# Helper functions for pvt properties.

#%% Gas

"""
`LeeGasViscosity(specificGravity, psiAbs, tempF, Z)`

Gas viscosity (μ_g) in centipoise.

Takes gas specific gravity, psia, °F, Z (deviation factor).

Lee et al 1966 method.
"""
function LeeGasViscosity(specificGravity, psiAbs, tempF, Z)

  tempR = tempF + 459.67
  molecularWeight = 28.967 * specificGravity
  density = (psiAbs * molecularWeight * 0.00149406) / (Z*tempR)

  K = ((0.00094 + 2.0* 10^-6.0 *molecularWeight) * tempR^1.5 ) / (200.0 + 19.0*molecularWeight + tempR)
  X = 3.5 + (986.0/tempR) + 0.01*molecularWeight
  Y = 2.4 - 0.2*X

  return K * exp(X * density^Y ) #in centipoise
end

"""
`HankinsonWithWichertPseudoCriticalTemp(specificGravity, molFracCO2, molFracH2S)`

Pseudo-critical temperature, adjusted pseudo-critical temperature in °R, and Wichert correction factor

Takes gas specific gravity, mol fraction of CO₂, mol fraction of H₂S.

Hankin-Thomas-Phillips method, with Wichert and Aziz correction for sour components.
"""
function HankinsonWithWichertPseudoCriticalTemp(specificGravity, molFracCO2, molFracH2S)
  #Hankinson-Thomas-Phillips pseudo-parameter:
  tempPseudoCritical = 170.5 + (307.3*specificGravity)

  #Wichert and Aziz pseudo-param correction for sour gas components:
  A = molFracCO2 + molFracH2S
  B = molFracH2S
  correctionFactor = 120.0*(A^0.9 - A^1.6) + 15.0*(B^0.5 - B^4.0)

  tempPseudoCriticalMod = tempPseudoCritical - correctionFactor

  return tempPseudoCritical, tempPseudoCriticalMod, correctionFactor
end

"""
`HankinsonWithWichertPseudoCriticalPressure(specificGravity, molFracCO2, molFracH2S)`

Pseudo-critical pressure in psia.

Takes gas specific gravity, mol fraction of CO₂, mol fraction of H₂S.

Hankin-Thomas-Phillips method, with Wichert and Aziz correction for sour components.
"""
function HankinsonWithWichertPseudoCriticalPressure(specificGravity, molFracCO2, molFracH2S)
  #Hankinson-Thomas-Phillips pseudo-parameter:
  pressurePseudoCritical = 709.6 - (58.7*specificGravity) #I think the 58.7 is incorrectly identified as 56.7 in Takacs text

  #pseudo-temperatures:
  tempPseudoCritical, tempPseudoCriticalMod, correctionFactor = HankinsonWithWichertPseudoCriticalTemp(specificGravity, molFracCO2, molFracH2S)
  B = molFracH2S

  return pressurePseudoCritical * tempPseudoCriticalMod / (tempPseudoCritical + B*(1-B)*correctionFactor)
end

"""
`PapayZFactor(pressurePseudoCritical, tempPseudoCriticalRankine, psiAbs, tempF)`

Natural gas compressibility deviation factor (Z).

Take pseudocritical pressure (psia), pseudocritical temperature (°R), pressure (psia), temperature (°F).

Papay 1968 method.
"""
function PapayZFactor(pressurePseudoCritical, tempPseudoCriticalRankine, psiAbs, tempF)

  pressurePseudoCriticalReduced = psiAbs/pressurePseudoCritical

  tempPseudoCriticalReduced = (tempF + 459.67)/tempPseudoCriticalRankine

  return 1 - (3.52*pressurePseudoCriticalReduced)/(10^ (0.9813*tempPseudoCriticalReduced)) +
  (0.274*pressurePseudoCriticalReduced*pressurePseudoCriticalReduced)/(10^(0.8157*tempPseudoCriticalReduced))
end

"""
`KareemEtAlZFactor(pressurePseudoCritical, tempPseudoCriticalRankine, psiAbs, tempF)`

Natural gas compressibility deviation factor (Z).

Take pseudocritical pressure (psia), pseudocritical temperature (°R), pressure (psia), temperature (°F).

Can use gauge pressures so long as unit basis matches.

Direct correlation continuous over 0.2 ≤ P_pr ≤ 15.

Kareem, Iwalewa, Al-Marhoun, 2016.
"""
function KareemEtAlZFactor(pressurePseudoCritical, tempPseudoCriticalRankine, psiAbs, tempF)

  P_pr = psiAbs/pressurePseudoCritical

  T_pr = (tempF + 459.67)/tempPseudoCriticalRankine

  if !(0.2 ≤ P_pr ≤ 15) | !(1.15 ≤ T_pr ≤ 3)
    @info "Using Kareem et al Z-factor correlation with values outside of 0.2 ≤ P_pr ≤ 15, 1.15 ≤ T_pr ≤ 3."
  end

  a1 = 0.317842
  a2 = 0.382216
  a3 = -7.768354
  a4 = 14.290531
  a5 = 0.000002
  a6 = -0.004693
  a7 = 0.096254
  a8 = 0.16672
  a9 = 0.96691
  a10 = 0.063069
  a11 = -1.966847
  a12 = 21.0581
  a13 = -27.0246
  a14 = 16.23
  a15 = 207.783
  a16 = -488.161
  a17 = 176.29
  a18 = 1.88453
  a19 = 3.05921

  t = 1 / T_pr
  A = a1 * t * exp(a2 * (1-t)^2) * P_pr
  B = a3 * t + a4 * t^2 + a5 * t^6 * P_pr^6
  C = a9 + a8 * t * P_pr + a7 * t^2 * P_pr^2 + a6 * t^3 * P_pr^3
  D = a10 * t * exp(a11 * (1-t)^2)
  E = a12 * t + a13 * t^2 + a14 * t^3
  F = a15 * t + a16 * t^2 + a17 * t^3
  G = a18 + a19 * t

  y = D * P_pr / ((1 + A^2) / C - (A^2 * B) / C^3)
  z = D * P_pr * (1 + y + y^2 - y^3) / (D * P_pr + E * y^2 - F * y^G) / (1 - y)^3

  return z
end

"""
`KareemEtAlZFactor_simplified(pressurePseudoCritical, tempPseudoCriticalRankine, psiAbs, tempF)`

Natural gas compressibility deviation factor (Z).

Take pseudocritical pressure (psia), pseudocritical temperature (°R), pressure (psia), temperature (°F).

Linearized form from Kareem, Iwalewa, Al-Marhoun, 2016.
"""
function KareemEtAlZFactor_simplified(pressurePseudoCritical, tempPseudoCriticalRankine, psiAbs, tempF)

  P_pr = psiAbs/pressurePseudoCritical

  T_pr = (tempF + 459.67)/tempPseudoCriticalRankine

  if !(0.2 ≤ P_pr ≤ 15) | !(1.15 ≤ T_pr ≤ 3)
    @info "Using Kareem et al Z-factor correlation with values outside of 0.2 ≤ P_pr ≤ 15, 1.15 ≤ T_pr ≤ 3."
  end

  a1 = 0.317842
  a2 = 0.382216
  a3 = -7.768354
  a4 = 14.290531
  a5 = 0.000002
  a6 = -0.004693
  a7 = 0.096254
  a8 = 0.16672
  a9 = 0.96691

  t = 1 / T_pr
  A = a1 * t * exp(a2 * (1-t)^2) * P_pr
  B = a3 * t + a4 * t^2 + a5 * t^6 * P_pr^6
  C = a9 + a8 * t * P_pr + a7 * t^2 * P_pr^2 + a6 * t^3 * P_pr^3

  z = (1+ A^2)/C - A^2 * B / C^3

  return z
end

"""
`gasVolumeFactor(pressureAbs, Z, tempF)`

Corrected gas volume factor (B_g).

Takes absolute pressure (psia), Z-factor, temp (°F).
"""
function gasVolumeFactor(pressureAbs, Z, tempF)

  return 0.0283 * (Z*(tempF+459.67)/pressureAbs)
end

"""
`gasDensity_insitu(specificGravityGas, Z_factor, abspressure, tempF)`

In-situ gas density in lb/ft³ (ρ_g).

Takes gas s.g., Z-factor, absolute pressure (psia), temperature (°F).
"""
function gasDensity_insitu(specificGravityGas, Z_factor, abspressure, tempF)

  tempR = tempF + 459.67

  return 2.7 * specificGravityGas * abspressure / (Z_factor * tempR)
end

#%% Oil
#TODO: add Vasquez-Beggs for solution GOR
#TODO: add Hanafy et al correlations

"""
`StandingBubblePoint(APIoil, sg_gas, R_b, tempF)`

Bubble point pressure in psia.

Takes oil gravity (°API), gas specific gravity, total solution GOR at pressures above bubble point (R_b, scf/bbl), temp (°F).
"""
function StandingBubblePoint(APIoil, sg_gas, R_b, tempF)
    y = 0.00091 * tempF - 0.0125 * APIoil
    return 18.2 * ((R_b/sg_gas)^0.83 * 10^y - 1.4)
end

"""
`StandingSolutionGOR(APIoil, specificGravityGas, psiAbs, tempF)`

Solution GOR (Rₛ) in scf/bbl.

Takes oil gravity (°API), gas specific gravity, pressure (psia), temp (°F), total solution GOR (R_b, scf/bbl), and bubblepoint value (psia).

Standing method.
"""
function StandingSolutionGOR(APIoil, specificGravityGas, psiAbs, tempF, R_b, bubblepoint::Real)

    if psiAbs >= bubblepoint
        return R_b
    else
        y = 0.00091*tempF - 0.0125*APIoil

        return specificGravityGas * (psiAbs / (18 * 10^y) )^1.205 #scf/bbl
    end
end

"""
`StandingSolutionGOR(APIoil, specificGravityGas, psiAbs, tempF)`

Solution GOR (Rₛ) in scf/bbl.

Takes oil gravity (°API), gas specific gravity, pressure (psia), temp (°F), total solution GOR (R_b, scf/bbl), and bubblepoint function.

Standing method.
"""
function StandingSolutionGOR(APIoil, specificGravityGas, psiAbs, tempF, R_b, bubblepoint::Function)

    if psiAbs >= bubblepoint(APIoil, specificGravityGas, R_b, tempF)
        return R_b
    else
        y = 0.00091*tempF - 0.0125*APIoil

        return specificGravityGas * (psiAbs / (18 * 10^y) )^1.205 #scf/bbl
    end
end

"""
`StandingOilVolumeFactor(APIoil, specificGravityGas, solutionGOR, psiAbs, tempF)`

Oil volume factor (Bₒ).

Takes oil gravity (°API), gas specific gravity, solution GOR (scf/bbl), absolute pressure (psia), temp (°F).

Standing method.
"""
function StandingOilVolumeFactor(APIoil, specificGravityGas, solutionGOR, psiAbs, tempF)

  fFactor = solutionGOR * sqrt(specificGravityGas/(141.5/(APIoil + 131.5))) + 1.25*tempF

  return 0.972 + (0.000147) * fFactor^1.175
end

"""
`oilDensity_insitu(APIoil,  specificGravityGas,  solutionGOR,  oilVolumeFactor)`

Oil density (ρₒ) in mass-lbs per ft³.

Takes oil gravity (°API), gas specific gravity, solution GOR (R_s, scf/bbl), oil volume factor.
"""
function oilDensity_insitu(APIoil,  specificGravityGas,  solutionGOR,  oilVolumeFactor)

  return (141.5/(APIoil + 131.5)*350.4 + 0.0764*specificGravityGas*solutionGOR)/(5.61 * oilVolumeFactor) #mass-lbs per ft³
end

"""
`BeggsAndRobinsonDeadOilViscosity(APIoil, tempF)`

Dead oil viscosity (μ_oD) in centipoise.

Takes oil gravity (°API), temp (°F).

Use with caution at 100-150° F: viscosity can be significantly overstated.

Beggs and Robinson method.
"""
function BeggsAndRobinsonDeadOilViscosity(APIoil, tempF)

  if 100 <= tempF <= 155
    @info "Warning: using Beggs and Robinson for dead oil viscosity at $(tempF)° F--consider using another correlation for 100-150° F."
  end

  y = 10^(3.0324 - 0.02023*APIoil)
  x = y*tempF^-1.163

  return 10^x - 1
end

"""
`GlasoDeadOilViscosity(APIoil, tempF)`

Dead oil viscosity (μ_oD) in centipoise.

Takes oil gravity (°API), temp (°F).

Glaso method.
"""
function GlasoDeadOilViscosity(APIoil, tempF)

  return ((3.141* 10^10) / tempF^3.444) * log10(APIoil)^(10.313*log10(tempF) - 36.447)
end

"""
`ChewAndConnallySaturatedOilViscosity(deadOilViscosity,  solutionGOR)`

Saturated oil viscosity (μₒ) in centipoise.

Takes dead oil viscosity (cp), solution GOR (scf/bbl).

Chew and Connally method to correct from dead to live oil viscosity.
"""
function ChewAndConnallySaturatedOilViscosity(deadOilViscosity,  solutionGOR)

   A = 0.2 + 0.8 / 10.0^(0.00081*solutionGOR)
   b = 0.43 + 0.57 / 10.0^(0.00072*solutionGOR)

  return A * deadOilViscosity^b
end

#%% Water

"""
`waterDensity_stb(waterGravity)`

Water density in lb per ft³.

Takes water specific gravity.
"""
function waterDensity_stb(waterGravity)
  return waterGravity * 62.4 #lb per ft^3
end

"""
`waterDensity_insitu(waterGravity, B_w)`

Water density in lb per ft³.

Takes specific gravity, B_w.
"""
function waterDensity_insitu(waterGravity, B_w)
  return waterGravity * 62.4 / B_w #lb per ft^3
end

"""
`GouldWaterVolumeFactor(pressureAbs,  tempF)`

Water volume factor (B_w).

Takes absolute pressure (psia), temp (°F).

Gould method.
"""
function GouldWaterVolumeFactor(pressureAbs,  tempF)

  return 1.0 + 1.21 * 10^-4 * (tempF-60)  + 10^-6 * (tempF-60)^2 - 3.33 * pressureAbs * 10^-6
end

const assumedWaterViscosity = 1.0 #centipoise

#%% Interfacial tension

"""
`gas_oil_interfacialtension(APIoil, pressureAbsolute, tempF)`

Gas-oil interfactial tension in dynes/cm.

Takes oil API, absolute pressure (psia), temp (°F).

Possibly Baker Swerdloff method; same method utilized by [Fekete](https://www.ihsenergy.ca/support/documentation_ca/Harmony/content/html_files/reference_material/calculations_and_correlations/pressure_loss_calculations.htm)
"""
function gas_oil_interfacialtension(APIoil, pressureAbsolute, tempF)

  if tempF <= 68.0
    σ_dead = 39 - 0.2571 * APIoil
  elseif tempF >= 100.0
    σ_dead = 37.5 - 0.2571 * APIoil
  else
    σ_68 = 39 - 0.2571 * APIoil
    σ_100 = 37.5 - 0.2571 * APIoil
    σ_dead = σ_68 + (tempF - 68.0) * (σ_100 - σ_68) / (100.0 - 68.0) # interpolate
  end

  C = 1.0 - 0.024 * pressureAbsolute^0.45
  return C * σ_dead
end

"""
`gas_water_interfacialtension(pressureAbsolute, tempF)`

Gas-water interfactial tension in dynes/cm.

Takes absolute pressure (psia), temp (°F).

Possibly Baker Swerdloff method; same method utilized by [Fekete](https://www.ihsenergy.ca/support/documentation_ca/Harmony/content/html_files/reference_material/calculations_and_correlations/pressure_loss_calculations.htm)
"""
function gas_water_interfacialtension(pressureAbsolute, tempF)

  if tempF <= 74
    return 75 - 1.018 * pressureAbsolute^0.349
  elseif tempF >= 280
    return 53 - 0.1048 * pressureAbsolute^0.637
  else
    σ_74 = 75 - 1.018 * pressureAbsolute^0.349
    σ_280 = 53 - 0.1048 * pressureAbsolute^0.637
    return σ_74 + (tempF - 74.0) * (σ_280 - σ_74) / (280.0 - 74.0) #interpolate
  end
end


using ApproxFun, SingularIntegralEquations, Test

@testset "GreensFun" begin
    cts = 100
    Γ,ψ = ones(cts),ones(cts)
    [(Γ[k],ψ[k]) = (k*Γ[k-1],ψ[k-1]+1/k) for k=2:cts]

    function FK1(z)
        if abs(z) <=.25
            upd = 1.
            ret = upd
            l=1
            zt2 = (.5z)^2
            while norm(upd) >= eps()
                upd = zt2^l/(Γ[l]*Γ[l+1])
                ret += upd
                l+=1
            end
        else
            ret = 2besseli(1,abs(z))/abs(z)
        end
        .5ret
    end
    function GK1(z)
        if abs(z) <= .25
            upd = 1.
            ret = upd
            l=1
            zt2 = (.5z)^2
            while norm(upd) >= eps()
                upd = zt2^l/(Γ[l]*Γ[l+1])*(ψ[l]+ψ[l+1])
                ret += upd
                l+=1
            end
        else
            abz = abs(z)
            ret = 4/abz^2 - 4(besselk(1,abz)-besseli(1,abz)*(log(abz/2)+γ))/abz
        end
        .5ret
    end

    d = Segment(-1.,1.)
    sp = Space(d)
    wsp = JacobiWeight(-0.5,-0.5,sp)
    ⨍ = DefiniteLineIntegral(wsp)
    x = Fun(identity,d)

    β = -2.0
    G = exp(β*x)

    Kf = (x,y)->(1/π*exp(β/2*(y-x))*β^2/2^2*(FK1(β*(y-x)/2)*(log(abs(β)/4) + γ) - GK1(β*(y-x)/2)/2))/sqrt(1-y^2)
    K = GreensFun(Kf,sp⊗wsp)
    @test K(0.1,0.2) ≈ Kf(0.1,0.2)

    K0f = (x,y)-> x == y ? abs(β)/(4sqrt(1-y^2)) : exp(β/2*(y-x))*abs(β)/2*besseli(1,abs(β*(y-x)/2))/abs(y-x)/sqrt(1-y^2)
    K0 = GreensFun(K0f,CauchyWeight(sp⊗wsp,0))
    @test K0(0.1,0.2) ≈ K0f(0.1,0.2)*logabs(0.1-0.2)/π

    K2f = (x,y)->exp(β/2*(y-x))/sqrt(1-y^2)
    K2 = GreensFun(K2f,CauchyWeight(sp⊗wsp,2))
    @test K2(0.1,0.2) ≈ K2f(0.1,0.2)/(0.1-0.2)^2/π
end


include("hydrogenic_orbitals.jl")

using SymPy
using Polynomials

function Base.:(/)(p::Polynomials.Poly, q::Polynomials.Poly)
    all(q.a .>= 0) || sum(q.a) == 1 ||
        throw(ArgumentError("True polynomial division not implemented"))
    i = findfirst(isequal(1), q.a)
    all(p.a[1:(i-1)] .== 0) ||
        throw(ArgumentError("Polynomial division by $q would not result in a polynomial"))
    Polynomials.Poly(p.a[i:end], p.var)
end

function Ykcomp(n,ℓ,n′,ℓ′,k,Z=1//1)
    N,ρ,P=hydredwfn(Tuple,n,ℓ,Z)
    N′,ρ′,P′=hydredwfn(Tuple,n′,ℓ′,Z)

    a = (polyder(ρ)*polyder(ρ′)).a[1]
    r = Polynomials.Poly([0,1], P.var)
    f = P*P′
    g = k > 0 ? f*r^k : f/r^(abs(k))
    I = Polynomials.Poly([0], f.var)

    i = 1
    while true
        I += (isodd(i) ? 1 : -1)*g/a^i

        g = polyder(g)
        i += 1
        iszero(g) && break
    end
    I,r,a
end

function Y(n,ℓ,n′,ℓ′,k,Z=1//1)
    N,ρ,P=hydredwfn(Tuple,n,ℓ,Z)
    N′,ρ′,P′=hydredwfn(Tuple,n′,ℓ′,Z)

    r = symbols("r", real=true, positive=true)

    ee = exp(-(polyval(ρ, r)+polyval(ρ′, r))/2)
    PP = polyval(P*P′, r)

    Ysmaller = integrate(r^k*PP*ee, (r,0,r))/r^k
    Ygreater = integrate(PP*ee/(r^(k+1)), (r,r,oo))*r^(k+1)
    Yᵏ = N*N′*expand(Ysmaller + Ygreater)
end

function triangle_range(a,b)
    kmin = abs(a-b)
    if !iseven(kmin + a + b)
        kmin += 1
    end
    kmin:2:(a+b)
end

function generate_Yᵏ_references()
    nℓs = [(1,0),
           (2,0), (2,1),
           (3,0), (3,1), (3,2),
           (4,0), (4,1), (4,2), (4,3)]
    ks = 0:3

    open("exact_Yks.jl", "w") do file
        write(file, "exact_Yᵏs = Dict(")
        for ((n,ℓ), (n′,ℓ′), k) in Iterators.product(nℓs, nℓs, ks)
            k ∉ triangle_range(ℓ,ℓ′) && continue
            YY = Y(n,ℓ,n′,ℓ′,k)
            write(file, "(($n,$ℓ), ($(n′),$(ℓ′)), $k) => r -> $YY,\n")
        end
        write(file, ")")
    end
end


using BcdV3
using StaticArrays
using MimuConstants
using LinearAlgebra
using Printf

# Noise less simulations
TT = Float64
active_imus = [3,4,5,6, 27,28, 29, 30]
r_all = InitialPosMIMU4444BT()
r_m = r_all[:, active_imus]
r_ref = r_m[:,1]
r_m .-= r_ref
r_all .-= r_ref

sig_a = get_rms_noise_acc()
sig_g = get_rms_noise_gyro()
Na = size(r_m, 2)
Ng = 8

N = 40 # Dyn
Nt = Na + Ng

r = SMatrix{3,Na, TT}(r_m)

w = [deg2rad(1000.0) * @SVector randn(3) for _ = 1:N]
w_dot = [deg2rad(40e3) * @SVector randn(3) for _ = 1:N]
s = [20.0 * @SVector randn(3) for _ = 1:N]

u = vcat(
    [Ω(w[n])^2*r[:,k] + Ω(w_dot[n])*r[:,k] + s[n] for k = 1:Na, n = 1:N],
    [w[n] for k = range(Na+1,length=Ng), n = 1:N]
)
Qu_inv = [SMatrix{3,3,Float64}(I)/( k <= Na ? sig_a^2 :  sig_g^2) for k = 1:Nt]
w0 = [w[n] + 0.1*randn(3) for  n = 1:N]

tol_gs =  1.0e-13
i_max_w = 200
w_hat, w_dot_hat, s_hat =  dynamics(w0, u, Qu_inv, r, Ng, Val{3*(Na+Ng)}(),
    tol_gs, i_max_w)


using Gadfly
using RDatasets
using Distributions

sds = [1, 1/2, 1/4, 1/8, 1/16, 1/32]
n = 10
ys = [mean(rand(Normal(0, sd), n)) for sd in sds]
ymins = ys .- (1.96 * sds / sqrt(n))
ymaxs = ys .+ (1.96 * sds / sqrt(n))
cs = [string(i) for i in 1:n]

plot(x=1:length(sds),
    x=ys, xmin=ymins, xmax=ymaxs,
    y=ys, ymin=ymins, ymax=ymaxs, color=cs, Geom.point, Geom.errorbar)


using CompScienceMeshes
using BEAST
using LinearAlgebra
using Test
using StaticArrays

o, x, y, z = euclidianbasis(3)

p1 = 2x
p2 = y
p3 = 3z
p4 = o
tet = simplex(p1, p2, p3, p4)

q = abs(CompScienceMeshes.relorientation([1,2,3],[1,2,3,4]))
tri = simplex(p1, p2, p3)
n = normal(tri)

i = 3
j = 3
edg = simplex(p1, p2)

T = Float64
x = BEAST.NDLCDRefSpace{T}()
y = BEAST.NDRefSpace{T}()

p = neighborhood(tet, [0.5, 0.5, 0.0])
r = neighborhood(tri, [0.5, 0.5])

@test carttobary(edg, cartesian(p)) ≈ [0.5]
@test carttobary(edg, cartesian(r)) ≈ [0.5]

xp = x(p)[j].value
yr = y(r)[i].value

a, b = extrema((n × xp) ./ yr)
@test a ≈ b

tgt = p2 - p1
a2 = dot(n × xp, tgt) / dot(yr, tgt)
@test a ≈ a2

z = a * yr
@test z ≈ n × xp

volume(simplex(p1,p2,p4))
volume(simplex(p1,p2))

Q = BEAST.ttrace(x, tet, q, tri)

p = neighborhood(tet, [1/3, 1/3, 1/3])
r = neighborhood(tri, [1/3, 1/3])
@test cartesian(p) ≈ cartesian(r)

xp = n × x(p)[j].value
yr = y(r)[i].value
@test xp ≈ yr * Q[i,j]

x = BEAST.NDLCCRefSpace{Float64}()
y = BEAST.RTRefSpace{Float64}()
for q in 1:4
    q = 3
    fc = BEAST.faces(tet)[q]
    Q = BEAST.ttrace(x, tet, q, fc)

    nbdi = CompScienceMeshes.center(fc)
    nbdj = neighborhood(tet, carttobary(tet, cartesian(nbdi)))

    xvals = x(nbdj)
    yvals = y(nbdi)

    for j in 1:6
        trc = sum(Q[i,j]*yvals[i].value for i in 1:3)
        @test isapprox(trc, normal(fc) × xvals[j].value, atol=1e-4)
    end
end

# test the case where intrinsic and extrinsic orientations differ
o, x, y, z = euclidianbasis(3)
fc = simplex(z,o,y)
x = BEAST.NDLCCRefSpace{Float64}()
y = BEAST.RTRefSpace{Float64}()
Q = BEAST.ttrace(x, tet, 3000, fc)

nbdi = CompScienceMeshes.center(fc)
nbdj = neighborhood(tet, carttobary(tet, cartesian(nbdi)))

@test cartesian(nbdi) ≈ cartesian(nbdj)

xvals = x(nbdj)
yvals = y(nbdi)

for j in 1:6
    trc = sum(Q[i,j]*yvals[i].value for i in 1:3)
    @test isapprox(trc, -normal(fc) × xvals[j].value, atol=1e-4)
end

o, x, y, z = euclidianbasis(3)
m = Mesh([x,y,z,o], [@SVector[1,2,3,4]])

m1 = skeleton(m,1)
X = BEAST.nedelecc3d(m, m1)
@test numfunctions(X) == 6

# m2 = skeleton(m,2)
m2 = boundary(m)
Y = BEAST.ttrace(X,m2)
@test numfunctions(Y) == 6

pa = Y.fns[1][1].cellid
pb = Y.fns[1][2].cellid

tria = chart(m2, cells(m2)[pa])
trib = chart(m2, cells(m2)[pb])

ra = Y.fns[1][1].refid
rb = Y.fns[1][2].refid

ctra = cartesian(CompScienceMeshes.center(CompScienceMeshes.edges(tria)[ra]))
ctrb = cartesian(CompScienceMeshes.center(CompScienceMeshes.edges(trib)[rb]))
@test ctra ≈ ctrb
# tri1 = chart(m, cells(m)[Y.fns[1][1].cellid])

# ctr1 = cartesian(center(CompScienceMeshes.edges(tri)[]))


module JPEGs

include("color_space_transformation.jl")
include("downsampling.jl")
include("block_splitting.jl")
include("dct.jl")
include("quantization.jl")
include("entropycoding.jl")
include("segments.jl")

function encode_image(img::Array{ColorTypes.RGB{FixedPointNumbers.Normed{UInt8,8}},2})
    i2 = JPEGs.rgb_to_ycbcr(img)
    y,cb,cr = JPEGs.split_channels(i2)
    #dcb = JPEGs.downsample_4_2_0(cb)
    #dcr = JPEGs.downsample_4_2_0(cr)
    y = JPEGs.split_into_blocks(y)
    cb = JPEGs.split_into_blocks(cb)
    cr = JPEGs.split_into_blocks(cr)
    y = JPEGs.dct.(y)
    cb = JPEGs.dct.(cb)
    cr = JPEGs.dct.(cr)

    y = JPEGs.quantize.(y)
    cb = JPEGs.quantize.(cb)
    cr = JPEGs.quantize.(cr)
    y = JPEGs.zigzag.(y)
    cb = JPEGs.zigzag.(cb)
    cr = JPEGs.zigzag.(cr)
    encoded_y = JPEGs.encode_zigzagged_block.(y)
    encoded_cb = JPEGs.encode_zigzagged_block.(cb)
    encoded_cr = JPEGs.encode_zigzagged_block.(cr)
    interlaced = [vcat(i...) for i in zip(encoded_y, encoded_cb, encoded_cr)]
    flattened = reshape(interlaced, *(size(interlaced)...))
    encoded_blocks = vcat(flattened...)
    serialized = JPEGs.serialize_jpeg_scan(encoded_blocks)
    return serialized
end

function decode_image(serialized_jpeg::Vector{UInt8})#::Array{ColorTypes.RGB{FixedPointNumbers.Normed{UInt8,8}},2}
    allbits = JPEGs.deserialize_into_jpeg_scan(serialized_jpeg)
    decoded_blocks = []
    while length(decoded_blocks) < (64*64*3)-1
        if length(decoded_blocks) == 12287
            println(allbits)
        end
        push!(decoded_blocks, decode_zigzagged_block(allbits))
    end
    y = []
    cb = []
    cr = []
    for b in 1:3:length(decoded_blocks)
        push!(y, JPEGs.unzigzag(decoded_blocks[b]))
        push!(cb, JPEGs.unzigzag(decoded_blocks[b+1]))
        push!(cr, JPEGs.unzigzag(decoded_blocks[b+2]))
    end
    y = JPEGs.unquantize.(y)
    cb = JPEGs.unquantize.(cb)
    cr = JPEGs.unquantize.(cr)
    y = JPEGs.invdct.(y)
    cb = JPEGs.invdct.(cb)
    cr = JPEGs.invdct.(cr)
    y = reshape(y, (64,64))
    cb = reshape(cb, (64,64))
    cr = reshape(cr, (64,64))
    y = JPEGs.join_blocks(y)
    cb = JPEGs.join_blocks(cb)
    cr = JPEGs.join_blocks(cr)
    i2 = JPEGs.join_channels((y,cb,cr))
    img = JPEGs.ycbcr_to_rgb(i2)
    return img
end

end


@testset "abc exponential" begin
    gs_p = randn()^2

    data = Dict(
        :y => rand(Exponential(gs_p), 1000)
    )

    mod = Model(
        y=Stochastic(1, (l) -> Exponential(l), false),
        l=Stochastic(() -> LogNormal(), true)
    )

    inits = [
        Dict{Symbol,Any}(
          :y => data[:y],
          :l => rand(LogNormal()),
            )
        for i in 1:3
    ]

    f = x -> [mean(x), std(x)]
    scheme = [ABC(:l, 0.01, f, 0.01, nsim = 3, maxdraw=15)]

    setsamplers!(mod, scheme)

    sim = mcmc(mod, data, inits, 100000, burnin=25000, thin=10, chains=2)

    while maximum(gelmandiag(sim).value[1,2,:]) > 1.1
        @show maximum(gelmandiag(sim).value[1,2,:])
        sim = mcmc(sim, 10000)
    end

    m = mean(sim.value[:,1,:])
    sd = std(sim.value[:,1,:])

    @test m - sd <= gs_p <= m + sd
end

@testset "abc Normal" begin
    gs_m = randn()
    gs_s = randn()^2
    data = Dict(
        :y => rand(Normal(gs_m, gs_s), 1000)
    )

    mod = Model(
        y=Stochastic(1, (m, s) -> Normal(m, s), false),
        m=Stochastic(() -> Normal(), true),
        s=Stochastic(() -> Exponential(), true)
    )

    inits = [
        Dict{Symbol,Any}(
          :y => data[:y],
          :m => gs_m,
          :s => rand(),
        )
        for i in 1:3
    ]

    f = x -> [mean(x), std(x)]
    scheme = [ABC(:m, 0.01, f, 0.001, nsim = 3, maxdraw=15),
              ABC(:s, 0.01, f, 0.001, nsim = 3, maxdraw=15)]

    setsamplers!(mod, scheme)

    sim = mcmc(mod, data, inits, 100000, burnin=25000, thin=10, chains=2)

    while maximum(gelmandiag(sim).value[1:2,2,:]) > 1.1
        @show maximum(gelmandiag(sim).value[1:2,2,:])
        sim = mcmc(sim, 10000)
    end

    m = mean(sim.value[:,1,:])
    sd = std(sim.value[:,1,:])

    @test m - sd <= gs_s <= m + sd

    m = mean(sim.value[:,2,:])
    sd = std(sim.value[:,2,:])

    @test m - sd <= gs_m <= m + sd
end


using ReachabilityBenchmarks, LazySets, MathematicalPredicates, MAT

function pde_specification()
    # initial set:
    # xᵢ = 0 for i ≤ 64
    # xᵢ ∈ [0.001, 0.0015] for i ∈ [65, 80]
    # xᵢ ∈ [-0.002, -0.0015] for i > 80
    X0 = Hyperrectangle(low=[zeros(64); fill(0.001, 16); fill(-0.002, 4)],
                        high=[zeros(64); fill(0.0015, 16); fill(-0.0015, 4)])

    # safety property: y ≤ 12 for linear combination y (defined in out.mat)
    y = read(matopen(@relpath "out.mat"), "M")[1, :]
    property = is_contained_in(HalfSpace(y, 12.0))

    # time horizon: 20 time units
    time_horizon = 20.0

    # specification
    O = Dict(:T => time_horizon, :property => property)

    return X0, O
end


# test for mat_center function
@testset "Test mat_center function $mat" for mat in ["her", "rec"]
    # test for Hermitian matrix
    if mat == "her"
        for idx = 1: 10
            @test abs(tr(mat_center(randn(3,3); mat = mat))) <= 10*eps(Float64);
        end
    end

    # test for rectangular case
    if mat == "rec"
        for idx = 1: 10
            @test sum(vec(mat_center(randn(3,3); mat ="rec"))) <= 10*eps(Float64)
        end
    end
end

# test for free_whiten function
@testset "Test free_whiten function $mat" for mat in ["her", "rec"]
    # test for Hermitian matrix
    if mat == "her"
        for idx = 1: 10
            G = randn(10, 10)
            Z = [G + G', G*G']
            Y = free_whiten(Z; mat = "her")[1]
            @test maximum(abs.( tr.(Y*Y')./10 - I)) < 10*eps(Float64)
        end
    end

    # test for rectangular matrix
    if mat == "rec"
        for idx = 1: 10
            Z = [randn(10, 20) for i = 1: 3]
            Y = free_whiten(Z; mat = "rec")[1]
            @test maximum(abs.( tr.(Y*Y')./10 - I)) < 10*eps(Float64)
        end
    end
end


"""
Unimodal Normal Distribution Crossover (UNDX).

See
    Kita, H., Ono, I., and Kobayashi, S., "Multi-parental Extension of the Unimodal Normal Distribution Crossover for Real-coded Genetic Algorithms," Proceedings of the 1999 Congress on Evolutionary Computation, pp. 1581-1588, 1999.
    Deb, K., Anand, A., and Joshi, D., "A Computationally Efficient Evolutionary Algorithm for Real-Parameter Optimization," Evolutionary Computation, vol. 10, no. 4, pp. 371-395, 2002.
"""
struct UnimodalNormalDistributionCrossover{NP} <: CrossoverOperator{NP,1}
	ζ::Float64 # sd for parent-defined orthogonal directions
    η::Float64 # sd for the directions orthogonal to the parent-defined ones, divided by √n

    function UnimodalNormalDistributionCrossover{NP}(ζ::Number, η::Number) where NP
        ζ > 0 || throw(ArgumentError("ζ must be positive"))
        η >= 0 || throw(ArgumentError("η must be non-negative"))
        new{NP}(ζ, η)
    end
    UnimodalNormalDistributionCrossover{NP}(params::Parameters) where NP =
        new{NP}(params[:UNDX_ζ], params[:UNDX_η])
end

const UNDX_DefaultOptions = ParamsDict(
    :UNDX_ζ => 0.5,
    :UNDX_η => 0.1
)

function apply!(xover::UnimodalNormalDistributionCrossover{NP},
                target::AbstractIndividual, targetIndex::Int,
                pop, parentIndices) where NP
    @assert length(parentIndices) == NP

    parents = pop[:, parentIndices]
    center = mean(parents, dims=2)
    parents .-= center
    sd = mean(map(sqrt, sum(abs2, parents, dims=2)))
    svdf = svd(parents, full=false)
    svals = svdf.S / norm(svdf.S)
    @inbounds for (i,s) in enumerate(svals)
        svals[i] = sd*randn() * (
            abs(s) > 1E-8 ? # check if parent subspace; FIXME is the threshold correct?
            s * xover.ζ : xover.η )
    end

    mul!(target, svdf.U, svals)
    @inbounds @simd for i in eachindex(target)
        target[i] += center[i,1]
    end
    return target
end


#import only the needed parts not to have conflict
import NNlib.conv, NNlib.conv!

### NNConv
function NNConv!(cLayer::Conv1D, Ai::AbstractArray{T,3}) where {T}
    padS = paddingSize(cLayer, Ai)
    cLayer.Z = conv(Ai, cLayer.W[end:-1:1, :, :], stride = cLayer.s, pad = padS)
    Z = cLayer.Z .+= cLayer.B[:, 1, :]
    actFun = cLayer.actFun
    cLayer.A = eval(:($actFun($Z)))

    return nothing
end #function img2colConvolve(cLayer::Conv1D

function NNConv!(cLayer::Conv2D, Ai::AbstractArray{T,4}) where {T}
    padS = paddingSize(cLayer, Ai)
    cLayer.Z = conv(Ai, cLayer.W[end:-1:1, end:-1:1, :, :], stride = cLayer.s, pad = padS)
    Z = cLayer.Z .+= cLayer.B[:, :, 1, :]
    actFun = cLayer.actFun
    cLayer.A = eval(:($actFun($Z)))

    return nothing
end #function img2colConvolve(cLayer::Conv2D

function NNConv!(cLayer::Conv3D, Ai::AbstractArray{T,5}) where {T}
    padS = paddingSize(cLayer, Ai)
    cLayer.Z = conv(
        Ai,
        cLayer.W[end:-1:1, end:-1:1, end:-1:1, :, :],
        stride = cLayer.s,
        pad = padS,
    )
    Z = cLayer.Z .+= cLayer.B[:, :, :, 1, :]
    actFun = cLayer.actFun
    cLayer.A = eval(:($actFun($Z)))

    return nothing
end #function img2colConvolve(cLayer::Conv3D

export NNConv!

### dNNConv!

#import only the needed parts not to have conflict
import NNlib.∇conv_data, NNlib.∇conv_filter, NNlib.DenseConvDims

function dNNConv!(
    cLayer::Conv1D,
    dZ::AbstractArray{T,3},
    Ai::AoN = nothing,
    # Ao::AoN = nothing,
) where {AoN<:Union{AbstractArray,Nothing},T}

    if Ai == nothing
        Ai = cLayer.prevLayer.A
    end
    # if Ao == nothing
    #     Ao = cLayer.A
    # end
    W = cLayer.W
    padS = paddingSize(cLayer, Ai)
    convdim = DenseConvDims(Ai, W, stride = cLayer.s, padding = padS)
    cLayer.dA = ∇conv_data(dZ, W[end:-1:1, :, :], convdim)
    cLayer.dW[end:-1:1, :, :] = ∇conv_filter(Ai, dZ, convdim)
    cLayer.dB = sum(permutedims(dZ, [1, 3, 2]), dims = 1:2)

    return nothing
end #function img2colConvolve(cLayer::Conv1D

function dNNConv!(
    cLayer::Conv2D,
    dZ::AbstractArray,
    Ai::AoN = nothing,
    # Ao::AoN = nothing,
) where {AoN<:Union{AbstractArray,Nothing},T}

    if Ai == nothing
        Ai = cLayer.prevLayer.A
    end
    # if Ao == nothing
    #     Ao = cLayer.A
    # end
    W = cLayer.W
    convdim = DenseConvDims(Ai, W, stride = cLayer.s)
    padS = paddingSize(cLayer, Ai)
    convdim = DenseConvDims(Ai, W, stride = cLayer.s, padding = padS)
    cLayer.dA = ∇conv_data(dZ, W[end:-1:1, end:-1:1, :, :], convdim)
    cLayer.dW[end:-1:1, end:-1:1, :, :] = ∇conv_filter(Ai, dZ, convdim)
    cLayer.dB = sum(permutedims(dZ, [1, 2, 4, 3]), dims = 1:3)

    return nothing
end #function img2colConvolve(cLayer::Conv2D

function dNNConv!(
    cLayer::Conv3D,
    dZ::AbstractArray,
    Ai::AoN = nothing,
    # Ao::AoN = nothing,
) where {AoN<:Union{AbstractArray,Nothing},T}

    if Ai == nothing
        Ai = cLayer.prevLayer.A
    end
    # if Ao == nothing
    #     Ao = cLayer.A
    # end
    W = cLayer.W
    convdim = DenseConvDims(Ai, W, stride = cLayer.s)
    padS = paddingSize(cLayer, Ai)
    convdim = DenseConvDims(Ai, W, stride = cLayer.s, padding = padS)
    cLayer.dA = ∇conv_data(dZ, W[end:-1:1, end:-1:1, end:-1:1, :, :], convdim)
    cLayer.dW[end:-1:1, end:-1:1, end:-1:1, :, :] = ∇conv_filter(Ai, dZ, convdim)
    cLayer.dB = sum(permutedims(dZ, [1, 2, 3, 5, 4]), dims = 1:4)

    return nothing
end #function img2colConvolve(cLayer::Conv3D

export dNNConv!


export train!

# train core 1 {{{
function train_core1(p::Real,q::Real)
	return p * q
end

function train_core1{T<:Real}(p::Complex{T},q::Complex{T})
	return p * conj(q)
end

function train_core1{T<:Real}(p::Quaternion{T},q::Quaternion{T})
	return p * conj(q)
end
# }}}

# train core 2 {{{
function train_core2(x::Real,nn::Real)
	return x/nn
end

function train_core2{T<:Real}(z::Complex{T},nn::Real)
	return z/(2.0*nn)
end

function train_core2{T<:Real}(q::Quaternion{T},nn::Real)
	return q/(4.0*nn)
end
# }}}

function train!{T<:Number}(net::HopfieldNetwork, patterns::Array{T,2})
	nn,np = size(patterns)

	# calc synaptic weight (Hebbian learning rule)
	for p in 1:nn
		for q in p+1:nn
			s = zero(T)
			for mu in 1:np
				s += train_core1(patterns[p, mu], patterns[q, mu])
			end
			s = train_core2(s,nn)
			net.weight[p, q] = s
			net.weight[q, p] = conj(s)
		end
	end

	return
end

function train_core{T<:Real}(patterns::AbstractArray{T},nn::Real)
	tmp = BLAS.syrk('U','T',1.0/nn, patterns')
	for i=1:nn
		tmp[i,i] = zero(T)
		for j=i+1:nn
			tmp[j,i] = tmp[i,j]
		end
	end

	return tmp
end

function train_fast!{T <: Number}(net::HopfieldNetwork, patterns::Array{T})
	nn = length(net.state) # num of neurons
	np = size(patterns, 2) # num of patterns

	# calc synaptic weight
	net.Weight = train_core(patterns,nn)
	return
end


function solve_ip(input::Input)

    init_log_ip(input)
    n = input.n
    m = input.m
    rho = input.rho
    alpha = input.alpha
    tol = input.tol
    A = input.A
    b = input.b
    c = input.c

    nx = (1:n)
    np = (n+1:m+n)
    ns = (m+n+1:m+2n)

    x = ones(n)
    s = ones(n)
    p = ones(m)
    X = x
    S = s
    P = p
    gap = []
    mu = rho*x'*s/n

    for iter in 1:input.max_iter
        F = [A zeros(m,m) zeros(m,n);
            zeros(n,n) A' I(n);
            Diagonal(s) zeros(n,m) Diagonal(x)]

        g = [A*x - b; A'p + s + c; x.*s .- mu]

        d = - F \ g

        d_x = d[nx]
        d_p = d[np]
        d_s = d[ns]

        filter_x = d_x .< 0.0
        filter_p = d_p .< 0.0
        filter_s = d_s .< 0.0

        if sum(filter_x) == 0
            beta_x = 1.0
        else
            beta_x = min(1.0, alpha*minimum((-x./d_x)[filter_x]))
        end

        if sum(filter_p) == 0
            beta_p = 1.0
        else
            beta_p = min(1.0, alpha*minimum((-p./d_p)[filter_p]))
        end

        if sum(filter_s) == 0
            beta_s = 1.0
        else
            beta_s = min(1.0, alpha*minimum((-s./d_s)[filter_s]))
        end

        x += beta_x * d_x
        p += beta_s * d_p
        s += beta_s * d_s
        z = c'x
        w = b'p
        dual_inf = x'*s

        if z > 1/tol
            output = OutputIP(x,s,p,X,S,P,gap,mu,Inf,2,input.max_iter)
            last_log(input, output)
            return output
        end

        if w > 1/tol
            output = OutputIP(x,s,p,X,S,P,gap,mu,-Inf,3,input.max_iter)
            last_log(input, output)
            return output
        end
        X = [X x]
        S = [S s]
        P = [P p]
        push!(gap,dual_inf)
        iteration_log(input,iter,z,dual_inf)
        mu = rho*dual_inf/n
        if mu < tol
            output = OutputIP(x,s,p,X,S,P,gap,mu,z,1,iter)
            last_log(input, output)
            return output
        end
    end
    z = c'x
    output = OutputIP(x,s,p,X,S,P,gap,mu,z,0,input.max_iter)
    last_log(input, output)
    return output
end

function crossover(input::Input, output::OutputIP)
    m = input.m
    n = input.n
    crossover_tol = input.crossover_tol
    x = output.x
    midterm = MidTerm()

    base = []
    nbase = []

    for i in 1:n
        if x[i] > 1E-3
            push!(base,i)
        else
            push!(nbase,i)
        end
    end

    midterm.base = base
    midterm.nbase = nbase

    B = view(input.A,:,midterm.base)
    for (k,w) in enumerate(midterm.nbase)
        sol = B \ input.A[:,w]
        if norm(B*sol - input.A[:,w]) > crossover_tol
            push!(midterm.base, w)
            deleteat!(midterm.nbase, k)
            if length(midterm.base) == m
                break
            end
        end
    end

    init_log_simplex2(input)
    while midterm.termination_status == 0 && midterm.iter < input.max_iter
        iterate!(input, midterm)
    end
    update_midterm!(input, midterm)
    output = write_output(input, midterm)
    return output
end


@testset "ff f4 certify" begin
    R, (x, y, z) = PolynomialRing(GF(2^31 - 1), ["x","y","z"], ordering=:lex)

    @test Groebner.groebner([x, y], certify=true) == [y, x]
    @test Groebner.groebner([y, x], certify=true) == [y, x]

    fs = [x^2 + y, x*y]
    @test Groebner.groebner(fs, certify=true) == [y^2, x*y, x^2 + y]
end

@testset "qq f4 certify" begin

    root = Groebner.change_ordering(Groebner.rootn(3, ground=QQ), :degrevlex)
    x1, x2, x3 = gens(parent(first(root)))
    gb = Groebner.groebner(root, certify=true)
    @test gb == [x1 + x2 + x3, x2^2 + x2*x3 + x3^2, x3^3 - 1]

    root = Groebner.change_ordering(Groebner.rootn(6, ground=QQ), :deglex)
    x1, x2, x3, x4, x5, x6 = gens(parent(first(root)))
    gb = Groebner.groebner(root, certify=true)
    @test gb == [
        x1 + x2 + x3 + x4 + x5 + x6,
        x2^2 + x2*x3 + x3^2 + x2*x4 + x3*x4 + x4^2 +
        x2*x5 + x3*x5 + x4*x5 + x5^2 + x2*x6 + x3*x6 + x4*x6 +
        x5*x6 + x6^2, x3^3 + x3^2*x4 + x3*x4^2 + x4^3 +
        x3^2*x5 + x3*x4*x5 + x4^2*x5 + x3*x5^2 + x4*x5^2 +
        x5^3 + x3^2*x6 + x3*x4*x6 + x4^2*x6 + x3*x5*x6 +
        x4*x5*x6 + x5^2*x6 + x3*x6^2 + x4*x6^2 + x5*x6^2 +
        x6^3, x4^4 + x4^3*x5 + x4^2*x5^2 + x4*x5^3 + x5^4 +
        x4^3*x6 + x4^2*x5*x6 + x4*x5^2*x6 + x5^3*x6 + x4^2*x6^2 +
        x4*x5*x6^2 + x5^2*x6^2 + x4*x6^3 + x5*x6^3 + x6^4, x5^5 +
        x5^4*x6 + x5^3*x6^2 + x5^2*x6^3 + x5*x6^4 + x6^5,
        x6^6 - 1]

    ku = Groebner.change_ordering(Groebner.ku10(ground=QQ), :degrevlex)
    x1, x2, x3, x4, x5, x6, x7, x8, x9, x10 = gens(parent(first(ku)))
    gb = Groebner.groebner(ku, certify=true)

    @test gb == [
        x9 + 230965//116706*x10 - 697789//116706,
        x8 + 92386//335511*x10 + 578636//335511,
        x7 + 5173616//8563059*x10 - 30862793//8563059,
        x6 + 87951472//192015891*x10 + 488096201//192015891,
        x5 + 131980//18759*x10 - 56944//18759,
        x4 + 32995//6084*x10 - 63415//6084,
        x3 - 263960//229671*x10 + 493631//229671,
        x2 - 65990//17043*x10 + 151205//17043,
        x1 - 26396//3273*x10 + 19850//3273,
        x10^2 - 71551//26396*x10 + 45155//26396]

end


# 64-QAM
"""
---
Calculate a euclidean distance based on input sample and alphabet distribution.
Used for symbolDemappingQAM function
# --- Syntax
	getDist!(out,sR,alphabet, a1...aN)
# --- Input parameters
- out	  : Float64 array of size N
- sR	  : Incoming real sequence
- alphabet : Gray base for decision
- a1...aN : N neigthbours for soft symbol demapper
# --- Output parameters
- []
# ---
# v 1.0 - Robin Gerzaguet.
"""
# 256-QAM
function getDist!(baseComp,sR,alphabet,a1,a2,a3,a4,a5,a6,a7,a8)
	baseComp[1] = abs2.(sR - alphabet[a1]);
	baseComp[2] = abs2.(sR - alphabet[a2]);
	baseComp[3] = abs2.(sR - alphabet[a3]);
	baseComp[4] = abs2.(sR - alphabet[a4]);
	baseComp[5] = abs2.(sR - alphabet[a5]);
	baseComp[6] = abs2.(sR - alphabet[a6]);
	baseComp[7] = abs2.(sR - alphabet[a7]);
	baseComp[8] = abs2.(sR - alphabet[a8]);
	return minimum(baseComp);
end

# 64-QAM
function getDist!(baseComp,sR,alphabet,a1,a2,a3,a4)
	baseComp[1] = abs2.(sR - alphabet[a1]);
	baseComp[2] = abs2.(sR - alphabet[a2]);
	baseComp[3] = abs2.(sR - alphabet[a3]);
	baseComp[4] = abs2.(sR - alphabet[a4]);
	return minimum(baseComp);
end
# 16-QAM
function getDist!(baseComp,sR,alphabet,a1,a2)
	baseComp[1] = abs2.(sR .- alphabet[a1]);
	baseComp[2] = abs2.(sR .- alphabet[a2]);
	return minimum(baseComp);
end

"""
---
Returns the final LLR value based on input distances
# --- Syntax
llr = calcLLR(e0,e1,c)
# --- Input parameters
- e0  : Minimal distance 1
- e1  : Minimal distance 2
- c	  : Channel estimate
# --- Output parameters
- llr : Max likelihood estimate
# ---
# v 1.0 - Robin Gerzaguet.
"""
function calcLLR(e0,e1,c)
	return (e0-e1) * c;
end

"""
---
# --- Description
Returns the log likelihood ratio of the incoming sequence qamSeq based on the channel estimates channelIn. Max log approximation is considered
qamSeq is an input noisy QAM sequence with same size of channel estimate vector
Output is a vector of soft output binary sequence to be fed in a FEC
# --- Syntax
 output	  = symbolDemappingQAM(mcs,qamSeq,channel)
# --- Input parameters
- mcs	  : Constellation size (from 4 to 256) [Int]
- qamSeq  : Complex noisy received sequence (after equalization) [Array{Float64},N]
- channel : Complex channel estimate [Array{Float64},N]
# --- Output parameters
- output  : Soft bits [Array{UInt8},N*log2(mcs)]
"""
function symbolDemappingQAM(mcs,qamSeq,channel)
	# --- Creating output array for allocation
	output = zeros(Float64,Int(length(qamSeq) * log2(mcs)));
	# --- Call bang method
	symbolDemappingQAM!(output,mcs,qamSeq,channel)
	# --- Return array
	return output;
end

"""
---
# --- Description
Returns the log likelihood ratio of the incoming sequence qamSeq based on the channel estimates channelIn
qamSeq is an input noisy QAM sequence with same size of channel estimate vector
Output is populated by  soft output binary sequence to be fed in a FEC
# --- Syntax
 output	  = :symbolDemappingQAM(mcs,qamSeq,channel)
# --- Input parameters
- output  : Soft bits [Array{UInt8},N*log2(mcs)]
- mcs	  : Constellation size (from 4 to 256) [Int]
- qamSeq  : Complex noisy received sequence (after equalization) [Array{Float64},N]
- channel : Complex channel estimate [Array{Float64},N]
# --- Output parameters
- []
# ---
# v 1.0 - Robin Gerzaguet.
"""
function symbolDemappingQAM!(output,mcs,qamSeq,channel)
	# ----------------------------------------------------
	# --- Overall parameters
	# ----------------------------------------------------
	# --- Defining scaling factor
	# Constellation output should have an unitary average power
	# σ^2 = ∑ p(a_i)  ⃒ a_i ⃒ ^2 = 1
	scalingFactor	= sqrt(2/3*(mcs-1));
	# --- Getting size of input
	nbSymb	= length(qamSeq);
	# ----------------------------------------------------
	## --- Symbol demapper
	# ----------------------------------------------------
	if mcs == 4
		# ----------------------------------------------------
		# --- QPSK scheme
		# ----------------------------------------------------
		# Decision is based on euclidean distance with 1/sqrt(2) and -1/sqrt(2).
		# We can do a direct form here.
		output[1:2:end] .= ((abs2.(real(qamSeq).+1/scalingFactor) .- abs2.(real(qamSeq).-1/scalingFactor)).*abs2.(channel))./2;
		output[2:2:end] .= ((abs2.(imag(qamSeq).+1/scalingFactor) .- abs2.(imag(qamSeq).-1/scalingFactor)).*abs2.(channel))./2;
	elseif mcs == 16
		# ----------------------------------------------------
		# --- 16-QAM scheme
		# ----------------------------------------------------
		#maximum log approximation
		# -------------------------%
		#   |      |     |     |
		#  10     11    01    00
		#  -3     -1     1     3
		alphabet  = [-3 -1 1 3]/ scalingFactor;
		# Trying to guess if the first bit  is a 0 or a 1
		# See bitMappingQAM: realSymb = 2bitSeq[4(i-1)+1] + bitSeq[4(i-1)+3]
		# * LSB is third bit for real part
		# * MSB is first bit for real part
		# --> 0 if -3 and 3
		# ---> 1 if -1 and 1
		# Compute here the maxlog approx --> minimum of distance
		# --- We create the container for comparison
		baseComp = zeros(Float64,2);
		# --- Iterative decision
		for iN = 1 : 1 : nbSymb
			c	= abs2(channel[iN]);
			# MSB -> Re
			e_porteur0 = getDist!(baseComp,qamSeq[iN].re,alphabet,3,4)
			e_porteur1 = getDist!(baseComp,qamSeq[iN].re,alphabet,1,2)
			output[4*(iN-1)+1]  = calcLLR(e_porteur0,e_porteur1,c);
			# LSB -> Re
			e_porteur0 = getDist!(baseComp,qamSeq[iN].re,alphabet,1,4)
			e_porteur1 = getDist!(baseComp,qamSeq[iN].re,alphabet,2,3)
			output[4*(iN-1)+3]  =calcLLR(e_porteur0,e_porteur1,c);
			# MSB -> Im
			e_porteur0 = getDist!(baseComp,qamSeq[iN].im,alphabet,3,4)
			e_porteur1 = getDist!(baseComp,qamSeq[iN].im,alphabet,1,2)
			output[4*(iN-1)+2]  =calcLLR(e_porteur0,e_porteur1,c);
			# LSB -> Im
			e_porteur0 = getDist!(baseComp,qamSeq[iN].im,alphabet,1,4)
			e_porteur1 = getDist!(baseComp,qamSeq[iN].im,alphabet,2,3)
			output[4*(iN-1)+4]  =calcLLR(e_porteur0,e_porteur1,c);
		end
	elseif mcs == 64
		# ----------------------------------------------------
		# --- 64-QAM Soft demapper
		# ----------------------------------------------------
		#maximum log approximation
		# ------------------------------------------------#
		#   |      |     |     |     |     |      |      |
		#  100    101   111   110   010   011    001    000
		#   -7     -5    -3    -1    +1    +3     +5     +7
		alphabet  = [-7 -5 -3 -1 1 3 5 7]/ scalingFactor;
		# --- Creating comparison container
		baseComp = zeros(Float64,4);
		# --- Iterative LLR computation
		for iN = 1 : 1 : nbSymb
			c	= abs2(channel[iN]);
			# --- Real part
			# MSB0 - Re
			e_porteur0 = getDist!(baseComp,qamSeq[iN].re,alphabet,5,6,7,8)
			e_porteur1 = getDist!(baseComp,qamSeq[iN].re,alphabet,1,2,3,4)
			output[6*(iN-1)+1]  = calcLLR(e_porteur0,e_porteur1,c);
			# MSB1 - Re
			e_porteur0 = getDist!(baseComp,qamSeq[iN].re,alphabet,1,2,7,8)
			e_porteur1 = getDist!(baseComp,qamSeq[iN].re,alphabet,3,4,5,6)
			output[6*(iN-1)+3]  =calcLLR(e_porteur0,e_porteur1,c);
			# LSB - Re
			e_porteur0 = getDist!(baseComp,qamSeq[iN].re,alphabet,1,4,5,8)
			e_porteur1 = getDist!(baseComp,qamSeq[iN].re,alphabet,2,3,6,7)
			output[6*(iN-1)+5]  =calcLLR(e_porteur0,e_porteur1,c);
			# --- Imag part
			# MSB0 - Im
			e_porteur0 = getDist!(baseComp,qamSeq[iN].im,alphabet,5,6,7,8);
			e_porteur1 = getDist!(baseComp,qamSeq[iN].im,alphabet,1,2,3,4);
			output[6*(iN-1)+2]  = calcLLR(e_porteur0,e_porteur1,c);
			# MSB1 - Im
			e_porteur0 = getDist!(baseComp,qamSeq[iN].im,alphabet,1,2,7,8);
			e_porteur1 = getDist!(baseComp,qamSeq[iN].im,alphabet,3,4,5,6);
			output[6*(iN-1)+4]  = calcLLR(e_porteur0,e_porteur1,c);
			# LSB - Im
			e_porteur0 = getDist!(baseComp,qamSeq[iN].im,alphabet,1,4,5,8);
			e_porteur1 = getDist!(baseComp,qamSeq[iN].im,alphabet,2,3,6,7);
			output[6*(iN-1)+6]  =calcLLR(e_porteur0,e_porteur1,c);
		end
	elseif mcs == 256
		# ----------------------------------------------------
		# --- 256-QAM Soft Demapper
		# ----------------------------------------------------
		#maximum log approximation
		# ------------------------------------------------#
		#	   |       |         |        |        |        |        |        |
		#	0100    0101 .    0111 .   0110 .   0010 .   0011 .   0001 .   0000
		#	 -15     -13 .     -11 .     -9 .     -7 .     -5 .     -3 .     -1
		#	   |       |         |        |        |        |        |        |
		#   1000 .   1001 .   1011 .   1010 .   1110 .   1111 .   1101 .   1100
		#     +1 .     +3 .     +5 .     +7 .     +9 .    +11 .	   +13      +15
		#output    = zeros(Float64,nbSymb*8);
		alphabet  = (-15:2:15)./ scalingFactor;
		# --- Creating comparison container
		baseComp = zeros(Float64,8);
		# --- Iterative LLR computation
		for iN = 1 : 1 : nbSymb
			c	= abs2(channel[iN]);
			# MSB0 - Re
			e_porteur0 = getDist!(baseComp,qamSeq[iN].re,alphabet,1,2,3,4,5,6,7,8);
			e_porteur1 = getDist!(baseComp,qamSeq[iN].re,alphabet,9,10,11,12,13,14,15,16);
			output[8*(iN-1)+1]  = calcLLR(e_porteur0,e_porteur1,c);
			# MSB1 - Re
			e_porteur0 = getDist!(baseComp,qamSeq[iN].re,alphabet,5,6,7,8,9,10,11,12);
			e_porteur1 = getDist!(baseComp,qamSeq[iN].re,alphabet,1,2,3,4,13,14,15,16);
			output[8*(iN-1)+3]  = calcLLR(e_porteur0,e_porteur1,c);
			# MSB2 - Re
			e_porteur0 = getDist!(baseComp,qamSeq[iN].re,alphabet,1,2,7,8,9,10,15,16);
			e_porteur1 = getDist!(baseComp,qamSeq[iN].re,alphabet,3,4,5,6,11,12,13,14);
			output[8*(iN-1)+5]  = calcLLR(e_porteur0,e_porteur1,c);
			# LSB - Re
			e_porteur0 = getDist!(baseComp,qamSeq[iN].re,alphabet,1,4,5,8,9,12,13,16);
			e_porteur1 = getDist!(baseComp,qamSeq[iN].re,alphabet,2,3,6,7,10,11,14,15);
			output[8*(iN-1)+7]  = calcLLR(e_porteur0,e_porteur1,c);
			# --- Imag part config
			# MSB0 - Im
			e_porteur0 = getDist!(baseComp,qamSeq[iN].im,alphabet,1,2,3,4,5,6,7,8);
			e_porteur1 = getDist!(baseComp,qamSeq[iN].im,alphabet,9,10,11,12,13,14,15,16);
			output[8*(iN-1)+2]  = calcLLR(e_porteur0,e_porteur1,c);
			# MSB1 - Im
			e_porteur0 = getDist!(baseComp,qamSeq[iN].im,alphabet,5,6,7,8,9,10,11,12);
			e_porteur1 = getDist!(baseComp,qamSeq[iN].im,alphabet,1,2,3,4,13,14,15,16);
			output[8*(iN-1)+4]  = calcLLR(e_porteur0,e_porteur1,c);
			# MSB2 - Im
			e_porteur0 = getDist!(baseComp,qamSeq[iN].im,alphabet,1,2,7,8,9,10,15,16);
			e_porteur1 = getDist!(baseComp,qamSeq[iN].im,alphabet,3,4,5,6,11,12,13,14);
			output[8*(iN-1)+6]  = calcLLR(e_porteur0,e_porteur1,c);
			# LSB - Im
			e_porteur0 = getDist!(baseComp,qamSeq[iN].im,alphabet,1,4,5,8,9,12,13,16);
			e_porteur1 = getDist!(baseComp,qamSeq[iN].im,alphabet,2,3,6,7,10,11,14,15);
			output[8*(iN-1)+8]  = calcLLR(e_porteur0,e_porteur1,c);
		end
	end
	return output
end

# ----------------------------------------------------
# --- Convert LLR to Unsigned Char Array (soft decoder in C)
# ----------------------------------------------------
"""
---
Convert a LLR floating array to a UInt LLR
Some method (espcially C functions as in libfec) expect LLR to be UInt value from  0 (Likely a 0) to 255 (Likely a 1).
symbolDemappingQAM create a Float64 array from -infty (likely a 0) to nfty (Likely a 1). This function take a Float64 LLR estimate and output a UInt8 vector
# --- Syntax
	llrToUInt!(llrUInt,llr)
# --- Input parameters
- llrUInt : Output LLR in UInt [Array{UInt},N]
- llr	  : Input LLR [Array{Float64},N]
# --- Output parameters
- []
# ---
# v 1.0 - Robin Gerzaguet.
"""
function llrToUInt!(llrUInt,llr)
    # 0 ---> Likely a 0
    # 255 ---> Likely a 1
    # We have the llRin: >0 1 <0 -1, higher the better reliability.
    # We assume that 1  --> certainly a 1 (i.e 255)
    #                -1 --> certainly a 0 (i.e 0)
    # We need to convert LLR into UInt8: LLR is converted between 0 and 1 and map to 256 values
    # 0 is a 0 with high probability
    # 1 is a 1 with high probability
    # --- Minimal and maximal bounds (-1 +1)
    # Then, Switch to [0;2]
    # Then Convert to [0,1]
	# Finally extend to 255
	llrUInt .= UInt8.( round.(255* (1 .+ map(x->min(x,1),map(x->max(x,-1),llr)))/2));
end

"""
---
Convert a LLR floating array to a UInt LLR
Some method (espcially C functions as in libfec) expect LLR to be UInt value from  0 (Likely a 0) to 255 (Likely a 1).
symbolDemappingQAM create a Float64 array from -infty to infty. This function take a Float64 LLR estimate and output a UInt8 vector
# --- Syntax
	llrUInt = llrToUInt(llr)
# --- Input parameters
- llr	  : Input LLR [Array{Float64},N]
# --- Output parameters
- llrUInt : Output LLR in UInt [Array{UInt},N]
# ---
# v 1.0 - Robin Gerzaguet.
"""
function llrToUInt(llr)
	# --- Create output vector
	llrUInt = zeros(UInt8,length(llr));
	# --- Call bang method
	llrToUInt!(llrUInt,llr);
	# --- Return LLR
    return llrUInt;
end

# ----------------------------------------------------
# --- Hard decoding from Soft LLRs
# ----------------------------------------------------
"""
---
Returns hard binary value from soft LLR estimate (no FEC decoder, only hard decision here!)
# --- Syntax
	llrToHardBits(hardD,llr)
# --- Input parameters
- hardD	  : Hard binary decision [Array{UInt},N]
- llr	  : Input LLR (Float64 or UInt8 array) [Union{[Array{UInt}],[Array{Float64}]}]
# --- Output parameters
- []
# ---
# v 1.0 - Robin Gerzaguet.
"""
function llrToHardBits!(hardD,llr::Array{Float64})
	# --- Direct LLR are given, sign is bit information
	hardD   .= map(x-> (x<0) ? 0 : 1,llr);
	return hardD
end

"""
---
Returns hard binary value from soft LLR estimate (no FEC decoder, only hard decision here!)
# --- Syntax
	llrToHardBits(hardD,llr)
# --- Input parameters
- hardD	  : Hard binary decision [Array{UInt},N]
- llr	  : Input LLR (Float64 or UInt8 array) [Union{[Array{UInt}],[Array{Float64}]}]
# --- Output parameters
- []
# ---
# v 1.0 - Robin Gerzaguet.
"""
function llrToHardBits!(hardD,llr::Array{UInt8})
	# --- UInt LLR (input of soft decoder): rule is 127
	hardD   .= map(x-> (x<0x7f) ? 0 : 1,llr);
	return hardD
end

"""
---
Returns hard binary value from soft LLR estimate (no FEC decoder, only hard decision here!)
# --- Syntax
	hardD = llrToHardBits(llr)
# --- Input parameters
- llr	  : Input LLR (Float64 or UInt8 array) [Union{[Array{UInt}],[Array{Float64}]}]
# --- Output parameters
- hardD	  : Hard binary decision [Array{UInt},N]
# ---
# v 1.0 - Robin Gerzaguet.
"""
function llrToHardBits(llr::Array{Float64})
	# --- Direct LLR are given, sign is bit information
	hardD   = map(x-> (x<0) ? 0 : 1,llr);
	return hardD
end

"""
---
Returns hard binary value from soft LLR estimate (no FEC decoder, only hard decision here!)
# --- Syntax
	hardD = llrToHardBits(llr)
# --- Input parameters
- llr	  : Input LLR (Float64 or UInt8 array) [Union{[Array{UInt}],[Array{Float64}]}]
# --- Output parameters
- hardD	  : Hard binary decision [Array{UInt},N]
# ---
# v 1.0 - Robin Gerzaguet.
"""
function llrToHardBits(llr::Array{UInt8})
	# --- UInt LLR (input of soft decoder): rule is 127
	hardD   = map(x-> (x<0x7f) ? 0 : 1,llr);
	return hardD
end


## Transmission Loss Demonstration

## Preamble
using Plots

## Lloyds Mirror
include("../LloydsMirror.jl")

f = 150.
r_src = 0.
z_src = 25.
z = range(0, 4e2, length = 500)
r = range(0, 5e3, length = 500)
c = 1500.

TL(r, z) = LloydsMirror.lloydsmirror_singlereflection.(c, f, r_src, r, z_src, z)

ptField = heatmap(r, z, TL,
	seriescolor = cgrad(:jet, rev = true),
	title = "TL (dB)",
	yaxis = (:flip, "Depth (m)"),
	xaxis = ("Range (m)"))
display(ptField)
savefig(pt, "img/SoundField_LloydsMirror_Simple.png")

ptSlice = plot(r, r -> TL(r, 200.),
	yaxis = :flip)
plot!(r, r -> 20log10(r))
display(ptSlice)
savefig(ptSlice, "img/SoundField_LloydsMirror_Simple_Slice.png")


include("testsystem.jl")
Random.seed!(11)
x = rand(StateObs(Gaussian(x0, P0), M.obs))
X = trace(DynamicIterators.Sampled(M), 1 => x, endtime(100))

@show X

Y = collect(t=>y for (t, (x,y)) in pairs(X))

Xf1, ll1 = kalmanfilter(O, 0 => Gaussian(x0, P0), Y)
@show Xf1

Xf2, ll2 = kalmanfilter(M, 0 => Gaussian(x0, P0), Y)
@show Xf2

kf = kalmanfilter(M)
Xf = trace(kf(Y), 0 => Gaussian(x0, P0), endtime(200), register = ((i, x),) -> i > 0)

@show Xf

@test Xf.t == Xf2.t == Xf1.t
@test first.(Xf.x) == Xf2.x == Xf1.x
@test Xf.x[end][3] == ll2


@doc raw"""
    BayesianSVM() -> BernoulliLikelihood

The [Bayesian SVM](https://arxiv.org/abs/1707.05532) is a Bayesian interpretation of the classical SVM.
```math
p(y|f) \propto \exp(2 \max(1-yf, 0))
```

---

For the analytic version of the likelihood, it is augmented via:

```math
p(y|f, ω) = \frac{1}{\sqrt(2\pi\omega)} \exp\left(-\frac{(1+\omega-yf)^2}{2\omega})\right)
```

where ``ω \sim 1[0,\infty)`` has an improper prior (his posterior is however has a valid distribution, a Generalized Inverse Gaussian). For reference [see this paper](http://ecmlpkdd2017.ijs.si/papers/paperID502.pdf).
"""
BayesianSVM() = BernoulliLikelihood(SVMLink())

struct SVMLink <: AbstractLink end

implemented(::BernoulliLikelihood{<:SVMLink}, ::AnalyticVI) = true

function (::SVMLink)(f::Real)
    return svmlikelihood(f)
end

# Return likelihood equivalent to SVM hinge loss
function svmlikelihood(f::Real)
    pos = svmpseudolikelihood(f)
    return pos ./ (pos .+ svmpseudolikelihood(-f))
end

# Return the pseudo likelihood of the SVM hinge loss
function svmpseudolikelihood(f::Real)
    return exp(-2 * max(1 - f, zero(f)))
end

function local_updates!(
    local_vars,
    ::BernoulliLikelihood{<:SVMLink},
    y::AbstractVector,
    μ::AbstractVector{T},
    diagΣ::AbstractVector,
) where {T}
    map!(local_vars.c, μ, diagΣ, y) do μ, σ², y
        abs2(one(T) - y * μ) + σ²
    end
    map!(inv ∘ sqrt, local_vars.θ, local_vars.c)
    return local_vars
end

@inline function ∇E_μ(
    ::BernoulliLikelihood{<:SVMLink}, ::AOptimizer, y::AbstractVector, state
)
    return (y .* (state.θ .+ one(eltype(state.θ))),)
end

@inline function ∇E_Σ(
    ::BernoulliLikelihood{<:SVMLink}, ::AOptimizer, ::AbstractVector, state
)
    return (state.θ / 2,)
end

## ELBO

function expec_loglikelihood(
    ::BernoulliLikelihood{<:SVMLink},
    ::AnalyticVI,
    y,
    μ::AbstractVector,
    diag_cov::AbstractVector,
    state,
)
    tot = -length(y) * logtwo / 2
    tot += dot(μ, y)
    tot += -dot(state.θ, diag_cov) / 2 + dot(state.θ, abs2.(one(eltype(μ)) .- y .* μ))
    return tot
end

function AugmentedKL(l::BernoulliLikelihood{<:SVMLink}, state, ::Any)
    Zygote.@ignore(GIGEntropy(l, state))
end

function GIGEntropy(::BernoulliLikelihood{<:SVMLink}, state)
    return sum(log.(state.c)) / 2 + sum(log.(2.0 * besselk.(0.5, sqrt.(state.c)))) -
           sum(sqrt.(state.c)) / 2
end


struct Equation
    coeffA :: Float64
    coeffB :: Float64
    coeffC :: Float64

    function Equation(numPots :: Int64, aMult :: Int64, bMult :: Int64)
        coeffA = convert(Float64, aMult + bMult);
        coeffB = convert(Float64, -2 * bMult * numPots);
        coeffC = convert(Float64, bMult * (numPots ^ 2));
        return new(coeffA, coeffB, coeffC)
    end
end

function find_equation_min(eq :: Equation)
    numer = -eq.coeffB
    denom = 2 * eq.coeffA
    return numer / denom
end

function solve_equation_at(eq :: Equation, coord :: Float64)
    aTerm = eq.coeffA * (coord ^ 2)
    bTerm = eq.coeffB * coord
    cTerm = eq.coeffC
    return aTerm + bTerm + cTerm
end

function solve_case(numPots, aMult, bMult) :: Int64
    eq = Equation(numPots, aMult, bMult);
    eq_min = find_equation_min(eq);
    soln = solve_equation_at(eq, round(eq_min));
    return round(soln);
end

function run_case()
    test_case_data = [parse(Int64, x) for x in split(readline())];
    soln = solve_case(test_case_data[1], test_case_data[2], test_case_data[3]);
    println(soln);
end

function main_fn()
    num_ranges = parse(Int, readline());

    for i in 1:num_ranges
        run_case();
    end

    return nothing
end

main_fn();


function TriMesh_ProjectDerivatives(x,eConn,u;nodeList=[])
#= """
  TriMesh_ProjectDerivatives - project the derivative of a C0 scalar field
             onto the continuous finite element space (a ZZ projection).

  Author: Jeff Borggaard, Virginia Tech
          part of FEMfunctions.jl

  Licensing:
     This code is distributed under the MIT license.

  Usage:
  ```julia
    dudx1, dudx2, elError, nodeList =
           TriMesh_ProjectDerivatives(x, eConn, u, nodeList)
  ```
                 Multiple scalar fields can be treated at once
                     u = [u1  u2  ... ]

                  in which case, the function returns
                     dudx1 = [∂(s1)/∂(x1)  ∂(s2)/∂(x1)  ... ]
                     dudx2 = [∂(s1)/∂(x1)  ∂(s2)/∂(x1)  ... ]

  Arguments:
  - `x`: Nodal coordinates
  - `eConn`: Element connectivity
  - `u`: Nodal values of scalar quantity

  - `nodeList`: (optional, can be omitted)
            the nodeList structure can be reused for multiple
            projections involving the same mesh, e.g. for
            time-dependent problems. It is generated and
            returned if not provided.

  Output arguments:
  - `dudx1`: Projection of the x-derivatives evaluated at nodes
  - `dudx2`: Projection of the y-derivatives evaluated at nodes

  - `elError`: element error (H1-seminorm for each field)
  - `nodeList`: a structure that contains an element list for each node
""" =#

  nNodes = size(x,1)
  nElements = size(eConn,1)

  nFields = size(u,2)

  #----------------------------------------------------------------------------
  #  For every vertex node, construct a list of elements that share it
  #----------------------------------------------------------------------------
  if typeof(nodeList) == typeof([])
    nodeList = zeros(UInt64,nNodes,15) # the first entry is reserved as a
                                       # counter and
                                       # we assume <= 14 elements per vertex

    for n=1:nElements  # local nodes 1,2, and 3 are vertices
      nodeList[eConn[n,1],1] = nodeList[eConn[n,1],1] + 1
      nodeList[eConn[n,1],nodeList[eConn[n,1],1]+1] = n

      nodeList[eConn[n,2],1] = nodeList[eConn[n,2],1] + 1
      nodeList[eConn[n,2],nodeList[eConn[n,2],1]+1] = n

      nodeList[eConn[n,3],1] = nodeList[eConn[n,3],1] + 1
      nodeList[eConn[n,3],nodeList[eConn[n,3],1]+1] = n
    end
  end

  #----------------------------------------------------------------------------
  #  Compute the least-squares projection over each patch,
  #     least-squares of gradients are performed at element quadrature points
  #----------------------------------------------------------------------------
  nQuadrature = 7
  r,s,w = twodQuadratureRule(nQuadrature)

  d1_p = fill(Inf,nNodes,nFields)
  d2_p = fill(Inf,nNodes,nFields)  # initialize to ridiculous values

  for n=1:nNodes
    if ( nodeList[n,1]>0 ) # this is a vertex node
      nel = nodeList[n,1]
      P   = zeros(nel*nQuadrature,6)
      d1  = zeros(nel*nQuadrature,nFields)
      d2  = zeros(nel*nQuadrature,nFields)

      for j=1:nodeList[n,1] # loop over elements in the patch
        el = nodeList[n,j+1]
        nodesLocal = eConn[el,:]
        xLocal = x[nodesLocal,:]
        uLocal = u[nodesLocal,:]

        x_g, w_g, ϕ, ϕ_x, ϕ_y = twodShape(xLocal, r, s, w)

        d_x = ϕ_x*uLocal   # calculate the finite element derivatives
        d_y = ϕ_y*uLocal   # on this element

        idx = 1+(j-1)*nQuadrature:j*nQuadrature
        P[idx,:] = hcat( ones(nQuadrature,1), x_g[:,1], x_g[:,2], x_g[:,1].^2, x_g[:,1].*x_g[:,2], x_g[:,2].^2 )
        d1[idx,:] = d_x
        d2[idx,:] = d_y

#       for i=1:n_quadrature
#         P  = [ P ; ...
#                1 x_g(i,1) x_g(i,2) x_g(i,1)^2 x_g(i,1)*x_g(i,2) x_g(i,2)^2 ]
#         d1 = [ d1; d_x(i) ]
#         d2 = [ d2; d_y(i) ]
#       end
      end # finished filling least-squares system

      # calculate the polynomial coefficients for each field
      for i=1:nFields
        a_x = P\d1[:,i]
        a_y = P\d2[:,i]

        #-------------------------------------------------------------------78--
        #  Compute projected derivatives at vertex node
        #-----------------------------------------------------------------------
        xp  = x[n,1]
        yp  = x[n,2]
        d1_p[n,i] = a_x[1]      + a_x[2]*xp    + a_x[3]*yp +
                    a_x[4]*xp^2 + a_x[5]*xp*yp + a_x[6]*yp^2
        d2_p[n,i] = a_y[1]      + a_y[2]*xp    + a_y[3]*yp +
                    a_y[4]*xp^2 + a_y[5]*xp*yp + a_y[6]*yp^2

        for j=1:nodeList[n,1]
          el = nodeList[n,j+1]
          nodesLocal = eConn[el,:]
          xLocal = x[nodesLocal,:]

          #---------------------------------------------------------------------
          #  Compute contribution to edge nodes.  These are either on the
          #  boundary or are averaged over two element faces.
          #---------------------------------------------------------------------
          if ( n==nodesLocal[1] )
            x4 = xLocal[4,1]
            y4 = xLocal[4,2]

            x6 = xLocal[6,1]
            y6 = xLocal[6,2]

            if ( d1_p[nodesLocal[4],i] == Inf ) # contribution not rec'd
              d1_p[nodesLocal[4],i] =
                  a_x[1]      + a_x[2]*x4    + a_x[3]*y4 +
                  a_x[4]*x4^2 + a_x[5]*x4*y4 + a_x[6]*y4^2
              d2_p[nodesLocal[4],i] =
                  a_y[1]      + a_y[2]*x4    + a_y[3]*y4 +
                  a_y[4]*x4^2 + a_y[5]*x4*y4 + a_y[6]*y4^2
            else
              d1_p[nodesLocal[4],i] = .5*( d1_p[nodesLocal[4],i] +
                  a_x[1]      + a_x[2]*x4    + a_x[3]*y4 +
                  a_x[4]*x4^2 + a_x[5]*x4*y4 + a_x[6]*y4^2 )
              d2_p[nodesLocal[4],i] = .5*( d2_p[nodesLocal[4],i] +
                  a_y[1]      + a_y[2]*x4    + a_y[3]*y4 +
                  a_y[4]*x4^2 + a_y[5]*x4*y4 + a_y[6]*y4^2 )
            end
            if ( d1_p[nodesLocal[6],i] == Inf )
              d1_p[nodesLocal[6],i] =
                  a_x[1]      + a_x[2]*x6    + a_x[3]*y6 +
                  a_x[4]*x6^2 + a_x[5]*x6*y6 + a_x[6]*y6^2
              d2_p[nodesLocal[6],i] =
                  a_y[1]      + a_y[2]*x6    + a_y[3]*y6 +
                  a_y[4]*x6^2 + a_y[5]*x6*y6 + a_y[6]*y6^2
            else
              d1_p[nodesLocal[6],i] = .5*( d1_p[nodesLocal[6],i] +
                  a_x[1]      + a_x[2]*x6    + a_x[3]*y6 +
                  a_x[4]*x6^2 + a_x[5]*x6*y6 + a_x[6]*y6^2 )
              d2_p[nodesLocal[6],i] = .5*( d2_p[nodesLocal[6],i] +
                  a_y[1]      + a_y[2]*x6    + a_y[3]*y6 +
                  a_y[4]*x6^2 + a_y[5]*x6*y6 + a_y[6]*y6^2 )
            end

          elseif ( n==nodesLocal[2] )
            x4 = xLocal[4,1]
            y4 = xLocal[4,2]

            x5 = xLocal[5,1]
            y5 = xLocal[5,2]

            if ( d1_p[nodesLocal[4],i] == Inf )
              d1_p[nodesLocal[4],i] =
                  a_x[1]      + a_x[2]*x4    + a_x[3]*y4 +
                  a_x[4]*x4^2 + a_x[5]*x4*y4 + a_x[6]*y4^2
              d2_p[nodesLocal[4],i] =
                  a_y[1]      + a_y[2]*x4    + a_y[3]*y4 +
                  a_y[4]*x4^2 + a_y[5]*x4*y4 + a_y[6]*y4^2
            else
              d1_p[nodesLocal[4],i] = .5*( d1_p[nodesLocal[4],i] +
                  a_x[1]      + a_x[2]*x4    + a_x[3]*y4 +
                  a_x[4]*x4^2 + a_x[5]*x4*y4 + a_x[6]*y4^2 )
              d2_p[nodesLocal[4],i] = .5*( d2_p[nodesLocal[4],i] +
                  a_y[1]      + a_y[2]*x4    + a_y[3]*y4 +
                  a_y[4]*x4^2 + a_y[5]*x4*y4 + a_y[6]*y4^2 )
            end
            if ( d1_p[nodesLocal[5],i] == Inf )
              d1_p[nodesLocal[5],i] =
                  a_x[1]      + a_x[2]*x5    + a_x[3]*y5 +
                  a_x[4]*x5^2 + a_x[5]*x5*y5 + a_x[6]*y5^2
              d2_p[nodesLocal[5],i] =
                  a_y[1]      + a_y[2]*x5    + a_y[3]*y5 +
                  a_y[4]*x5^2 + a_y[5]*x5*y5 + a_y[6]*y5^2
            else
              d1_p[nodesLocal[5],i] = .5*( d1_p[nodesLocal[5],i] +
                  a_x[1]      + a_x[2]*x5    + a_x[3]*y5 +
                  a_x[4]*x5^2 + a_x[5]*x5*y5 + a_x[6]*y5^2 )
              d2_p[nodesLocal[5],i] = .5*( d2_p[nodesLocal[5],i] +
                  a_y[1]      + a_y[2]*x5    + a_y[3]*y5 +
                  a_y[4]*x5^2 + a_y[5]*x5*y5 + a_y[6]*y5^2 )
            end

          elseif ( n==nodesLocal[3] )
            x5 = xLocal[5,1]
            y5 = xLocal[5,2]

            x6 = xLocal[6,1]
            y6 = xLocal[6,2]

            if ( d1_p[nodesLocal[5],i] == Inf )
              d1_p[nodesLocal[5],i] =
                  a_x[1]      + a_x[2]*x5    + a_x[3]*y5 +
                  a_x[4]*x5^2 + a_x[5]*x5*y5 + a_x[6]*y5^2
              d2_p[nodesLocal[5],i] =
                  a_y[1]      + a_y[2]*x5    + a_y[3]*y5 +
                  a_y[4]*x5^2 + a_y[5]*x5*y5 + a_y[6]*y5^2
            else
              d1_p[nodesLocal[5],i] = .5*( d1_p[nodesLocal[5],i] +
                  a_x[1]      + a_x[2]*x5    + a_x[3]*y5 +
                  a_x[4]*x5^2 + a_x[5]*x5*y5 + a_x[6]*y5^2 )
              d2_p[nodesLocal[5],i] = .5*( d2_p[nodesLocal[5],i] +
                  a_y[1]      + a_y[2]*x5    + a_y[3]*y5 +
                  a_y[4]*x5^2 + a_y[5]*x5*y5 + a_y[6]*y5^2 )
            end
            if ( d1_p[nodesLocal[6],i] == Inf )
              d1_p[nodesLocal[6],i] =
                  a_x[1]      + a_x[2]*x6    + a_x[3]*y6 +
                  a_x[4]*x6^2 + a_x[5]*x6*y6 + a_x[6]*y6^2
              d2_p[nodesLocal[6],i] =
                  a_y[1]      + a_y[2]*x6    + a_y[3]*y6 +
                  a_y[4]*x6^2 + a_y[5]*x6*y6 + a_y[6]*y6^2
            else
              d1_p[nodesLocal[6],i] = .5*( d1_p[nodesLocal[6],i] +
                  a_x[1]      + a_x[2]*x6    + a_x[3]*y6 +
                  a_x[4]*x6^2 + a_x[5]*x6*y6 + a_x[6]*y6^2 )
              d2_p[nodesLocal[6],i] = .5*( d2_p[nodesLocal[6],i] +
                  a_y[1]      + a_y[2]*x6    + a_y[3]*y6 +
                  a_y[4]*x6^2 + a_y[5]*x6*y6 + a_y[6]*y6^2 )
            end
          else
            fprintf("Problem identifying node at a vertex")
          end

        end
      end
    end
  end
#  Compare d1_p with the true gradient
#d_true=-2*x(:,1).*(1-x(:,2).^2)     % elliptic_2da example
#disp([d1_p d_true])

#  Compare d2_p with the true gradient
#d_true=-2*x(:,2).*(1-x(:,1).^2)
#disp([d2_p d_true])
  computeElementError = false  # later define this based on vargout?
  if ( computeElementError )
    #---------------------------------------------------------------------------
    #  Calculate the H1-seminorm of the error on each element
    #---------------------------------------------------------------------------
    eError = zeros(Float64,nElements,nFields)

    nQuadrature = 7;
    r,s,w   = twodQuadratureRule(nQuadrature)

    for nEl=1:nElements
      nodesLocal       = eConn[nEl,:]
      xLocal           = x[nodesLocal,:]
      uLocal           = u[nodesLocal,:]
      d1Local          = d1_p[nodesLocal,:]
      d2Local          = d2_p[nodesLocal,:]

      x_g,w_g,ϕ,ϕ_x,ϕ_y = twodShape(xLocal,r,s,w)

      # calculate the finite element derivatives and their projections at x_g
      dx_fe           = ϕ_x*uLocal
      dy_fe           = ϕ_y*uLocal

      dx_p            = ϕ*d1Local
      dy_p            = ϕ*d2Local

  # % exact derivatives as a test
  # dx_p = -2*x_g(:,1).*(1-x_g(:,2).^2)
  # dy_p = -2*x_g(:,2).*(1-x_g(:,1).^2)
      for i=1:nFields
        sqError     = (dx_fe[:,i]-dx_p[:,i]).^2 + (dy_fe[:,i]-dy_p[:,i]).^2
        eError[nEl,i] = sqError'*w_g

      end

    eError = sqrt.(eError)
    end
  end # computeElementError conditional

  return d1_p, d2_p, nodeList #, eError
end


module RTFESpacesTests
##
using Test
using Gridap
using Gridap.MapApply
using Gridap.CellValuesGallery
using LinearAlgebra

D = 2
order = 1

dom = fill(4,D)
model = CartesianDiscreteModel(partition=tuple(dom...))

p = Polytope(fill(HEX_AXIS,D)...)
reffe = RTRefFE(p,Float64,order)

grid = Grid(model,D)
trian = Triangulation(grid)
graph = GridGraph(model)
graph.data[2,1]

labels = FaceLabels(model)
tags = [5,6,7,8]
fun(x) = VectorValue(x[1],x[2])
# tags = Int[]

fesp = ConformingFESpace(reffe,trian,graph,labels,tags)
dfesp = TrialFESpace(fesp,fun)

trian = Triangulation(model)
quad = CellQuadrature(trian,degree=2)

uh = interpolate(dfesp,fun)

e = fun - uh

el2 = sqrt(sum(integrate(inner(e,e),trian,quad)))
@test el2 < 1.0e-10

##

end # module


const ev_to_K = 11604.52500617u"K"
const e_statC = 4.80320425e-10u"cm^(3/2) * g^(1/2) * s^−1"

"""
    electron_gyrofrequency(B)

The electron gyrofrequency, or the angular frequency of the circular motion of an
electron in the plane perpendicular to the magnetic field.

# Arguments
- `B`: magnetic flux density, typically in units of tesla
"""
function electron_gyrofrequency(B)
    return (e_statC * B) / (ElectronMass * SpeedOfLightInVacuum)
end

"""
    ion_gyrofrequency(B)

The ion gyrofrequency, or the angular frequency of the circular motion of an ion in
the plane perpendicular to the magnetic field.

# Arguments
 - `B`: magnetic flux density, typically in units of tesla
 - `Z`: charge state, unitless
 - `m_i`: ion mass, typically in grams
"""
function ion_gyrofrequency(B, Z, m_i)
    return (Z * e_statC * B) / (m_i * SpeedOfLightInVacuum)
end

"""
    electron_plasma_frequency(n_e)

Electron plasma frequency, or the frequency at which the electrons in the plasma oscillate

# Arguments
 - `n_e`: electron number density, typically in units of 1/cm^3
"""
function electron_plasma_frequency(n_e)
    return sqrt((4π * n_e * e_statC^2) / (ElectronMass |> u"g"))
end

"""
    ion_plasma_frequency(n_i, m_i, Z)

Ion plasma frequency, or the frequency at which the ions in the plasma oscillate

# Arguments
 - `n_i`: ion number density, typically in units of 1/cm^3
 - `Z`: charge state, dimensionless
 - `m_i`: ion mass, e.g., grams
"""
function ion_plasma_frequency(n_i, m_i, Z)
    return sqrt((4π * n_i * Z^2 * e_statC^2) / m_i)
end

"""
# Arguments
 - ``:
"""
function electron_trapping_rate()
end

"""
# Arguments
 - ``:
"""
function ion_trapping_rate()
end

"""
# Arguments
 - ``:
"""
function electron_collision_rate()
end

"""
# Arguments
 - ``:
"""
function ion_collision_rate()
end

"""
# Arguments
 - ``:
"""
function electron_debroglie_length()
end

"""
# Arguments
 - ``:
"""
function classical_minimum_approach_dist()
end

"""
# Arguments
 - ``:
"""
function electron_gryoradius()
end

"""
# Arguments
 - ``:
"""
function ion_gyroradius()
end

"""
# Arguments
 - ``:
"""
function Debye_length()
end

"""
# Arguments
 - ``:
"""
function magnetic_debye_length()
end

"""
    electron_thermal_velocity(T_e)

# Arguments
 - `T_e`: electron temperature in eV
"""
function electron_thermal_velocity(T_e::Unitful.Energy)
    T = ustrip(u"eV", T_e) * ev_to_K
	electron_thermal_velocity(T)
end

function electron_thermal_velocity(T_e::Unitful.Temperature)
	sqrt(T_e * BoltzmannConstant / ElectronMass) |> u"cm/s"
end

"""
# Arguments
 - ``:
"""
function ion_thermal_velocity(T_i, m_i)
    return sqrt((BoltzmannConstant * T_i) / m_i)
end

"""
    ion_sound_velocity(γ, Z, T_e, m_i)

Ion sound speed, the speed of the longitudinal waves resulting from the
mass of the ions and the pressure of the electrons.

# Arguments
 - `γ`: adiabatic index
 - `Z`: charge state
 - `T_e`: electron temperature in eV
 - `m_i`: ion mass
"""
function ion_sound_velocity(γ, Z, T_e, m_i)
    return sqrt(γ * Z * k_B * T_e / m_i)
end

"""
    alfven_velocity(B, n_i, m_i)

The Alfven velocity, the speed of the waves resulting from the mass of the
ions and the restoring force of the magnetic field

# Arguments
 - `B`: magnetic flux, typically in teslas
 - `n_i`: ion number density, typically in 1/cm^3
 - `m_i`: ion mass, typically in grams
"""
function alfven_velocity(B, n_i, m_i)
    return B / sqrt(4π * n_i * m_i)
end


using SymbolicWedderburn
using DynamicPolynomials
using SparseArrays

using GroupsCore
include(joinpath(pkgdir(GroupsCore), "test", "cyclic.jl"))

G = CyclicGroup(2)
x,y = @polyvar x y
# (x, y) → (x+y, x-y)

struct By90Rotation <: SymbolicWedderburn.ByLinearTransformation end
# x -> linear combination of basis elements;

SymbolicWedderburn.coeff_type(::By90Rotation) = Float64
function SymbolicWedderburn.action(::By90Rotation, g::CyclicGroupElement, m::Monomial)
    isone(g) && return m
    x, y = variables(m)
    return m(x => (x+y)/sqrt(2), y => (x-y)/sqrt(2)) # we need the action to be orthogonal
end

function SymbolicWedderburn.decompose(k::AbstractPolynomial, hom::SymbolicWedderburn.InducedActionHomomorphism)
    # correct only if features(hom) == monomials

    indcs = [hom[m] for m in monomials(k)]
    coeffs = coefficients(k)

    return indcs, coeffs
end

using Test

@testset "Decompose in basis" begin
    g = gens(G, 1)

    basis = monomials([x,y], 0:4)
    k = SymbolicWedderburn.action(By90Rotation(), g, basis[2])
    ehom = SymbolicWedderburn.ExtensionHomomorphism(By90Rotation(), basis)
    idcs, vals = SymbolicWedderburn.decompose(k, ehom)
    @test sum(c*basis[i] for (c,i) in zip(vals, idcs)) == k

    for b in basis
        k = SymbolicWedderburn.action(By90Rotation(), g, b)
        idcs, vals = SymbolicWedderburn.decompose(k, ehom)
        @test sum(c*basis[i] for (c,i) in zip(vals, idcs)) == k
    end
end

# let us check that we get an actual matrix representation of G:

@testset "induced Matrix Representation" begin
    g = gens(G, 1)
    monomial_basis = monomials([x,y], 0:4)
    ehom = SymbolicWedderburn.ExtensionHomomorphism(By90Rotation(), monomial_basis)
    m = droptol!(SymbolicWedderburn.induce(By90Rotation(), ehom, g), 1e-15)
    @test eltype(m) == Float64

    @test !(m ≈ one(m))
    @test m^2 ≈ one(m)

    ssimple_basis = SymbolicWedderburn.symmetry_adapted_basis(Float64, G, By90Rotation(), monomial_basis);
    degs = SymbolicWedderburn.degree.(ssimple_basis)
    mlts = multiplicity.(ssimple_basis)
    @test sum(d*m for (d,m) in zip(degs, mlts)) == length(monomial_basis)
    @test sum(first∘size, ssimple_basis) == length(monomial_basis)
end


import Clustering, NearestNeighbors, LinearAlgebra
import Statistics
import StaticArrays

function get_transform_between_two_registered_clouds(c1, c2)
    centroid_1 = mean(c1, dims=2)
    centroid_2 = mean(c2, dims=2)
    centered_1 = c1 .- centroid_1
    centered_2 = c2 .- centroid_2
    H = centered_1 * centered_2'
    s = LinearAlgebra.svd(H)
    R = (s.V * permutedims(s.U))
    if LinearAlgebra.det(R) < 0
        s = LinearAlgebra.svd(R)
        V = s.V
        V[:,3] .= -1.0 .* V[:,3]
        R = V * permutedims(s.U)
    end
    rot = Rotations.RotMatrix(SMatrix{3,3}(R))

    T = (centroid_2 .- rot * centroid_1)
    T = T[:]
    Pose(T, rot)
end

"""
Returns the rigid transformation `T::Pose` such that `T * csource` is as close
as possible to `ctarget`.  Here `ctarget` and `csource` are point clouds, that
is, 3×something matrices where each column represents a point.
"""
function icp(c1, c2; iterations=10, c1_tree=nothing)
    T = IDENTITY_POSE
    if isnothing(c1_tree)
        c1_tree = NearestNeighbors.KDTree(c1)
    else
        @assert length(c1_tree.data) == size(c1)[2]
    end
    for _ in 1:iterations
        idxs, dists = NearestNeighbors.nn(c1_tree, c2)
        neighbors = c1[:,idxs]
        new_T = get_transform_between_two_registered_clouds(c2, neighbors)
        c2 = new_T.pos .+ (new_T.orientation * c2)
        T = get_c_relative_to_a(new_T, T)
    end

    T
end

function icp_object_pose(init_pose, obs, get_cloud_func;
    c1_tree=nothing, random_rotation_initialization=false, outer_iterations=5, iterations=10)
    p = init_pose
    if random_rotation_initialization
        p = Pose(p.pos, uniform_rot3())
    end

    if isnothing(c1_tree)
        c1_tree = NearestNeighbors.KDTree(obs)
    else
        @assert length(c1_tree.data) == size(obs)[2]
    end

    c = get_cloud_func(p)
    for _ in 1:outer_iterations
        T = icp(obs, c; iterations=iterations, c1_tree=c1_tree)
        p = get_c_relative_to_a(T,p)
        c = get_cloud_func(p)
    end

    p
end

function icp_project_to_planar_contact(
    obs, get_cloud_func, parent_pose, parent_box, parent_face, child_box, child_face,
    x,y,ang; c1_tree=nothing, outer_iterations=5, iterations=10)

    if isnothing(c1_tree)
        c1_tree = NearestNeighbors.KDTree(obs)
    else
        @assert length(c1_tree.data) == size(obs)[2]
    end

    contact = S.ShapeContact(parent_face, Real[], child_face, Real[], S.PlanarContact(x,y,ang))
    p = parent_pose * S.getRelativePoseFromContact(parent_box, child_box, contact)
    c = get_cloud_func(p)

    for _ in 1:outer_iterations
        T = icp(obs, c; iterations=iterations, c1_tree=c1_tree)
        p = get_c_relative_to_a(T,p)

        (parent_face_idx, child_face_idx),_ = get_closest_planar_contact(parent_pose, parent_box, p, child_box)
        new_parent_face, new_child_face = S.BOX_SURFACE_IDS[parent_face_idx], S.BOX_SURFACE_IDS[child_face_idx]
        pc = S.closestApproximatingContact(
            (parent_pose * S.getContactPlane(parent_box, new_parent_face)) \
            (p * S.getContactPlane(child_box, new_child_face)),
        )
        x,y,ang = pc.x, pc.y, pc.angle
        contact = S.ShapeContact(parent_face, Real[], child_face, Real[], S.PlanarContact(x,y,ang #= And no slack term. =#))
        p = parent_pose * S.getRelativePoseFromContact(parent_box, child_box, contact)

        c = get_cloud_func(p)
    end

    (x,y,ang)
end

export get_transform_between_two_registered_clouds, icp, icp_object_pose

PyCall.py"""
import cv2 as cv
import numpy as np
def registerModelToScene(c1, c2):
    icp = cv.ppf_match_3d_ICP(6)
    return icp.registerModelToScene(np.array(c1).transpose().astype(np.float32),
                             np.array(c2).transpose().astype(np.float32))
def registerModelToSceneWithPoses(c1, c2, poses):
    icp = cv.ppf_match_3d_ICP(100)
    return icp.registerModelToScene(np.array(c1).transpose().astype(np.float32),
                             np.array(c2).transpose().astype(np.float32),
                             poses)
def pose_R_t_to_cv2_pose(R,t):
    p = cv.ppf_match_3d_Pose3D()
    p.updatePose(np.array(R), np.array(t))
    return p
"""
function cv2_pose_to_pose(pose::PyCall.PyObject)
   mat = pose.pose
   Pose(mat[:,4], RotMatrix3{Float64}(mat[1:3,1:3]))
end
function cv2_pose_to_pose(pose::Matrix)
   mat = pose
   Pose(mat[:,4], RotMatrix3{Float64}(mat[1:3,1:3]))
end
function pose_to_cv2_pose(pose)
    PyCall.py"pose_R_t_to_cv2_pose"(pose.orientation, pose.pos)
end

export icp_tree, icp_object_poses


"""
Copyright Marcus Greiff 2020

Tests of the attitude conversions and distances pertaining to SO(3)
"""

# Include modules and recompile if running test standalone
if !isdefined(Main, :runAllTests)
    using Test, AerialVehicleControl, LinearAlgebra
    recompile();
end

@verbose(1, "Testing the SO(3) maps...")
@testset "Attitude maps" begin
    @testset "Test the SO(3) hat map" begin
        @testset "Good inputs" begin
            # Fixture
            A, Ac, Am = get_matrix_and_copy(rand(3,3))
            u, uc, um = get_matrix_and_copy(rand(3,1))
            # Test
            status = ccall((:cont_SO3_hat, AerialVehicleControl.CONT_LIB_PATH),
                Cint,
                (Ref{matrix_double_t}, Ref{matrix_double_t}),
                um,
                Am)
            # Assert
            @test isapprox(status,1);
            @test isapprox(u, uc);
            @test !isapprox(A, Ac);
            @test isapprox(A, skew(u));
        end
        @testset "Bad inputs" begin
            # Fixture
            A, Ac, Am = get_matrix_and_copy(rand(3,4))
            u, uc, um = get_matrix_and_copy(rand(3,1))
            # Test
            status = ccall((:cont_SO3_hat, AerialVehicleControl.CONT_LIB_PATH),
                Cint,
                (Ref{matrix_double_t}, Ref{matrix_double_t}),
                um,
                Am)
            # Assert
            @test isapprox(status, 0);
        end
    end
    @testset "Test the SO(3) vee map" begin
        @testset "Good inputs" begin
            # Fixture
            u               = rand(3,1);
            S, Sc, Sm       = get_matrix_and_copy(skew(u))
            out, outc, outm = get_matrix_and_copy(rand(3,1))
            # Test
            status = ccall((:cont_SO3_vee, AerialVehicleControl.CONT_LIB_PATH),
                Cint,
                (Ref{matrix_double_t}, Ref{matrix_double_t}),
                Sm,
                outm)
            # Assert
            @test isapprox(status,1);
            @test isapprox(S, Sc);
            @test !isapprox(out, outc);
            @test isapprox(u, out);
        end
        @testset "Bad inputs" begin
            # Fixture
            u               = rand(3,1);
            S, Sc, Sm       = get_matrix_and_copy(skew(u))
            out, outc, outm = get_matrix_and_copy(rand(3,2))
            # Test
            status = ccall((:cont_SO3_vee, AerialVehicleControl.CONT_LIB_PATH),
                Cint,
                (Ref{matrix_double_t}, Ref{matrix_double_t}),
                Sm,
                outm)
            # Assert
            @test isapprox(status, 0);
        end
    end
    @testset "Test the SO(3) Psi distance" begin
        @testset "Good inputs" begin
            for i = 1:10
                # Fixture
                R1, R1c, R1m  = get_matrix_and_copy(exp(skew(rand(3,1))))
                R2, R2c, R2m  = get_matrix_and_copy(exp(skew(rand(3,1))))
                # Test
                value = ccall((:cont_SO3_distance, AerialVehicleControl.CONT_LIB_PATH),
                    Cdouble,
                    (Ref{matrix_double_t}, Ref{matrix_double_t}),
                    R1m,
                    R2m)
                # Assert
                @test isapprox(value, tr(I-R1'R2)/2.0);
                @test isapprox(R1, R1c)
                @test isapprox(R2, R2c)
            end
        end
        # TODO write bad input tests
    end
    @testset "Test the SO(3) exponential map" begin
        @testset "Good inputs" begin
            for i = 1:5
                # Fixture
                u, uc, um = get_matrix_and_copy(rand(3,1));
                R, Rc, Rm = get_matrix_and_copy(rand(3,3));
                # Test
                status = ccall((:cont_SO3_Exp, AerialVehicleControl.CONT_LIB_PATH),
                    Cint,
                    (Ref{matrix_double_t}, Ref{matrix_double_t}),
                    um,
                    Rm)
                # Assert
                @test isapprox(status,1);
                @test isapprox(u, uc);
                @test !isapprox(R, Rc);
                @test isapprox(R, exp(skew(uc)));
            end
        end
        # TODO write bad input tests
    end
    @testset "Test the SO(3) logarithmic map" begin
        @testset "Good inputs" begin
            for i = 1:5
                # Fixture
                u, uc, um = get_matrix_and_copy(rand(3,1));
                R, Rc, Rm = get_matrix_and_copy(exp(skew(u)));
                out, outc, outm = get_matrix_and_copy(rand(3,1));
                # Test
                status = ccall((:cont_SO3_Log, AerialVehicleControl.CONT_LIB_PATH),
                    Cint,
                    (Ref{matrix_double_t}, Ref{matrix_double_t}),
                    Rm,
                    outm)

                theta = acos((tr(R) - 1)/2)
                # Assert
                @test isapprox(status,1);
                # We should have changed out, but not R or u
                @test isapprox(R, Rc);
                @test isapprox(u, uc);
                @test !isapprox(out, outc);
                # We chould be able to recover u from R
                @test isapprox(u, out);
            end
        end
        # TODO write bad input tests
    end
end


@kwdef struct BalancedAcceptance{I <: Integer} <: SpatialSampler
    numpoints::I = 50
    α = 1.0
end

function _generate!(piv::BalancedAcceptance, uncertainty::M) where {M<:AbstractMatrix}
    seed = Int32.([floor(10^7*rand()),floor(10^7*rand())])
    np, α = piv.numpoints, piv.α
    x,y = size(uncertainty)

    stduncert = StatsBase.transform(StatsBase.fit(ZScoreTransform, uncertainty, dims=2), uncertainty)
    reluncert = broadcast(x->exp(α*x)/(1+exp(α*x)), stduncert)
    coords = []

    ptct = 0
    while length(coords) < np
        i,j = haltonvalue(seed[1]+ptct,2), haltonvalue(seed[2]+ptct,3)
        candcoord = CartesianIndex(convert.(Int32, [ceil(x*i), ceil(y*j)])...)
        prob = reluncert[candcoord]
        rand() < prob && push!(coords, candcoord)
        ptct += 1
    end

    return coords
end