import ThresholdStability: midpoint, shift

@testset "Midpoints" begin
      @test midpoint(1, 2, 1) == 1.5
      @test midpoint(2, 3, 1) == 2.5
      @test midpoint(Inf, 2., 1.) == 1.
      @test midpoint(1., Inf, 1.) == 2.
      @test midpoint(Inf, Inf, 1.) == 0.
end

C = [1 2 -3 4 5 6;
     -6 5 4 -3 2 1;
     9 -8 7 6 5 4]
Cstar = [1 0; -1 1; 0 1]
Fstar = [1; -1; 0]
βtilde = [0, -1]

@testset "CKSVAR" begin
            Σ, X = CKSVAR_to_TAR(C, Cstar, βtilde, 2)
         @test Σ == [[1.0 2.0 -2.0 4.0 5.0 6.0; -6.0 5.0 3.0 -3.0 2.0 2.0; 9.0 -8.0 7.0 6.0 5.0 5.0; 1.0 0.0 0.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 0.0 0.0; 0.0 0.0 1.0 0.0 0.0 0.0],
               [1.0 2.0 -2.0 4.0 5.0 0.0
         -6.0 5.0 3.0 -3.0 2.0 1.0; 9.0 -8.0 7.0 6.0 5.0 1.0; 1.0 0.0 0.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0 0.0 0.0; 0.0 0.0 1.0 0.0 0.0 0.0],
         [1.0 2.0 3.0 4.0 5.0 6.0; -6.0 5.0 4.0 -3.0 2.0 2.0; 9.0 -8.0 -8.0 6.0 5.0 5.0; 1.0 0.0 0.0 0.0 0.0 0.0; 0.0 1.0 1.0 0.0 0.0 0.0; 0.0 0.0 1.0 0.0 0.0 0.0],
          [1.0 2.0 3.0 4.0 5.0 0.0; -6.0 5.0 4.0 -3.0 2.0 1.0
           9.0 -8.0 -8.0 6.0 5.0 1.0; 1.0 0.0 0.0 0.0 0.0 0.0; 0.0 1.0 1.0 0.0 0.0 0.0
           0.0 0.0 1.0 0.0 0.0 0.0]]
           @test X == [[[0.0 0.0 1.0 0.0 0.0 0.0; 0.0 0.0 0.0 0.0 0.0 1.0], zeros(1, 6)],
     [[0.0 0.0 1.0 0.0 0.0 0.0; 0.0 0.0 0.0 0.0 0.0 -1.0], zeros(1, 6)],
     [[0.0 0.0 -1.0 0.0 0.0 0.0; 0.0 0.0 0.0 0.0 0.0 1.0], zeros(1, 6)],
     [[0.0 0.0 -1.0 0.0 0.0 0.0; 0.0 0.0 0.0 0.0 0.0 -1.0], zeros(1, 6)]]
     @test CKSVAR_to_companion(C, Cstar, βtilde, 2) == [[1.0 2.0 -2.0 4.0 5.0 6.0 0.0; -6.0 5.0 3.0 -3.0 2.0 1.0 1.0;
           9.0 -8.0 7.0 6.0 5.0 4.0 1.0; 1.0 0.0 0.0 0.0 0.0 0.0 0.0;
           0.0 1.0 0.0 0.0 0.0 0.0 0.0; 0.0 0.0 1.0 0.0 0.0 0.0 0.0;
           0.0 0.0 1.0 0.0 0.0 0.0 0.0],
           [1.0 2.0 3.0 4.0 5.0 6.0 0.0; -6.0 5.0 4.0 -3.0 2.0 1.0 1.0;
            9.0 -8.0 -8.0 6.0 5.0 4.0 1.0; 1.0 0.0 0.0 0.0 0.0 0.0 0.0;
            0.0 1.0 1.0 0.0 0.0 0.0 0.0; 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
            0.0 0.0 1.0 0.0 0.0 0.0 0.0]]
      @test CKSVAR_to_companionFD(C, Fstar, βtilde, 2) == [[1.0 2.0 -2.0 4.0 5.0 0.0; -6.0 5.0 3.0 -3.0 2.0 0.0;
        9.0 -8.0 8.0 6.0 5.0 0.0; 1.0 0.0 0.0 0.0 0.0 0.0;
        0.0 1.0 -0.0 0.0 0.0 0.0; 0.0 0.0 1.0 0.0 0.0 0.0],
       [1.0 2.0 3.0 4.0 5.0 0.0; -6.0 5.0 4.0 -3.0 2.0 0.0;
         9.0 -8.0 -7.0 6.0 5.0 0.0; 1.0 0.0 -0.0 0.0 0.0 0.0;
         0.0 1.0 1.0 0.0 0.0 0.0; 0.0 0.0 0.0 0.0 0.0 0.0]]
      @test CKSVAR_to_companionFD(C, Fstar, βtilde, 2, diff=false) == [[1.0 2.0 -2.0 4.0 5.0 3.0 -1.0; -6.0 5.0 3.0 -3.0 2.0 -4.0 1.0;
        9.0 -8.0 8.0 6.0 5.0 -7.0 0.0;  1.0 0.0 0.0 0.0 0.0 0.0 0.0;
        0.0 1.0 0.0 0.0 0.0 0.0 0.0;  0.0 0.0 1.0 0.0 0.0 0.0 0.0;
        0.0 0.0 1.0 0.0 0.0 0.0 0.0],
       [1.0 2.0 3.0 4.0 5.0 3.0 -1.0; -6.0 5.0 3.0 -3.0 2.0 -4.0 1.0;
        9.0 -8.0 -7.0 6.0 5.0 -7.0 0.0;  1.0 0.0 0.0 0.0 0.0 0.0 0.0;
        0.0 1.0 1.0 0.0 0.0 0.0 0.0;  0.0 0.0 0.0 0.0 0.0 0.0 0.0;
        0.0 0.0 1.0 0.0 0.0 0.0 0.0]]
end
