@testset "Sudoku" begin

    @testset "Sudoku MOI/Integers/Int8" begin
        grid = Int8[
            0 2 1 0 7 9 0 8 5
            0 4 5 3 1 0 0 0 9
            0 7 0 0 4 0 0 1 0
            0 0 0 1 0 8 0 3 6
            0 6 0 0 0 0 2 0 8
            0 0 0 0 0 3 0 0 4
            6 0 8 0 0 0 0 0 0
            0 9 4 0 0 7 8 0 0
            2 0 0 5 0 0 0 4 0
        ]
        grid .-= 6
        grid[grid .== 3] .= 10
        grid[grid .== -2] .= 7

        m = CS.Optimizer()
        # don't use 1-9 here but some other integers to test offset and alldifferent without all numbers
        x = [
            [
                MOI.add_constrained_variable(
                    m,
                    CS.Integers([-5, -4, -3, 7, -1, 0, 1, 2, 10]),
                ) for i in 1:9
            ] for j in 1:9
        ]

        # set variables
        for r in 1:9, c in 1:9
            if grid[r, c] != -6
                sat = [MOI.ScalarAffineTerm(Int8(1), x[r][c][1])]
                MOI.add_constraint(
                    m,
                    MOI.ScalarAffineFunction{Int8}(sat, 0),
                    MOI.EqualTo(grid[r, c]),
                )
            end
        end

        # sudoku constraints
        moi_add_sudoku_constr!(m, x)
        MOI.set(m, MOI.TimeLimitSec(), 10.0)

        MOI.optimize!(m)
        @test MOI.get(m, MOI.TerminationStatus()) == MOI.OPTIMAL
        solution = zeros(Int, 9, 9)
        for r in 1:9
            solution[r, :] = [MOI.get(m, MOI.VariablePrimal(), x[r][c][1]) for c in 1:9]
        end
        @test jump_fulfills_sudoku_constr(solution)
        @test CS.get_inner_model(m).options.time_limit == 10.0
    end

    @testset "Hard sudoku with table constraint" begin
        grid = zeros(Int, (9, 9))

        grid[1, :] = [3 8 0 6 0 0 0 0 0]
        grid[2, :] = [0 0 9 0 0 0 0 0 0]
        grid[3, :] = [0 2 0 0 3 0 5 1 0]
        grid[4, :] = [0 0 0 0 0 5 0 0 0]
        grid[5, :] = [0 3 0 0 1 0 0 6 0]
        grid[6, :] = [0 0 0 4 0 0 0 0 0]
        grid[7, :] = [0 1 7 0 5 0 0 8 0]
        grid[8, :] = [0 0 0 0 0 0 9 0 0]
        grid[9, :] = [0 0 0 0 0 7 0 3 2]

        offset = -2
        grid .+= offset

        m = Model(optimizer_with_attributes(CS.Optimizer))
        @variable(m, 1 + offset <= x[1:9, 1:9] <= 9 + offset, Int)
        # set variables
        nvars_set = 0
        for r in 1:9, c in 1:9
            if grid[r, c] != offset
                @constraint(m, x[r, c] == grid[r, c])
                nvars_set += 1
            end
        end

        table = Array{Int64}(undef, (factorial(9), 9))
        i = 1
        for row in permutations(1:9)
            table[i, :] = row .+ offset
            i += 1
        end

        # sudoku constraints
        for rc in 1:9
            @constraint(m, x[rc, :] in CS.TableSet(table))
            @constraint(m, x[:, rc] in CS.TableSet(table))
        end

        for br in 0:2
            for bc in 0:2
                @constraint(
                    m,
                    vec(x[(br * 3 + 1):((br + 1) * 3), (bc * 3 + 1):((bc + 1) * 3)]) in
                    CS.TableSet(table)
                )
            end
        end

        optimize!(m)
        @test JuMP.termination_status(m) == MOI.OPTIMAL
        @test jump_fulfills_sudoku_constr(JuMP.value.(x))
    end

    @testset "Hard sudoku infeasible" begin
        grid = [
            0 0 0 5 4 6 0 0 9
            0 2 0 0 0 0 0 0 7
            0 0 3 9 0 0 0 0 4
            9 0 5 0 0 0 0 7 3
            7 0 0 0 0 0 0 2 0
            0 0 0 0 9 3 0 0 0
            0 5 6 0 0 8 0 0 0
            0 1 0 0 3 9 0 0 0
            0 0 0 0 0 0 8 0 6
        ]

        m = Model(CSJuMPTestOptimizer(; branch_strategy = :ABS))
        @variable(m, 1 <= x[1:9, 1:9] <= 9, Int)
        # set variables
        for r in 1:9, c in 1:9
            if grid[r, c] != 0
                @constraint(m, x[r, c] == grid[r, c])
            end
        end
        # sudoku constraints
        jump_add_sudoku_constr!(m, x)

        optimize!(m)
        @test JuMP.termination_status(m) == MOI.INFEASIBLE
    end

    @testset "Hard fsudoku repo" begin
        grid = zeros(Int, (9, 9))
        grid[1, :] = [0 0 0 0 0 0 0 0 0]
        grid[2, :] = [0 1 0 6 2 0 0 9 0]
        grid[3, :] = [0 0 2 0 0 9 3 1 0]
        grid[4, :] = [0 0 4 0 0 6 0 8 0]
        grid[5, :] = [0 0 8 7 0 2 1 0 0]
        grid[6, :] = [0 3 0 8 0 0 5 0 0]
        grid[7, :] = [0 6 9 1 0 0 4 0 0]
        grid[8, :] = [0 8 0 0 7 3 0 5 0]
        grid[9, :] = [0 0 0 0 0 0 0 0 0]

        m = Model(CSJuMPTestOptimizer())
        @variable(m, 1 <= x[1:9, 1:9] <= 9, Int)
        # set variables
        nvars_set = 0
        for r in 1:9, c in 1:9
            if grid[r, c] != 0
                @constraint(m, x[r, c] == grid[r, c])
                nvars_set += 1
            end
        end

        @test nvars_set == length(filter(n -> n != 0, grid))

        # sudoku constraints
        jump_add_sudoku_constr!(m, x)

        optimize!(m)

        @test JuMP.termination_status(m) == MOI.OPTIMAL
        com = CS.get_inner_model(m)
        @test_reference "refs/hard_fsudoku" test_string([
            constraint.indices for constraint in com.constraints
        ])

        # check that it actually solves the given sudoku
        for r in 1:9, c in 1:9
            if grid[r, c] != 0
                @test JuMP.value(x[r, c]) == grid[r, c]
            end
        end

        @test jump_fulfills_sudoku_constr(JuMP.value.(x))
    end

    @testset "Infeasible sudoku at start" begin
        grid = zeros(Int, (9, 9))
        grid[1, :] = [0 0 0 0 0 0 0 0 0]
        grid[2, :] = [0 1 0 6 2 0 0 9 0]
        grid[3, :] = [0 0 2 0 0 9 3 1 0]
        grid[4, :] = [0 0 4 0 0 6 0 8 0]
        grid[5, :] = [0 0 8 7 0 2 1 0 0]
        grid[6, :] = [0 0 0 0 0 0 5 0 0]
        grid[7, :] = [0 6 9 1 0 0 2 1 0]
        grid[8, :] = [0 0 0 0 7 0 0 5 0]
        grid[9, :] = [0 0 1 0 0 0 0 0 0]

        m = Model(CSJuMPTestOptimizer())
        @variable(m, 1 <= x[1:9, 1:9] <= 9, Int)
        # set variables
        nvars_set = 0
        for r in 1:9, c in 1:9
            if grid[r, c] != 0
                @constraint(m, x[r, c] == grid[r, c])
                nvars_set += 1
            end
        end

        # sudoku constraints
        jump_add_sudoku_constr!(m, x)

        optimize!(m)

        @test JuMP.termination_status(m) == MOI.INFEASIBLE
    end

    @testset "top95 some use backtracking" begin
        grids = sudokus_from_file("data/top95")
        c = 0
        for grid in grids
            m = Model(optimizer_with_attributes(
                CS.Optimizer,
                "solution_type" => Int8,
                "logging" => [],
            ))

            @variable(m, 1 <= x[1:9, 1:9] <= 9, Int)
            # set variables
            for r in 1:9, c in 1:9
                if grid[r, c] != 0
                    @constraint(m, x[r, c] == grid[r, c])
                end
            end

            # sudoku constraints
            jump_add_sudoku_constr!(m, x)
            @objective(m, Min, x[1, 1])

            optimize!(m)
            com = CS.get_inner_model(m)

            @test typeof(com.best_sol) == Int8
            @test JuMP.objective_value(m) == JuMP.value(x[1, 1]) == com.best_sol
            @test JuMP.termination_status(m) == MOI.OPTIMAL
            @test is_solved(com)
            c += 1
        end
        # check that actually all 95 problems were tested
        @test c == 95
    end

    @testset "Number 7 in top95.txt w/o backtracking" begin
        m = Model(optimizer_with_attributes(
            CS.Optimizer,
            "backtrack" => false,
            "logging" => [],
        ))

        grid = Int[
            6 0 2 0 5 0 0 0 0
            0 0 0 0 0 3 0 4 0
            0 0 0 0 0 0 0 0 0
            4 3 0 0 0 8 0 0 0
            0 1 0 0 0 0 2 0 0
            0 0 0 0 0 0 7 0 0
            5 0 0 2 7 0 0 0 0
            0 0 0 0 0 0 0 8 1
            0 0 0 6 0 0 0 0 0
        ]
        grid = transpose(reshape(grid, (9, 9)))

        @variable(m, 1 <= x[1:9, 1:9] <= 9, Int)
        # set variables
        for r in 1:9, c in 1:9
            if grid[r, c] != 0
                @constraint(m, x[r, c] == grid[r, c])
            end
        end

        # sudoku constraints
        jump_add_sudoku_constr!(m, x)

        optimize!(m)

        com = CS.get_inner_model(m)
        @test JuMP.termination_status(m) == MOI.OTHER_LIMIT

        @test !com.info.backtracked
        @test com.info.backtrack_fixes == 0
        @test com.info.in_backtrack_calls == 0
        @show com.info
    end

    @testset "Two solutions" begin
        m = Model(optimizer_with_attributes(
            CS.Optimizer,
            "all_solutions" => true,
            "logging" => [],
        ))

        grid = Int[
            9 0 6 0 7 0 4 0 3
            0 0 0 4 0 0 2 0 0
            0 7 0 0 2 3 0 1 0
            5 0 0 0 0 0 1 0 0
            0 4 0 2 0 8 0 6 0
            0 0 3 0 0 0 0 0 5
            0 3 0 7 0 0 0 5 0
            0 0 7 0 0 5 0 0 0
            4 0 5 0 1 0 7 0 8
        ]

        @variable(m, 1 <= x[1:9, 1:9] <= 9, Int)
        # set variables
        for r in 1:9, c in 1:9
            if grid[r, c] != 0
                @constraint(m, x[r, c] == grid[r, c])
            end
        end

        # sudoku constraints
        jump_add_sudoku_constr!(m, x)

        optimize!(m)

        com = CS.get_inner_model(m)

        @test JuMP.result_count(m) == 2
        @test JuMP.value.(x) != JuMP.value.(x, result = 2)
        if JuMP.value.(x[6, 5:6]) == [9, 4]
            @test JuMP.value.(x[7, 5:6]) == [4, 9]
        else
            @test JuMP.value.(x[6, 5:6]) == [4, 9]
            @test JuMP.value.(x[7, 5:6]) == [9, 4]
        end
    end

    @testset "Two solutions but optimize on one value" begin
        # all solutions should give all solutions even if they are not optimal
        m = Model(optimizer_with_attributes(
            CS.Optimizer,
            "all_solutions" => true,
            "logging" => [],
        ))

        grid = Int[
            9 0 6 0 7 0 4 0 3
            0 0 0 4 0 0 2 0 0
            0 7 0 0 2 3 0 1 0
            5 0 0 0 0 0 1 0 0
            0 4 0 2 0 8 0 6 0
            0 0 3 0 0 0 0 0 5
            0 3 0 7 0 0 0 5 0
            0 0 7 0 0 5 0 0 0
            4 0 5 0 1 0 7 0 8
        ]

        @variable(m, 1 <= x[1:9, 1:9] <= 9, Int)
        # set variables
        for r in 1:9, c in 1:9
            if grid[r, c] != 0
                @constraint(m, x[r, c] == grid[r, c])
            end
        end

        # sudoku constraints
        jump_add_sudoku_constr!(m, x)
        @objective(m, Max, x[6, 5])

        optimize!(m)

        com = CS.get_inner_model(m)

        @test JuMP.result_count(m) == 2
        @test JuMP.value.(x) != JuMP.value.(x, result = 2)
        # the better one should be solution 1 of course
        @test JuMP.objective_value(m) == 9
        @test JuMP.value.(x[6, 5:6]) == [9, 4]
        @test JuMP.value.(x[7, 5:6]) == [4, 9]
        # second worse solution
        @test JuMP.objective_value(m, result = 2) == 4
        @test JuMP.value.(x[6, 5:6], result = 2) == [4, 9]
        @test JuMP.value.(x[7, 5:6], result = 2) == [9, 4]
    end

    @testset "Two solutions but only one optimal" begin
        # all solutions should give all solutions even if they are not optimal
        m = Model(optimizer_with_attributes(
            CS.Optimizer,
            "all_optimal_solutions" => true,
            "logging" => [],
        ))

        grid = Int[
            9 0 6 0 7 0 4 0 3
            0 0 0 4 0 0 2 0 0
            0 7 0 0 2 3 0 1 0
            5 0 0 0 0 0 1 0 0
            0 4 0 2 0 8 0 6 0
            0 0 3 0 0 0 0 0 5
            0 3 0 7 0 0 0 5 0
            0 0 7 0 0 5 0 0 0
            4 0 5 0 1 0 7 0 8
        ]

        @variable(m, 1 <= x[1:9, 1:9] <= 9, Int)
        # set variables
        for r in 1:9, c in 1:9
            if grid[r, c] != 0
                @constraint(m, x[r, c] == grid[r, c])
            end
        end

        # sudoku constraints
        jump_add_sudoku_constr!(m, x)
        @objective(m, Max, x[6, 5])

        optimize!(m)

        com = CS.get_inner_model(m)

        @test JuMP.result_count(m) == 1
        # the better one should be solution 1 of course
        @test JuMP.objective_value(m) == 9
        @test JuMP.value.(x[6, 5:6]) == [9, 4]
        @test JuMP.value.(x[7, 5:6]) == [4, 9]
    end

    @testset "Two solutions but both optimal" begin
        # all solutions should give all solutions even if they are not optimal
        m = Model(optimizer_with_attributes(
            CS.Optimizer,
            "all_optimal_solutions" => true,
            "logging" => [],
        ))

        grid = Int[
            9 0 6 0 7 0 4 0 3
            0 0 0 4 0 0 2 0 0
            0 7 0 0 2 3 0 1 0
            5 0 0 0 0 0 1 0 0
            0 4 0 2 0 8 0 6 0
            0 0 3 0 0 0 0 0 5
            0 3 0 7 0 0 0 5 0
            0 0 7 0 0 5 0 0 0
            4 0 5 0 1 0 7 0 8
        ]

        @variable(m, x[1:9, 1:9], CS.Integers(1:9))
        # set variables
        for r in 1:9, c in 1:9
            if grid[r, c] != 0
                @constraint(m, x[r, c] == grid[r, c])
            end
        end

        # sudoku constraints
        jump_add_sudoku_constr!(m, x)
        @objective(m, Max, x[1, 1])

        optimize!(m)

        com = CS.get_inner_model(m)
        @test com.info.n_constraint_types.alldifferent == 27
        @test length(com.constraints) == 27
        @test length(com.search_space) == 81

        # the better one should be solution 1 of course
        @test JuMP.objective_value(m) == 9
        @test JuMP.result_count(m) == 2
        @test JuMP.value.(x) != JuMP.value.(x, result = 2)
        if JuMP.value.(x[6, 5:6]) == [9, 4]
            @test JuMP.value.(x[7, 5:6]) == [4, 9]
        else
            @test JuMP.value.(x[6, 5:6]) == [4, 9]
            @test JuMP.value.(x[7, 5:6]) == [9, 4]
        end
    end

    @testset "All optimal solutions" begin
        n = 9
        g = 3

        grid = zeros(Int, (n, n))

        m = Model(optimizer_with_attributes(
            CS.Optimizer,
            "all_optimal_solutions" => true,
            "time_limit" => 1.0,
            "keep_logs" => true,
            "logging" => [],
        ))

        @variable(m, 1 <= x[1:n, 1:n] <= n, Int)
        # set variables
        for r in 1:n, c in 1:n
            if grid[r, c] != 0
                @constraint(m, x[r, c] == grid[r, c])
            end
        end
        for rc in 1:n
            @constraint(m, x[rc, :] in CS.AllDifferentSet())
            @constraint(m, x[:, rc] in CS.AllDifferentSet())
        end
        for br in 0:(g - 1)
            for bc in 0:(g - 1)
                @constraint(
                    m,
                    vec(x[(br * g + 1):((br + 1) * g), (bc * g + 1):((bc + 1) * g)]) in
                    CS.AllDifferentSet()
                )
            end
        end

        optimize!(m)
        @test MOI.get(m, MOI.SolveTime()) >= 1.0
        # at least more than 1 but in that time frame it should find a lot ;)
        @test MOI.get(m, MOI.ResultCount()) >= 10
        @test JuMP.termination_status(m) == MOI.TIME_LIMIT
        com = CS.get_inner_model(m)
        general_tree_test(com)
        logs = CS.get_logs(com)
        @test CS.sanity_check_log(logs[:tree])
    end

    @testset "All solutions" begin
        n = 9
        g = 3

        grid = zeros(Int, (n, n))

        m = Model(optimizer_with_attributes(
            CS.Optimizer,
            "all_solutions" => true,
            "time_limit" => 1.0,
            "logging" => [],
        ))

        @variable(m, 1 <= x[1:n, 1:n] <= n, Int)
        # set variables
        for r in 1:n, c in 1:n
            if grid[r, c] != 0
                @constraint(m, x[r, c] == grid[r, c])
            end
        end
        for rc in 1:n
            @constraint(m, x[rc, :] in CS.AllDifferentSet())
            @constraint(m, x[:, rc] in CS.AllDifferentSet())
        end
        for br in 0:(g - 1)
            for bc in 0:(g - 1)
                @constraint(
                    m,
                    vec(x[(br * g + 1):((br + 1) * g), (bc * g + 1):((bc + 1) * g)]) in
                    CS.AllDifferentSet()
                )
            end
        end

        optimize!(m)
        @test MOI.get(m, MOI.SolveTime()) >= 1.0
        # at least more than 1 but in that time frame it should find a lot ;)
        @test MOI.get(m, MOI.ResultCount()) >= 10
        @test JuMP.termination_status(m) == MOI.TIME_LIMIT
    end
end


import Gen
import Distributions
import Rotations

const Rot3 = Rotations.UnitQuaternion{Float64}

# area of 3-sphere times two
total_area(::Type{Rot3}) = pi * pi

@inline function _to_array(x::Rot3)
    v = Rotations.vector(x)
    return Float64[Rotations.scalar(x), v[1], v[2], v[3]]
end

export Rot3

################################################
# uniform distribution (Haar measure on SO(3)) #
################################################

struct UniformRot3 <: Gen.Distribution{Rot3} end

const uniform_rot3 = UniformRot3()

function Gen.logpdf(::UniformRot3, x::Rot3)
    return -log(total_area(Rot3))
end

function Gen.logpdf_grad(::UniformRot3, x::Rot3)
    error("Not implemented") # TODO implement
    return (nothing,)
end

function Gen.random(::UniformRot3)
    w = randn()
    x = randn()
    y = randn()
    z = randn()
    return Rotations.UnitQuaternion{Float64}(w, x, y, z, true)
end

Gen.has_output_grad(::UniformRot3) = false

Gen.has_argument_grads(::UniformRot3) = ()

(::UniformRot3)() = Gen.random(UniformRot3())

export uniform_rot3

################################################
# von Mises Fisher based distribution on SO(3) #
################################################

struct VMFRot3 <: Gen.Distribution{Rot3} end

const vmf_rot3 = VMFRot3()

function Gen.logpdf(::VMFRot3, x::Rot3, mu::Rot3, k::Real)
    dist = Distributions.VonMisesFisher(_to_array(mu), k)
    x_unit_vec = _to_array(x)
    # mixture of anti-podal Von Mises Fisher distributions
    # NOTE: 0.5 is indeed not needed, because of how the base measure is defined
    l1 = Distributions.logpdf(dist, x_unit_vec)
    l2 = Distributions.logpdf(dist, -x_unit_vec)
    return _logsumexp(l1, l2)
end

function Gen.logpdf_grad(::VMFRot3, x::Rot3, mu::Rot3, k::Real)
    error("Not implemented") # TODO implement
    return (nothing,nothing,nothing)
end

function Gen.random(::VMFRot3, mu::Rot3, k::Real)
    dist = Distributions.VonMisesFisher(_to_array(mu), k)
    v = rand(dist)
    return Rotations.UnitQuaternion(v[1], v[2], v[3], v[4])
end

Gen.has_output_grad(::VMFRot3) = false

Gen.has_argument_grads(::VMFRot3) = (false, false)

(::VMFRot3)(mu, k) = Gen.random(VMFRot3(), mu, k)

export vmf_rot3

###########################################################################
# mixture of von Mises Fisher based distribution and uniform distribution #
###########################################################################

struct UniformVMFRot3 <: Gen.Distribution{Rot3} end

const uniform_vmf_rot3 = UniformVMFRot3()

function Gen.logpdf(
        ::UniformVMFRot3, x::Rot3, mu::Rot3,
        k::Real, prob_uniform::Real)
    lp_vmf = logpdf(vmf_rot3, x, mu, k) + log(1.0 - prob_uniform)
    lp_uniform = logpdf(uniform_rot3, x) + log(prob_uniform)
    return _logsumexp(lp_vmf, lp_uniform)
end

function Gen.logpdf_grad(::UniformVMFRot3, x::Rot3, mu::Rot3, k::Real)
    error("Not implemented") # TODO implement
    return (nothing, nothing, nothing, nothing)
end

function Gen.random(
        ::UniformVMFRot3, mu::Rot3,
        k::Real, prob_uniform::Real)
    if Gen.bernoulli(prob_uniform)
        return uniform_rot3()
    else
        return vmf_rot3(mu, k)
    end
end

Gen.has_output_grad(::UniformVMFRot3) = false

Gen.has_argument_grads(::UniformVMFRot3) = (false, false, false, false)

function (::UniformVMFRot3)(mu, k, prob_uniform)
    return Gen.random(UniformVMFRot3(), mu, k, prob_uniform)
end

export uniform_vmf_rot3


#####
##### transformations
#####

"""
$(TYPEDEF)

Abstract type used internally for simplifying forwarding. Assumes a slot `ℓ`, and forwards
`dimension` to it.
"""
abstract type LogDensityTransformation <: SamplingLogDensity end

dimension(ℓ::LogDensityTransformation) = dimension(ℓ.ℓ)

####
#### linear transformation
####

struct Linear{L,M,S,T} <: LogDensityTransformation
    ℓ::L
    A::M
    divA::S
    logabsdetA::T
end

"""
$(SIGNATURES)

Internal method for returning an object that makes `A \\ x` fast and stable.
"""
_fastdiv(A::AbstractMatrix) = lu(A)
_fastdiv(A::AbstractTriangular) = A
_fastdiv(A::Diagonal) = A

"""
$(SIGNATURES)

Internal method for logabsdet, work around
https://github.com/JuliaLang/julia/issues/32988
"""
_logabsdet(A::AbstractMatrix) = first(logabsdet(A))
_logabsdet(A::Diagonal) = sum(log ∘ abs, diag(A))

"""
$(SIGNATURES)

Transform a distribution on `x` to `y = Ax`, where `A` is a conformable square matrix.

Since the log Jacobian is constant, it is dropped in the log density.
"""
function linear(A::AbstractMatrix, ℓ)
    K = dimension(ℓ)
    @argcheck checksquare(A) == K
    Linear(ℓ, A, _fastdiv(A), _logabsdet(A))
end

logdensity(ℓ::Linear, x) = logdensity(ℓ.ℓ, ℓ.divA \ x) - ℓ.logabsdetA

function logdensity_and_gradient(ℓ::Linear, x)
    f, ∇f = logdensity_and_gradient(ℓ.ℓ, ℓ.divA \ x)
    f - ℓ.logabsdetA, (ℓ.divA') \ ∇f # note here \ is needed because / is not defined for LU
end

hypercube_transform(ℓ::Linear, x) = ℓ.A * hypercube_transform(ℓ.ℓ, x)

####
#### shift (translation)
####

struct Shift{L, T <: AbstractVector} <: LogDensityTransformation
    ℓ::L
    b::T
end

"""
$(SIGNATURES)

Transform a distribution on `x` to `y = x + b`, where `b` is a conformable vector.
"""
function shift(b::AbstractVector, ℓ)
    @argcheck length(b) == dimension(ℓ)
    Shift(ℓ, b)
end

logdensity(ℓ::Shift, x) = logdensity(ℓ.ℓ, x - ℓ.b)

# The log Jacobian adjustment is zero
logdensity_and_gradient(ℓ::Shift, x) = logdensity_and_gradient(ℓ.ℓ, x - ℓ.b)

hypercube_transform(ℓ::Shift, x) = ℓ.b .+ hypercube_transform(ℓ.ℓ, x)

####
#### elongate
####

struct Elongate{L, T <: Real} <: LogDensityTransformation
    ℓ::L
    k::T
end

"""
$(SIGNATURES)

Transform a distribution on `x` to ``y = (1 + ‖x‖²)ᵏ⋅x`` where ``‖ ‖`` is the Euclidean norm
and `k` is a real number. `k > 0` values make the tails heavier.
"""
function elongate(k::Real, ℓ)
    Elongate(ℓ, k)
end

"""
$(SIGNATURES)

Helper function that solves
```math
y = x ⋅ (1 + x²)ᵏ
```
for `x`, given `y ≥ 0` and `k`, using Newton's method.

Absolute tolerance `atol` should not be set *too* low to avoid occasional numerical cycles
near the root.
"""
function _find_x_norm(y, k; x = zero(y), atol = 16 * eps(y))
    for _ in 1:100
        x2 = abs2(x)
        A = (1 + x2)^k
        f = A * x - y
        abs(f) ≤ atol && return x
        B = 2 * k * x2 * (1 + x2)^(k - 1)
        x = (B * x + y) / (A + B)
    end
    error("internal error: reached maximum number of iterations, y = $(y), k = $(k)")
end

function logdensity_and_gradient(ℓ::Elongate, y)
    @unpack ℓ, k = ℓ
    d = dimension(ℓ)
    ynorm = norm(y, 2)
    xnorm = _find_x_norm(ynorm, k)
    xnorm2 = abs2(xnorm)
    D = (1 + xnorm2)^(-k)       # x = D ⋅ y
    x = D .* y
    ℓx, ∇ℓx = logdensity_and_gradient(ℓ, x)
    ℓy = ℓx - (k*d - 1) * log1p(xnorm2) - log1p((1 + 2*k) * xnorm2)
    A = 1 + xnorm2
    B = 1 + (1 + 2*k) * xnorm2
    L1 = (I - (2 * k / B) .* (x * x')) * ∇ℓx .* D
    L2 = (((k * d - 1)/A + (1 + 2 * k)/B) * 2 * A^(1 - 2*k) / B) .* y
    ℓy, L1 .- L2
end

function hypercube_transform(ℓ::Elongate, z)
    @unpack ℓ, k = ℓ
    x = hypercube_transform(ℓ, z)
    (1 + sum(abs2, x))^k .* x
end


function part1(input)
    map = readlines(input)
    return trees(map, 3, 1)
end

function part2(input)
    map = readlines(input)
    slopes = [(1, 1), (3, 1), (5, 1), (7, 1), (1, 2)]
    return reduce(*, [trees(map, slope...) for slope in slopes])
end

function trees(map, slope_x, slope_y)
    width = length(map[1])
    x, y = 1, 1
    trees = 0
    while y <= length(map)
        trees += map[y][mod1(x, width)] == '#'
        x += slope_x
        y += slope_y
    end
    return trees
end


using Plots
using Random: randn;

@testset "plot wavelet transformed" begin
    X = randn(1024);
    wt = ContinuousWaveletTransform(CauchyWavelet(10), LinRange(2,200,64))
    wX = transform(wt, X)
    @test_nowarn contourf(wX)
end

@testset "plot coherence" begin
    N, M = 1024, 10;
    X = zeros(Float64, N, M)
    Y = zeros(Float64, N, M)
    for i in 1:M
        X[:,i] = map(t -> sin(2π*t/50), 1:N) + randn(N)
        Y[:,i] = map(t -> sin(2π*t/50), 1:N) + randn(N)
    end
    wt = ContinuousWaveletTransform(CauchyWavelet(10), LinRange(2,200,64))
    C = coherence(wt, X,Y)
    @test_nowarn contourf(C);
end


#
# A Lee Group as a trait to manifolds
#
#
# Manopt.jl, R. Bergmann, 2018-07-07
export IsLieGroupM, IsLieGroupP, IsLieGroupV, ⊗

"""
    IsLieGroupM{X}
Indicates that `X` is a [`Manifold`](@ref) with a Lie group structure. This also
introdcues a group operation [`⊗`](@ref) of two [`MPoint`](@ref)s of `X`.
"""
@traitdef IsLieGroupM{X}
"""
    IsLieGroupP{X}
An abstract [`MPoint`](@ref) belonging to a Lie group manifold.
"""
@traitdef IsLieGroupP{X}
"""
    IsLieGroupV{X}
An abstract [`TVector`](@ref) belonging to a Lie group manifold.
"""
@traitdef IsLieGroupV{X}
@doc doc"""
    ⊗(x,y)

the binary operator `x ⊗ y` represents the Lie group action for a Lie group
[`Manifold`](@ref), see [`IsLieGroupM`](@ref).
"""
⊗(x::P,y::P) where {P <: MPoint}= LieGroupOp(x,y)
@traitfn function LieGroupOp(x::P,y::P) where {P <: MPoint; !IsLieGroupP{P}}
    sig1 = string( typeof(x) )
    throw( ErrorException("The manifold point of type $sig1 does not belong to a Lie Group") );
end


@testset "Krylov Trust Region" begin

@testset "Toy test problem 1" begin
    # Test on actual optimization problems.
    function f(x::Vector)
        (x[1] - 5.0)^4
    end

    function fg!(g, x)
        g[1] = 4.0 * (x[1] - 5.0)^3
        f(x)
    end

    function fg2!(_f, g, x)
        g  === nothing || (g[1] = 4.0 * (x[1] - 5.0)^3)
        _f === nothing || return f(x)
    end

    function hv!(Hv, x, v)
        Hv[1] = 12.0 * (x[1] - 5.0)^2 * v[1]
    end

    d = Optim.TwiceDifferentiableHV(f, fg!, hv!, [0.0])
    d2 = Optim.TwiceDifferentiableHV(NLSolversBase.only_fg_and_hv!(fg2!, hv!), [0.0])

    result = Optim.optimize(d, [0.0], Optim.KrylovTrustRegion())
    @test norm(Optim.minimizer(result) - [5.0]) < 0.01
    result = Optim.optimize(d2, [0.0], Optim.KrylovTrustRegion())
    @test norm(Optim.minimizer(result) - [5.0]) < 0.01
end

@testset "Toy test problem 2" begin
    eta = 0.9

    function f2(x::Vector)
        0.5 * (x[1]^2 + eta * x[2]^2)
    end

    function fg2!(storage::Vector, x::Vector)
        storage[:] = [x[1], eta * x[2]]
        f2(x)
    end

    function hv2!(Hv::Vector, x::Vector, v::Vector)
        Hv[:] = [1.0 0.0; 0.0 eta] * v
    end

    d2 = Optim.TwiceDifferentiableHV(f2, fg2!, hv2!, Float64[127, 921])

    result = Optim.optimize(d2, Float64[127, 921], Optim.KrylovTrustRegion())
    @test result.g_converged
    @test norm(Optim.minimizer(result) - [0.0, 0.0]) < 0.01
end

@testset "Stock test problems" begin
    for (name, prob) in MultivariateProblems.UnconstrainedProblems.examples
      if prob.istwicedifferentiable
            hv!(storage::Vector, x::Vector, v::Vector) = begin
                n = length(x)
                H = Matrix{Float64}(undef, n, n)
                MVP.hessian(prob)(H, x)
                storage .= H * v
            end
            fg!(g::Vector, x::Vector) = begin
                MVP.gradient(prob)(g,x)
                MVP.objective(prob)(x)
            end
        ddf = Optim.TwiceDifferentiableHV(MVP.objective(prob), fg!, hv!, prob.initial_x)
        result = Optim.optimize(ddf, prob.initial_x, Optim.KrylovTrustRegion())
        @test norm(Optim.minimizer(result) - prob.solutions) < 1e-2
      end
    end
end

end


using IsingModels

d = Ising(100, isferromagnetic = true)

rand(d)


include("dipole.jl")

const c = 299792458.
const mu0 = 4*pi*1e-7
const eps0 = 1/(mu0*c^2)

#dipoles:
const N=1000;
const distance=1000+1000*rand(N)
const theta=2*acos(rand(N)-1)
const phi=2*pi*rand(N)
xyz = hcat(distance.*sin(theta).*cos(phi), distance.*sin(theta).*sin(phi), distance.*cos(theta))
#moments:
const th_dip=2*acos(rand(N)-1)
const phi_dip=2*pi*rand(N)

#dipole moment
#total time averaged radiated power P= 1 W dipole moment => |p|=sqrt(12πcP/µOω⁴)
const Pow=1
const amp_max=sqrt(12*pi*c*Pow/(mu0*(2*pi*freq)^4))/N
const amplitude=amp_max*rand(N)
const phase=2*pi*rand(N)
I=hcat(amplitude.*sin(th_dip).*cos(phi_dip), amplitude.*sin(th_dip).*sin(phi_dip), amplitude.*cos(th_dip))

const freq=1e9

#observation points
M=500
#observation points
x=rand(M)*(10)-5
y=rand(M)*(10)-5
z=rand(M)*(10)-5

println("Computing the radiation...")
Z=zeros(M)
for i=1:M
  perc=round(i/M*100)
  println("$perc %")
  r=[x[i],y[i],z[i]]
  E=zeros(Complex128,3)
  B=zeros(Complex128,3)
  for m=1:N
    p=vec(I[m,:])
    R=vec(xyz[m,:])
    Ed,Bd=Hertz_dipole (r, p, R, phase[m], freq)
    E+=Ed
    B+=Bd
  end
  Z[i]=sqrt(sum(abs(E).^2))/sqrt(sum(abs(B).^2))*mu0 #real(E).^2#0.5*numpy.cross(E.T,conjugate(B.T))
end

using PyPlot
plt.hist(Z)


@testset "$(rpad("Nonlinear solvers",80))" begin

    n = 1
    T = Float64

    function F(x::Vector, b::Vector)
        b[:] = x.^2
    end

    function J(x::Vector, A::Matrix)
        A[:,:] = 2x
    end

    for Solver in (NewtonSolver, QuasiNewtonSolver)
        x = ones(T, n)
        nl = Solver(x, F)
        solve!(nl)
        # println(nl.status.i, ", ", nl.status.rₐ,", ",  nl.status.rᵣ,", ",  nl.status.rₛ)
        for x in nl.x
            @test x ≈ 0 atol=1E-7
        end

        x = ones(T, n)
        nl = Solver(x, F, J=J)
        solve!(nl)
        # println(nl.status.i, ", ", nl.status.rₐ,", ",  nl.status.rᵣ,", ",  nl.status.rₛ)
        for x in nl.x
            @test x ≈ 0 atol=1E-7
        end
    end

end


@inline function C_se(CellData,s,z,Def)
    """
    Concentration Solid-Electrolyte Transfer Function
    # Add Ins and Outs
        # Cell Data
        # Frequency Vector
        # Discretisation Locations
        # Electrode Definition
    """

 if Def == "Pos"
    Electrode = CellData.Pos #Electrode Length
 else
    Electrode = CellData.Neg #Electrode Length
 end

κ_eff = CellData.Const.κ*Electrode.ϵ_e^Electrode.κ_brug #Effective Electrolyte Conductivity
σ_eff = Electrode.σ*Electrode.ϵ_s^Electrode.σ_brug #Effective Electrode Conductivity

#Defining SOC
θ = CellData.Const.SOC * (Electrode.θ_100-Electrode.θ_0) + Electrode.θ_0

#Beta's
β = @. Electrode.Rs*sqrt(s/Electrode.Ds)

#Prepare for j0
cs0 = Electrode.cs_max * θ

#Current Flux Density
if CellData.Const.CellTyp == "Doyle_94"
    κ = Electrode.k_norm/Electrode.cs_max/CellData.Const.ce0^(1-Electrode.α)
    j0 = κ*(CellData.Const.ce0*(Electrode.cs_max-cs0))^(1-Electrode.α)*cs0^Electrode.α
else
    j0 = Electrode.k_norm*(CellData.Const.ce0*(cs0/Electrode.cs_max*(1-cs0/Electrode.cs_max)))^(1-Electrode.α)
end

#Resistances
Rtot = R*CellData.Const.T/(j0*F^2) + Electrode.RFilm

#∂Uocp_Def
∂Uocp_elc = CellData.Const.∂Uocp(Def,θ)/Electrode.cs_max

cse_res = -3/(Electrode.as*F*Electrode.L*CellData.Const.CC_A*Electrode.Rs) #Residual Variable for Pole Removal - eq. 4.45
ν = @. Electrode.L*sqrt((Electrode.as/σ_eff+Electrode.as/κ_eff)/(Rtot+∂Uocp_elc*(Electrode.Rs/(F*Electrode.Ds))*(tanh(β)/(tanh(β)-β)))) #Condensing Variable - eq. 4.13

cse_tf = @. ν*Electrode.Rs*(σ_eff*cosh(ν*z)+κ_eff*cosh(ν*(z-1)))*tanh(β)/(Electrode.as*F*Electrode.L*CellData.Const.CC_A*Electrode.Ds*(κ_eff+σ_eff)*sinh(ν)*(tanh(β)-β)) #Transfer Function - eq. 4.17
cse_tf = @. cse_tf-cse_res/s  #Pole removal - eq. 4.43
zero_tf = @. (5*Electrode.as*Electrode.Ds*F*Electrode.L^2*(κ_eff*(2-6*z+3*z^2)+(3*z^2-1)*σ_eff)-6*∂Uocp_elc*Electrode.Rs*κ_eff*σ_eff)/(30*CellData.Const.CC_A*Electrode.as*Electrode.Ds*∂Uocp_elc*F*Electrode.L*κ_eff*σ_eff) #For s = 0 / Wolfram Alpha
cse_tf[:,findall(s.==0)] .= zero_tf[:,findall(s.==0)]
D = zeros(length(z))
D_term = "zeros(length($z))"

if Def == "Pos"
    cse_tf = -cse_tf
    cse_res = -cse_res

    if Debug == 1
        println("D:C_se:Pos",D)
        println("z:C_se:Pos",z)
        println("zero_tf:C_se:Pos",zero_tf[:,1])
        println("κ_eff:C_se:Pos",κ_eff)
        println("σ_eff:C_se:Pos",σ_eff)
        #println("ν_∞:C_se:Pos",ν_∞)
        println("j0:C_se:Pos",j0)
        println("as:C_se:Pos",Electrode.as)
        println("Rtot:C_se:Pos",Rtot)
        println("σ_eff:C_se:Pos",σ_eff)
        println("∂Uocp_elc:C_se:Pos",∂Uocp_elc)
        println("Electrode.L:C_se:Pos",Electrode.L)
    end
end
cse_res = cse_res*ones(length(z))
return cse_tf, D, cse_res, D_term

end


# Define a dummy function for automatic differentiation:
function G3(param::Array{T,1}) where {T <: Real}
return G3(param[1],param[2])
end

function G3(gamma::T,beta::T;gc=convert(T,0.5)) where {T <: Real}
sqb = sqrt(abs(beta))
#x = sqb*s
if gamma < gc
  return G3_series(gamma,beta)
else
  if beta >= 0
    return (gamma-sin(gamma))/(sqb*beta)
  else
    return (gamma-sinh(gamma))/(sqb*beta)
  end
end
end

y = zeros(2)
dG3 = y -> ForwardDiff.gradient(G3,y);

function H2(gamma::T,beta::T;gc=convert(T,0.5)) where {T <: Real}
sqb = sqrt(abs(beta))
#x=sqb*s
if gamma < gc
  return H2_series(gamma,beta)
else
  if beta >= 0
    return (sin(gamma)-gamma*cos(gamma))/(sqb*beta)
  else
    return (sinh(gamma)-gamma*cosh(gamma))/(sqb*beta)
  end
end
end

function H1(gamma::T,beta::T;gc=convert(T,0.5)) where {T <: Real}
#x=sqrt(abs(beta))*s
if gamma < gc
  return H1_series(gamma,beta)
else
  if beta >= 0
    return (4sin(0.5*gamma)^2 -gamma*sin(gamma))/beta^2
  else
    return (-4sinh(0.5*gamma)^2+gamma*sinh(gamma))/beta^2
  end
end
end

function G3_series(gamma::T,beta::T) where {T <: Real}
epsilon = eps(gamma)
# Computes G_3(\beta,s) using a series tailored to the precision of s.
#x2 = -beta*s^2
x2 = -sign(beta)*gamma^2
term = one(T)
g3 = one(T)
g31 = 2g3
g32 = 2g3
n=0
iter = 0
ITMAX = 100
# Terminate series when required precision reached:
#while abs(term) > epsilon*abs(g3)
while true
  g32 = g31
  g31 = g3
  n += 1
  term *= x2/((2n+3)*(2n+2))
  g3 += term
  iter +=1
  if iter >= ITMAX || g3 == g32 || g3 == g31
    break
  end
end
g3 *= gamma^3/(6*sqrt(abs(beta^3)))
return g3::T
end

dG3_series = y -> ForwardDiff.gradient(G3_series,y);

# Define a dummy function for automatic differentiation:
function G3_series(param::Array{T,1}) where {T <: Real}
return G3_series(param[1],param[2])
end

function H2_series(gamma::T,beta::T) where {T <: Real}
# Computes H_2(\beta,s) using a series tailored to the precision of s.
epsilon = eps(gamma)
#x2 = -beta*s^2
x2 = -sign(beta)*gamma^2
term = one(T)
h2 = one(T)
h21 = 2h2
h22 = 2h2
n=0
iter = 0
ITMAX = 100
# Terminate series when required precision reached:
#while abs(term) > epsilon*abs(h2)
while true
  h22 = h21
  h21 = h2
  n += 1
  term *= x2
  term /= (4n+6)*n
  h2 += term
  iter += 1
  if iter >= ITMAX || h2 == h22 || h2 == h21
    break
  end
end
h2 *= gamma^3/(3*sqrt(abs(beta^3)))
return h2::T
end

function H1_series(gamma::T,beta::T) where {T <: Real}
# Computes H_1(\beta,s) using a series tailored to the precision of s.
epsilon = eps(gamma)
#x2 = -beta*s^2
x2 = -sign(beta)*gamma^2
term = one(T)
h1 = one(T)
h11 = 2h1
h12 = 2h1
n=0
iter = 0
ITMAX = 100
# Terminate series when required precision reached:
#while abs(term) > epsilon*abs(h1)
while true
  h12 = h11
  h11 = h1
  n += 1
  term *= x2*(n+1)
  term /= (2n+4)*(2n+3)*n
  h1 += term
  iter +=1
  if iter >= ITMAX || h1 == h12 || h1 == h11
    break
  end
end
h1 *= gamma^4/(12*beta^2)
return h1::T
end

function H5(gamma::T,beta::T;gc=convert(T,0.5)) where {T <: Real}
# This is G_1 G_2 -(2+G_0) G_3 = H_2 - 2 G_3:
if gamma < gc
  return H5_series(gamma,beta)
else
  if beta >= 0
    return (3sin(gamma)-2gamma-gamma*cos(gamma))/(beta*sqrt(beta))
  else
    return (3sinh(gamma)-2gamma-gamma*cosh(gamma))/(beta*sqrt(-beta))
  end
end
end

function H5_series(gamma::T,beta::T) where {T <: Real}
# Computes H_5(\beta,s) using a series tailored to the precision of gamma:
epsilon = eps(gamma)
#x2 = -beta*s^2
x2 = -sign(beta)*gamma^2
term = one(T)/60
h5 = copy(term)
h51 = 2h5
h52 = 2h5
n=0
iter = 0
ITMAX = 100
# Terminate series when required precision reached:
#while abs(term) > epsilon*abs(h5)
while true
  h52 = h51
  h51 = h5
  n += 1
  term *= x2*(n+1)
  term /= (2n+5)*(2n+4)*n
  h5 += term
  iter += 1
  if iter >= ITMAX || h5 == h52 || h5 == h51
    break
  end
end
h5 *= -gamma^5/(beta*sqrt(abs(beta)))
return h5::T
end

function H6(gamma::T,beta::T;gc=convert(T,0.5)) where {T <: Real}
# This is 2 G_2^2 -3 G_1 G_3:
if gamma < gc
  return H6_series(gamma,beta)
else
  if beta >= 0
    return (9-8cos(gamma) -cos(2gamma) -6gamma*sin(gamma))/(2beta^2)
  else
    return (9-8cosh(gamma)-cosh(2gamma)+6gamma*sinh(gamma))/(2beta^2)
  end
end
end

function H6_series(gamma::T,beta::T) where {T <: Real}
# Computes H_6(\beta,s) using a series tailored to the precision of gamma:
epsilon = eps(gamma)
#x2 = -beta*s^2
x2 = -sign(beta)*gamma^2
term = convert(T,1//360)
h6 = convert(T,1//40)
h61 = 2h6
h62 = 2h6
n=0
iter = 0
ITMAX = 100
# Terminate series when required precision reached:
#while abs(term) > epsilon*abs(h6)
while true
  h62 = h61
  h61 = h6
  n += 1
  term *= x2
  term /= (2n+5)*(2n+6)
  h6 += term*(4^(n+2)-3*n-7)
  iter +=1
  if iter >= ITMAX || h6 == h62 || h6 == h61
    break
  end
end
h6 *= gamma^6/(beta*abs(beta))
return h6::T
end

function H3(gamma::T,beta::T;gc=convert(T,0.5)) where {T <: Real}
# This is H_3 = G_1 G_2 - 3 G_3:
if gamma < gc
  return H3_series(gamma,beta)
else
  if beta >= 0
    return (4*sin(gamma)-sin(gamma)*cos(gamma)-3*gamma)/(beta*sqrt(beta))
  else
    return (4*sinh(gamma)-sinh(gamma)*cosh(gamma)-3*gamma)/(beta*sqrt(-beta))
  end
end
end

function H3_series(gamma::T,beta::T) where {T <: Real}
# Computes H_3(\beta,s) using a series tailored to the precision of gamma:
epsilon = eps(gamma)
#x2 = -beta*s^2
x2 = -sign(beta)*gamma^2
term = convert(T,1//30)
h3 = convert(T,1//10)
h31 = 2h3
h32 = 2h3
n=0
iter = 0
ITMAX = 100
# Terminate series when required precision reached:
#while abs(term) > epsilon*abs(h3)
while true
  h32 = h31
  h31 = h3
  n += 1
  term *= x2
  term /= (2n+4)*(2n+5)
  h3 += term*(4^(n+1)-1)
  iter += 1
  if iter >= ITMAX || h3 == h32 || h3 == h31
    break
  end
end
h3 *= -gamma^5/(beta*sqrt(abs(beta)))
return h3::T
end

function H7(gamma::T,beta::T;gc=convert(T,0.5)) where {T <: Real}
# This is H_7 = G_1 G_2 (1-2 G_0) + 3 G_0^2 G_3:
if gamma < gc
  return H7_series(gamma,beta)
else
  if beta >= 0
    return (3*cos(gamma)*(gamma*cos(gamma)-sin(gamma))+sin(gamma)^3)/(beta*sqrt(beta))
  else
    return (3*cosh(gamma)*(gamma*cosh(gamma)-sinh(gamma))-sinh(gamma)^3)/(beta*sqrt(-beta))
  end
end
end

function H7_series(gamma::T,beta::T) where {T <: Real}
# Computes H_7(\beta,s) using a series tailored to the precision of gamma:
epsilon = eps(gamma)
#x2 = -beta*s^2
x2 = -sign(beta)*gamma^2
term = -convert(T,3//20160)*x2
h7 = convert(T,1//10-11//840*x2)
h71 = 2h7
h72 = 2h7
n=0
iter = 0
ITMAX = 100
# Terminate series when required precision reached:
#while abs(term) > epsilon*abs(h7)
while true
  h72 = h71
  h71 = h7
  n += 1
  term *= x2
  term /= (2n+6)*(2n+7)
  h7 += term*(9^(3+n)-1-(5+2*n)*2^(7+2*n))
  iter += 1
  if iter >= ITMAX || h7 == h72 || h7 == h71
    break
  end
end
h7 *= gamma^5/(beta*sqrt(abs(beta)))
return h7::T
end

function H8(gamma::T,beta::T;gc=convert(T,0.5)) where {T <: Real}
# This is H_8 = G_1 G_2 - 3 G_0 G_3:
if gamma < gc
  return H8_series(gamma,beta)
else
  if beta >= 0
    return (-3gamma*cos(gamma) +sin(gamma) +sin(2gamma))/(beta*sqrt(beta))
  else
    return (-3gamma*cosh(gamma)+sinh(gamma)+sinh(2gamma))/(beta*sqrt(-beta))
  end
end
end

function H8_series(gamma::T,beta::T) where {T <: Real}
# Computes H_8(\beta,s) using a series tailored to the precision of gamma:
epsilon = eps(gamma)
#x2 = -beta*s^2
x2 = -sign(beta)*gamma^2
term = convert(T,1//120)
h8 = convert(T,3//20)
h81 = 2h8
h82 = 2h8
n=0
iter = 0
ITMAX = 100
# Terminate series when required precision reached:
#while abs(term) > epsilon*abs(h8)
while true
  h82 = h81
  h81 = h8
  n += 1
  term *= x2
  term /= (2n+4)*(2n+5)
  h8 += term*(2^(5+2n)-14-6n)
  iter +=1
  if iter >= ITMAX || h8 == h82 || h8 == h81
    break
  end
end
h8 *= gamma^5/(beta*sqrt(abs(beta)))
return h8::T
end


using SimpleGF2, SimpleGraphs, SimplePosets, Memoize, SimpleGraphAlgorithms

export CircleRepresentation

# AUTHOR: Tara Abrishami

"""
`CircleRepresentation(G)` returns a circle graph representation of the
graph `G` or throws an error if there is none.
"""
function CircleRepresentation(G::SimpleGraph)
    if cache_check(G, :CircleRepresentation)
        return cache_recall(G, :CircleRepresentation)
    end
    V = eltype(G)
    numVert = Dict{Int,V}()
    vertNum = Dict{V,Int}()
    counter = 1
    verts = NV(G)
    z = zeros(GF2, verts * verts + 1)
    C = deepcopy(z)
    vertices = vlist(G)
    for v in vertices
        vertNum[v] = counter
        numVert[counter] = v
        counter = counter + 1
    end
    for e in elist(G) #assigns the beta equations where there are edges
        x = deepcopy(z)
        x[(vertNum[e[1]]-1)*verts+vertNum[e[2]]] = 1
        x[(vertNum[e[2]]-1)*verts+vertNum[e[1]]] = 1
        x[verts*verts+1] = 1
        C = hcat(C, x)
    end
    n = length(vertices)
    for v in vertices
        for i = 1:n
            for j = 1+i:n
                v1 = vertices[i]
                v2 = vertices[j]
                if (has(G, v, v1) && !has(G, v, v2)) ||
                   (has(G, v, v2) && !has(G, v, v1)) ||
                   v == v1 ||
                   v == v2 ||
                   v1 == v2
                    continue
                elseif has(G, v, v1) && has(G, v, v2) #assigns the beta equation in the third case
                    if !has(G, v1, v2)
                        x = deepcopy(z)
                        x[(vertNum[v]-1)*verts+vertNum[v1]] = 1
                        x[(vertNum[v]-1)*verts+vertNum[v2]] = 1
                        x[(vertNum[v1]-1)*verts+vertNum[v2]] = 1
                        x[(vertNum[v2]-1)*verts+vertNum[v1]] = 1
                        x[verts*verts+1] = 1
                        C = hcat(C, x)
                    end
                elseif !has(G, v, v1) && !has(G, v, v2) #assigns the beta equation in the second case
                    if has(G, v1, v2)
                        x = deepcopy(z)
                        x[(vertNum[v]-1)*verts+vertNum[v1]] = 1
                        x[(vertNum[v]-1)*verts+vertNum[v2]] = 1
                        C = hcat(C, x)
                    end
                end
            end
        end
    end
    C = C'
    sol = Array{GF2}(undef)
    try
        sol = solve_augmented(C)
    catch
        error("Not a circle graph")
    end

    ret = Array{Int}(0)
    comps = collect(parts(components(G)))
    for c in comps
        lis = assignOrder(c, G, vertNum, sol, numVert)
        lis = mod.(lis - 1, verts) + 1
        ret = vcat(ret, flipdim(flipdim(lis, 1), 1))
    end
    retn = Array{V}(length(ret))
    for q = 1:length(ret)
        retn[q] = numVert[ret[q]]
    end
    cache_save(G, :CircleRepresentation, retn)
    return retn
end

function assignOrder(c, G, vertNum, sol, numVert)
    order = SimplePoset(Int)
    V = eltype(G)
    verts = NV(G)
    siz = length(c)
    arr = Array{V}(siz)
    i = 1
    for x in c
        arr[i] = x
        i = i + 1
    end
    firstVertex = arr[1]
    add!(order, vertNum[firstVertex], vertNum[firstVertex] + verts)
    for i = 2:siz
        add!(order, vertNum[firstVertex], vertNum[arr[i]])
        add!(order, vertNum[firstVertex], vertNum[arr[i]] + verts)
        if has(G, firstVertex, arr[i]) &&
           sol[(vertNum[firstVertex]-1)*verts+vertNum[arr[i]]] == 1
            add!(order, vertNum[arr[i]], vertNum[firstVertex] + verts)
            add!(order, vertNum[firstVertex] + verts, vertNum[arr[i]] + verts)
        elseif has(G, firstVertex, arr[i]) &&
               sol[(vertNum[firstVertex]-1)*verts+vertNum[arr[i]]] == 0
            add!(order, vertNum[firstVertex] + verts, vertNum[arr[i]])
            add!(order, vertNum[arr[i]] + verts, vertNum[firstVertex] + verts)
        elseif !has(G, firstVertex, arr[i]) &&
               sol[(vertNum[firstVertex]-1)*verts+vertNum[arr[i]]] == 1
            add!(order, vertNum[firstVertex] + verts, vertNum[arr[i]])
            add!(order, vertNum[firstVertex] + verts, vertNum[arr[i]] + verts)
        elseif !has(G, firstVertex, arr[i]) &&
               sol[(vertNum[firstVertex]-1)*verts+vertNum[arr[i]]] == 0
            add!(order, vertNum[arr[i]], vertNum[firstVertex] + verts)
            add!(order, vertNum[arr[i]] + verts, vertNum[firstVertex] + verts)
        end
        for elt in above(order, vertNum[firstVertex]) #there are ambiguity problems with cases 1, 2, 5, and 6
            j = mod(elt, siz)
            if j == 0
                j = siz
            end
            _compareG(order, i, j, vertNum, sol, arr, verts, firstVertex, G)
        end
    end
    m = 1
    while (m != 0)
        orig = length(incomparables(order))
        for x in incomparables(order)
            i = mod(x[1], siz)
            j = mod(x[2], siz)
            if i == 0
                i = siz
            end
            if j == 0
                j = siz
            end
            _compareG(order, i, j, vertNum, sol, arr, verts, firstVertex, G)
            _compareG(order, j, i, vertNum, sol, arr, verts, firstVertex, G)
        end
        fin = length(incomparables(order))
        m = fin - orig
    end
    return _final(order, G, numVert, vertNum)
end

function _compareG(order, i, j, vertNum, sol, arr, verts, firstVertex, G)
    if i == j
        if !has(G, firstVertex, arr[i]) &&
           sol[(vertNum[firstVertex]-1)*verts+vertNum[arr[i]]] == 1 &&
           sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 1
            add!(order, vertNum[arr[i]], vertNum[arr[i]] + verts)
        elseif !has(G, firstVertex, arr[i]) &&
               sol[(vertNum[firstVertex]-1)*verts+vertNum[arr[i]]] == 1 &&
               sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 0
            add!(order, vertNum[arr[i]] + verts, vertNum[arr[i]])
        elseif !has(G, firstVertex, arr[i]) &&
               sol[(vertNum[firstVertex]-1)*verts+vertNum[arr[i]]] == 0 &&
               sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 1
            add!(order, vertNum[arr[i]], vertNum[arr[i]] + verts)
        elseif !has(G, firstVertex, arr[i]) &&
               sol[(vertNum[firstVertex]-1)*verts+vertNum[arr[i]]] == 0 &&
               sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 0
            add!(order, vertNum[arr[i]] + verts, vertNum[arr[i]])
        end
    elseif has(G, arr[i], arr[j]) && sol[(vertNum[arr[j]]-1)*verts+vertNum[arr[i]]] == 1 #if there is an edge and beta is 1
        if (
            has(order, vertNum[arr[j]] + verts, vertNum[firstVertex] + verts) &&
            has(order, vertNum[arr[j]], vertNum[arr[j]] + verts)
        ) #first case
            add!(order, vertNum[arr[j]], vertNum[arr[i]])
            add!(order, vertNum[arr[i]], vertNum[arr[j]] + verts)
            if has(order, vertNum[arr[i]] + verts, vertNum[firstVertex] + verts) &&
               sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 1
                add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]] + verts)
            elseif has(order, vertNum[arr[i]] + verts, vertNum[firstVertex] + verts) &&
                   sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 0
                add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]])
            end
        elseif (
            has(order, vertNum[arr[j]], vertNum[arr[j]] + verts) &&
            has(order, vertNum[firstVertex] + verts, vertNum[arr[j]])
        ) #sixth case
            add!(order, vertNum[arr[j]], vertNum[arr[i]])
            add!(order, vertNum[arr[i]], vertNum[arr[j]] + verts)
            if has(order, vertNum[firstVertex] + verts, vertNum[arr[i]]) &&
               sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 1
                add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]] + verts)
            elseif has(order, vertNum[firstVertex] + verts, vertNum[arr[i]]) &&
                   sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 0
                add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]])
            end
        elseif (
            has(order, vertNum[arr[j]], vertNum[firstVertex] + verts) &&
            has(order, vertNum[arr[j]] + verts, vertNum[arr[j]])
        ) #second case
            add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]] + verts)
            add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]])
            if has(order, vertNum[arr[i]], vertNum[firstVertex] + verts) &&
               sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 1
                add!(order, vertNum[arr[i]], vertNum[arr[j]] + verts)
            elseif has(order, vertNum[arr[i]], vertNum[firstVertex] + verts) &&
                   sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 0
                add!(order, vertNum[arr[j]], vertNum[arr[i]])
            end
        elseif (
            has(order, vertNum[firstVertex] + verts, vertNum[arr[j]] + verts) &&
            has(order, vertNum[arr[j]] + verts, vertNum[arr[j]])
        ) #fifth case
            add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]] + verts)
            add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]])
            if has(order, vertNum[firstVertex] + verts, vertNum[arr[i]] + verts) &&
               sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 1
                add!(order, vertNum[arr[j]], vertNum[arr[i]] + verts)
            elseif has(order, vertNum[firstVertex] + verts, vertNum[arr[i]] + verts) &&
                   sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 0
                add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]] + verts)
            end
        elseif has(order, vertNum[arr[j]], vertNum[arr[j]] + verts) &&
               has(order, vertNum[firstVertex] + verts, vertNum[arr[j]] + verts) &&
               has(G, arr[j], firstVertex) #case 3
            add!(order, vertNum[arr[j]], vertNum[arr[i]])
            add!(order, vertNum[arr[i]], vertNum[arr[j]] + verts)
            if has(order, vertNum[arr[i]] + verts, vertNum[firstVertex] + verts)
                add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]])
            else
                add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]] + verts)
            end
        elseif has(order, vertNum[arr[j]] + verts, vertNum[arr[j]]) &&
               has(order, vertNum[firstVertex] + verts, vertNum[arr[j]]) &&
               has(G, arr[j], firstVertex) #case 4
            add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]] + verts)
            add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]])
            if has(order, vertNum[arr[i]], vertNum[firstVertex] + verts)
                add!(order, vertNum[arr[i]], vertNum[arr[j]] + verts)
            else
                add!(order, vertNum[arr[j]], vertNum[arr[i]])
            end
        end
    elseif has(G, arr[i], arr[j]) && sol[(vertNum[arr[j]]-1)*verts+vertNum[arr[i]]] == 0 #if there is an edge and beta is 0
        if (
            has(order, vertNum[arr[j]] + verts, vertNum[firstVertex] + verts) &&
            has(order, vertNum[arr[j]], vertNum[arr[j]] + verts)
        ) #first case
            add!(order, vertNum[arr[j]], vertNum[arr[i]] + verts)
            add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]] + verts)
            if has(order, vertNum[arr[i]], vertNum[firstVertex] + verts) &&
               sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 1
                add!(order, vertNum[arr[i]], vertNum[arr[j]])
            elseif has(order, vertNum[arr[i]], vertNum[firstVertex] + verts) &&
                   sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 0
                add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]])
            end
        elseif has(order, vertNum[arr[j]], vertNum[arr[j]] + verts) &&
               has(order, vertNum[firstVertex] + verts, vertNum[arr[j]]) #sixth case
            add!(order, vertNum[arr[j]], vertNum[arr[i]] + verts)
            add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]] + verts)
            if has(order, vertNum[firstVertex] + verts, vertNum[arr[i]]) &&
               sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 1
                add!(order, vertNum[arr[i]], vertNum[arr[j]])
            elseif has(order, vertNum[firstVertex] + verts, vertNum[arr[i]]) &&
                   sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 0
                add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]])
            end
        elseif (
            has(order, vertNum[arr[j]], vertNum[firstVertex] + verts) &&
            has(order, vertNum[arr[j]] + verts, vertNum[arr[j]])
        ) #second case
            add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]])
            add!(order, vertNum[arr[i]], vertNum[arr[j]])
            if has(order, vertNum[arr[i]] + verts, vertNum[firstVertex] + verts) &&
               sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 1
                add!(order, vertNum[arr[j]], vertNum[arr[i]] + verts)
            elseif has(order, vertNum[arr[i]] + verts, vertNum[firstVertex] + verts) &&
                   sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 0
                add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]] + verts)
            end
        elseif has(order, vertNum[firstVertex] + verts, vertNum[arr[j]] + verts) &&
               has(order, vertNum[arr[j]] + verts, vertNum[arr[j]]) #fifth case
            add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]])
            add!(order, vertNum[arr[i]], vertNum[arr[j]])
            if has(order, vertNum[firstVertex] + verts, vertNum[arr[i]] + verts) &&
               sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 1
                add!(order, vertNum[arr[j]], vertNum[arr[i]] + verts)
            elseif has(order, vertNum[firstVertex] + verts, vertNum[arr[i]] + verts) &&
                   sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 0
                add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]] + verts)
            end
        elseif has(order, vertNum[arr[j]], vertNum[arr[j]] + verts) &&
               has(order, vertNum[firstVertex] + verts, vertNum[arr[j]] + verts) &&
               has(G, arr[j], firstVertex) #case 3
            add!(order, vertNum[arr[j]], vertNum[arr[i]] + verts)
            add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]] + verts)
            if has(order, vertNum[arr[i]], vertNum[firstVertex] + verts)
                add!(order, vertNum[arr[i]], vertNum[arr[j]])
            else
                add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]])
            end
        elseif has(order, vertNum[arr[j]] + verts, vertNum[arr[j]]) &&
               has(order, vertNum[firstVertex] + verts, vertNum[arr[j]]) &&
               has(G, arr[j], firstVertex) #case 4
            add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]])
            add!(order, vertNum[arr[i]], vertNum[arr[j]])
            if has(order, vertNum[arr[i]] + verts, vertNum[firstVertex] + verts)
                add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]] + verts)
            else
                add!(order, vertNum[arr[j]], vertNum[arr[i]] + verts)
            end
        end
    elseif !has(G, arr[i], arr[j]) && sol[(vertNum[arr[j]]-1)*verts+vertNum[arr[i]]] == 1 #if there is not an edge and beta is 1
        if has(order, vertNum[arr[j]] + verts, vertNum[arr[j]]) &&
           has(order, vertNum[firstVertex] + verts, vertNum[arr[j]]) &&
           has(G, arr[j], firstVertex) ||
           has(order, vertNum[firstVertex] + verts, vertNum[arr[j]] + verts) &&
           has(order, vertNum[arr[j]] + verts, vertNum[arr[j]]) ||
           has(order, vertNum[arr[j]], vertNum[firstVertex] + verts) &&
           has(order, vertNum[arr[j]] + verts, vertNum[arr[j]]) #case 4, 5 and 2
            add!(order, vertNum[arr[i]], vertNum[arr[j]])
            add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]])
            add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]])
            add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]] + verts)
        elseif has(order, vertNum[arr[j]], vertNum[arr[j]] + verts) &&
               has(order, vertNum[firstVertex] + verts, vertNum[arr[j]] + verts) &&
               has(G, arr[j], firstVertex) #third case
            if has(order, vertNum[arr[i]], vertNum[firstVertex])
                add!(order, vertNum[arr[i]], vertNum[arr[j]])
            else
                add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]])
            end
            if has(order, vertNum[arr[i]] + verts, vertNum[firstVertex])
                add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]])
            else
                add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]] + verts)
            end
        elseif has(order, vertNum[arr[j]] + verts, vertNum[firstVertex] + verts) &&
               has(order, vertNum[arr[j]], vertNum[arr[j]] + verts) #case 1
            if has(G, arr[i], firstVertex)
                if sol[(vertNum[firstVertex]-1)*verts+vertNum[arr[i]]] == 1 &&
                   sol[(vertNum[arr[i]]-1)*verts+vertNum[arr[j]]] == 1
                    add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]])
                elseif sol[(vertNum[firstVertex]-1)*verts+vertNum[arr[i]]] == 1 &&
                       sol[(vertNum[arr[i]]-1)*verts+vertNum[arr[j]]] == 0
                    add!(order, vertNum[arr[i]], vertNum[arr[j]])
                elseif sol[(vertNum[firstVertex]-1)*verts+vertNum[arr[i]]] == 0 &&
                       sol[(vertNum[arr[i]]-1)*verts+vertNum[arr[j]]] == 1
                    add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]])
                elseif sol[(vertNum[firstVertex]-1)*verts+vertNum[arr[i]]] == 0 &&
                       sol[(vertNum[arr[i]]-1)*verts+vertNum[arr[j]]] == 0
                    add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]] + verts)
                end
            elseif sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 1 &&
                   sol[(vertNum[arr[i]]-1)*verts+vertNum[arr[j]]] == 0
                add!(order, vertNum[arr[i]], vertNum[arr[j]])
                add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]] + verts)
            elseif sol[(vertNum[arr[i]]-1)*verts+vertNum[firstVertex]] == 0 &&
                   sol[(vertNum[arr[i]]-1)*verts+vertNum[arr[j]]] == 1
                add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]])
                add!(order, vertNum[arr[j]] + verts, vertNum[arr[i]])
            end
        elseif has(order, vertNum[arr[j]], vertNum[arr[j]] + verts) &&
               has(order, vertNum[firstVertex] + verts, vertNum[arr[j]])
        end
    elseif !has(G, arr[i], arr[j]) && sol[(vertNum[arr[j]]-1)*verts+vertNum[arr[i]]] == 0 #if there is not an edge and beta is 0
        if (
            has(order, vertNum[arr[j]] + verts, vertNum[firstVertex] + verts) &&
            has(order, vertNum[arr[j]], vertNum[arr[j]] + verts)
        ) ||
           has(order, vertNum[arr[j]], vertNum[arr[j]] + verts) &&
           has(order, vertNum[firstVertex] + verts, vertNum[arr[j]]) #first case and sixth case
            add!(order, vertNum[arr[j]], vertNum[arr[i]])
            add!(order, vertNum[arr[j]], vertNum[arr[i]] + verts)
            add!(order, vertNum[arr[i]], vertNum[arr[j]] + verts)
            add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]] + verts)
        elseif has(order, vertNum[arr[j]], vertNum[arr[j]] + verts) &&
               has(order, vertNum[firstVertex] + verts, vertNum[arr[j]] + verts) &&
               has(G, arr[j], firstVertex) #case 3
            add!(order, vertNum[arr[j]], vertNum[arr[i]])
            add!(order, vertNum[arr[j]], vertNum[arr[i]] + verts)
            add!(order, vertNum[arr[i]], vertNum[arr[j]] + verts)
            add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]] + verts)
        elseif has(order, vertNum[arr[j]] + verts, vertNum[arr[j]]) &&
               has(order, vertNum[firstVertex] + verts, vertNum[arr[j]]) &&
               has(G, arr[j], firstVertex) #case 4
            if has(order, vertNum[arr[i]], vertNum[firstVertex] + verts)
                add!(order, vertNum[arr[i]], vertNum[arr[j]] + verts)
            else
                add!(order, vertNum[arr[j]], vertNum[arr[i]])
            end
            if has(order, vertNum[arr[i]] + verts, vertNum[firstVertex] + verts)
                add!(order, vertNum[arr[i]] + verts, vertNum[arr[j]] + verts)
            else
                add!(order, vertNum[arr[j]], vertNum[arr[i]] + verts)
            end
        end
    end
end

@memoize function _final(P::SimplePoset, G::SimpleGraph, numVert, vertNum)
    T = element_type(P)
    M = maximals(P)
    for x in M
        PP = deepcopy(P)
        delete!(PP, x)
        PP_exts = all_linear_extensions(PP)
        for L in PP_exts
            append!(L, [x])
            L = mod.(L - 1, NV(G)) + 1
            GG = CircleGraph(L)
            X = relabel(G, vertNum)
            if GG == induce(X, Set(vlist(GG)))
                return L
            end
        end
    end
end


nt = 1000
seed = 99
m = ACQR_simula(nt,seed)
y = m["y"]

############################### starting point
nx = 4
i = nx+1
Ai,Ci,Qi,Ri,Si = ACQRS_sub(y,nx,i)
m1i = Ci\y[:,1:1]
P1i = 0*Ai

#######################
max_iter=10
tol=1e-6
txo=false
print("Testing ACQR_em: ")
A,C,Q,R,m1,P1,loglik,aic = ACQR_em(y,Ai,Ci,Qi,Ri,m1i,P1i,max_iter,tol,txo)
if sum(A.^2) > 1e-6
    println("OK")
end


#######################################
#
# Global field power
#
#######################################

function gfp(x::Array)

    samples, sensors = size(x)

    @info("Computing global field power for $sensors sensors and $samples samples")

    result = zeros(samples, 1)

    for sample = 1:samples
        u = vec(x[sample, :]) .- Statistics.mean(x[sample, :])
        sumsqdif = 0
        for sensor = 1:sensors
            for sensor2 = 1:sensors
                sumsqdif += (u[sensor] - u[sensor2])^2
            end
        end
        result[sample] = sqrt.(sumsqdif / (2 * length(samples)))
    end

    return result
end


abstract type Constraint end

mutable struct EpipolarConstraint <: Constraint
end

# Triangulation from Two Views Revisited: Hartley-Sturm vs. Optimal Correction
function satisfy(entity::FundamentalMatrix, constraint::EpipolarConstraint, 𝐅::AbstractArray, 𝒟::Tuple{AbstractArray, Vararg{AbstractArray}})
    ℳ, ℳʹ = 𝒟
    𝒪 = similar(ℳ)
    𝒪ʹ = similar(ℳʹ)

    N = length(ℳ)
    𝐏₂ = SMatrix{3,3,Float64,3^2}([1 0 0; 0 1 0; 0 0 0])

    I = 10
    for n = 1:N
        𝐦 = hom(ℳ[n])
        𝐦ʹ = hom(ℳʹ[n])
        𝐦ₕ = init_correction_view_1(𝐅, 𝐦, 𝐦ʹ, 𝐏₂)
        𝐦ₕʹ= init_correction_view_2(𝐅, 𝐦, 𝐦ʹ, 𝐏₂)
        for i = 1:I
            𝐦ₜ =  𝐦 - 𝐦ₕ
            𝐦ₜʹ =  𝐦ʹ - 𝐦ₕʹ
            𝐦ₕ = update_correction_view_1(𝐅, 𝐦, 𝐦ₕ, 𝐦ₜ, 𝐦ʹ, 𝐦ₕʹ, 𝐦ₜʹ, 𝐏₂)
            𝐦ₕʹ = update_correction_view_2(𝐅, 𝐦, 𝐦ₕ, 𝐦ₜ, 𝐦ʹ, 𝐦ₕʹ, 𝐦ₜʹ, 𝐏₂)
        end
        𝒪[n] = hom⁻¹(𝐦ₕ)
        𝒪ʹ[n] = hom⁻¹(𝐦ₕʹ)
    end
    𝒪 ,𝒪ʹ
end

function init_correction_view_1(𝐅::AbstractArray, 𝐦::AbstractVector, 𝐦ʹ::AbstractVector, 𝐏₂::AbstractArray)
    𝐦 -  dot(𝐦,𝐅*𝐦ʹ)*𝐏₂*𝐅*𝐦ʹ / ( dot(𝐅*𝐦ʹ,𝐏₂*𝐅*𝐦ʹ) + dot(𝐅'*𝐦, 𝐏₂*𝐅'*𝐦) )
end

function init_correction_view_2(𝐅::AbstractArray, 𝐦::AbstractVector, 𝐦ʹ::AbstractVector, 𝐏₂::AbstractArray)
    𝐦ʹ -  dot(𝐦,𝐅*𝐦ʹ)*𝐏₂*𝐅'*𝐦 / ( dot(𝐅*𝐦ʹ,𝐏₂*𝐅*𝐦ʹ) + dot(𝐅'*𝐦, 𝐏₂*𝐅'*𝐦) )
end

function update_correction_view_1(𝐅::AbstractArray, 𝐦::AbstractVector, 𝐦ₕ::AbstractVector, 𝐦ₜ::AbstractVector,  𝐦ʹ::AbstractVector, 𝐦ₕʹ::AbstractVector, 𝐦ₜʹ::AbstractVector,  𝐏₂::AbstractArray)
    𝐦 -  ( ( dot(𝐦ₕ,𝐅*𝐦ₕʹ) + dot(𝐅*𝐦ₕʹ, 𝐦ₜ) + dot(𝐅'*𝐦ₕ, 𝐦ₜʹ)  ) * 𝐏₂*𝐅*𝐦ₕʹ)   / (dot(𝐅*𝐦ₕʹ, 𝐏₂*𝐅*𝐦ₕʹ) + dot(𝐅'*𝐦ₕ, 𝐏₂*𝐅'*𝐦ₕ) )
end

function update_correction_view_2(𝐅::AbstractArray, 𝐦::AbstractVector, 𝐦ₕ::AbstractVector, 𝐦ₜ::AbstractVector,  𝐦ʹ::AbstractVector, 𝐦ₕʹ::AbstractVector, 𝐦ₜʹ::AbstractVector,  𝐏₂::AbstractArray)
    𝐦ʹ -  ( ( dot(𝐦ₕ,𝐅*𝐦ₕʹ) + dot(𝐅*𝐦ₕʹ, 𝐦ₜ) + dot(𝐅'*𝐦ₕ, 𝐦ₜʹ)  ) * 𝐏₂*𝐅'*𝐦ₕ)   / (dot(𝐅*𝐦ₕʹ, 𝐏₂*𝐅*𝐦ₕʹ) + dot(𝐅'*𝐦ₕ, 𝐏₂*𝐅'*𝐦ₕ) )
end


using FreeRay
using Test, Plots

#### Geometry
# Point source 1000 Hz at 80 m
source = Source(
    frequency = 100,
    height = -5
)

# Multiple recievers on grid
receiver = Receiver(
    depth_point = 1001,
    range_point = 10001,
    depth = Vec2(-1000f0,0f0),
    range = Vec2(0f0,10f0)
)

# Terrain geometry
terrain = Terrain(
    interp_type = "C",
    profile = (Vec2([0f0,5f0,10f0],[0f0,0f0,0f0]))
)

### Boundary condition
Zc = 12.81 + 11.62im
Theta, Rmag, Rphase = R_coeff(Zc;len=100)

brc = Vec3(Theta,Rmag,Rphase)
trc = Vec3([0f0,45f0,90f0],[0f0,0f0,0f0],[0f0,0f0,0f0])

reflection = Reflection_Coeff(
    top_coeff = trc,
    bottom_coeff = brc
)

### Sound speed profile
#sspl = Vec2([-1000f0,-500f0,0f0],[243f0,293,343]) # case 3
sspl = Vec2([-1000f0,-500f0,0f0],[343f0,343f0,343f0]) # case 1
#sspl = Vec2([-1000f0,-500f0,0f0],[443f0,393f0,343f0]) # case 2
#sspl = Vec2([-1000f0,-300f0,-100f0,0f0],[333f0,333f0,353f0,343f0]) #case 4
ssp = SSP(sound_speed_profile= sspl)

### Analysis
opt = Analysis(
    filename = "Case0_Bellhop_f10",
    analyse = "RG",
    option1 = "CFW",
    option2 = "F*",
    num_ray = 161,
    alpha = Vec2(-80.0f0,80.0f0),
    box = Vec2(10f0,1000.0f0),
    step= 0
)

Environment(opt,source, receiver,ssp,terrain,reflection)

# Run Bellhop
fn= opt.filename
filename = "$fn"
run_bellhop = `bellhop $filename`
@time run(run_bellhop)

p1 = PlotRay("$filename.ray",
        xlabs = "Range, m",
        ylabs = "Height, m")

#savefig(p1,"ray1.png")

#p2 = PlotShd("$filename.shd";
#        xlabs = "Range, m",
#        ylabs = "Transmission loss, dB",
#        cblabs = "dB",
#        climb = (40,80))

#savefig(p2,"transc4.png")

#PlotTlr("$filename.shd", -7;
#        xlabs = "Range, m",
#        ylabs = "Transmission loss, dB",
#        xlim = (0,10000),
#        ylim = (20,120))

rmfile(filename)

@testset "FreeRay.jl" begin
    # Write your tests here.
end

#read_shd("Case0_Bellhop_f10.shd")


# -----------------------------------------------------------------------------
# Types

"""
The supertype for all sky coordinate systems.
"""
abstract type AbstractSkyCoords end

"""
    ICRSCoords(ra, dec)

[International Celestial Reference System](https://en.wikipedia.org/wiki/International_Celestial_Reference_System)

This is the current standard adopted by the International Astronomical Union notably due to its high level of accuracy compared to standard equatorial coordinate systems. What sets this apart from [`FK5Coords`](@ref) is that it is completely defined using extragalactic radio sources rather than a geocentric frame, which means the reference frame will not change due to Earth's motion.

# Coordinates
- `ra` - Right ascension in radians (0, 2π)
- `dec` - Declination in radians (-π/2, π/2)
"""
struct ICRSCoords{T<:AbstractFloat} <: AbstractSkyCoords
    ra::T
    dec::T
    ICRSCoords{T}(ra, dec) where {T<:AbstractFloat} = new(mod2pi(ra), dec)
end
ICRSCoords(ra::T, dec::T) where {T<:AbstractFloat} = ICRSCoords{T}(ra, dec)
ICRSCoords(ra::Real, dec::Real) = ICRSCoords(promote(float(ra), float(dec))...)
ICRSCoords(c::T) where {T<:AbstractSkyCoords} = convert(ICRSCoords, c)
ICRSCoords{F}(c::T) where {F,T<:AbstractSkyCoords} = convert(ICRSCoords{F}, c)

"""
    GalCoords(l, b)

[Galactic Coordinate System](https://en.wikipedia.org/wiki/Galactic_coordinate_system)

This coordinate system is defined based on the projection of the Milky Way galaxy onto our celestial sphere, with (0, 0) being approximately the center of our galaxy.

# Coordinates
- `l` - Galactic longitude in radians (-π, π)
- `b` - Galactic latitude in radians (-π/2, π/2)
"""
struct GalCoords{T<:AbstractFloat} <: AbstractSkyCoords
    l::T
    b::T
    GalCoords{T}(l, b) where {T<:AbstractFloat} = new(mod2pi(l), b)
end
GalCoords(l::T, b::T) where {T<:AbstractFloat} = GalCoords{T}(l, b)
GalCoords(l::Real, b::Real) = GalCoords(promote(float(l), float(b))...)
GalCoords(c::T) where {T<:AbstractSkyCoords} = convert(GalCoords, c)
GalCoords{F}(c::T) where {F,T<:AbstractSkyCoords} = convert(GalCoords{F}, c)

"""
    FK5Coords{equinox}(ra, dec)

[Equatorial Coordinate System](https://en.wikipedia.org/wiki/Equatorial_coordinate_system)

This coordinate system maps the celestial sphere based on a geocentric observer. Historically the oldest, this coordinate system has been shown to be inaccurate due to its definitions based on the Earth, which has long-scale precession causing the reference frame to change. Because of this, an equinox must be provided (typically 2000, commonly known as J2000) which defines the reference frame.

# Coordinates
- `ra` - Right ascension in radians (0, 2π)
- `dec` - Declination in radians (-π/2, π/2)
"""
struct FK5Coords{e,T<:AbstractFloat} <: AbstractSkyCoords
    ra::T
    dec::T
    FK5Coords{e,T}(ra, dec) where {T<:AbstractFloat,e} = new(mod2pi(ra), dec)
end
FK5Coords{e}(ra::T, dec::T) where {e,T<:AbstractFloat} = FK5Coords{e,T}(ra, dec)
FK5Coords{e}(ra::Real, dec::Real) where {e} =
    FK5Coords{e}(promote(float(ra), float(dec))...)
FK5Coords{e}(c::T) where {e,T<:AbstractSkyCoords} = convert(FK5Coords{e}, c)
FK5Coords{e,F}(c::T) where {e,F,T<:AbstractSkyCoords} = convert(FK5Coords{e,F}, c)

# Scalar coordinate conversions
Base.convert(::Type{T}, c::T) where {T<:AbstractSkyCoords} = c

function Base.convert(::Type{T}, c::S) where {T<:AbstractSkyCoords,S<:AbstractSkyCoords}
    r = rotmat(T, S) * coords2cart(c)
    lon, lat = cart2coords(r)
    T(lon, lat)
end

Base.:(==)(a::T, b::T) where {T<:AbstractSkyCoords} = lon(a) == lon(b) && lat(a) == lat(b)


# MIT license
# Copyright (c) Microsoft Corporation. All rights reserved.
# See LICENSE in the project root for full license information.

#########################################################################################################

#"pseudo-types" of aspheres
"""
AphericSurfaces polynomial evaluation is optimized for no terms `CONIC`, odd terms `ODD`, even terms `EVEN` or any `ODDEVEN`
"""
@enum AsphericSurfaceType CONIC ODD EVEN ODDEVEN

"""
    AsphericSurface{T,N,Q,M} <: ParametricSurface{T,N}

Surface incorporating an aspheric polynomial - radius, conic and aspherics are defined relative to absolute semi-diameter,.
`T` is the datatype, `N` is the dimensionality, `Q` is the number of aspheric terms, and `M` is the type of aspheric polynomial.

```julia
AsphericSurface(semidiameter; radius, conic, aspherics=nothing, normradius = semidiameter)
```

The surface is centered at the origin and treated as being the cap of an infinite cylinder, thus creating a true half-space.
Outside of `0 <= ρ <= 1` the height of the surface is not necessarily well defined, so NaN may be returned.

`aspherics`  aspherics should be vectors containing tuples of the form (i, v) where i is the polynomial power of the aspheric term

The sag is defined by the equation

```math
z(r,\\phi) = \\frac{cr^2}{1 + \\sqrt{1 - (1+k)c^2r^2}} + \\sum_{i}^{Q}\\alpha_ir^{i}
```

where ``\\rho = \\frac{r}{\\texttt{normradius}}``, ``c = \\frac{1}{\\texttt{radius}}``, and ``k = \\texttt{conic}`` .

The function checks if the aspheric terms are missing, even, odd or both and uses an efficient polynomial evaluation strategy.

"""
struct AsphericSurface{T,N,Q,M} <: ParametricSurface{T,N}
    semidiameter::T
    curvature::T
    conic::T
    aspherics::SVector{Q,T}
    normradius::T
    boundingcylinder::Cylinder{T,N}

    function AsphericSurface(M::AsphericSurfaceType, semidiameter::T, curvature::T, conic::T, aspherics::SVector{Q,T}, normradius::T, boundingcylinder) where {T<:Real,Q}
        new{T,3,Q,M}(semidiameter, curvature, conic, aspherics, normradius, boundingcylinder)
    end

    function AsphericSurface(semidiameter::T; radius::T = typemax(T), conic::T = zero(T), aspherics::Union{Nothing,Vector{Tuple{Int,T}}} = nothing, normradius::T = semidiameter) where {T<:Real}
        @assert semidiameter > 0
        @assert !isnan(semidiameter) && !isnan(radius) && !isnan(conic)
        @assert one(T) - (1 / radius)^2 * (conic + one(T)) * semidiameter^2 > 0 "Invalid surface (conic/radius combination: $radius, $conic)"

        acs = []
        if aspherics===nothing
            M = CONIC
         else
            asphericTerms = [i for (i, ) in aspherics]
            minAsphericTerm = minimum(asphericTerms)
            maxAsphericTerm = maximum(asphericTerms)
            @assert minAsphericTerm > 0 "Aspheric Terms must be Order 1 or higher ($minAsphericTerm)"
            acs = zeros(T, maxAsphericTerm )
            for (i, k) in aspherics
                acs[i] = k
            end
            odd = any([isodd(i) && a != zero(T) for (i, a) in enumerate(acs)])
            even = any([iseven(i) && a != zero(T) for (i, a) in enumerate(acs)])
            if odd && even
                M = ODDEVEN
            elseif even
                M = EVEN
                acs = [acs[2i] for i in 1:(maxAsphericTerm ÷ 2)]
            elseif odd
                M = ODD
                acs = [acs[2i-1] for i in 1:((maxAsphericTerm+1) ÷ 2)]
            else #there are no nonzero aspherics terms in the list
                M = CONIC
                acs = []
            end
        end
        Q = length(acs)
        surf = new{T,3, Q, M}(semidiameter, 1/radius, conic, acs, normradius, Cylinder(semidiameter, interface = opaqueinterface(T)))
        return surf
    end

end

"""
    EvenAsphericSurface(semidiameter, curvature::T, conic::T, aspherics::Vector{T}; normradius::T=semidiameter)

Surface incorporating an aspheric polynomial - radius, conic and aspherics are defined relative to absolute semi-diameter.

`aspherics` should be an array of the even coefficients of the aspheric polynomial starting with A2

"""
function EvenAsphericSurface(semidiameter::T, curvature::T, conic::T, aspherics::Vector{T}; normradius::T=semidiameter) where T<:Real
    Q=length(aspherics)
    AsphericSurface(EVEN, semidiameter, curvature, conic, SVector{Q,T}(aspherics), normradius, Cylinder(semidiameter, interface = opaqueinterface(T)))
end

"""
    OddAsphericSurface(semidiameter, curvature::T, conic::T, aspherics::Vector{T}; normradius::T=semidiameter)

Surface incorporating an aspheric polynomial - radius, conic and aspherics are defined relative to absolute semi-diameter.

`aspherics`  should be an array of the odd coefficients of the aspheric polynomial starting with A1

"""
function OddAsphericSurface(semidiameter::T, curvature::T, conic::T, aspherics::Vector{T}; normradius::T=semidiameter) where T<:Real
    Q=length(aspherics)
    AsphericSurface(ODD, semidiameter, curvature, conic, SVector{Q,T}(aspherics), normradius, Cylinder(semidiameter, interface = opaqueinterface(T)))
end

"""
    OddEvenAsphericSurface(semidiameter, curvature::T, conic::T, aspherics::Vector{T}; normradius::T=semidiameter)

Surface incorporating an aspheric polynomial - radius, conic and aspherics are defined relative to absolute semi-diameter.

`aspherics` should be an array of the both odd and even coefficients of the aspheric polynomial starting with A1

"""
function OddEvenAsphericSurface(semidiameter::T, curvature::T, conic::T, aspherics::Vector{T}; normradius::T=semidiameter) where T<:Real
    Q=length(aspherics)
    AsphericSurface(ODDEVEN, semidiameter, curvature, conic, SVector{Q,T}(aspherics), normradius, Cylinder(semidiameter, interface = opaqueinterface(T)))
end

#don't have a function for CONIC as it would better to directly solve for the surface intersection instead of the rootfinding of a ParametricSurface

"""
    asphericType(surf::AsphericSurface)

Query the polynomial type of `asp.  Returns CONIC, ODD, EVEN, or ODDEVEN. CONIC corresponds to no aspheric terms, ODD
means it only has odd aspheric terms, EVEN means only even aspheric terms and ODDEVEN means both even and odd terms.

This function is to enable proper interpretation of `surf.aspherics` by any optimization routines that directly query the aspheric coefficients.
"""
asphericType(z::AsphericSurface{T,3,Q,M}) where {T<:Real,Q,M} = M

export AsphericSurface, EvenAsphericSurface, OddAsphericSurface, OddEvenAsphericSurface, asphericType, EVEN, CONIC, ODD, ODDEVEN

uvrange(::AsphericSurface{T,N}) where {T<:Real,N} = ((zero(T), one(T)), (-T(π), T(π))) # ρ and ϕ

semidiameter(z::AsphericSurface{T}) where {T<:Real} = z.semidiameter
halfsizeu(z::AsphericSurface{T}) where {T<:Real} = semidiameter(z)
halfsizev(z::AsphericSurface{T}) where {T<:Real} = semidiameter(z)

boundingobj(z::AsphericSurface{T}) where {T<:Real} = z.boundingcylinder

prod_step(z::AsphericSurface{T,N,Q,ODDEVEN}, r, r2) where {T<:Real,N,Q} = r, r
prod_step(z::AsphericSurface{T,N,Q,ODD}, r, r2) where {T<:Real,N,Q} = r, r2
prod_step(z::AsphericSurface{T,N,Q,EVEN}, r, r2) where {T<:Real,N,Q} = r2, r2

function point(z::AsphericSurface{T,3,Q,M}, ρ::T, ϕ::T)::SVector{3,T} where {T<:Real,Q,M}
    rad = z.semidiameter
    r = ρ * rad
    r2 = r^2
    t = one(T) - z.curvature^2 * (z.conic + one(T)) * r^2
    if t < zero(T)
        return SVector{3,T}(NaN, NaN, NaN)
    end
    h = z.curvature * r2 / (one(T) + sqrt(t))
   # sum aspheric
    if M != CONIC
        prod, step = prod_step(z, r, r2)  #multiple dispatch on M
        asp,rest = Iterators.peel(z.aspherics)
        h += asp * prod
        for asp in rest
            prod *= step
            h += asp * prod
        end
    end
    return SVector{3,T}(r * cos(ϕ), r * sin(ϕ), h)
end

partial_prod_step(z::AsphericSurface{T,3,Q,EVEN}, r::T, r2::T) where {T<:Real,Q} = r, r2, 2:2:2Q
partial_prod_step(z::AsphericSurface{T,3,Q,ODD}, r::T, r2::T) where {T<:Real,Q} = one(T), r2, 1:2:(2Q-1)
partial_prod_step(z::AsphericSurface{T,3,Q,ODDEVEN}, r::T, r2::T) where {T<:Real,Q} = one(T), r, 1:1:Q

function partials(z::AsphericSurface{T,3,Q,M}, ρ::T, ϕ::T)::Tuple{SVector{3,T},SVector{3,T}} where {T<:Real,Q,M}
    rad = z.semidiameter
    r = ρ * rad
    r2 = r*r
    t = one(T) - z.curvature^2 * (z.conic + one(T)) * r^2
    if t < zero(T)
        return SVector{3,T}(NaN, NaN, NaN), SVector{3,T}(NaN, NaN, NaN)
    end
    dhdρ = rad * z.curvature * r * sqrt(t) / t
    # sum aspherics partial
    if M != CONIC
        prod, step, mIter = partial_prod_step(z, r, r2)

        ((m, asp), rest) = Iterators.peel(zip(mIter,z.aspherics))
        dhdρ += rad * asp * 2 * prod #first term m=1*2 and prod = r
        for (m,asp) in rest
            prod *= step
            dhdρ += rad * m * asp * prod
        end
    end
    dhdϕ = zero(T)
    cosϕ = cos(ϕ)
    sinϕ = sin(ϕ)
    return SVector{3,T}(rad * cosϕ, rad * sinϕ, dhdρ), SVector{3,T}(r * -sinϕ, r * cosϕ, dhdϕ)
end

function normal(z::AsphericSurface{T,3}, ρ::T, ϕ::T)::SVector{3,T} where {T<:Real}
    du, dv = partials(z, ρ, ϕ)
    if ρ == zero(T) && norm(dv) == zero(T)
        # in cases where there is no δϕ at ρ = 0 (i.e. anything which is rotationally symetric)
        # then we get some big problems, hardcoding this case solves the problems
        return SVector{3,T}(0, 0, 1)
    end
    return normalize(cross(du, dv))
end

function uv(z::AsphericSurface{T,3}, p::SVector{3,T}) where {T<:Real}
    # avoid divide by zero for ForwardDiff
    ϕ = NaNsafeatan(p[2], p[1])
    if p[1] == zero(T) && p[2] == zero(T)
        ρ = zero(T)
    else
        ρ = sqrt(p[1]^2 + p[2]^2) / semidiameter(z)
    end
    return SVector{2,T}(ρ, ϕ)
end

function onsurface(surf::AsphericSurface{T,3}, p::SVector{3,T}) where {T<:Real}
    ρ, ϕ = uv(surf, p)
    if ρ > one(T)
        return false
    else
        surfpoint = point(surf, ρ, ϕ)
        return samepoint(p[3], surfpoint[3])
    end
end

function inside(surf::AsphericSurface{T,3}, p::SVector{3,T}) where {T<:Real}
    ρ, ϕ = uv(surf, p)
    if ρ > one(T)
        return false
    else
        surfpoint = point(surf, ρ, ϕ)
        return p[3] < surfpoint[3]
    end
end

#########################################################################################################

# Assumes the ray has been transformed into the canonical coordinate frame which has the vertical axis passing through (0,0,0) and aligned with the z axis.
function surfaceintersection(surf::AcceleratedParametricSurface{T,3,AsphericSurface{T,3}}, r::AbstractRay{T,3}) where {T<:Real}
    cylint = surfaceintersection(surf.surface.boundingcylinder, r)
    if cylint isa EmptyInterval{T}
        return EmptyInterval(T)
    else
        if doesintersect(surf.triangles_bbox, r) || inside(surf.triangles_bbox, origin(r))
            surfint = triangulatedintersection(surf, r)
            if !(surfint isa EmptyInterval{T})
                return intervalintersection(cylint, surfint)
            end
        end
        # hasn't hit the surface
        if lower(cylint) isa RayOrigin{T} && upper(cylint) isa Infinity{T}
            if inside(surf.surface, origin(r))
                return Interval(RayOrigin(T), Infinity(T))
            else
                return EmptyInterval(T)
            end
            # otherwise check that the intersection is underneath the surface
        else
            p = point(closestintersection(cylint, false))
            ρ, ϕ = uv(surf, p)
            surfpoint = point(surf.surface, ρ, ϕ)
            if p[3] < surfpoint[3]
                return cylint # TODO!! UV (and interface) issues?
            else
                return EmptyInterval(T)
            end
        end
    end
end

function AcceleratedParametricSurface(surf::T, numsamples::Int = 17; interface::NullOrFresnel{S} = NullInterface(S)) where {S<:Real,N,T<:AsphericSurface{S,N}}
    # Zernike uses ρ, ϕ uv space so need to modify extension of triangulation
    a = AcceleratedParametricSurface(surf, triangulate(surf, numsamples, true, false, true, false), interface = interface)
    emptytrianglepool!(S)
    return a
end

function asphericSag(surf::AsphericSurface{T,3,Q, EVEN}) where {T<:Real,Q}
    amin = sum(k < zero(T) ? k * surf.semidiameter^(2m) : zero(T) for (m, k) in enumerate(surf.aspherics))
    amax = sum(k > zero(T) ? k * surf.semidiameter^(2m) : zero(T) for (m, k) in enumerate(surf.aspherics))
    return amin, amax
end

function asphericSag(surf::AsphericSurface{T,3,Q, ODD}) where {T<:Real,Q}
    amin = sum(k < zero(T) ? k * surf.semidiameter^(2m-1) : zero(T) for (m, k) in enumerate(surf.aspherics))
    amax = sum(k > zero(T) ? k * surf.semidiameter^(2m-1) : zero(T) for (m, k) in enumerate(surf.aspherics))
    return amin, amax
end

function asphericSag(surf::AsphericSurface{T,3,Q, ODDEVEN}) where {T<:Real,Q}
    amin = sum(k < zero(T) ? k * surf.semidiameter^(m) : zero(T) for (m, k) in enumerate(surf.aspherics))
    amax = sum(k > zero(T) ? k * surf.semidiameter^(m) : zero(T) for (m, k) in enumerate(surf.aspherics))
    return amin, amax
end

function asphericSag(surf::AsphericSurface{T,3,Q, CONIC}) where {T<:Real,Q}
    amin = zero(T)
    amax = zero(T)
    return amin, amax
end

function BoundingBox(surf::AsphericSurface{T,3}) where {T<:Real}
    xmin = -semidiameter(surf)
    xmax = semidiameter(surf)
    ymin = -semidiameter(surf)
    ymax = semidiameter(surf)
    # aspherics can be more comlpicated, so just take sum of all negative and all positives
    amin, amax = asphericSag(surf)
    zmin = amin
    zmax = amax
    # curvature only goes one way
    q = one(T) - (one(T) + surf.conic) * surf.curvature^2 * surf.semidiameter^2
    if q < zero(T)
        throw(ErrorException("The surface is invalid, no bounding box can be constructed"))
    end
    hmax = surf.curvature * surf.semidiameter^2 / (one(T) + sqrt(q))
    if hmax > zero(T)
        zmax += hmax
    else
        zmin += hmax
    end
    return BoundingBox(xmin, xmax, ymin, ymax, zmin, zmax)
end


"""
`estimate_entrainment_area(arnold_tongue, I_max)`

Estimate entrainment area of the Arnold tongue.

**Arguments**
- `arnold_tongue`: A DataFrame representing an Arnold tongue. The DataFrame has
    two columns names `amplitudes` and `periods`. Periods indicate a position
    at x-axis and amplitudes indices a maximal height at y-axis at which was the
    system entrained.
- `I_max`: Maximal input period that was considered for the Arnold tongue.
"""
function estimate_entrainment_area(arnold_tongue, I_max)
    amplitudes = arnold_tongue[:, "amplitudes"]
    entrainment_area = 1 - mean(amplitudes ./ I_max)
    return entrainment_area
end

"""
`plot_arnold_tongue(arnold_tongue, I_max; ax=gca())`

Plot the Arnold tongue.

**Arguments**
- `arnold_tongue`: A DataFrame representing an Arnold tongue. The DataFrame has
    two columns names `amplitudes` and `periods`. Periods indicate a position
    at x-axis and amplitudes indices a maximal height at y-axis at which was the
    system entrained.
- `I_max`: Maximal input period that was considered for the Arnold tongue.

**Keyword Arguments**
- `ax`: PyPlot axes.
"""
function plot_arnold_tongue(arnold_tongue, I_max; ax=gca())
    periods = arnold_tongue[:, "periods"]
    amplitudes = arnold_tongue[:, "amplitudes"]
    n = length(amplitudes)
    arnold_tongue_matrix = fill(0.0, n, n)

    for i_period = 1:n
        for i_amplitude = 1:n
            amplitude = amplitudes[i_period]
            entrained = (I_max * (i_amplitude/n) ) < amplitude
            arnold_tongue_matrix[i_amplitude, i_period] = entrained
        end
    end
    amplitude_ticks = range(0, I_max, length=n)
    ax.pcolor(periods, amplitude_ticks, arnold_tongue_matrix, rasterized=true)
    ax.set_xticks(range(minimum(periods), maximum(periods), length=3))
    ax.set_yticks(range(minimum(amplitude_ticks), maximum(amplitude_ticks),
        length=4))
    ax.set_xlabel("Input period")
    ax.set_ylabel("Input amplitude")
    ax.set_title("Arnold tongue")

    return ax
end