text:
module Mathematica

using MathLink

const exprs = quote

  # Typed Mathematica functions to import, in alphabetical order.
  D::Expr
  Integrate::Expr
  Prime(Integer)::Int
  RandomReal(Number)::Float64
  RandomReal(Number, Integer)::Vector{Float64}
  ToString::UTF8String

end

const macros = quote

  # Macros to import, in alphabetical order.
  Integrate::Expr
  D::Expr

end

# -----------
# Import Code
# -----------

getsym(expr) = typeof(expr) == Symbol ? expr : getsym(expr.args[1])
macrosym(s) = Symbol(string("@", s))

# Functions

for expr in exprs.args
  if typeof(expr) == Symbol || (typeof(expr) == Expr && expr.head != :line)
    @eval @mmimport $(expr)
    eval(Expr(:export, getsym(expr)))
  end
end

for expr in macros.args
  if typeof(expr) == Symbol || (typeof(expr) == Expr && expr.head != :line)
    @eval @mmacro $(expr)
    eval(Expr(:export, macrosym(getsym(expr))))
  end
end

for name in @math Names("System`*")
  f = Symbol(name)
  mf = macrosym(name)

  if !isdefined(f)
    @eval @mmimport $f
    eval(Expr(:export, f))
  end

  if !isdefined(mf)
    @eval @mmacro $f
    eval(Expr(:export, mf))
  end
end

end


text:
export AARectangle

export
    set_min!, set_max!

import ..Nodes.draw

mutable struct AARectangle <: Ranger.AbstractNode
    base::Nodes.NodeData
    transform::Nodes.TransformProperties{Float64}

    color::Rendering.Palette

    min::Geometry.Point{Float64}
    max::Geometry.Point{Float64}
    buc_min::Geometry.Point{Float64}
    buc_max::Geometry.Point{Float64}

    function AARectangle(world::Ranger.World, name::String, parent::Ranger.AbstractNode)
        o = new()

        o.base = NodeData(Ranger.gen_id(world), name, parent)
        o.transform = TransformProperties{Float64}()
        o.color = Rendering.White()

        o.min = Geometry.Point{Float64}()
        o.max = Geometry.Point{Float64}()
        o.buc_min = Geometry.Point{Float64}()
        o.buc_max = Geometry.Point{Float64}()

        o
    end
end

function Nodes.draw(node::AARectangle, context::Rendering.RenderContext)
    if Nodes.is_dirty(node)
        Math.transform!(context, node.min, node.max, node.buc_min, node.buc_max)
        Nodes.set_dirty!(node, false)
    end

    Rendering.set_draw_color(context, node.color)
    Rendering.render_aa_rectangle(context, node.buc_min, node.buc_max, Rendering.FILLED);
end

function set_min!(node::AARectangle, x::Float64, y::Float64)
    Geometry.set!(node.min, x, y)
    Nodes.set_dirty!(node, true)
end

function set_max!(node::AARectangle, x::Float64, y::Float64)
    Geometry.set!(node.max, x, y)
    Nodes.set_dirty!(node, true)
end

ext: jl

max_stars_repo_name: wdevore/Ranger-Julia-SDL

max_issues_repo_name: wdevore/Ranger-Julia-SDL

max_forks_repo_name: wdevore/Ranger-Julia-SDL


text:
# test yaml config file import

## test isntequal()
@testset "test isntequal itself" begin
    nt1 = (a=1.5, b=3, c=[1, 2, 3])
    nt2 = (c=[1, 2, 3], b=3, a=1.5)
    @test isntequal(nt1, nt1)
    @test isntequal(nt1, nt2)

    nt3 = (c=[1, 2, 3], b=3.0, a=1.5)
    @test isntequal(nt1, nt3)
    @test isntequal(nt2, nt3)

    nt4 = (c=[1, 2, 3], d=3.1, a=1.5)
    nt5 = (c=[1, 2, 3], d=3, a=1.5)
    @test ! isntequal(nt1, nt4)
    @test ! isntequal(nt2, nt4)
    @test ! isntequal(nt3, nt4)
    @test ! isntequal(nt1, nt5)
    @test ! isntequal(nt2, nt5)
    @test ! isntequal(nt3, nt5)
    @test ! isntequal(nt4, nt5)

    # nested name tuples
    nt11 = (sphere = nt1, beta=2, gamma="str1")
    nt1_ = (beta=2, gamma="str1", sphere = nt1)
    nt13 = (beta=2, sphere = nt3, gamma="str1")
    nt12 = (sphere = nt2, gamma="str1", beta=2)
    eqnts = [nt11, nt1_, nt13, nt12]
    eqnts_size = size(eqnts, 1)
    for i in 1:eqnts_size
        for j in i:eqnts_size
            @test isntequal(eqnts[i], eqnts[j])
        end
    end

    # not equal nesteds
    nnt11 = (sphere = nt1, beta=2.1, gamma="str1")
    nnt1_ = (beta=2, gamma="str2", sphere = nt1)
    nnt13 = (beta=2, sphere = nt4, gamma="str1")
    nnt12 = (sphere = nt4, gamma="str1", beta=2.1)
    nnt15 = (sphere = nt5, name="str1", beta=2.1)

    noteqnts = [nnt11, nnt1_, nnt13, nnt12, nnt15]
    noteqnts_size = size(noteqnts, 1)
    for i in 1:eqnts_size
        for j in i:noteqnts_size
            @test ! isntequal(eqnts[i], noteqnts[j])
        end
    end

    # nest moar
    nt111 = (squirrel = nt11, beta=2, gamma="str1")
    nt133 = (beta=2, squirrel = nt13, gamma="str1")
    @test isntequal(nt111, nt133)
end

@testset "yaml config read - t1.yml" begin
    f1 = joinpath(pwd(), "yaml", "t1.yml")

    conf1 = readconfig(f1)

    p1 = ransacparameters()
    p1 = ransacparameters(p1,; sphere=(ϵ=0.2, α=0.05, sphere_par=0.01,), plane=(ϵ=0.1, α=0.01,))
    p1 = ransacparameters(p1,; cylinder=(α=0.0872,), cone=(ϵ=1, α=3.14, minconeopang=1.,))
    p1 = ransacparameters(p1,; iteration=(drawN=9, minsubsetN=2, prob_det=0.999,τ=10000, itermax=100000, shape_types=[FittedPlane, FittedSphere],))
    p1 = ransacparameters(p1,; common=(parallelthrdeg=0.5, collin_threshold=0.3,))

    @test isntequal(conf1, p1)
end

@testset "yaml config read - t2.yml" begin
    f1 = joinpath(pwd(), "yaml", "t2.yml")

    conf1 = readconfig(f1)

    p1 = ransacparameters()
    p1 = ransacparameters(p1, plane=(ϵ=0.35, α=1.0872,), sphere=(sphere_par=0.025,))
    p1 = ransacparameters(p1, iteration=(itermax=100,))
    p1 = ransacparameters(p1, common=(collin_threshold=0.22, parallelthrdeg=1.2,))

    @test isntequal(conf1, p1)
end


text:
using Profile, ProfileView

include("util.jl")

mutable struct Circle
    marbles::LinkedList{Int64}
    next_marble::Int64
end

function rotate_marbles(c::Circle, idx::Int64)
    pre = c.marbles[idx:end]
    post = c.marbles[1:idx-1]
    c.marbles = vcat(pre,post)
end

function insert_marble(circle::Circle)::Int64
    marble_to_insert = circle.next_marble
    if marble_to_insert % 23 == 0
        score = marble_to_insert + circle.marbles[-6]
        deleteat!(circle.marbles, -6)
        circshift!(circle.marbles, 6)
        #rotate_marbles(circle, mod(-5,length(circle)))
        circle.next_marble += 1
        return score
    else
        insert!(circle.marbles, 2, marble_to_insert)
        circshift!(circle.marbles, -2)
        #rotate_marbles(circle, 3)
        circle.next_marble += 1
        return 0
    end
end

function part_1(num_players::Int64, last_marble::Int64)
    a = LinkedListNode(3, nothing, nothing)
    b = LinkedListNode(0, a, nothing)
    c = LinkedListNode(2, b, nothing)
    d = LinkedListNode(1, c, nothing)
    c.next = d
    b.next = c
    a.next = b
    circle = Circle(LinkedList(a,d,4), 4)
    player_scores::Array{Int64} = zeros(num_players)
    player = 1
    while circle.next_marble <= last_marble
        if circle.next_marble % 72170 == 0
            #println(circle.next_marble/72170)
        end
        player_scores[player] += insert_marble(circle)
        player += 1
        player = (player % num_players)
        if player == 0
            player = num_players
        end
    end
    maximum(player_scores)
end

part_1(10,25)
part_1(10,1618)
part_1(13,7999)
part_1(17,1104)
part_1(470,72170)
part_2 = part_1(470,72170*100)

Profile.clear()
@profile part_1(470,72170*100)
ProfileView.view()

@time part_2 = part_1(470,72170*100)


text:
NIFgen_Misc_Dict = Dict()

"""
fgen = connect!(obj::T) where {T<:NIFgen}
"""
function connect!(obj::T) where {T<:NIFgen}
	obj_ptr = ViPSession(0)
	obj_Int = [UInt8.(collect(obj.address)); UInt8(0)] # terminate with NULL char
	status = niFgen_init(obj_Int, Int16(0), Int16(0), obj_ptr)
	obj.handle = obj_ptr[]
	obj.initialized = true
	return obj
end

# close
"""
status = disconnect!(obj::T) where {T<:NIFgen}
"""
function disconnect!(obj::T) where {T<:NIFgen}
	status = niFgen_close(obj.handle)
	obj.initialized = false
	return status
end

# IDN
function get_idn(obj::T) where {T<:NIFgen}
	chlist = [UInt8.(collect("")); UInt8(0)] # terminate with NULL char
	bufSize = Int32(256)
	res    = zeros(UInt8,bufSize)
	pres   = Ref(res)
	status = niFgen_GetAttributeViString(obj.handle,
		chlist, NIFGEN_ATTR_INSTRUMENT_MODEL, bufSize, res)
	return (status, unsafe_string(pointer(res)))
end


text:
using LinearAlgebra

# k_local(Elastic Modulus,thickness,height,length,number of nodes,row/column one,row/column two, ==1 adjusts for fixed node)

#creates local stiffness matrix and maps to global
function k_local(E,t,h,l,i,n1,n2,fixed=0)
    k_global = zeros(i,i)
    A = t*h
    k = ((E*A)/l)
    k_global[n1,n1] = k
    k_global[n1,n2] = -k
    k_global[n2,n1] = -k
    k_global[n2,n2] = k
    if fixed == 1
      adj = zeros(1,i)
      adj[n1] = 1
      k_global[n1,:] = adj
      k_global[:,n1] = adj
    end
    return k_global
end

# constants from problem statement
E = 28*10^3
t = 1/8
h16 = 4
h2345 = 2.5/4
l = 2
l2345 = 8

# combining all globally mapped matrices
kG = k_local(E,t,h16,l,4,1,2,1)+
     k_local(E,t,h2345,l2345,4,2,3)+
     k_local(E,t,h2345,l2345,4,2,3)+
     k_local(E,t,h2345,l2345,4,2,3)+
     k_local(E,t,h2345,l2345,4,2,3)+
     k_local(E,t,h16,l,4,3,4)

# Force Vector
f = Float64[0 0 0 500]
F = transpose(f)

# Displacement vector
u = kG\F
println("Nodal Displacement Vector (inches)")
display(u)

# Strain Function
ϵ(x2,x1,l) = (x2 - x1)/l

#Strain Vector
ϵM = [ϵ(u[2],u[1],l)
      ϵ(u[3],u[2],l2345)
      ϵ(u[3],u[2],l2345)
      ϵ(u[3],u[2],l2345)
      ϵ(u[3],u[2],l2345)
      ϵ(u[4],u[3],l)]

# Stress Function
σ(E,ϵ) = E*ϵ

# Stress Vector
σM = [σ(E,ϵM[1])
      σ(E,ϵM[2])
      σ(E,ϵM[3])
      σ(E,ϵM[4])
      σ(E,ϵM[5])
      σ(E,ϵM[6])]
println("Element Stress Vector (psi)")
display(σM)


text:
@with_kw mutable struct EmergencySystem <: DriverModel{LatLonAccel}

    timestep::Float64 = 0.05
    t_current::Float64 = 0
    tick::Int64 = 0

    ego_vehicle::Vehicle = Vehicle(VehicleState(VecSE2(0., 0., 0.), 0.), VehicleDef(), 1)
    a::LatLonAccel = LatLonAccel(0.0, 0)

    env::CrosswalkEnv = CrosswalkEnv(CrosswalkParams())
    sensor::AutomotiveSensors.GaussianSensor = AutomotiveSensors.GaussianSensor(AutomotiveSensors.LinearNoise(10, 0., 0.),
                                                               AutomotiveSensors.LinearNoise(10, 0., 0.), 0, 0, MersenneTwister(1))

    sensor_observations::Vector{Vehicle} = []
    objects_tracked = Dict{Int64, Float64}()

    update_tick_emergency_system::Int64 = 1

    state_brake_system::Int32 = 0

    a_request::Float64 = 0
    a_current::Float64 = 0
    t_brake_trigger::Float64 = 0

    brake_request::Bool = false
    brake_release::Bool = false

    THRESHOLD_COLLISION_RATE::Float64 = 0.5
    THRESHOLD_TIME_TO_REACT::Float64 = 0.99

    TS_BREAK::Float64 = 0.2
    TD_BREAK::Float64 = 0.2
    AX_MAX::Float64 = -10.0
    SAFETY_DISTANCE_LON::Float64 = 1.0

    collision_rate::Float64 = 0.0
    risk::Float64 = 0.0
    ttc::Float64 = 0.0

    obstacles::Vector{ConvexPolygon}
    prediction_obstacle::Array{Float64} = []

end

function AutomotiveDrivingModels.observe!(model::EmergencySystem, scene::Scene, roadway::Roadway, egoid::Int)

    model.ego_vehicle = scene[findfirst(egoid, scene)]
    for i=2:scene.n
        object = scene[findfirst(i, scene)]
        if ( collision_checker(model.ego_vehicle.state, object.state, model.ego_vehicle.def, object.def) == true )
            println("collision with object: ", i, " ego.state: ", model.ego_vehicle.state, " object.state: ", object.state)
        end
    end

    model.sensor_observations = measure(model.sensor, model.ego_vehicle, scene, roadway, model.obstacles)

    # add sensor delay 0.2s
    model.sensor_observations = objects_time_delay(model)

   # println(model.sensor_observations)
    ################ Emergency System #####################################################
    (ttb, stop_distance) = get_stop_distance_vehicle(model, model.ego_vehicle.state.v, 10.0, 1.0)

    model.risk = 0
    model.collision_rate = 0
    model.brake_request = false
    model.prediction_obstacle = []

    emergency_system_brake_request = false

    if ( model.tick % model.update_tick_emergency_system == 0 )

    	phi_dot = 0.0; t0 = 0.0; x0 = 0.0; y0 = 0.0; phi0 = 0.0; TS = 0.01; T = 3.0
        ego_data = caclulate_CTRA_prediction_simple(t0, x0, 0., y0, 0., phi0, 0., model.ego_vehicle.state.v, 0., TS, T, phi_dot, model.a_current)

        # calculate for every observation the risk
        for object in model.sensor_observations

            # calculation of the object data in the Frenet frame
            object_posF = Frenet(proj(object.state.posG, get_lane(model.env.roadway, model.ego_vehicle.state), model.env.roadway, move_along_curves=false),model.env.roadway)
            delta_s = object_posF.s - model.ego_vehicle.state.posF.s - model.ego_vehicle.def.length/2
            delta_d = object_posF.t - model.ego_vehicle.state.posF.t
            delta_theta = object_posF.ϕ - model.ego_vehicle.state.posF.ϕ

            # object is only in front of the ego vehicle relevant
            if (delta_s > 0 && model.ego_vehicle.state.v > 0.1)

                # in general, trajectory should come from a prediction model!!
                # here a simple linear single track model is used!
                object_data = caclulate_CTRA_prediction_simple(t0, delta_s, 0.1, delta_d, 0.1,
                                                                delta_theta, 0.1, object.state.v, 0.1, TS, T, 0., 0.)

                # calculate the point/time where the two trajectories are closest
                (idx, ti) = get_conflict_zone(ego_data, object_data)

                # calculate collision probability based on pedestrian prediction
                (ttc, collision_rate, prediction_obstacle) = calculate_collision(ego_data, object_data, idx)

                # for visualization, only consider critical objects
                if ( collision_rate > 0.2 )
                    model.collision_rate = collision_rate
                    model.prediction_obstacle = prediction_obstacle

                    ttc_m = mean(ttc)
                    ttc_std = std(ttc)

                    model.ttc = ttc_m-2*ttc_std  # be safer
                    model.risk = min(ttb / model.ttc, 1.0)

                #   print(" collision_rate: ", model.collision_rate, " ttc_m: ", ttc_m, " ttc_std: ", ttc_std)
                #   println("Risk: ", model.risk, " ttc_min: ", model.ttc)
                end

                # intervention necessary: time to react < threashold AND collision risk < threashold
                if ( model.risk > model.THRESHOLD_TIME_TO_REACT && model.collision_rate >= model.THRESHOLD_COLLISION_RATE )
                    emergency_system_brake_request = true
                end
            end
        end

        if ( emergency_system_brake_request == true )
            model.brake_request = true
        else
            model.brake_request = false
        end
    end

    # state machine for the brake system (limitation of required deceleration request)
    model = brake_system_state_machine(model, model.ego_vehicle.state)
    model.a = LatLonAccel(0., model.a_current)

    model.t_current = model.t_current + model.timestep
    model.tick += 1

end

function AutomotiveDrivingModels.propagate(veh::Vehicle, action::LatLonAccel, roadway::Roadway, Δt::Float64)

    # new velocity
    v_ = veh.state.v + action.a_lon*Δt
    v_ = clamp(v_, 0., v_)

    # lateral offset
    delta_y = action.a_lat * Δt   # a_lat corresponds to lateral velocity --> a_lat == v_lat
    if v_ <= 0.
        delta_y = 0.
    end
    y_ = veh.state.posG.y + delta_y
    y_ = clamp(y_, -1.0, 1.0)

    s_new = v_ * Δt

    # longitudional distance based on required velocity and lateral offset
    delta_x = sqrt(s_new^2 - delta_y^2 )
    delta_x = clamp(delta_x, 0., delta_x)
    x_ = veh.state.posG.x + delta_x

    return VehicleState(VecSE2(x_, y_, veh.state.posG.θ), roadway, v_)
end

function AutomotiveDrivingModels.get_name(model::EmergencySystem)
    return "Emergency Braking System"
end

Base.rand(rng::AbstractRNG, model::EmergencySystem) = model.a

"""
    is_crash(scene::Scene)
return true if the ego car is in collision in the given scene, do not check for collisions between
other participants
"""
function is_crash(scene::Scene)
    ego = get_by_id(scene, EGO_ID)
    if ego.state.v ≈ 0
        return false
    end
    for veh in scene
        if veh.id != EGO_ID
            if collision_checker(ego, veh)
                return true
            end
        end
    end
    return false
end

"""
    brake_system_state_machine(model::EmergencySystem, ego::VehicleState)
State machine to model the behavior of the brake system. Modification of the brake.state and the required deceleration
"""
function brake_system_state_machine(model::EmergencySystem, ego::VehicleState)

    if model.brake_release == true
        model.state_brake_system = 0
    end

    if model.state_brake_system == 0
        #println("standby")
        model.a_current = 0
        if ( model.brake_request == true )
            model.state_brake_system = 1
        end
    end

    if model.state_brake_system == 1
        #println("brake_request")
        model.a_current = 0
        model.a_request = model.AX_MAX
        if ( model.a.a_lon > -0.5)    # no break delay if brake system is already active
            model.t_brake_trigger = model.t_current + model.TD_BREAK
        else
            model.t_brake_trigger = model.t_current
        end
        model.state_brake_system = 2
    end

    if model.state_brake_system == 2
        #println("brake_delay")
        model.a_current = 0
        if (model.t_current >= model.t_brake_trigger)
          model.state_brake_system = 3
        end
    end

    if model.state_brake_system == 3
        #println("brake_ramp")
        model.a_current = model.a_current - 50 * model.timestep
        if ( model.a_request >=  model.a_current )
            model.state_brake_system = 4
        end
    end
    if model.state_brake_system == 4
        #println("brake_max")
        if (ego.v > 0)
            model.a_current = model.a_request
        else
            model.a_current = 0.0
            model.state_brake_system = 0
        end
    end

    return model
end

"""
    get_stop_distance_vehicle(model::EmergencySystem, delta_v::Float64, ax_max::Float64, friction_coefficient::Float64)
Caclulation of the stop distance of the vehicle, based on the delay, ramp up phase and the maximum deceleration
return: time to brake and the distance to stop
"""
function get_stop_distance_vehicle(model::EmergencySystem, delta_v::Float64, ax_max::Float64, friction_coefficient::Float64)

    a = min(ax_max,friction_coefficient*10);

    v_s = delta_v - (a / 2) *  model.TS_BREAK;      # velocity after ramp up phase
    s_s = v_s *  model.TS_BREAK;                    # distance during ramp up phase
    s_v = (v_s * v_s) / (2 * a);                    # distance to stop with full acceleration

    if ( model.a.a_lon < -2. )
        distance_to_stop = s_v + model.SAFETY_DISTANCE_LON
    else
        distance_to_stop = model.TD_BREAK * delta_v + s_s + s_v;
        distance_to_stop = distance_to_stop + model.SAFETY_DISTANCE_LON
    end

    if ( distance_to_stop < 0.0)
        distance_to_stop = 0
    end
    ttb = distance_to_stop / delta_v

    return ttb, distance_to_stop
  end

@with_kw mutable struct Trajectory
    a::Float32 = 0
    phi_dot::Float64 = 0
    X::Matrix{Float64} = Array{Float64}(undef, 0,9)
end

"""
    caclulate_CTRA_prediction_simple(t0::Float64, x0::Float64, x_s::Float64, y0::Float64, y_s::Float64,
                            phi0::Float64, phi_s::Float64, v0::Float64, v_s::Float64, TS::Float64, T::Float64, phi_dot::Float64, a::Float64)
Simple linear prediction for an object observation. A single track model with increasing standard deviation for the relevant states is used (time based).
Calculation of the trajectory based on the current state and the current input (a and phi_dot).
return: array with {t, x, y, phi, v, x_std, y_std, phi_std, v_std}
"""
function caclulate_CTRA_prediction_simple(t0::Float64, x0::Float64, x_s::Float64, y0::Float64, y_s::Float64,
                            phi0::Float64, phi_s::Float64, v0::Float64, v_s::Float64, TS::Float64, T::Float64, phi_dot::Float64, a::Float64)

    trajectory = Trajectory()
    trajectory.a = a;
    trajectory.phi_dot = phi_dot;

    time = 0:TS:(T-TS)
    X = Matrix(undef, length(time), 9)
    x = [t0 x0 y0 phi0 v0 x_s y_s phi_s v_s]
    X[1,:] = x

    STD_X = 0.2
    STD_Y = 0.1
    STD_NN = 0.1
    for i=2:length(time)
        phi = x[4]
        v = x[5]
        x = x + TS *[1 v*cos(phi) v*sin(phi) trajectory.phi_dot trajectory.a STD_X STD_Y STD_NN STD_NN ]
        X[i,:] = x
    end

    trajectory.X = X
    return trajectory
end

"""
    calculate_collision(object_1::Trajectory, object_2::Trajectory, critical_index::Int)
Calculates for the object prediction the collision risk and the time to collision in the Frenet frame
return: time to collision distribution, collision rate and predictions ot the object at the critical point
"""
function calculate_collision(object_1::Trajectory, object_2::Trajectory, critical_index::Int)

    time = object_1.X[:,1]
    ego_state_t0 = object_1.X[1,:]

    ttc = Float64[]
    collisions = 0

    ellipses = []

    if (critical_index < length(time) )

        ego = object_1.X[critical_index,:]
        ego_def = VehicleDef()
        ego_state = VehicleState(VecSE2(ego[2],ego[3],ego[4]),ego[5])

        object = object_2.X[critical_index,:]
        object_state = VehicleState(VecSE2(object[2],object[3],object[4]),object[5])

        # get objects position on ellipse
        ellipses = get_object_confidence_interval(object[2], object[6], object[3], object[7], object[4])
        object_positions = size(ellipses)[1]

        # collision check for every object position on the ellipse
        for i = 1:object_positions

            object_state = VehicleState(VecSE2(ellipses[i,1],ellipses[i,2],object[4]),object[5])
            collision = AutomotivePOMDPs.collision_checker(ego_state, object_state, ego_def, AutomotivePOMDPs.PEDESTRIAN_DEF)
            if (collision)
                collisions = collisions + 1
                ttc_tmp = (ellipses[i,1]-ego_state_t0[2] + AutomotivePOMDPs.PEDESTRIAN_DEF.width/2) / ego_state_t0[5]
                push!(ttc, ttc_tmp)
            end
        end
        collision_rate = collisions/object_positions
    else
        collision_rate = 0
        push!(ttc, 100.0)
    end
    return ttc, collision_rate, ellipses
end

"""
    get_object_confidence_interval(x::Float64, x_sigma::Float64, y::Float64, y_sigma::Float64, theta::Float64)
Returns object positions based on the object state (prediction) for the 99% confidence interval error ellipse
"""
function get_object_confidence_interval(x::Float64, x_sigma::Float64, y::Float64, y_sigma::Float64, theta::Float64)

    # get the 99% confidence interval error ellipse
    confidence = 0.99
    k = (-2*log(1-confidence))^0.5

    x_s = k*x_sigma;
    y_s = k*y_sigma;

    # calculation of the ellipse
    theta_grid = LinRange(0.0, 2*pi, 40)

    ellipse_x  = x_s * cos.(theta_grid);
    ellipse_y  = y_s * sin.(theta_grid)

    # rotation via theta
    R = [ cos(theta) sin(theta); -sin(theta) cos(theta) ];
    ellipse = [ellipse_x ellipse_y] * R;

    # transform to x/y-position
    ellipse = [(ellipse[:,1] .+ x) (ellipse[:,2] .+ y)]
    return ellipse
end

"""
    get_conflict_zone(object_1::Trajectory, object_2::Trajectory)
Get the point/time where two trajectories are closest to each other
"""
function get_conflict_zone(object_1::Trajectory, object_2::Trajectory)
    time = object_1.X[:,1]
    min_di = 500.
    min_di_idx = 500
    for i = 1:length(time)
        di = sqrt((object_1.X[i,2] - object_2.X[i,2])^2 + (object_1.X[i,3] - object_2.X[i,3])^2 )
        if ( di <= min_di )
            min_di = di
            min_di_idx = i
        end
    end
    return min_di_idx, time[min_di_idx]
end

function objects_time_delay(model::EmergencySystem)

    objects_tracked = Vehicle[]

    for object in model.sensor_observations

	    if haskey(model.objects_tracked, object.id) == false   # new object
            model.objects_tracked[object.id] = model.t_current + 0.2
            #println(model.objects_tracked)
        else
	        if ( model.t_current >= model.objects_tracked[object.id] )   # how long is the object already visible
                model.objects_tracked[object.id] = model.t_current
                push!(objects_tracked, object)
            end
        end
    end

    # delete objects if they are older than 0.2s, because if object occurs again, the tracker needs again some time
    for oid in keys(model.objects_tracked)
        if ( model.objects_tracked[oid] < model.t_current - 0.2 )
            delete!(model.objects_tracked, oid)
        end
    end

    model.sensor_observations = deepcopy(objects_tracked)
    return model.sensor_observations
end


text:
import RNG: gen_seed, union_uint

"The key for AESNI."
type AESNIKey
    key1::UInt128
    key2::UInt128
    key3::UInt128
    key4::UInt128
    key5::UInt128
    key6::UInt128
    key7::UInt128
    key8::UInt128
    key9::UInt128
    key10::UInt128
    key11::UInt128
    AESNIKey() = new(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
end

"""
```julia
AESNI1x <: R123Generator1x{UInt128}
AESNI1x([seed])
```

AESNI1x is one kind of AESNI Counter-Based RNGs. It generates one `UInt128` number at a time.

`seed` is an `Integer` which will be automatically converted to `UInt128`.

Only available when [`R123_USE_AESNI`](@ref).
"""
type AESNI1x <: R123Generator1x{UInt128}
    x::UInt128
    key::AESNIKey
    ctr::UInt128
    function AESNI1x(seed::Integer=gen_seed(UInt128))
        r = new(0, AESNIKey(), 0)
        srand(r, seed)
    end
end

function srand(r::AESNI1x, seed::Integer=gen_seed(UInt128))
    initkey(r, seed % UInt128)
    r.ctr = 0
    random123_r(r)
    r
end

"""
```julia
AESNI4x <: R123Generator4x{UInt32}
AESNI4x([seed])
```

AESNI4x is one kind of AESNI Counter-Based RNGs. It generates four `UInt32` numbers at a time.

`seed` is a `Tuple` of four `Integer`s which will all be automatically converted to `UInt32`.

Only available when [`R123_USE_AESNI`](@ref).
"""
type AESNI4x <: R123Generator4x{UInt32}
    x1::UInt32
    x2::UInt32
    x3::UInt32
    x4::UInt32
    key::AESNIKey
    ctr1::UInt128
    p::Int
    function AESNI4x(seed::NTuple{4, Integer}=gen_seed(UInt32, 4))
        r = new(0, 0, 0, 0, AESNIKey(), 0, 0)
        srand(r, seed)
    end
end

function srand(r::AESNI4x, seed::NTuple{4, Integer}=gen_seed(UInt32, 4))
    key = union_uint(map(x -> x % UInt32, seed))
    initkey(r, key)
    r.ctr1 = 0
    p = 0
    random123_r(r)
    r
end

@inline function initkey(r, key)
    k = Ptr{UInt128}(pointer_from_objref(r.key))
    ccall((:keyinit, librandom123), Void, (
    Ptr{UInt128}, Ptr{UInt128}, Ptr{UInt128}, Ptr{UInt128},
    Ptr{UInt128}, Ptr{UInt128}, Ptr{UInt128}, Ptr{UInt128},
    Ptr{UInt128}, Ptr{UInt128}, Ptr{UInt128}, Ptr{UInt128}
    ), Ptr{UInt128}(pointer_from_objref(key)), k, k + 16, k + 32, k + 48, k + 64, k + 80, k + 96,
    k + 112, k + 128, k + 144, k + 160)
end

@inline function aesni1xm128i(r, ctr)
    k = Ptr{UInt128}(pointer_from_objref(r.key))
    p = Ptr{UInt128}(pointer_from_objref(r))
    ccall((:aesni1xm128i, librandom123), Void, (
    Ptr{UInt128}, Ptr{UInt128}, Ptr{UInt128}, Ptr{UInt128},
    Ptr{UInt128}, Ptr{UInt128}, Ptr{UInt128}, Ptr{UInt128},
    Ptr{UInt128}, Ptr{UInt128}, Ptr{UInt128}, Ptr{UInt128},
    Ptr{UInt128}
    ), Ptr{UInt128}(pointer_from_objref(ctr)), k, k + 16, k + 32, k + 48, k + 64, k + 80, k + 96,
    k + 112, k + 128, k + 144, k + 160, p)
    unsafe_load(p, 1)
end

@inline function random123_r(r::AESNI1x)
    (aesni1xm128i(r, r.ctr),)
end

@inline function random123_r(r::AESNI4x)
    aesni1xm128i(r, r.ctr1)
    x = unsafe_wrap(Array, Ptr{UInt32}(pointer_from_objref(r)), 4)
    x[1], x[2], x[3], x[4]
end


text:
"""
    InflationFunctions

Funciones para computar estimadores muestrales de inflación.
"""
module InflationFunctions

    using CPIDataBase
    using Statistics
    using StatsBase
    using SparseArrays
    using RecipesBase

    ## Métodos a extender
    import CPIDataBase: measure_name, measure_tag, params

    ## Media simple interanual
    export InflationSimpleMean
    include("InflationSimpleMean.jl")

    ## Media ponderada interanual
    export InflationWeightedMean
    include("InflationWeightedMean.jl")

    ## Método de medias móviles y suavizamiento exponencial simple (SES)
    export InflationMovingAverage, InflationExpSmoothing
    include("InflationMovingAverage.jl")
    include("InflationExpSmoothing.jl")

    ## Percentiles equiponderados
    export InflationPercentileEq
    include("InflationPercentileEq.jl")

    ## Percentiles ponderados
    export InflationPercentileWeighted
    include("InflationPercentileWeighted.jl")

    ## Variación interanual IPC con cambio de base sintético
    export InflationTotalRebaseCPI
    include("InflationTotalRebaseCPI.jl")

    ## Media Truncada Equiponderada
    export InflationTrimmedMeanEq
    include("InflationTrimmedMeanEq.jl")

    ## Media Truncada Ponderada
    export InflationTrimmedMeanWeighted
    include("InflationTrimmedMeanWeighted.jl")

    ## Exclusión Fija de gastos básicos
    export InflationFixedExclusion, InflationFixedExclusionCPI
    include("InflationFixedExclusionCPI.jl")
    include("InflationFixedExclusion.jl")

    ## Subyacente MAI (muestra ampliada implícitamente)
    export MaiG, MaiF, MaiFP
    export InflationCoreMai
    include("mai/TransversalDistr.jl")
    include("mai/renormalize.jl")
    include("mai/InflationCoreMai.jl")

    # Funciones de inflación MAI simplificadas
    export InflationCoreMaiF, InflationCoreMaiG, InflationCoreMaiFP
    include("mai/InflationCoreMaiMethods.jl")

    ## Exclusión dinámica
    export InflationDynamicExclusion
    include("InflationDynamicExclusion.jl")

    ## Inflación constante
    export InflationConstant
    include("InflationConstant.jl")

    ## Etiquetas
    include("inflation_tags.jl")

    ## Desarrollo
    include("dev/totalcpi_methods.jl")

    ## Recetas para Gráficas
    include("recipes/plotrecipes.jl")

end


text:
"""
Creates Ohms constraints for shiftable PSTs / OLTCs

```
p[f_idx] == g*vm_tap[f_idx]^2 + (-g)*(vm_tap[f_idx]*vm_tap[t_idx]*cos((va[f_bus] - va_shift[f_idx]) - (va[t_bus] - va_shift[t_idx]))) + (-b)*(vm_tap[f_idx]*vm_tap[t_idx]*sin((va[f_bus] - va_shift[f_idx]) - (va[t_bus] - va_shift[t_idx]))))
q[f_idx] == -(b+c/2)*vm_tap[f_idx]^2 - (-b)*(vm_tap[f_idx]*vm_tap[t_idx]*cos((va[f_bus] - va_shift[f_idx]) - (va[t_bus] - va_shift[t_idx]))) + (-g)*(vm_tap[f_idx]*vm_tap[t_idx]*sin((va[f_bus] - va_shift[f_idx]) - (va[t_bus] - va_shift[t_idx]))))
vm_tap[f_idx] * tap_min <= vm[f_bus] <= vm_tap[f_idx] * tap_max
```
"""
function constraint_variable_transformer_y_from(pm::GenericPowerModel{T}, n::Int, cnd::Int, f_bus, t_bus, f_idx, t_idx, g, b, c, g_shunt, tap_min, tap_max)  where T <: PowerModels.AbstractACPForm
    p_fr = PowerModels.var(pm, n, cnd, :p, f_idx)
    q_fr = PowerModels.var(pm, n, cnd, :q, f_idx)
    vm_fr = PowerModels.var(pm, n, cnd, :vm, f_bus)
    vm_to = PowerModels.var(pm, n, cnd, :vm, t_bus)
    va_fr = PowerModels.var(pm, n, cnd, :va, f_bus)
    va_to = PowerModels.var(pm, n, cnd, :va, t_bus)
    va_shift_fr = PowerModels.var(pm, n, cnd, :va_shift, f_idx)
    va_shift_to = PowerModels.var(pm, n, cnd, :va_shift, t_idx)
    vm_tap_fr = PowerModels.var(pm, n, cnd, :vm_tap, f_idx)
    vm_tap_to = PowerModels.var(pm, n, cnd, :vm_tap, t_idx)

    @NLconstraint(pm.model, p_fr == (g + g_shunt / 2)*vm_tap_fr^2 + (-g)*(vm_tap_fr*vm_tap_to*cos((va_fr - va_shift_fr) - (va_to - va_shift_to))) + (-b)*(vm_tap_fr*vm_tap_to*sin((va_fr - va_shift_fr) - (va_to - va_shift_to))))
    @NLconstraint(pm.model, q_fr == -(b+c/2)*vm_tap_fr^2 - (-b)*(vm_tap_fr*vm_tap_to*cos((va_fr - va_shift_fr) - (va_to - va_shift_to))) + (-g)*(vm_tap_fr*vm_tap_to*sin((va_fr - va_shift_fr) - (va_to - va_shift_to))))
    @constraint(pm.model, vm_tap_fr * tap_min <= vm_fr)
    @constraint(pm.model, vm_fr <= vm_tap_fr * tap_max)
end
"""
Creates Ohms constraints for shiftable PSTs / OLTCs

```
p[t_idx] == g*vm_tap[t_idx]^2 + (-g)*(vm_tap[t_idx]*vm_tap[f_idx]*cos((va[t_bus] - va_shift[t_idx]) - (va[f_bus] - va_shift[f_idx]))) + (-b)*(vm_tap[t_idx]*vm_tap[f_idx]*sin((va[t_bus] - va_shift[t_idx]) - (va[f_bus] - va_shift[f_idx]))))
q[t_idx] == -(b+c/2)*vm_tap[t_idx]^2 - (-b)*(vm_tap[t_idx]*vm_tap[f_idx]*cos((va[t_bus] - va_shift[t_idx]) - (va[f_bus] - va_shift[f_idx]))) + (-g)*(vm_tap[t_idx]*vm_tap[f_idx]*sin((va[t_bus] - va_shift[t_idx]) - (va[f_bus] - va_shift[f_idx]))))
vm_tap[t_idx] * tap_min <= vm[t_bus] <= vm_tap[t_idx] * tap_max
```
"""
function constraint_variable_transformer_y_to(pm::GenericPowerModel{T}, n::Int, cnd::Int, f_bus, t_bus, f_idx, t_idx, g, b, c, g_shunt, tap_min, tap_max) where T <: PowerModels.AbstractACPForm
    p_to = PowerModels.var(pm, n, cnd :p, t_idx)
    q_to = PowerModels.var(pm, n, cnd, :q, t_idx)
    vm_fr = PowerModels.var(pm, n, cnd, :vm, f_bus)
    vm_to = PowerModels.var(pm, n, cnd, :vm, t_bus)
    va_fr = PowerModels.var(pm, n, cnd, :va, f_bus)
    va_to = PowerModels.var(pm, n, cnd, :va, t_bus)
    va_shift_fr = PowerModels.var(pm, n, cnd, :va_shift, f_idx)
    va_shift_to = PowerModels.var(pm, n, cnd, :va_shift, t_idx)
    vm_tap_fr = PowerModels.var(pm, n, cnd, :vm_tap, f_idx)
    vm_tap_to = PowerModels.var(pm, n, cnd, :vm_tap, t_idx)

    @NLconstraint(pm.model, p_to == (g + g_shunt / 2)*vm_tap_to^2 + (-g)*(vm_tap_to*vm_tap_fr*cos((va_to - va_shift_to) - (va_fr - va_shift_fr))) + (-b)*(vm_tap_to*vm_tap_fr*sin((va_to - va_shift_to) - (va_fr - va_shift_fr))))
    @NLconstraint(pm.model, q_to == -(b+c/2)*vm_tap_to^2 - (-b)*(vm_tap_to*vm_tap_fr*cos((va_to - va_shift_to) - (va_fr - va_shift_fr))) + (-g)*(vm_tap_to*vm_tap_fr*sin((va_to - va_shift_to) - (va_fr - va_shift_fr))))
    @constraint(pm.model, vm_tap_to * tap_min <= vm_to)
    @constraint(pm.model, vm_to <= vm_tap_to * tap_max)
end
"""
Links voltage magnitudes of not tappable transformers with node voltage magnitudes

```
vm_tap[f_idx] * tap == vm[f_bus]
vm_tap[t_idx] * tap == vm[t_bus]
```
"""
function constraint_link_voltage_magnitudes(pm::GenericPowerModel{T}, n::Int, cnd::Int, f_bus, t_bus, f_idx, t_idx, tap_fr, tap_to) where T <: PowerModels.AbstractACPForm
    vm_fr = PowerModels.var(pm, n, cnd, :vm, f_bus)
    vm_to = PowerModels.var(pm, n, cnd, :vm, t_bus)
    vm_tap_fr = PowerModels.var(pm, n, cnd, :vm_tap, f_idx)
    vm_tap_to = PowerModels.var(pm, n, cnd, :vm_tap, t_idx)
    @constraint(pm.model, vm_tap_fr * tap_fr == vm_fr)
    @constraint(pm.model, vm_tap_to * tap_to == vm_to)
end
"""
Aggregated unit representation in the nodes
```
sum(p[a] for a in bus_arcs) + sum(p_dc[a_dc] for a_dc in bus_arcs_dc) == pnode - gs*v^2
sum(q[a] for a in bus_arcs) + sum(q_dc[a_dc] for a_dc in bus_arcs_dc) == qnode + bs*v^2
```
"""
function constraint_kcl_shunt_aggregated(pm::GenericPowerModel{T}, n::Int, cnd::Int, i, bus_arcs, bus_arcs_dc, bus_gs, bus_bs) where T <: PowerModels.AbstractACPForm
    vm = PowerModels.var(pm, n, cnd, :vm, i)
    p = PowerModels.var(pm, n, cnd, :p)
    q = PowerModels.var(pm, n, cnd, :q)
    pnode = PowerModels.var(pm, n, cnd, :pnode, i)
    qnode = PowerModels.var(pm, n, cnd, :qnode, i)
    p_dc = PowerModels.var(pm, n, cnd, :p_dc)
    q_dc = PowerModels.var(pm, n, cnd, :q_dc)

    @constraint(pm.model, sum(p[a] for a in bus_arcs) + sum(p_dc[a_dc] for a_dc in bus_arcs_dc) == pnode - sum(gs for gs in values(bus_gs))*vm^2)
    @constraint(pm.model, sum(q[a] for a in bus_arcs) + sum(q_dc[a_dc] for a_dc in bus_arcs_dc) == qnode + sum(bs for bs in values(bus_bs))*vm^2)
end
function constraint_redispatch_active_power_gen(pm::GenericPowerModel{T}, n::Int, cnd::Int, i::Int, pref::AbstractFloat) where T <: PowerModels.AbstractACPForm
    pg_delta = PowerModels.var(pm, n, cnd, :pg_delta, i)
    pg = PowerModels.var(pm, n, cnd, :pg, i)

    if pm.setting["relax_absolute_value"] == false
        @NLconstraint(pm.model, pg_delta == abs(pg - pref))
    else
        @constraint(pm.model, pg_delta >= pg - pref)
        @constraint(pm.model, pg_delta >= -(pg - pref))
    end
end
function constraint_redispatch_reactive_power_gen(pm::GenericPowerModel{T}, n::Int,  cnd::Int, i::Int, qref::AbstractFloat) where T <: PowerModels.AbstractACPForm
    qg_delta = PowerModels.var(pm, n, cnd, :qg_delta, i)
    qg = PowerModels.var(pm, n, cnd, :qg, i)

    if pm.setting["relax_absolute_value"] == false
        @NLconstraint(pm.model, qg_delta == abs(qg - qref))
    else
        @constraint(pm.model, qg_delta >= qg - qref)
        @constraint(pm.model, qg_delta >= -(qg - qref))
    end
end
function constraint_second_stage_redispatch_active_power_gen(pm::GenericPowerModel{T}, n::Int, cnd::Int, i::Int, first_stage_network_id) where T <: PowerModels.AbstractACPForm
    pg_delta = PowerModels.var(pm, n, cnd, :pg_delta, i)
    pg = PowerModels.var(pm, n, cnd, :pg, i)
    pg_first_stage = PowerModels.var(pm, first_stage_network_id, cnd, :pg, i)

    if pm.setting["relax_absolute_value"] == false
        @NLconstraint(pm.model, pg_delta == abs(pg - pg_first_stage))
    else
        @constraint(pm.model, pg_delta >= pg - pg_first_stage)
        @constraint(pm.model, pg_delta >= -(pg - pg_first_stage))
    end
end
function constraint_second_stage_redispatch_reactive_power_gen(pm::GenericPowerModel{T}, n::Int, cnd::Int, i::Int, first_stage_network_id) where T <: PowerModels.AbstractACPForm
    qg_delta = PowerModels.var(pm, n, cnd, :qg_delta, i)
    qg = PowerModels.var(pm, n, cnd, :qg, i)
    qg_first_stage = PowerModels.var(pm, first_stage_network_id, cnd, :qg, i)

    if pm.setting["relax_absolute_value"] == false
        @NLconstraint(pm.model, qg_delta == abs(qg - qg_first_stage))
    else
        @constraint(pm.model, qg_delta >= qg - qg_first_stage)
        @constraint(pm.model, qg_delta >= -(qg - qg_first_stage))
    end
end
function constraint_redispatch_active_power_load(pm::GenericPowerModel{T}, n::Int, cnd::Int, i::Int, pref::AbstractFloat) where T <: PowerModels.AbstractACPForm
    pl_delta = PowerModels.var(pm, n, cnd, :pl_delta, i)
    pl = PowerModels.var(pm, n, cnd, :pl, i)

    if pm.setting["relax_absolute_value"] == false
        @NLconstraint(pm.model, pl_delta == abs(pl - pref))
    else
        @constraint(pm.model, pl_delta >= pl - pref)
        @constraint(pm.model, pl_delta >= -(pl - pref))
    end
end
function constraint_redispatch_reactive_power_load(pm::GenericPowerModel{T}, n::Int, cnd::Int, i::Int, qref::AbstractFloat) where T <: PowerModels.AbstractACPForm
    ql_delta = PowerModels.var(pm, n, cnd, :ql_delta, i)
    ql = PowerModels.var(pm, n, cnd, :ql, i)

    if pm.setting["relax_absolute_value"] == false
        @NLconstraint(pm.model, ql_delta == abs(ql - qref))
    else
        @constraint(pm.model, ql_delta >= ql - qref)
        @constraint(pm.model, ql_delta >= -(ql - qref))
    end
end
function constraint_second_stage_redispatch_active_power_load(pm::GenericPowerModel{T}, n::Int, cnd::Int, i::Int, first_stage_network_id)  where T <: PowerModels.AbstractACPForm
    pl_delta = PowerModels.var(pm, n, cnd, :pl_delta, i)
    pl = PowerModels.var(pm, n, cnd, :pl, i)
    pl_first_stage = PowerModels.var(pm, first_stage_network_id, cnd, :pl, i)

    if pm.setting["relax_absolute_value"] == false
        @NLconstraint(pm.model, pl_delta == abs(pl - pl_first_stage))
    else
        @constraint(pm.model, pl_delta >= pl - pl_first_stage)
        @constraint(pm.model, pl_delta >= -(pl - pl_first_stage))
    end
end
function constraint_second_stage_redispatch_reactive_power_load(pm::GenericPowerModel{T}, n::Int, cnd::Int, i::Int, first_stage_network_id)  where T <: PowerModels.AbstractACPForm
    ql_delta = PowerModels.var(pm, n, cnd, :ql_delta, i)
    ql = PowerModels.var(pm, n, cnd, :ql, i)
    ql_first_stage = PowerModels.var(pm, first_stage_network_id, cnd, :ql, i)

    if pm.setting["relax_absolute_value"] == false
        @NLconstraint(pm.model, ql_delta == abs(ql - ql_first_stage))
    else
        @constraint(pm.model, ql_delta >= ql - ql_first_stage)
        @constraint(pm.model, ql_delta >= -(ql - ql_first_stage))
    end
end

max_issues_repo_name: frederikgeth/PowerModelsReliability

max_forks_repo_name: frederikgeth/PowerModelsReliability


text:
################################################################################
#
#   COLLECTION OF FUNCTIONS FOR
#   RECIPROCAL LATTICES / UNITCELLS
#
#   FILE CONTAINS
#   - construction of reciprocal unitcells from real space unitcells
#   - testing of reciprocality between unitcell and reciprocal unitcell
#   - reciprocal point information, shifting to 1st BZ
#
################################################################################

################################################################################
#
#   RECIPROCAL LATTICE / UNITCELL CONSTRUCTION
#
################################################################################

############
# FALLBACK #
############

# N=?,D=?
function getReciprocalUnitcell(
        ::Type{R},
        unitcell :: U
    ) :: R where {D,N,LS,LB,S<:AbstractSite{LS,D},B<:AbstractBond{LB,N},U<:AbstractUnitcell{S,B},
                  LBR,BR<:AbstractBond{LBR,N},P<:AbstractReciprocalPoint{D},R<:AbstractReciprocalUnitcell{P,BR}}

    # give an error
    error("Cannot build a reciprocal unitcell for realspace lattice dimensions D=" * string(D) * " and N=" * string(N) * ".")
end

###########
# WRAPPER #
###########

# N=?,D=?
function getReciprocalUnitcell(
        unitcell :: U
    ) :: ReciprocalUnitcell{ReciprocalPoint{D},Bond{LB,N}} where {D,N,LS,LB,S<:AbstractSite{LS,D},B<:AbstractBond{LB,N},U<:AbstractUnitcell{S,B},
                  LBR,BR<:AbstractBond{LBR,N},P<:AbstractReciprocalPoint{D},R<:AbstractReciprocalUnitcell{P,BR}}

    # pass on to general interface
    return getReciprocalUnitcell(ReciprocalUnitcell{ReciprocalPoint{D},Bond{LB,N}}, unitcell)
end

###########
#   1D    #
###########

# N=1,D=?
function getReciprocalUnitcell(
        ::Type{R},
        unitcell :: U
    ) :: R where {D,LS,LB,S<:AbstractSite{LS,D},B<:AbstractBond{LB,1},U<:AbstractUnitcell{S,B},
                  LBR,BR<:AbstractBond{LBR,1},P<:AbstractReciprocalPoint{D},R<:AbstractReciprocalUnitcell{P,BR}}

    # build lattice vector
    latvec_b1 = a1(unitcell) .* 2*pi/sqrt(dot(a1(unitcell), a1(unitcell)))

    # return a new reciprocal unitcell
    return newReciprocalUnitcell(
        # pass the given type
        R,
        # pass the single lattice vector
        Vector{Float64}[
            latvec_b1,
        ],
        # pass the two relevant bonds
        BR[
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1,)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1,))
        ]
    )
end

###########
#   2D    #
###########

# N=2, D=2
function getReciprocalUnitcell(
        ::Type{R},
        unitcell :: U
    ) :: R where {LS,LB,S<:AbstractSite{LS,2},B<:AbstractBond{LB,2},U<:AbstractUnitcell{S,B},
                  LBR,BR<:AbstractBond{LBR,2},P<:AbstractReciprocalPoint{2},R<:AbstractReciprocalUnitcell{P,BR}}

    # build reciprocal lattice vectors
    latvec_b1 = [a2(unitcell)[2], -a2(unitcell)[1]]
    latvec_b2 = [a1(unitcell)[2], -a1(unitcell)[1]]

    # normalize the reciprocal vectors
    latvec_b1 .*= 2*pi/dot(latvec_b1, a1(unitcell))
    latvec_b2 .*= 2*pi/dot(latvec_b2, a2(unitcell))

    # return a new reciprocal unitcell
    return newReciprocalUnitcell(
        # pass the given type
        R,
        # pass the single lattice vector
        Vector{Float64}[
            latvec_b1,
            latvec_b2
        ],
        # pass the two relevant bonds
        BR[
            newBond(BR, 1,1, getDefaultLabel(LBR), ( 0,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), ( 0,-1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1, 0)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1, 0)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1,-1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1,-1))
        ]
    )
end

# N=2, D=3
function getReciprocalUnitcell(
        ::Type{R},
        unitcell :: U
    ) :: R where {LS,LB,S<:AbstractSite{LS,3},B<:AbstractBond{LB,2},U<:AbstractUnitcell{S,B},
                  LBR,BR<:AbstractBond{LBR,2},P<:AbstractReciprocalPoint{3},R<:AbstractReciprocalUnitcell{P,BR}}

    # build a orthogonal vector
    ortho = cross(a1(unitcell), a2(unitcell))

    # build reciprocal lattice vectors
    latvec_b1 = cross(a2(unitcell), ortho)
    latvec_b2 = cross(a1(unitcell), ortho)

    # normalize the reciprocal vectors
    latvec_b1 .*= 2*pi/dot(latvec_b1, a1(unitcell))
    latvec_b2 .*= 2*pi/dot(latvec_b2, a2(unitcell))

    # return a new reciprocal unitcell
    return newReciprocalUnitcell(
        # pass the given type
        R,
        # pass the two lattice vectors
        Vector{Float64}[
            latvec_b1,
            latvec_b2
        ],
        # pass the 8 (3^2 - 1) relevant bonds
        BR[
            newBond(BR, 1,1, getDefaultLabel(LBR), ( 0,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), ( 0,-1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1, 0)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1, 0)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1,-1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1,-1))
        ]
    )
end

# N=2, D<2 not possible
# N=2, D>3 not implemented

###########
#   3D    #
###########

# N=2, D=3
function getReciprocalUnitcell(
        ::Type{R},
        unitcell :: U
    ) :: R where {LS,LB,S<:AbstractSite{LS,3},B<:AbstractBond{LB,3},U<:AbstractUnitcell{S,B},
                  LBR,BR<:AbstractBond{LBR,3},P<:AbstractReciprocalPoint{3},R<:AbstractReciprocalUnitcell{P,BR}}

    # construct reciprocal lattice vectors
    latvec_b1 = cross(a2(unitcell), a3(unitcell))
    latvec_b2 = cross(a3(unitcell), a1(unitcell))
    latvec_b3 = cross(a1(unitcell), a2(unitcell))
    # normalize the vectors
    latvec_b1 .*= 2*pi/sum(latvec_b1.*a1(unitcell))
    latvec_b2 .*= 2*pi/sum(latvec_b2.*a2(unitcell))
    latvec_b3 .*= 2*pi/sum(latvec_b3.*a3(unitcell))

    # return a new reciprocal unitcell
    return newReciprocalUnitcell(
        # pass the given type
        R,
        # pass the three lattice vectors
        Vector{Float64}[
            latvec_b1,
            latvec_b2,
            latvec_b3
        ],
        # pass the 26 (3^3 - 1) relevant bonds
        BR[
            newBond(BR, 1,1, getDefaultLabel(LBR), ( 0, 0,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), ( 0, 0,-1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), ( 0,+1, 0)),
            newBond(BR, 1,1, getDefaultLabel(LBR), ( 0,-1, 0)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1, 0, 0)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1, 0, 0)),
            newBond(BR, 1,1, getDefaultLabel(LBR), ( 0,+1,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), ( 0,-1,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), ( 0,+1,-1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), ( 0,-1,-1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1, 0,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1, 0,-1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1, 0,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1, 0,-1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1,+1, 0)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1,-1, 0)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1,+1, 0)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1,-1, 0)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1,+1,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1,-1,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1,+1,-1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (+1,-1,-1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1,+1,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1,-1,+1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1,+1,-1)),
            newBond(BR, 1,1, getDefaultLabel(LBR), (-1,-1,-1))
        ]
    )
end

# N=3, D<3 not possible
# N=3, D>3 not implemented

# export the function
export getReciprocalUnitcell

################################################################################
#
#   RECIPROCAL LATTICE / UNITCELL TESTING
#
################################################################################

# Test function
function testReciprocality(
        uc_real :: U,
        uc_reci :: R,
        error_allowed :: Float64 = 1e-8
    ) :: Bool where {N,UL,US,UB<:AbstractBond{UL,N},U<:AbstractUnitcell{US,UB},
                     RL,RS,RB<:AbstractBond{RL,N},R<:AbstractReciprocalUnitcell{RS,RB}}

    # the complete overlap
    overlap_sum = 0.0

    # test the overlap
    for l1 in 1:length(latticeVectors(uc_real))
    for l2 in 1:length(latticeVectors(uc_reci))
        # the value that it should take
        v_correct = l1==l2 ? 2*pi : 0.0
        # calculate the value
        v_taken = dot(latticeVectors(uc_real)[l1],latticeVectors(uc_reci)[l2])
        # add to the overlap sum
        overlap_sum += abs(v_correct - v_taken)
    end
    end

    # check if the overall overlap is sufficiently small
    return overlap_sum < error_allowed ? true : false
end

# export the function
export testReciprocality

################################################################################
#
#   RECIPROCAL POINT INFORMATION / SHIFTING TO 1st BZ
#
################################################################################

# TESTING IF IN 1st BZ
function isInFirstBZ(
            reci_unitcell :: RU,
            k             :: Vector{<:Real}
        ) :: Bool where {D,N,L,P<:AbstractReciprocalPoint{D},B<:AbstractBond{L,N},RU<:AbstractReciprocalUnitcell{P,B}}

    # build up the minimal distance to any neighbor
    min_distance_squared = 1e40

    # iterate over all neighbors and find distance
    for b in bonds(reci_unitcell)
        # find out the position of the neighbor
        pos_neighbor = Float64[
            sum([latticeVectors(reci_unitcell)[j][i] * wrap(b)[j] for j in 1:N])  for i in 1:D
        ]
        # add the k point given
        pos_neighbor .+= k
        # check the minimum squared distance
        min_distance_squared = min(min_distance_squared, dot(pos_neighbor, pos_neighbor))
    end

    # return if the minimal distance to the neighbors is smaller than the distance to the origin (@[0,0,0,..])
    return (min_distance_squared >= dot(k,k))
end

# export the test
export isInFirstBZ

# SHIFTING TO 1st BZ
function shiftToFirstBZ(
            reci_unitcell :: RU,
            k             :: Vector{<:Real}
        ) :: Vector{Float64} where {D,N,L,P<:AbstractReciprocalPoint{D},B<:AbstractBond{L,N},RU<:AbstractReciprocalUnitcell{P,B}}

    # return the inplace function with a copied input
    return shiftToFirstBZ!(reci_unitcell, deepcopy(k))
end

# SHIFTING TO 1st BZ (inplace)
function shiftToFirstBZ!(
            reci_unitcell :: RU,
            k             :: Vector{<:Real}
        ) :: Vector{Float64} where {D,N,L,P<:AbstractReciprocalPoint{D},B<:AbstractBond{L,N},RU<:AbstractReciprocalUnitcell{P,B}}

    # find out the current distance to gamma point
    current_distance = dot(k,k)

    # create a shifted k vector
    k_shifted = k

    # boolean indicating if the point was relocated in the last iteration
    relocated = true
    # try to shift as long as there is a possiblity to decrease the distance
    while relocated
        # assume that the point is not relocated in this iteration
        relocated = false
        # iterate over all neighbors and try to shift the point
        for b in bonds(reci_unitcell)
            # get the offset from the neighbor wrap
            offset = Float64[
                sum([latticeVectors(reci_unitcell)[j][i] * wrap(b)[j] for j in 1:N])  for i in 1:D
            ]
            # see if the distance is lowered and maybe shift
            if dot(k_shifted .+ offset, k_shifted .+ offset) < current_distance
                # shift the point
                k_shifted .+= offset
                # set the current distance correctly
                current_distance = dot(k_shifted,k_shifted)
                # denote as relocated
                relocated = true
            end
        end
    end

    # return the final shifted lattice vector
    return k_shifted
end

# export the test
export shiftToFirstBZ, shiftToFirstBZ!


text:
module TestVar

using Base.Test

using Bio: Seq, Var
using TestFunctions
using PairwiseListMatrices
using IntervalTrees: IntervalValue

typealias PWM PairwiseListMatrix

function generate_testcase{A<:Union{DNAAlphabet{4}, DNAAlphabet{2}, RNAAlphabet{4}, RNAAlphabet{2}}}(::Type{A}, len::Int)
    a = [convert(Char, i)  for i in alphabet(A)]
    probs = Vector{Float64}(length(a))
    fill!(probs, 1 / length(a))
    return BioSequence{A}(random_seq(len, a, probs))
end

function generate_possibilities_tester{A<:Union{DNAAlphabet{4}, DNAAlphabet{2}, RNAAlphabet{4}, RNAAlphabet{2}}}(::Type{A})
    symbols = alphabet(A)
    arra = Vector{eltype(A)}()
    arrb = Vector{eltype(A)}()
    for i in 1:length(symbols), j in i:length(symbols)
        push!(arra, symbols[i])
        push!(arrb, symbols[j])
    end
    return BioSequence{A}(arra), BioSequence{A}(arrb)
end

for alph in (:DNAAlphabet, :RNAAlphabet)
    @eval function generate_possibilities_tester(::Type{$alph{2}}, ::Type{$alph{4}})
        arra = Vector{eltype($alph)}()
        arrb = Vector{eltype($alph)}()
        for i in alphabet($alph{2}), j in alphabet($alph{4})
            push!(arra, i)
            push!(arrb, j)
        end
        return BioSequence{$alph{2}}(arra), BioSequence{$alph{4}}(arrb)
    end
end

@testset "Var" begin
    @testset "Site counting and identification" begin
        @testset "Naive methods" begin

            alphabets = (DNAAlphabet{4}, DNAAlphabet{2}, RNAAlphabet{4}, RNAAlphabet{2})

            for alph in alphabets

                # Answers to these tests were worked out manually to verify count_sites_naive was working.
                # seqA and seqB contain all possible observations of sites.

                istwobit = Seq.bitsof(alph) == 2

                seqA, seqB = generate_possibilities_tester(alph)

                # Test methods which work on single sequences.
                @test count_sites_naive(Certain, seqA) == ifelse(istwobit, length(seqA), 49)
                @test count_sites_naive(Certain, seqB) == ifelse(istwobit, length(seqB), 19)
                @test count_sites_naive(Gap, seqA) == ifelse(istwobit, 0, 16)
                @test count_sites_naive(Gap, seqB) == ifelse(istwobit, 0, 1)
                @test count_sites_naive(Ambiguous, seqA) == ifelse(istwobit, 0, length(seqA) - 65)
                @test count_sites_naive(Ambiguous, seqB) == ifelse(istwobit, 0, length(seqB) - 20)

                # Test methods which work on two sequences.
                # Test when sequences are of the same bitencoding.

                @test count_sites_naive(Certain, seqA, seqB) == count_sites_naive(Certain, seqB, seqA) == 10
                @test count_sites_naive(Gap, seqA, seqB) == count_sites_naive(Gap, seqB, seqA) == ifelse(istwobit, 0, 16)
                @test count_sites_naive(Ambiguous, seqA, seqB) == count_sites_naive(Ambiguous, seqB, seqA) == ifelse(istwobit, 0, 121)
                @test count_sites_naive(Match, seqA, seqB) == count_sites_naive(Match, seqB, seqA) == length(alphabet(alph))
                @test count_sites_naive(Mismatch, seqA, seqB) == count_sites_naive(Mismatch, seqB, seqA) == (length(seqA) - length(alphabet(alph)))

                @test count_sites_naive(Mutated, seqA, seqB) == count_sites_naive(Mutated, seqB, seqA) == (6, ifelse(istwobit, 0, 126))
                @test count_sites_naive(Conserved, seqA, seqB) == count_sites_naive(Conserved, seqB, seqA) == (4, ifelse(istwobit, 0, 126))
                @test count_sites_naive(Transition, seqA, seqB) == count_sites_naive(Transition, seqB, seqA) == (2, ifelse(istwobit, 0, 126))
                @test count_sites_naive(Transversion, seqA, seqB) == count_sites_naive(Transversion, seqB, seqA) == (4, ifelse(istwobit, 0, 126))
            end

            # Test for when sequences are of different bitencodings.
            for alphs in [(DNAAlphabet{2}, DNAAlphabet{4}),
                          (RNAAlphabet{2}, RNAAlphabet{4})]
                seqA, seqB = generate_possibilities_tester(alphs...)
                @test count_sites_naive(Certain, seqA, seqB) == count_sites_naive(Certain, seqB, seqA) == 16
                @test count_sites_naive(Gap, seqA, seqB) == count_sites_naive(Gap, seqB, seqA) == 4
                @test count_sites_naive(Ambiguous, seqA, seqB) == count_sites_naive(Ambiguous, seqB, seqA) == 44
                @test count_sites_naive(Match, seqA, seqB) == count_sites_naive(Match, seqB, seqA) == 4
                @test count_sites_naive(Mismatch, seqA, seqB) == count_sites_naive(Mismatch, seqB, seqA) == 60

                @test count_sites_naive(Mutated, seqA, seqB) == count_sites_naive(Mutated, seqB, seqA) == (12, 48)
                @test count_sites_naive(Conserved, seqA, seqB) == count_sites_naive(Conserved, seqB, seqA) == (4, 48)
                @test count_sites_naive(Transition, seqA, seqB) == count_sites_naive(Transition, seqB, seqA) == (4, 48)
                @test count_sites_naive(Transversion, seqA, seqB) == count_sites_naive(Transversion, seqB, seqA) == (8, 48)
            end
        end

        @testset "Pairwise methods" begin
            dnas = [dna"ATCGCCA-", dna"ATCGCCTA", dna"ATCGCCT-", dna"GTCGCCTA"]
            rnas = [rna"AUCGCCA-", rna"AUCGCCUA", rna"AUCGCCU-", rna"GUCGCCUA"]
            answer_mismatch = PWM{Int, false}([0 2 1 3; 2 0 1 1; 1 1 0 2; 3 1 2 0])
            answer_match = PWM{Int, false}([0 6 7 5; 6 0 7 7; 7 7 0 6; 5 7 6 0])
            for i in (dnas, rnas)
                @test count_sites(Mismatch, i) == answer_mismatch
                @test count_sites(Match, i) == answer_match
                @test count_sites(Certain, i) == PWM{Int, false}([0 7 7 7; 7 0 7 8; 7 7 0 7; 7 8 7 0])
                @test count_sites(Ambiguous, i) == PWM{Int, false}([0 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0])
                @test count_sites(Gap, i) == PWM{Int, false}([0 1 1 1; 1 0 1 0; 1 1 0 1; 1 0 1 0])
                ambigs = PWM{Int, false}([0 1 1 1; 1 0 1 0; 1 1 0 1; 1 0 1 0])
                @test count_sites(Conserved, i) == (PWM{Int, false}([0 6 6 5; 6 0 7 7; 6 7 0 6; 5 7 6 0]), ambigs)
                @test count_sites(Mutated, i) == (PWM{Int, false}([0 1 1 2; 1 0 0 1; 1 0 0 1; 2 1 1 0]), ambigs)
            end
        end

        @testset "Windowed methods" begin
            dnaA = dna"ATCGCCA-M"
            dnaB = dna"ATCGCCTAA"
            rnaA = rna"AUCGCCA-M"
            rnaB = rna"AUCGCCUAA"
            matches = [3, 3, 3, 3, 2, 1, 0]
            idxes = [1:3, 2:4, 3:5, 4:6, 5:7, 6:8, 7:9]
            for seqs in ((dnaA, dnaB), (rnaA, rnaB))
                @test count_sites(Certain, seqs[1], seqs[2], 3, 1) == [IntervalValue(1, 3, 3),
                                                                       IntervalValue(2, 4, 3),
                                                                       IntervalValue(3, 5, 3),
                                                                       IntervalValue(4, 6, 3),
                                                                       IntervalValue(5, 7, 3),
                                                                       IntervalValue(6, 8, 2),
                                                                       IntervalValue(7, 9, 1)]
                @test count_sites(Ambiguous, seqs[1], seqs[2], 3, 1) == [IntervalValue(1, 3, 0),
                                                                         IntervalValue(2, 4, 0),
                                                                         IntervalValue(3, 5, 0),
                                                                         IntervalValue(4, 6, 0),
                                                                         IntervalValue(5, 7, 0),
                                                                         IntervalValue(6, 8, 0),
                                                                         IntervalValue(7, 9, 1)]
                @test count_sites(Gap, seqs[1], seqs[2], 3, 1) == [IntervalValue(1, 3, 0),
                                                                   IntervalValue(2, 4, 0),
                                                                   IntervalValue(3, 5, 0),
                                                                   IntervalValue(4, 6, 0),
                                                                   IntervalValue(5, 7, 0),
                                                                   IntervalValue(6, 8, 1),
                                                                   IntervalValue(7, 9, 1)]
                @test count_sites(Match, seqs[1], seqs[2], 3, 1) == [IntervalValue(1, 3, 3),
                                                                     IntervalValue(2, 4, 3),
                                                                     IntervalValue(3, 5, 3),
                                                                     IntervalValue(4, 6, 3),
                                                                     IntervalValue(5, 7, 2),
                                                                     IntervalValue(6, 8, 1),
                                                                     IntervalValue(7, 9, 0)]
                @test count_sites(Mismatch, seqs[1], seqs[2], 3, 1) == [IntervalValue(1, 3, 0),
                                                                        IntervalValue(2, 4, 0),
                                                                        IntervalValue(3, 5, 0),
                                                                        IntervalValue(4, 6, 0),
                                                                        IntervalValue(5, 7, 1),
                                                                        IntervalValue(6, 8, 2),
                                                                        IntervalValue(7, 9, 3)]

                @test count_sites(Conserved, seqs[1], seqs[2], 3, 1) == ([IntervalValue(1, 3, 3),
                                                                          IntervalValue(2, 4, 3),
                                                                          IntervalValue(3, 5, 3),
                                                                          IntervalValue(4, 6, 3),
                                                                          IntervalValue(5, 7, 2),
                                                                          IntervalValue(6, 8, 1),
                                                                          IntervalValue(7, 9, 0)],
                                                                         [IntervalValue(1, 3, 0),
                                                                          IntervalValue(2, 4, 0),
                                                                          IntervalValue(3, 5, 0),
                                                                          IntervalValue(4, 6, 0),
                                                                          IntervalValue(5, 7, 0),
                                                                          IntervalValue(6, 8, 1),
                                                                          IntervalValue(7, 9, 2)])
                @test count_sites(Mutated, seqs[1], seqs[2], 3, 1) == ([IntervalValue(1, 3, 0),
                                                                        IntervalValue(2, 4, 0),
                                                                        IntervalValue(3, 5, 0),
                                                                        IntervalValue(4, 6, 0),
                                                                        IntervalValue(5, 7, 1),
                                                                        IntervalValue(6, 8, 1),
                                                                        IntervalValue(7, 9, 1)],
                                                                       [IntervalValue(1, 3, 0),
                                                                        IntervalValue(2, 4, 0),
                                                                        IntervalValue(3, 5, 0),
                                                                        IntervalValue(4, 6, 0),
                                                                        IntervalValue(5, 7, 0),
                                                                        IntervalValue(6, 8, 1),
                                                                        IntervalValue(7, 9, 2)])
                @test count_sites(Transition, seqs[1], seqs[2], 3, 1) == ([IntervalValue(1, 3, 0),
                                                                           IntervalValue(2, 4, 0),
                                                                           IntervalValue(3, 5, 0),
                                                                           IntervalValue(4, 6, 0),
                                                                           IntervalValue(5, 7, 0),
                                                                           IntervalValue(6, 8, 0),
                                                                           IntervalValue(7, 9, 0)],
                                                                          [IntervalValue(1, 3, 0),
                                                                           IntervalValue(2, 4, 0),
                                                                           IntervalValue(3, 5, 0),
                                                                           IntervalValue(4, 6, 0),
                                                                           IntervalValue(5, 7, 0),
                                                                           IntervalValue(6, 8, 1),
                                                                           IntervalValue(7, 9, 2)])
                @test count_sites(Transversion, seqs[1], seqs[2], 3, 1) == ([IntervalValue(1, 3, 0),
                                                                           IntervalValue(2, 4, 0),
                                                                           IntervalValue(3, 5, 0),
                                                                           IntervalValue(4, 6, 0),
                                                                           IntervalValue(5, 7, 1),
                                                                           IntervalValue(6, 8, 1),
                                                                           IntervalValue(7, 9, 1)],
                                                                          [IntervalValue(1, 3, 0),
                                                                           IntervalValue(2, 4, 0),
                                                                           IntervalValue(3, 5, 0),
                                                                           IntervalValue(4, 6, 0),
                                                                           IntervalValue(5, 7, 0),
                                                                           IntervalValue(6, 8, 1),
                                                                           IntervalValue(7, 9, 2)])
            end
        end

    end
end

#=
@testset "Distance Computation" begin

    dnas1 = [dna"ATTG-ACCTGGNTTTCCGAA", dna"A-ACAGAGTATACRGTCGTC"]
    m1 = seqmatrix(dnas1, :seq)

    dnas2 = [dna"attgaacctggntttccgaa",
             dna"atacagagtatacrgtcgtc"]
    dnas3 = [dna"attgaacctgtntttccgaa",
             dna"atagaacgtatatrgccgtc"]
    m2 = seqmatrix(dnas2, :seq)

    @test distance(Count{AnyMutation}, dnas1) == ([12], [16])
    @test distance(Count{TransitionMutation}, dnas1) == ([4], [16])
    @test distance(Count{TransversionMutation}, dnas1) == ([8], [16])
    @test distance(Count{Kimura80}, dnas1) == ([4], [8], [16])
    @test distance(Count{AnyMutation}, m1) == ([12], [16])
    @test distance(Count{TransitionMutation}, m1) == ([4], [16])
    @test distance(Count{TransversionMutation}, m1) == ([8], [16])
    @test distance(Count{Kimura80}, m1) == ([4], [8], [16])

    @test distance(Count{AnyMutation}, dnas2, 5, 5)[1][:] == [2, 4, 3, 3]
    @test distance(Count{AnyMutation}, dnas2, 5, 5)[2][:] == [5, 5, 3, 5]
    @test distance(Count{TransitionMutation}, dnas2, 5, 5)[1][:] == [0, 2, 1, 1]
    @test distance(Count{TransitionMutation}, dnas2, 5, 5)[2][:] == [5, 5, 3, 5]
    @test distance(Count{TransversionMutation}, dnas2, 5, 5)[1][:] == [2, 2, 2, 2]
    @test distance(Count{TransversionMutation}, dnas2, 5, 5)[2][:] == [5, 5, 3, 5]
    @test distance(Count{Kimura80}, dnas1) == ([4], [8], [16])

    @test distance(Count{AnyMutation}, dnas2) == ([12], [18])
    @test distance(Count{TransitionMutation}, dnas2) == ([4], [18])
    @test distance(Count{TransversionMutation}, dnas2) == ([8], [18])
    @test distance(Count{Kimura80}, dnas2) == ([4], [8], [18])
    @test distance(Count{AnyMutation}, m2) == ([12], [18])
    @test distance(Count{TransitionMutation}, m2) == ([4], [18])
    @test distance(Count{TransversionMutation}, m2) == ([8], [18])
    @test distance(Count{Kimura80}, m2) == ([4], [8], [18])

    d = distance(Proportion{AnyMutation}, dnas2, 5, 5)
    a = [0.4, 0.8, 1.0, 0.6]
    for i in 1:length(d[1])
        @test_approx_eq_eps d[1][i] a[i] 1e-4
    end
    @test d[2][:] == [5, 5, 3, 5]
    d = distance(Proportion{TransitionMutation}, dnas2, 5, 5)
    a = [0.0, 0.4, 0.333333, 0.2]
    for i in 1:length(d[1])
        @test_approx_eq_eps d[1][i] a[i] 1e-4
    end
    @test d[2][:] == [5, 5, 3, 5]
    d = distance(Proportion{TransversionMutation}, dnas2, 5, 5)
    a = [0.4, 0.4, 0.666667, 0.4]
    for i in 1:length(d[1])
        @test_approx_eq_eps d[1][i] a[i] 1e-4
    end
    @test d[2][:] == [5, 5, 3, 5]

    @test distance(Proportion{AnyMutation}, dnas1) == ([(12 / 16)], [16])
    @test distance(Proportion{TransitionMutation}, dnas1) == ([(4 / 16)], [16])
    @test distance(Proportion{TransversionMutation}, dnas1) == ([(8 / 16)], [16])
    @test distance(Proportion{AnyMutation}, m1) == ([(12 / 16)], [16])
    @test distance(Proportion{TransitionMutation}, m1) == ([(4 / 16)], [16])
    @test distance(Proportion{TransversionMutation}, m1) == ([(8 / 16)], [16])

    @test distance(Proportion{AnyMutation}, dnas2) == ([(12 / 18)], [18])
    @test distance(Proportion{TransitionMutation}, dnas2) == ([(4 / 18)], [18])
    @test distance(Proportion{TransversionMutation}, dnas2) == ([(8 / 18)], [18])
    @test distance(Proportion{AnyMutation}, m2) == ([(12 / 18)], [18])
    @test distance(Proportion{TransitionMutation}, m2) == ([(4 / 18)], [18])
    @test distance(Proportion{TransversionMutation}, m2) == ([(8 / 18)], [18])

    @test distance(JukesCantor69, dnas1) == ([Inf], [Inf]) # Returns infinity as 12/16 is 0.75 - mutation saturation.
    @test distance(JukesCantor69, m1) == ([Inf], [Inf])

    @test round(distance(JukesCantor69, dnas2)[1][1], 3) == 1.648
    @test round(distance(JukesCantor69, dnas2)[2][1], 3) == 1
    @test round(distance(JukesCantor69, m2)[1][1], 3) == 1.648
    @test round(distance(JukesCantor69, m2)[2][1], 3) == 1
    @test_throws DomainError distance(JukesCantor69, dnas2, 5, 5)
    d = distance(JukesCantor69, dnas3, 5, 5)
    a = [0.232616, 0.571605, 0.44084, 0.571605]
    v = [0.0595041, 0.220408, 0.24, 0.220408]
    for i in 1:length(d[1])
        @test_approx_eq_eps d[1][i] a[i] 1e-5
        @test_approx_eq_eps d[2][i] v[i] 1e-5
    end

    @test round(distance(Kimura80, dnas2)[1][1], 3) == 1.648
    @test round(distance(Kimura80, dnas2)[2][1], 3) == 1
    @test round(distance(Kimura80, m2)[1][1], 3) == 1.648
    @test round(distance(Kimura80, m2)[2][1], 3) == 1

end
<<<<<<< HEAD

@testset "VCF" begin
    metainfo = VCFMetaInfo()
    @test !isfilled(metainfo)
    @test ismatch(r"^Bio.Var.VCFMetaInfo: <not filled>", repr(metainfo))
    @test_throws ArgumentError metainfotag(metainfo)

    metainfo = VCFMetaInfo(b"##source=foobar1234")
    @test isfilled(metainfo)
    @test metainfotag(metainfo) == "source"
    @test metainfoval(metainfo) == "foobar1234"

    metainfo = VCFMetaInfo("##source=foobar1234")
    @test isfilled(metainfo)
    @test metainfotag(metainfo) == "source"
    @test metainfoval(metainfo) == "foobar1234"

    metainfo = VCFMetaInfo(metainfo)
    @test isa(metainfo, VCFMetaInfo)
    metainfo = VCFMetaInfo(metainfo, tag="date")
    @test metainfotag(metainfo) == "date"
    metainfo = VCFMetaInfo(metainfo, value="2017-01-30")
    @test metainfoval(metainfo) == "2017-01-30"
    metainfo = VCFMetaInfo(metainfo, tag="INFO", value=["ID"=>"DP", "Number"=>"1", "Type"=>"Integer", "Description"=>"Total Depth"])
    @test metainfo["ID"] == "DP"
    @test metainfo["Number"] == "1"
    @test metainfo["Type"] == "Integer"
    @test metainfo["Description"] == "Total Depth"
    @test metainfotag(metainfo) == "INFO"
    @test metainfoval(metainfo) == """<ID=DP,Number=1,Type=Integer,Description="Total Depth">"""

    record = VCFRecord()
    @test !isfilled(record)
    @test ismatch(r"^Bio.Var.VCFRecord: <not filled>", repr(record))
    @test_throws ArgumentError chromosome(record)

    record = VCFRecord("20\t302\t.\tT\tTA\t999\t.\t.\tGT")
    @test isfilled(record)
    @test haschromosome(record)
    @test chromosome(record) == "20"
    @test hasleftposition(record)
    @test leftposition(record) == 302
    @test !hasidentifier(record)
    @test_throws MissingFieldException identifier(record)
    @test hasreference(record)
    @test reference(record) == "T"
    @test hasalternate(record)
    @test alternate(record) == ["TA"]
    @test hasquality(record)
    @test quality(record) == 999
    @test !hasfilter_(record)
    @test_throws MissingFieldException filter_(record)
    @test infokeys(record) == String[]
    @test !hasinformation(record)
    @test_throws MissingFieldException information(record)
    @test hasformat(record)
    @test format(record) == ["GT"]

    # empty data is not a valid VCF record
    @test_throws ArgumentError VCFRecord("")
    @test_throws ArgumentError VCFRecord(b"")

    record = VCFRecord(b".\t.\t.\t.\t.\t.\t.\t.\t")
    @test isfilled(record)
    @test !haschromosome(record)
    @test !hasleftposition(record)
    @test !hasidentifier(record)
    @test !hasreference(record)
    @test !hasalternate(record)
    @test !hasquality(record)
    @test !hasfilter_(record)
    @test !hasinformation(record)

    record = VCFRecord(record)
    @test isa(record, VCFRecord)
    record = VCFRecord(record, chromosome="chr1")
    @test chromosome(record) == "chr1"
    record = VCFRecord(record, position=1234)
    @test leftposition(record) == 1234
    record = VCFRecord(record, identifier="rs1111")
    @test identifier(record) == ["rs1111"]
    record = VCFRecord(record, reference="A")
    @test reference(record) == "A"
    record = VCFRecord(record, alternate=["AT"])
    @test alternate(record) == ["AT"]
    record = VCFRecord(record, quality=11.2)
    @test quality(record) == 11.2
    record = VCFRecord(record, filter="PASS")
    @test filter_(record) == ["PASS"]
    record = VCFRecord(record, information=Dict("DP" => 20, "AA" => "AT", "DB"=>nothing))
    @test information(record, "DP") == "20"
    @test information(record, "AA") == "AT"
    @test information(record, "DB") == ""
    @test infokeys(record) == ["DP", "AA", "DB"]
    record = VCFRecord(record, genotype=[Dict("GT" => "0/0", "DP" => [10,20])])
    @test format(record) == ["DP", "GT"]
    @test genotype(record) == [["10,20", "0/0"]]

    let header = VCFHeader()
        @test isempty(header)
        push!(header, "##reference=file:///seq/references/1000GenomesPilot-NCBI36.fasta")
        @test !isempty(header)
        @test length(header) == 1
        unshift!(header, "##fileformat=VCFv4.3")
        @test length(header) == 2
        @test collect(header) == [
            VCFMetaInfo("##fileformat=VCFv4.3"),
            VCFMetaInfo("##reference=file:///seq/references/1000GenomesPilot-NCBI36.fasta")]
        @test startswith(repr(header), "Bio.Var.VCFHeader:")
    end

    let header = VCFHeader(["##fileformat=VCFv4.3"], ["Sample1"])
        @test !isempty(header)
        @test length(header) == 1
        @test header.sampleID == ["Sample1"]
        @test first(header) == VCFMetaInfo("##fileformat=VCFv4.3")
    end

    # minimum header
    data = b"""
    ##fileformat=VCFv4.3
    #CHROM	POS	ID	REF	ALT	QUAL	FILTER	INFO
    """
    reader = VCFReader(BufferedInputStream(data))
    @test isa(header(reader), VCFHeader)
    let header = header(reader)
        @test length(header.metainfo) == 1
        @test metainfotag(header.metainfo[1]) == "fileformat"
        @test metainfoval(header.metainfo[1]) == "VCFv4.3"
        @test isempty(header.sampleID)
    end

    # realistic header
    data = b"""
    ##fileformat=VCFv4.2
    ##fileDate=20090805
    ##source=myImputationProgramV3.1
    ##reference=file:///seq/references/1000GenomesPilot-NCBI36.fasta
    ##contig=<ID=20,length=62435964,assembly=B36,md5=f126cdf8a6e0c7f379d618ff66beb2da,species="Homo sapiens",taxonomy=x>
    ##phasing=partial
    ##INFO=<ID=NS,Number=1,Type=Integer,Description="Number of Samples With Data">
    ##INFO=<ID=DP,Number=1,Type=Integer,Description="Total Depth">
    ##INFO=<ID=AF,Number=A,Type=Float,Description="Allele Frequency">
    ##INFO=<ID=AA,Number=1,Type=String,Description="Ancestral Allele">
    #CHROM	POS	ID	REF	ALT	QUAL	FILTER	INFO	FORMAT	NA00001	NA00002	NA00003
    """
    reader = VCFReader(BufferedInputStream(data))
    @test isa(header(reader), VCFHeader)

    let header = header(reader)
        @test length(header.metainfo) == 10

        let metainfo = header.metainfo[1]
            @test metainfotag(metainfo) == "fileformat"
            @test metainfoval(metainfo) == "VCFv4.2"
            @test_throws ArgumentError keys(metainfo)
            @test_throws ArgumentError values(metainfo)
        end
        @test length(find(header, "fileformat")) == 1
        @test first(find(header, "fileformat")) == header.metainfo[1]

        let metainfo = header.metainfo[2]
            @test metainfotag(metainfo) == "fileDate"
            @test metainfoval(metainfo) == "20090805"
            @test_throws ArgumentError keys(metainfo)
            @test_throws ArgumentError values(metainfo)
        end

        let metainfo = header.metainfo[5]
            @test metainfotag(metainfo) == "contig"
            @test metainfoval(metainfo) == """<ID=20,length=62435964,assembly=B36,md5=f126cdf8a6e0c7f379d618ff66beb2da,species="Homo sapiens",taxonomy=x>"""
            @test keys(metainfo) == ["ID", "length", "assembly", "md5", "species", "taxonomy"]
            @test values(metainfo) == ["20", "62435964", "B36", "f126cdf8a6e0c7f379d618ff66beb2da", "Homo sapiens", "x"]
            @test metainfo["ID"] == "20"
            @test metainfo["md5"] == "f126cdf8a6e0c7f379d618ff66beb2da"
            @test metainfo["taxonomy"] == "x"
        end

        let metainfo = header.metainfo[7]
            @test metainfotag(metainfo) == "INFO"
            @test metainfoval(metainfo) == """<ID=NS,Number=1,Type=Integer,Description="Number of Samples With Data">"""
            @test keys(metainfo) == ["ID", "Number", "Type", "Description"]
            @test values(metainfo) == ["NS", "1", "Integer", "Number of Samples With Data"]
            @test metainfo["ID"] == "NS"
            @test metainfo["Type"] == "Integer"
        end
        @test length(find(header, "INFO")) == 4

        @test header.sampleID == ["NA00001", "NA00002", "NA00003"]
    end

    data = b"""
    ##fileformat=VCFv4.3
    ##contig=<ID=chr1>
    ##contig=<ID=chr2>
    ##INFO=<ID=DP,Number=1,Type=Integer,Description="Total Depth">
    ##INFO=<ID=AF,Number=A,Type=Float,Description="Allele Frequency">
    ##FORMAT=<ID=GT,Number=1,Description="Genotype">
    #CHROM\tPOS\tID\tREF\tALT\tQUAL\tFILTER\tINFO\tFORMAT\tNA00001\tNA00002
    chr1\t1234\trs001234\tA\tC\t30\tPASS\tDP=10;AF=0.3\tGT\t0|0\t0/1
    chr2\t4\t.\tA\tAA,AAT\t.\t.\tDP=5\tGT:DP\t0|1:42\t0/1
    """
    reader = VCFReader(BufferedInputStream(data))
    record = VCFRecord()

    @test read!(reader, record) === record
    @test chromosome(record) == "chr1"
    @test leftposition(record) === 1234
    @test identifier(record) == ["rs001234"]
    @test reference(record) == "A"
    @test alternate(record) == ["C"]
    @test quality(record) === 30.0
    @test filter_(record) == ["PASS"]
    @test information(record) == ["DP" => "10", "AF" => "0.3"]
    @test information(record, "DP") == "10"
    @test information(record, "AF") == "0.3"
    @test format(record) == ["GT"]
    @test genotype(record) == [["0|0"], ["0/1"]]
    @test genotype(record, 1) == genotype(record)[1]
    @test genotype(record, 2) == genotype(record)[2]
    @test genotype(record, 1, "GT") == "0|0"
    @test genotype(record, 2, "GT") == "0/1"
    @test genotype(record, 1:2, "GT") == ["0|0", "0/1"]
    @test genotype(record, :, "GT") == genotype(record, 1:2, "GT")
    @test ismatch(r"^Bio.Var.VCFRecord:\n.*", repr(record))

    @test read!(reader, record) === record
    @test chromosome(record) == "chr2"
    @test leftposition(record) == 4
    @test !hasidentifier(record)
    @test reference(record) == "A"
    @test alternate(record) == ["AA", "AAT"]
    @test !hasquality(record)
    @test !hasfilter_(record)
    @test information(record) == ["DP" => "5"]
    @test information(record, "DP") == "5"
    @test_throws KeyError information(record, "AF")
    @test format(record) == ["GT", "DP"]
    @test genotype(record) == [["0|1", "42"], ["0/1", "."]]
    @test genotype(record, 1) == genotype(record)[1]
    @test genotype(record, 2) == genotype(record)[2]
    @test genotype(record, 1, "GT") == "0|1"
    @test genotype(record, 1, "DP") == "42"
    @test genotype(record, 2, "GT") == "0/1"
    @test genotype(record, 2, "DP") == "."
    @test genotype(record, 1:2, "GT") == ["0|1", "0/1"]
    @test genotype(record, 1:2, "DP") == ["42", "."]
    @test genotype(record, :, "DP") == genotype(record, 1:2, "DP")
    @test_throws KeyError genotype(record, :, "BAD")

    @test_throws EOFError read!(reader, record)

    # round-trip test
    vcfdir = joinpath(dirname(@__FILE__), "..", "BioFmtSpecimens", "VCF")
    for specimen in YAML.load_file(joinpath(vcfdir, "index.yml"))
        filepath = joinpath(vcfdir, specimen["filename"])
        records = VCFRecord[]
        reader = open(VCFReader, filepath)
        output = IOBuffer()
        writer = VCFWriter(output, header(reader))
        for record in reader
            write(writer, record)
            push!(records, record)
        end
        close(reader)
        flush(writer)

        records2 = VCFRecord[]
        for record in VCFReader(IOBuffer(takebuf_array(output)))
            push!(records2, record)
        end
        @test records == records2
    end
end

function parsehex(str)
    return map(x -> parse(UInt8, x, 16), split(str, ' '))
end

@testset "BCF" begin
    record = BCFRecord()
    @test !isfilled(record)
    @test ismatch(r"^Bio.Var.BCFRecord: <not filled>", repr(record))
    @test_throws ArgumentError chromosome(record)

    record = BCFRecord()
    record.sharedlen = 0x1c
    record.indivlen = 0x00
    # generated from bcftools 1.3.1 (htslib 1.3.1)
    record.data = parsehex("00 00 00 00 ff ff ff ff 01 00 00 00 01 00 80 7f 00 00 01 00 00 00 00 00 07 17 2e 00")
    record.filled = 1:endof(record.data)
    @test chromosome(record) == 1
    record = BCFRecord(record)
    @test isa(record, BCFRecord)
    record = BCFRecord(record, chromosome=4)
    @test chromosome(record) == 4
    record = BCFRecord(record, position=1234)
    @test leftposition(record) == 1234
    record = BCFRecord(record, quality=12.3)
    @test quality(record) == 12.3f0
    record = BCFRecord(record, identifier="rs1234")
    @test identifier(record) == "rs1234"
    record = BCFRecord(record, reference="AT")
    @test reference(record) == "AT"
    record = BCFRecord(record, alternate=["ATT", "ACT"])
    @test alternate(record) == ["ATT", "ACT"]
    record = BCFRecord(record, filter=[2, 3])
    @test filter_(record) == [2, 3]
    record = BCFRecord(record, information=Dict(1 => Int8[42]))
    @test information(record) == [(1, 42)]
    @test information(record, simplify=false) == [(1, [42])]
    @test information(record, 1) == 42
    @test information(record, 1, simplify=false) == [42]

    bcfdir = joinpath(dirname(@__FILE__), "..", "BioFmtSpecimens", "BCF")
    reader = BCFReader(open(joinpath(bcfdir, "example.bcf")))
    let header = header(reader)
        @test length(find(header, "fileformat")) == 1
        @test find(header, "fileformat")[1] == VCFMetaInfo("##fileformat=VCFv4.2")
        @test length(find(header, "FORMAT")) == 4
    end
    record = BCFRecord()
    @test read!(reader, record) === record
    @test chromosome(record) == 1
    @test leftposition(record) == 14370
    @test identifier(record) == "rs6054257"
    @test reference(record) == "G"
    @test alternate(record) == ["A"]
    @test quality(record) == 29.0
    @test filter_(record) == [1]
    @test information(record) == [(1,3),(2,14),(3,0.5),(5,nothing),(6,nothing)]
    @test information(record, simplify=false) == [(1,[3]),(2,[14]),(3,[0.5]),(5,[]),(6,[])]
    @test genotype(record) == [(9,[[2,3],[4,3],[4,4]]),(10,[[48],[48],[43]]),(2,[[1],[8],[5]]),(11,[[51,51],[51,51],[-128,-128]])]
    @test genotype(record, 1) == [(9, [2,3]), (10, [48]), (2, [1]), (11, [51,51])]
    @test genotype(record, 1, 9) == [2,3]
    @test genotype(record, 1, 10) == [48]
    @test genotype(record, 2, 9) == [4,3]
    @test genotype(record, :, 9) == [[2,3],[4,3],[4,4]]
    @test ismatch(r"^Bio.Var.BCFRecord:\n.*", repr(record))
    close(reader)

    # round-trip test
    for specimen in YAML.load_file(joinpath(bcfdir, "index.yml"))
        filepath = joinpath(bcfdir, specimen["filename"])
        records = BCFRecord[]
        reader = open(BCFReader, filepath)
        output = IOBuffer()
        writer = BCFWriter(output, header(reader))
        for record in reader
            write(writer, record)
            push!(records, record)
        end
        # HACK: take the data buffer before closing the writer
        data = output.data
        close(reader)
        close(writer)

        records2 = BCFRecord[]
        for record in BCFReader(IOBuffer(data))
            push!(records2, record)
        end
        @test records == records2
    end
end

=======
=#
end # module TestVar


text:
include(Pkg.dir("SDE","src", "Diffusion.jl"))

using Diffusion
srand(8)

@test brown1(3,2,1) == [3.0]
@test length(brown1(3,4,2)) == 2

#test brown1(0,2,5)[end] sim N(0, 2)

#tests with alpha 0.01
r = 2.576
n = 1000

varmu(x, mu) = dot(x-mu, x-mu)/length(x)
var0(x) = dot(x, x)/length(x)

#testing mean and variance of Brownian motion sampled at few points
@test abs(mean([brown1(0,2,5)[end] for i in 1:n])) < r*sqrt(2/n)
chiupper = 1118.95 #upper 0.005 percentile for n = 1000
chilower = 888.56 #lower 0.005 percentile for n = 1000
@test chiupper >1000.*var([brown1(0,2,5)[end] for i in 1:1000])/2 > chilower

@test brown(5., 1, 2, 1) == [5. 5.]'
@test abs(mean(mean([brown([5.,2.], 2, 2, 5)[:,end] for i in 1:n/2]))-3.5) <  r*sqrt(2/n)

#quadratic variance is proportional to time plus sampling bias
qu = [quvar(brown1(0,2,1000)) for j in 1: 1000]
s2 = var(qu)
@test abs(mean(qu)-2)/sqrt(s2)*sqrt(1000) < 3

@test diff(ito([1., 2., 3.])) == [1., 2., 3.]

# int0^T w_t dw_t = w_T^2/2 - T/2
@test abs((b -> (ito(ydx(b, diff(b)))[end] - (0.5b[end]^2 - 1)))(brown1(0, 2, 10000))) < 0.1
@test abs((dw -> ito((ydx(ito(dw), dw)))[end] - (0.5ito(dw)[end]^2 - 1) )(dW1(2., 10000))) < 0.1
@test abs((dw -> ito((ydx(ito(dw), dw)))[1,end] - (0.5ito(dw)[1,end]^2 - 1) )(dW(2.,1, 10000))) < 0.1
@test abs((dw -> ito(ito(dw), dw)[end] - (0.5ito(dw)[end]^2 - 1) )(dW1(2., 10000))) < 0.1

# E int0^T w_t dw_t = E W_T^2/2 - T/2 = 0
# takes too long to test...
# mean([(dw -> ito((ydx(ito(dw), dw)))[end])(dW1(2., 10000)) for i in 1:100000]) approx 0

#test (roughly) the quadratic variance of Euler approximation
qu = [quvar(euler(0, 1, (t,x)-> -5x, (t,x) -> 1, 1/1000., dW1(1.,1000))) for j in 1: 1000]
s2 = var(qu)
@test abs(mean(qu)-1)/sqrt(s2)*sqrt(1000) < 5.

#  B(t) = (1-t) W(t/(1-t)).

@test abs(mean([bb(0,1,1,3)[2] for i in 1:1000])-0.5)/sqrt(0.25)*sqrt(1000) < r
@test abs(mean([bb(0,1,1,5)[3] for i in 1:1000])-0.5)/sqrt(0.25)*sqrt(1000) < r
@test chilower < 1000*abs(var0([bb(0,1,1,3)[2] for i in 1:1000]-0.5))/.25 < chiupper
@test chilower < 1000*abs(var0([bb(0,1,1,5)[3] for i in 1:1000]-0.5))/.25 < chiupper

@test_approx_eq sum(dWcond(2, 5, 10)) 2

# Covariance of a Brownian bridge from t_1 to t_2 (t_2-t)(s-t_1)/(t_2-t_1)
# here (2-1)(1)/2 = 1/2

@test chilower < 1000*abs(var0([bb(0,1,2,5)[3] for i in 1:1000]-0.5))*2 < chiupper
@test chilower < 1000*abs(var0([cumsum0(dWcond(2, 2, 5))[3] for i in 1:1000]-1))*2. < chiupper

a1 = sum([abs(2 - quvar(ito(aug(dW1(2., 100),1/50,10)))) for j in 1:1000])
a2 = sum([abs(2 - quvar(ito(aug(dW1(2., 100),1/51,10)))) for j in 1:1000])
a3 = sum([abs(2 - quvar(ito(aug(dW1(2., 100),1/49,10)))) for j in 1:1000])

@test a1 < min(a2, a3)


text:
# alias for equivalent uniform measures
# TODO: If we generalize Haar for G-manifolds, then we can add Haar under left-action of O(n)
# here.
const UniformStiefelMeasures{n,k,P} = Hausdorff{Stiefel{n,k,ℝ},P}

# In general, given matrix z ∈ ℝ^(n × k) with IID std normal elements, p=z(z'z)^(-1/2) is drawn
# from the Hausdorff measure on St(n,k).
# This implementation uses the unique QR decomposition z=QR where R[i,i] > 0, where Q is then
# drawn from the Hausdorff measure on St(n,k).
# See Theorem 2.3.19 of Gupta AK, Nagar DK. Matrix variate distributions. CRC Press; 2018
function Base.rand(rng::AbstractRNG, ::Type, μ::Hausdorff{<:Stiefel{n,k,ℝ}}) where {n,k}
    p = randn!(rng, similar(μ.point))
    Q, R = qr(p)
    p .= Matrix(Q) .* sign.(view(R, diagind(R))')
    return p
end

function MeasureTheory.logdensity(
    ::UniformStiefelMeasures{n,k}, ::UniformStiefelMeasures{n,k}, x::AbstractMatrix
) where {n,k}
    return zero(eltype(x))
end

# Chikuse, 2003 Eq. 1.4.8
volume(::Stiefel{n,k,ℝ}) where {n,k} = 2^k * π^(k * n//2) / mvgamma(k, n//2)


text:
module NoiseContrastiveEstimation
using ReverseDiff, Random, LinearAlgebra
abstract type AbstractCNCE end

Base.@kwdef struct CNCE{T,F,dF} <: AbstractCNCE
    f::F
    grad_f::dF = (d, p) -> ReverseDiff.gradient(x -> f(d, x), p)
    data::Vector{T}
    noised::Matrix{T}
end

Base.@kwdef struct StochasticCNCE{F,dF,T,pT,RNG} <: AbstractCNCE
    f::F
    grad_f::dF = (d, p) -> ReverseDiff.gradient(x -> f(d, x), p)
    data::Vector{T}
    perturbator::pT
    K::Int = 5
    minibatch::Int
    rng::RNG = Random.GLOBAL_RNG
end

tsum(a, b) = (a[1] .+ b[1], a[2] .+ b[2])

function (J::StochasticCNCE)(θ)
    κ = J.K
    N = length(J.data)
    obs = 0.0
    step = N ÷ J.minibatch
    T = foldl(
        tsum,
        begin
            rg = (1+ri*step):min(step + ri * step, N)
            res = (0.0, 0.0)
            if !isempty(rg)
                i = rand(J.rng, rg)
                bϕ = J.f(J.data[i], θ)
                dbϕ = J.grad_f(J.data[i], θ)
                res = foldl(tsum, begin
                    noised = J.perturbator(J.data[i])
                    G = bϕ - J.f(noised, θ)
                    dG = dbϕ - J.grad_f(noised, θ)
                    eG = 1 + exp(-G)
                    grad = (1 / eG - 1) * dG
                    fval = log(eG)
                    obs += 1
                    (fval, grad)
                end for j = 1:κ)
            end
            res
        end for ri = 0:J.minibatch
    )
    T = T .* (2 / obs)
    (J = T[1], dJ = T[2])
end

function (J::CNCE)(θ)
    κ = size(J.noised, 1)
    N = length(J.data)
    T = foldl(
        tsum,
        begin
            bϕ = J.f(J.data[i], θ)
            dbϕ = J.grad_f(J.data[i], θ)
            foldl(tsum, begin
                G = bϕ - J.f(J.noised[j, i], θ)
                dG = dbϕ - J.grad_f(J.noised[j, i], θ)
                eG = 1 + exp(-G)
                grad = 2 / (κ * N) * (1 / eG - 1) * dG
                fval = 2 / (κ * N) * log(eG)
                (fval, grad)
            end for j = 1:κ)
        end for i = 1:N
    )
    (J = T[1], dJ = T[2])
end

function approxtol(a, b, rtol, atol)
    a = abs(a)
    b = abs(b)
    aδ = abs(b - a)
    rδ = aδ / max(a, b)
    rδ <= rtol || aδ <= atol
end

function nesterov(
    J::AbstractCNCE,
    x0,
    μ,
    η;
    atol = 0,
    rtol = 1e-7,
    maxiter = 1e4,
    verbose = false,
)
    C, v = J(x0)
    C0 = C
    i = 1
    x = x0 + zero(η) * v
    while i < maxiter
        f, gf = J(x - μ * v)
        v = μ * v + (η / i) * gf
        x = x - v
        verbose && println(i, " ", f, " ", norm(gf), " ", norm(v))
        i > 1 && approxtol(f, C, rtol, atol) && break
        C = f
        i += 1
    end
    (sol = x, cost = C, cost_fractional_reduction = 1 - C / C0, epochs = i)
end

export StochasticCNCE, CNCE, nesterov
end


text:
#-------------------------------------------------------------------------------
# Dixon's Mothod
# In developing
#-------------------------------------------------------------------------------
export dixon
function dixon(
    g::AbstractFiniteGroup,
    h::AbstractArray{<:Integer, 3}
)
    n, p = findprime(g)
    Z = primitive_root(n, p)
    ξ = exp(1im * 2π/n)
    NC = size(h, 1)
    v = [I(NC)]
    for i = 1:NC
        hi = h[i, :, :]
        v = vcat([vec_split(hi, vi, p) for vi in v]...)
        all(isone(size(vi, 2)) for vi in v) && break
    end
    mat = vcat(transpose.(v)...)
    for i = 1:size(mat, 1)
        normalize_modp!(g, view(mat, i, :), p)
    end
    maptocomplex.(mat, Z, n, ξ)
end

function eigvecs_modp(m::AbstractMatrix{<:Integer}, modp::Integer)
    vecs = []
    for λ = 0:modp-1
        sol = eigvecs_modp(m, λ, modp)
        if size(sol, 2) > 0
            push!(vecs, sol)
        end
    end
    @assert sum(size(vec, 2) for vec in vecs) == size(m, 1) "Solution imcomplete."
    vecs
end

function eigvecs_modp(m::AbstractMatrix{<:Integer}, λ::Integer, modp::Integer)
    n = size(m, 1)
    herm = hermiteform!([m-λ*I; I], modp)
    C = 1
    while C <= n
        iszero(sum(herm[1:n, C])) && break
        C += 1
    end
    herm[n+1:end, C:end]
end

function vec_split(
    h::AbstractMatrix{<:Integer},
    vs::AbstractVecOrMat{<:Integer},
    modp::Integer
)
    isone(size(vs, 2)) && return [vs]
    php = vs' * h * vs
    v = eigvecs_modp(php, modp)
    [mod!(vs * vi, modp) for vi in v]
end

function normalize_modp!(g::AbstractFiniteGroup, v::AbstractVector{<:Integer}, modp::Integer)
    v0 = copy(v)
    for i = 1:modp
        v .+= v0
        mod!(v, modp)
        isone(v[1]) && break
    end
    d2 = order(g) // sum(v[i] * inv_modp(v[i], modp) // mult(g, i) for i =1:length(v))
    d2 = mod(Int(d2), modp)
    d = round(Int, sqrt(d2))
    @assert d^2 == d2
    v .*= d
    mod!(v, modp)
end

#-------------------------------------------------------------------------------
# Calculation on prime integer field Fₚ
#-------------------------------------------------------------------------------
function inv_modp(i::Integer, p::Integer)
    for j = 2:p-1
        isone(mod(i*j, p)) && (return j)
    end
    error("Did not find inverse.")
end

function primitive_root(n::Integer, modp::Integer)
    isone(n) && (return n)
    for i = 2:modp
        p = i
        flag = true
        for j = 1:n-2
            p = mod(i * p, modp)
            isone(p) && (flag = false; break)
        end
        flag && isone(mod(i * p, modp)) && (return i)
    end
end

function findprime(g::AbstractFiniteGroup)
    n = lcm([order(g, c[1]) for c in class(g)])
    primes = [2, 3, 5, 7, 11, 13, 15, 17, 19, 23, 29, 31]
    i = ceil(Int64, 2 * sqrt(order(g)))
    i = i^2 > 4 * order(g) ? min(32, i) : min(32, i+1)
    while true
        Q = true
        for p in primes
            d, r = divrem(i, p)
            iszero(r) && (Q=false; break)
            (d <= p) && break
        end
        if Q
            iszero(mod(i-1, n)) ? (return (n, i)) : push!(primes, i)
        end
        i += 1
    end
end

function maptocomplex(N::Integer, Z::Integer, n::Integer, ξ::Number)
    list = zeros(Int, n)
    for i = 1:n-1
        N, list[i] = divrem(N, Z)
    end
    list[n] = N
    reverse!(list)
    C = list[1]
    for i = 2:n
        C = ξ * C + list[i]
    end
    C
end

#-------------------------------------------------------------------------------
# Hermitian normal form on prime integer field Fₚ
#-------------------------------------------------------------------------------
"""
    hermiteform!(m::AbstractMatrix{<:Integer}, modp::Integer)

Compute the Hermite form of a matrix in the prime integer field Fₚ
"""
function hermiteform!(m::AbstractMatrix{<:Integer}, modp::Integer)
    mod!(m, modp)
    pointer = 1
    x, y = size(m)
    for i = 1:y-1
        while pointer <= x && set_prime_column!(m, i, row=pointer)
            pointer += 1
        end
        pointer > x && break
        for j = i+1:y
            column_reduce!(view(m, :, i), view(m, :, j), modp, row=pointer)
        end
        pointer += 1
    end
    m
end

"""
    column_reduce!(v1, v2, modp; col)

Reduce column `v2` using column `v1`, with prime row set to `row`.
"""
function column_reduce!(
    v1::AbstractVector{<:Integer},
    v2::AbstractVector{<:Integer},
    modp::Integer;
    row::Integer=1
)
    iszero(v2[row]) && (return nothing)
    v1[row] > v2[row] && swap!(v1, v2)
    kill_column!(v1, v2, modp, row=row)
    column_reduce!(v1, v2, modp, row=row)
end

"""
Basic column manipulation.
"""
function kill_column!(
    v1::AbstractVector{<:Integer},
    v2::AbstractVector{<:Integer},
    modp::Integer;
    row::Integer=1
)
    v2 .-= (v2[row] ÷ v1[row]) * v1
    mod!(v2, modp)
end

"""
    set_prime_column!(m, row; col)

Swap with the remaining column so that m[row, col] > 0.
Return true if its impossible, and false otherwise.
"""
function set_prime_column!(
    m::AbstractMatrix{<:Integer},
    col::Integer;
    row::Integer=1
)
    iszero(m[row, col]) || (return false)
    for j = col+1:size(m, 2)
        if !iszero(m[row, j])
            swap!(view(m, :, col), view(m, :, j))
            return false
        end
    end
    true
end

"""
In place swap of `v1` and `v2`.
"""
@inline function swap!(v1::AbstractArray, v2::AbstractArray)
    for i = 1:length(v1)
        v1[i], v2[i] = v2[i], v1[i]
    end
end

"""
In place mod p on `v`.
"""
@inline function mod!(v::AbstractArray, modp::Integer)
    for i = 1:length(v)
        v[i] = mod(v[i], modp)
    end
    v
end