# Author: Ritchie Lee, ritchie.lee@sv.cmu.edu
# Date: 04/14/2015
# Implements Lincoln Lab scripted dynamics model

module LLADMImpl

export
addObserver,

initialize,
update,

initializeDynamicModel,
simulateDynamicModel,

LLADM,
LLADMState,
LLADMOutputState,
LLADMCommand

using AbstractDynamicModelImpl
using AbstractDynamicModelInterfaces
using CommonInterfaces
using RLESUtils, Observers

using Base.Test

import CommonInterfaces.addObserver
import CommonInterfaces.initialize
import CommonInterfaces.update
import AbstractDynamicModelInterfaces.initializeDynamicModel
import AbstractDynamicModelInterfaces.simulateDynamicModel

# Note: Angles are all in radians in this module unless otherwise indicated
# CASSATT_TIMING is false

type LLADMState

  t::Float64       #time (s)
  v::Float64       #airspeed (ft/s)
  N::Float64       #position north (ft)
  E::Float64       #position east (ft)
  h::Float64       #position altitude (ft)
  psi::Float64     #heading angle (rad), N is zero, clockwise is positive
  theta::Float64   #pitch angle (rad)
  phi::Float64     #bank angle (rad)
  a::Float64       #airspeed acceleration (ft/s^2)
end

type LLADMOutputState #mitcas state in scripted_dynamics.h

  t::Float64
  x::Float64
  y::Float64
  h::Float64
  vx::Float64   #DX in scripted_dynamics.h
  vy::Float64   #DY in scripted_dynamics.h
  vh::Float64   #DH in scripted_dynamics.h
  psi::Float64
  theta::Float64
  phi::Float64
end

LLADMOutputState() = LLADMOutputState(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0)

type LLADMCommand

  t::Float64        #time of command (s)
  hdot::Float64     #commanded vertical rate (ft/s)
  psidot::Float64   #commanded turn rate (rad/s)
  a::Float64        #commanded acceleration (ft/s^2)
  dh_min::Float64          #dh_min of advisory being followed (ft/s)
  dh_max::Float64          #dh_max of advisory being followed (ft/s)
  target_rate::Float64     #target_rate of advisory being followed (ft/s)
  ddh::Float64             #ddh of advisory being followed (ft/s^2)

end

immutable LLADMConsts

  _dt::Float64           # minor time step
  _K::Float64            # integrator gain
  _g::Float64            # acceleration of gravity (ft/s^2)
  _qmax::Float64         # as in CASSATT (rad/s)
  _rmax::Float64         # as in CASSATT GA_psidotMAX = 1e6
  _phi_max::Float64
  _phidotmax::Float64
  _v_high::Float64       # airspeed limits
  _v_low::Float64
  LIMIT_DDH::Bool
  _min_vertical_rate::Float64
  _max_vertical_rate::Float64
  _min_vertical_accel::Float64
  _max_vertical_accel::Float64

end

type LLADM <: AbstractDynamicModel

  state::LLADMState
  update::Union{LLADMCommand, Void}
  timestep::Float64
  params::LLADMConsts
  output_state::LLADMOutputState

  observer::Observer

  function LLADM(; timestep::Float64 = 1.0, #major time step
                 _dt::Float64 = 0.1, #minor time step
                 _K::Float64 = 1.0,
                 _g::Float64 = 32.2,
                 _qmax::Float64 = deg2rad(3.0),
                 _rmax::Float64 = 1000000.0,
                 _phi_max::Float64 = deg2rad(75.0),
                 _phidotmax::Float64 = 0.524,
                 _v_high::Float64 = 100000.0,
                 _v_low::Float64 = 1.7,
                 LIMIT_DDH::Bool = true,
                 _min_vertical_rate::Float64 = -9999.0,
                 _max_vertical_rate::Float64 = 9999.0,
                 _min_vertical_accel::Float64 = -9.66,
                 _max_vertical_accel::Float64 = 9.66,
                 )

    obj = new()

    obj.update = nothing
    obj.timestep = timestep

    obj.params = LLADMConsts(_dt,
                               _K,
                               _g,
                               _qmax,
                               _rmax,
                               _phi_max,
                               _phidotmax,
                               _v_high,
                               _v_low,
                               LIMIT_DDH,
                               _min_vertical_rate,
                               _max_vertical_rate,
                               _min_vertical_accel,
                               _max_vertical_accel
                               )

    obj.output_state = LLADMOutputState()

    obj.observer = Observer()

    return obj
  end
end

addObserver(adm::LLADM, f::Function) = add_observer(adm.observer, f)
addObserver(adm::LLADM, tag::AbstractString, f::Function) = add_observer(adm.observer, tag, f)

initialize(adm::LLADM, state) = initialize(adm,convert(LLADMState, state))

function initialize(adm::LLADM, state::LLADMState)
  adm.state = state

  # Intialize the mitcas state
  # Note: x is E and y is N in scripted_dynamics.h
  adm.output_state.t     = state.t
  adm.output_state.h     = state.h
  adm.output_state.vh    = sin(state.theta) * state.v
  #adm.output_state.x     = state.E
  #adm.output_state.vx    = sin(state.psi) * cos(state.theta) * state.v
  #adm.output_state.y     = state.N
  #adm.output_state.vy    = cos(state.psi) * cos(state.theta) * state.v
  adm.output_state.psi   = state.psi
  adm.output_state.theta = state.theta
  adm.output_state.phi   = state.phi

  # RLEE: Override x and y to match LLCEM.  X is north
  adm.output_state.x     = state.N
  adm.output_state.vx    = cos(state.psi) * cos(state.theta) * state.v
  adm.output_state.y     = state.E
  adm.output_state.vy    = sin(state.psi) * cos(state.theta) * state.v

  adm.update = nothing

  return adm.output_state
end

# Mirrors state_update() from scripted_dynamics.h
function state_update(C::LLADMConsts, state::LLADMState, ctrl::LLADMCommand, hddcmd::Float64, dt::Float64)
  # Inputs:
  # C = constants
  # state = current state (adm.state)
  # ctrl_update = current command (update::LLADMCommand)
  # hddcmd = vertical acceleration to apply
  # dt = time step

  const s_theta = sin(state.theta)
  const c_theta = cos(state.theta)
  const t_theta = tan(state.theta)
  const s_phi = sin(state.phi)
  const c_phi = cos(state.phi)
  const s_psi = sin(state.psi)
  const c_psi = cos(state.psi)

  # save v and saturated v
  const v = state.v
  const v_saturated = max(v,1.0)

  # compute and saturate q
  q = saturate(1.0 / (v_saturated * c_phi) * (hddcmd / c_theta + C._g * c_theta * s_phi * s_phi - ctrl.a * t_theta), -C._qmax, C._qmax)

  # compute and saturate r
  r = saturate(C._g * s_phi * c_theta / v_saturated, -C._rmax, C._rmax)

  # sometimes the aircraft bank angle is so large that it would prevent reaching desired h_dot,
  # so we dynamically saturate the bank angle so that h_dot is always acheived
  # compute phimax (in the Plopt/compute q block)
  hdd_cmd_phi = min(hddcmd, v_saturated * C._qmax * c_phi * c_theta)

  # calculate discriminant
  sqrt_arg = v_saturated * v_saturated * C._qmax * C._qmax - 4 * C._g * ctrl.a * s_theta + 4 * C._g * hdd_cmd_phi + 4 * C._g * C._g * c_theta * c_theta
  phi_max_2 = 0

  if sqrt_arg < 0
    phi_max_2 = 10000
  else
    # calculate cos(phi)
    cphi1 = (sqrt(sqrt_arg) - v_saturated * C._qmax) / (2 * C._g * c_theta)

    if abs(cphi1) < 1
      phi_max_2 = acos(cphi1) * 0.98 # add a small buffer to prevent jittering
    end
  end

  phimax = min(C._phi_max, phi_max_2)

  # compute and saturate p
  psidot_if_no_change = (q * s_phi + r * c_phi) / c_theta
  dpsidot = ctrl.psidot - psidot_if_no_change
  p = saturate(20.0 * dpsidot, -C._phidotmax, C._phidotmax) # 20 is user tuned parameter from CASSATT

  # limit max bank angle
  if (state.phi + p * dt > phimax)

    p = (phimax - state.phi) / dt
  end

  if (state.phi + p * dt < -phimax)

    p = (-phimax - state.phi) / dt
  end

  state.v += dt * (ctrl.a)
  state.N += dt * (v * c_theta * c_psi)
  state.E += dt * (v * c_theta * s_psi)
  state.h += dt * (v * s_theta)
  state.psi += dt * (q * s_phi / c_theta + r * c_phi / c_theta)
  state.psi = to_plusminus_pi(state.psi) #RLEE added: mod back to [-pi, pi]
  state.theta += dt * (q * c_phi - r * s_phi)
  state.theta = to_plusminus_pi(state.theta) #RLEE added: mod back to [-pi, pi]
  state.phi += dt * (p + q * s_phi * t_theta + r * c_phi * t_theta)
  state.phi = to_plusminus_pi(state.phi) #RLEE added: mode back to [-pi, pi]

  state.v = saturate(state.v, 1.7, C._v_high - 0.000001)
  state.t += dt

end

update(adm::LLADM, ctrl) = update(adm,convert(LLADMCommand, ctrl))

function update(adm::LLADM, ctrl::LLADMCommand)

  state = adm.state
  C = adm.params #constants

  t_start = state.t

  for i = 1:Int64(round(adm.timestep / C._dt))
    substep(state, ctrl, C)
  end

  #snap so small errors don't accumulate
  state.t = snap(state.t, t_start + adm.timestep)

  #make sure timing is in sync
  @test state.t == t_start + adm.timestep

  # calculate the new mitcas state vars
  # Note: x is E and y is N in scripted_dynamics.h
  adm.output_state.t     = state.t
  adm.output_state.h     = state.h
  adm.output_state.vh    = sin(state.theta) * state.v
  #adm.output_state.x     = state.E
  #adm.output_state.vx    = sin(state.psi) * cos(state.theta) * state.v
  #adm.output_state.y     = state.N
  #adm.output_state.vy    = cos(state.psi) * cos(state.theta) * state.v
  adm.output_state.psi   = state.psi
  adm.output_state.theta = state.theta
  adm.output_state.phi   = state.phi

  # RLEE: Override x and y to match LLCEM.  X is north
  adm.output_state.x     = state.N
  adm.output_state.vx    = cos(state.psi) * cos(state.theta) * state.v
  adm.output_state.y     = state.E
  adm.output_state.vy    = sin(state.psi) * cos(state.theta) * state.v

  return adm.output_state
end

function substep(state::LLADMState, ctrl::LLADMCommand, C::LLADMConsts)

  dh = state.v * sin(state.theta)

  dummy, ddh_script = response(dh, (ctrl.hdot - dh) / C._dt,
                                       ctrl.dh_min, ctrl.dh_max,
                                       C._min_vertical_rate, C._max_vertical_rate, C._dt)

  dummy, ddh_cmd = response(dh, ctrl.dh_min, ctrl.dh_max, 0.0, 0.0, ctrl.ddh,
                                 C._min_vertical_rate, C._max_vertical_rate, C._dt)

  ddh2apply = resolve_TCAS_and_script(ddh_cmd,ddh_script)

  #debug
  #println("ddh_script=", ddh_script, ", ddh_cmd=", ddh_cmd, ", ddh2apply=", ddh2apply)

  if C.LIMIT_DDH
    ddh2apply = saturate(ddh2apply, C._min_vertical_accel, C._max_vertical_accel)
  end

  state_update(C, state, ctrl, ddh2apply, C._dt)
end

function saturate(x::Float64, limit_min::Float64, limit_max::Float64)
  @test limit_min <= limit_max

  return min(max(x,limit_min),limit_max)
end

function response(dh::Float64, ddh_in::Float64, dh_cmd_min::Float64, dh_cmd_max::Float64,
                  min_dh::Float64, max_dh::Float64, dt::Float64)

  # default value for ddh_out
	ddh_out = ddh_in

	# new projected vertical rate
	dh_proj = dh + ddh_out * dt

	# whether or not inside green zone
	dh_inside_green = saturate(dh, dh_cmd_min, dh_cmd_max) == dh

	if (dh > dh_cmd_max && dh_proj < dh_cmd_min) || (dh_inside_green && dh_proj > dh_cmd_max)

		# OUTSIDE GREEN ZONE
		# cross max edge from above and go below min edge => hit max edge
		# OR
		# INSIDE GREEN ZONE
		# cross max edge from below and go above max edge => hit max edge

		dh_proj = dh_cmd_max

	elseif (dh < dh_cmd_min && dh_proj > dh_cmd_max) || (dh_inside_green && dh_proj < dh_cmd_min)

		# OUTSIDE GREEN ZONE
		# cross min edge from below and go above max edge => hit min edge
		# OR
		# INSIDE GREEN ZONE
		# cross min edge from above and go below min edge => hit min edge

		dh_proj = dh_cmd_min

	elseif dh_proj < min_dh

		# cross max descent rate => hit max descent rate
		dh_proj = min_dh

	elseif dh_proj > max_dh

		# cross max climb rate => hit max climb rate
		dh_proj = max_dh

  end

	# recompute ddh
	ddh_out = dt != 0.0 ? (dh_proj - dh) / dt : 0.0

	# compute next dh
	dh_next = snap(dh + ddh_out * dt, dh_cmd_min)
	dh_next = snap(dh_next, dh_cmd_max)
	dh_next = snap(dh_next, min_dh)
	dh_next = snap(dh_next, max_dh)

  return dh_next, ddh_out
end

function response(dh::Float64, dh_cmd_min::Float64, dh_cmd_max::Float64, ddh_no_response::Float64,
                  ddh_response::Float64, cmd_ddh::Float64, min_dh::Float64, max_dh::Float64, dt::Float64)

  ddh_in = acceleration_to_apply(dh, ddh_no_response, ddh_response, dh_cmd_min, dh_cmd_max, cmd_ddh)

  return response(dh, ddh_in, dh_cmd_min, dh_cmd_max, min_dh, max_dh, dt)
end

function acceleration_to_apply(dh::Float64, ddh_no_response::Float64, ddh_response::Float64,
                               dh_cmd_min::Float64, dh_cmd_max::Float64, cmd_ddh::Float64)

  ddh = ddh_no_response

  # resolution advisory handling
	if (saturate(dh, dh_cmd_min, dh_cmd_max) != dh)

		# commanded acceleration
		ddh = cmd_ddh

		# determine direction
		if (dh > dh_cmd_max)
      ddh = -ddh
    end

		# add response noise
		ddh += ddh_response;
  end

	return ddh
end

function resolve_TCAS_and_script(ddh_TCAS::Float64, ddh_script::Float64)

  opposite_senses = ddh_script * ddh_TCAS < 0
  same_senses = !opposite_senses

  cmd_greater = abs(ddh_TCAS) > abs(ddh_script)

  return (ddh_TCAS != 0.0 && ((same_senses && cmd_greater) || opposite_senses)) ? ddh_TCAS : ddh_script
end

snap(x, y) = approxeq(x, y, 1e-8) ? y : x

approxeq(x, y, eps) = (x == y) ? true : abs(x-y) < eps

#mods x to the range [-b, b]
function to_plusminus_b(x::AbstractFloat, b::AbstractFloat)
  z = mod(x, 2 * b)
  return (z > b) ? (z - 2 * b) : z
end

to_plusminus_pi(x::AbstractFloat) = to_plusminus_b(x, Float64(pi))

end


#
# This file is part of module
#   GenerateMediumDict (ModiaMedia/dict/GenerateMediumDict.jl)
#

const Modelica_Media_IdealGases_Common_DataRecord = ModiaMedia.SingleGasNasaData
const singleGasesData = Dict{AbstractString, ModiaMedia.SingleGasNasaData}()
const fluidData       = Dict{AbstractString, ModiaMedia.IdealGasFluidConstants}()

# Include data from Modelica.Media.IdealGases.Common.SingleGasesData.jl into dict
include(joinpath(dirname(@__FILE__), "Modelica.Media.IdealGases.Common.SingleGasesData.jl"))

# Include data from Modelica.Media.IdealGases.Common.FluidData.jl into dict
include(joinpath(dirname(@__FILE__), "Modelica.Media.IdealGases.Common.FluidData.jl"))

# Construct Medium
"""
    medium = SimpleGasNasa(name, fluidConstants, data)

Return an ideal gas medium of a single substance, or an ideal gas medium
consisting of several substances where the mass fractions are fixed.
Density is a function of T and p. All other quantities are solely a function of T.

# Sources for model and literature

Original Data: *Computer program for calculation of complex chemical equilibrium compositions and applications*.
Part 1: Analysis Document ID: 19950013764 N (95N20180)
File Series: NASA Technical Reports Report
Number: NASA-RP-1311 E-8017 NAS 1.61:1311
Authors: Gordon, Sanford (NASA Lewis Research Center) Mcbride, Bonnie J. (NASA Lewis Research Center)
Published: Oct 01, 1994.

The data to computpe the specific enthalpy is from
McBride B.J., Zehe M.J., and Gordon S. (2002):
*NASA Glenn Coefficients for Calculating Thermodynamic Properties of Individual Species*.
NASA report TP-2002-211556.
https://www.grc.nasa.gov/WWW/CEAWeb/TP-2002-211556.pdf

Known limits of validity: The data is valid for temperatures between 200K and 6000K.
A few of the data sets for monatomic gases have a discontinuous 1st derivative at 1000K,
but this never caused problems so far.

This model has been adapted from the Modelica.Media.IdealGases.Common.SingleGasNasa
Modelica package, which in turn was copied from the ThermoFluid library and
adapted to Modelica.
"""
function SingleGasNasa(name::AbstractString, fluidConstants::ModiaMedia.IdealGasFluidConstants, data::ModiaMedia.SingleGasNasaData)
    ModiaMedia.SingleGasNasa(mediumName     = name,
                             fluidConstants = fluidConstants,
                             fluidLimits    = ModiaMedia.FluidLimits(TMIN=200.0, TMAX=6000.0),
                             data           = data)
end

# Construct SingleGasNasa objects and store them in the medium dict
function storeSingleGasNasaMedium!(mediumDict)
    global fluidData
    global singleGasesData
    for (name,fluidConstants) in fluidData
        dict[name] = SingleGasNasa(name, fluidConstants, singleGasesData[name])
    end
end

# Manually insert air into the dictionary according to model from Modelica.Media.Air.MoistAir; should probably fix somehow
dict["Air"] = SingleGasNasa("Air", fluidData["N2"], singleGasesData["Air"])

storeSingleGasNasaMedium!(dict)


abstract type
    HydroGen <: Generator
end

struct TechHydro <: TechnicalParams
    installedcapacity::Float64
    activepower::Float64 # [MW]
    activepowerlimits::NamedTuple{(:min, :max),Tuple{Float64,Float64}} # [MW]
    reactivepower::Union{Float64,Nothing} # [MVAr]
    reactivepowerlimits::Union{NamedTuple{(:min, :max),Tuple{Float64,Float64}},Nothing} # [MVAr]
    ramplimits::Union{NamedTuple{(:up, :down),Tuple{Float64,Float64}},Nothing} #MW/Hr
    timelimits::Union{NamedTuple{(:up, :down),Tuple{Float64,Float64}},Nothing} # Hrs
    function TechHydro(installedcapacity, activepower, activepowerlimits, reactivepower, reactivepowerlimits, ramplimits, timelimits)

        new(installedcapacity, activepower, PowerSystems.orderedlimits(activepowerlimits, "Real Power"), reactivepower, PowerSystems.orderedlimits(reactivepowerlimits, "Reactive Power"), ramplimits, timelimits)

    end
end

TechHydro(;installedcapacity = 0.0,
          activepower = 0.0,
          activepowerlimits = (min = 0.0, max = 0.0),
          reactivepower = nothing,
          reactivepowerlimits = nothing,
          ramplimits = nothing,
          timelimits = nothing
        ) = TechHydro(installedcapacity, activepower, activepowerlimits, reactivepower, reactivepowerlimits, ramplimits, timelimits)

struct EconHydro <: TechnicalParams
    curtailpenalty::Float64 # [$/MWh]
    variablecost::Union{Float64,Nothing} # [$/MWh]
end

EconHydro(; cost = 0.0, curtailcost = 0.0) = EconHydro(cost, curtailcost)

struct HydroFix <: HydroGen
    name::String
    available::Bool
    bus::Bus
    tech::TechHydro
    scalingfactor::TimeSeries.TimeArray
end

HydroFix(; name="init",
        status = false,
        bus = Bus(),
        tech = TechHydro(),
        scalingfactor = TimeSeries.TimeArray(Dates.today(),ones(1))) = HydroFix(name, status, bus, tech, scalingfactor)

struct HydroCurtailment <: HydroGen
    name::String
    available::Bool
    bus::Bus
    tech::TechHydro
    econ::Union{EconHydro,Nothing}
    scalingfactor::TimeSeries.TimeArray # [0-1]
    function HydroCurtailment(name, status, bus, tech, curtailcost::Float64, scalingfactor)
        econ = EconHydro(curtailcost, nothing)
        new(name, status, bus, tech, econ, scalingfactor)
    end
end

HydroCurtailment(; name = "init",
                status = false,
                bus= Bus(),
                tech = TechHydro(),
                curtailcost = 0.0,
                scalingfactor = TimeSeries.TimeArray(Dates.today(),ones(1))) = HydroCurtailment(name, status, bus, tech, curtailcost, scalingfactor)

struct HydroStorage <: HydroGen
    name::String
    available::Bool
    bus::Bus
    tech::TechHydro
    econ::Union{EconHydro,Nothing}
    storagecapacity::Float64 #[m^3]
    scalingfactor::TimeSeries.TimeArray
end

HydroStorage(; name = "init",
                status = false,
                bus= Bus(),
                tech = TechHydro(),
                econ = EconHydro(),
                storagecapacity = 0.0,
                scalingfactor = TimeSeries.TimeArray(Dates.today(),ones(1))) = HydroStorage(name, status, bus, tech, econ, storagecapacity, scalingfactor)


using Oceananigans.Advection: AbstractAdvectionScheme
using Oceananigans.Operators: ℑxᶠᵃᵃ, ℑxᶜᵃᵃ, ℑyᵃᶠᵃ, ℑyᵃᶜᵃ, ℑzᵃᵃᶠ, ℑzᵃᵃᶜ
using Oceananigans.TurbulenceClosures: AbstractTurbulenceClosure, AbstractTimeDiscretization

const ATC = AbstractTurbulenceClosure
const ATD = AbstractTimeDiscretization

struct RasterDepthMask end

struct GridFittedBoundary{M, S} <: AbstractImmersedBoundary
    mask :: S
    mask_type :: M
end

GridFittedBoundary(mask; mask_type=nothing) = GridFittedBoundary(mask, mask_type)

@inline is_immersed(i, j, k, underlying_grid, ib::GridFittedBoundary) = ib.mask(node(c, c, c, i, j, k, underlying_grid)...)
@inline is_immersed(i, j, k, underlying_grid, ib::GridFittedBoundary{<:RasterDepthMask}) = ib.mask(i, j)

const IBG = ImmersedBoundaryGrid

const c = Center()
const f = Face()

#####
##### GridFittedImmersedBoundaryGrid
#####

const GFIBG = ImmersedBoundaryGrid{FT, TX, TY, TZ, G, <:GridFittedBoundary} where {FT, TX, TY, TZ, G}

@inline solid_cell(i, j, k, ibg) = is_immersed(i, j, k, ibg.grid, ibg.immersed_boundary)

@inline solid_node(LX, LY, LZ, i, j, k, ibg) = solid_cell(i, j, k, ibg) # fallback (for Center or Nothing LX, LY, LZ)

@inline solid_node(::Face, LX, LY, i, j, k, ibg) = solid_cell(i, j, k, ibg) || solid_cell(i-1, j, k, ibg)
@inline solid_node(LX, ::Face, LZ, i, j, k, ibg) = solid_cell(i, j, k, ibg) || solid_cell(i, j-1, k, ibg)
@inline solid_node(LX, LY, ::Face, i, j, k, ibg) = solid_cell(i, j, k, ibg) || solid_cell(i, j, k-1, ibg)

@inline solid_node(::Face, ::Face, LZ, i, j, k, ibg) = solid_node(c, f, c, i, j, k, ibg) || solid_node(c, f, c, i-1, j, k, ibg)
@inline solid_node(::Face, LY, ::Face, i, j, k, ibg) = solid_node(c, c, f, i, j, k, ibg) || solid_node(c, c, f, i-1, j, k, ibg)
@inline solid_node(LX, ::Face, ::Face, i, j, k, ibg) = solid_node(c, f, c, i, j, k, ibg) || solid_node(c, f, c, i, j, k-1, ibg)

@inline solid_node(::Face, ::Face, ::Face, i, j, k, ibg) = solid_node(c, f, f, i, j, k, ibg) || solid_node(c, f, f, i-1, j, k, ibg)

@inline conditional_flux_ccc(i, j, k, ibg::IBG{FT}, flux, args...) where FT = ifelse(solid_node(c, c, c, i, j, k, ibg), zero(FT), flux(i, j, k, ibg, args...))
@inline conditional_flux_ffc(i, j, k, ibg::IBG{FT}, flux, args...) where FT = ifelse(solid_node(f, f, c, i, j, k, ibg), zero(FT), flux(i, j, k, ibg, args...))
@inline conditional_flux_fcf(i, j, k, ibg::IBG{FT}, flux, args...) where FT = ifelse(solid_node(f, c, f, i, j, k, ibg), zero(FT), flux(i, j, k, ibg, args...))
@inline conditional_flux_cff(i, j, k, ibg::IBG{FT}, flux, args...) where FT = ifelse(solid_node(c, f, f, i, j, k, ibg), zero(FT), flux(i, j, k, ibg, args...))

@inline conditional_flux_fcc(i, j, k, ibg::IBG{FT}, flux, args...) where FT = ifelse(solid_node(f, c, c, i, j, k, ibg), zero(FT), flux(i, j, k, ibg, args...))
@inline conditional_flux_cfc(i, j, k, ibg::IBG{FT}, flux, args...) where FT = ifelse(solid_node(c, f, c, i, j, k, ibg), zero(FT), flux(i, j, k, ibg, args...))
@inline conditional_flux_ccf(i, j, k, ibg::IBG{FT}, flux, args...) where FT = ifelse(solid_node(c, c, f, i, j, k, ibg), zero(FT), flux(i, j, k, ibg, args...))

#####
##### Advective fluxes
#####

# ccc, ffc, fcf
 @inline _viscous_flux_ux(i, j, k, ibg::GFIBG, args...) = conditional_flux_ccc(i, j, k, ibg, viscous_flux_ux, args...)
 @inline _viscous_flux_uy(i, j, k, ibg::GFIBG, args...) = conditional_flux_ffc(i, j, k, ibg, viscous_flux_uy, args...)
 @inline _viscous_flux_uz(i, j, k, ibg::GFIBG, args...) = conditional_flux_fcf(i, j, k, ibg, viscous_flux_uz, args...)

 # ffc, ccc, cff
 @inline _viscous_flux_vx(i, j, k, ibg::GFIBG, args...) = conditional_flux_ffc(i, j, k, ibg, viscous_flux_vx, args...)
 @inline _viscous_flux_vy(i, j, k, ibg::GFIBG, args...) = conditional_flux_ccc(i, j, k, ibg, viscous_flux_vy, args...)
 @inline _viscous_flux_vz(i, j, k, ibg::GFIBG, args...) = conditional_flux_cff(i, j, k, ibg, viscous_flux_vz, args...)

 # fcf, cff, ccc
 @inline _viscous_flux_wx(i, j, k, ibg::GFIBG, args...) = conditional_flux_fcf(i, j, k, ibg, viscous_flux_wx, args...)
 @inline _viscous_flux_wy(i, j, k, ibg::GFIBG, args...) = conditional_flux_cff(i, j, k, ibg, viscous_flux_wy, args...)
 @inline _viscous_flux_wz(i, j, k, ibg::GFIBG, args...) = conditional_flux_ccc(i, j, k, ibg, viscous_flux_wz, args...)

# fcc, cfc, ccf
@inline _diffusive_flux_x(i, j, k, ibg::GFIBG, args...) = conditional_flux_fcc(i, j, k, ibg, diffusive_flux_x, args...)
@inline _diffusive_flux_y(i, j, k, ibg::GFIBG, args...) = conditional_flux_cfc(i, j, k, ibg, diffusive_flux_y, args...)
@inline _diffusive_flux_z(i, j, k, ibg::GFIBG, args...) = conditional_flux_ccf(i, j, k, ibg, diffusive_flux_z, args...)

#####
##### Advective fluxes
#####

# dx(uu), dy(vu), dz(wu)
# ccc,    ffc,    fcf
@inline _advective_momentum_flux_Uu(i, j, k, ibg::GFIBG, args...) = conditional_flux_ccc(i, j, k, ibg, advective_momentum_flux_Uu, args...)
@inline _advective_momentum_flux_Vu(i, j, k, ibg::GFIBG, args...) = conditional_flux_ffc(i, j, k, ibg, advective_momentum_flux_Vu, args...)
@inline _advective_momentum_flux_Wu(i, j, k, ibg::GFIBG, args...) = conditional_flux_fcf(i, j, k, ibg, advective_momentum_flux_Wu, args...)

# dx(uv), dy(vv), dz(wv)
# ffc,    ccc,    cff
@inline _advective_momentum_flux_Uv(i, j, k, ibg::GFIBG, args...) = conditional_flux_ffc(i, j, k, ibg, advective_momentum_flux_Uv, args...)
@inline _advective_momentum_flux_Vv(i, j, k, ibg::GFIBG, args...) = conditional_flux_ccc(i, j, k, ibg, advective_momentum_flux_Vv, args...)
@inline _advective_momentum_flux_Wv(i, j, k, ibg::GFIBG, args...) = conditional_flux_cff(i, j, k, ibg, advective_momentum_flux_Wv, args...)

# dx(uw), dy(vw), dz(ww)
# fcf,    cff,    ccc
@inline _advective_momentum_flux_Uw(i, j, k, ibg::GFIBG, args...) = conditional_flux_fcf(i, j, k, ibg, advective_momentum_flux_Uw, args...)
@inline _advective_momentum_flux_Vw(i, j, k, ibg::GFIBG, args...) = conditional_flux_cff(i, j, k, ibg, advective_momentum_flux_Vw, args...)
@inline _advective_momentum_flux_Ww(i, j, k, ibg::GFIBG, args...) = conditional_flux_ccc(i, j, k, ibg, advective_momentum_flux_Ww, args...)

   @inline _advective_tracer_flux_x(i, j, k, ibg::GFIBG, args...) = conditional_flux_fcc(i, j, k, ibg, advective_tracer_flux_x, args...)
   @inline _advective_tracer_flux_y(i, j, k, ibg::GFIBG, args...) = conditional_flux_cfc(i, j, k, ibg, advective_tracer_flux_y, args...)
   @inline _advective_tracer_flux_z(i, j, k, ibg::GFIBG, args...) = conditional_flux_ccf(i, j, k, ibg, advective_tracer_flux_z, args...)

#####
##### "Boundary-aware" interpolation
#####
##### Don't interpolate dead cells.
#####

@inline near_x_boundary(i, j, k, ibg, ::AbstractAdvectionScheme{0}) = false
@inline near_y_boundary(i, j, k, ibg, ::AbstractAdvectionScheme{0}) = false
@inline near_z_boundary(i, j, k, ibg, ::AbstractAdvectionScheme{0}) = false

@inline near_x_boundary(i, j, k, ibg, ::AbstractAdvectionScheme{1}) = solid_cell(i - 1, j, k, ibg) || solid_cell(i, j, k, ibg) || solid_cell(i + 1, j, k, ibg)
@inline near_y_boundary(i, j, k, ibg, ::AbstractAdvectionScheme{1}) = solid_cell(i, j - 1, k, ibg) || solid_cell(i, j, k, ibg) || solid_cell(i, j + 1, k, ibg)
@inline near_z_boundary(i, j, k, ibg, ::AbstractAdvectionScheme{1}) = solid_cell(i, j, k - 1, ibg) || solid_cell(i, j, k, ibg) || solid_cell(i, j, k + 1, ibg)

@inline near_x_boundary(i, j, k, ibg, ::AbstractAdvectionScheme{2}) = solid_cell(i - 2, j, k, ibg) || solid_cell(i - 1, j, k, ibg) || solid_cell(i, j, k, ibg) || solid_cell(i + 1, j, k, ibg) || solid_cell(i + 2, j, k, ibg)
@inline near_y_boundary(i, j, k, ibg, ::AbstractAdvectionScheme{2}) = solid_cell(i, j - 2, k, ibg) || solid_cell(i, j - 1, k, ibg) || solid_cell(i, j, k, ibg) || solid_cell(i, j + 1, k, ibg) || solid_cell(i, j + 2, k, ibg)
@inline near_z_boundary(i, j, k, ibg, ::AbstractAdvectionScheme{2}) = solid_cell(i, j, k - 2, ibg) || solid_cell(i, j, k - 1, ibg) || solid_cell(i, j, k, ibg) || solid_cell(i, j, k + 1, ibg) || solid_cell(i, j, k + 2, ibg)

# Takes forever to compile, but works.
# @inline near_x_boundary(i, j, k, ibg, ::AbstractAdvectionScheme{buffer}) where buffer = any(ntuple(δ -> solid_cell(i - buffer - 1 + δ, j, k, ibg), Val(2buffer + 1)))
# @inline near_y_boundary(i, j, k, ibg, ::AbstractAdvectionScheme{buffer}) where buffer = any(ntuple(δ -> solid_cell(i, j - buffer - 1 + δ, k, ibg), Val(2buffer + 1)))
# @inline near_z_boundary(i, j, k, ibg, ::AbstractAdvectionScheme{buffer}) where buffer = any(ntuple(δ -> solid_cell(i, j, k - buffer - 1 + δ, ibg), Val(2buffer + 1)))

for bias in (:symmetric, :left_biased, :right_biased)
    for (d, ξ) in enumerate((:x, :y, :z))

        code = [:ᵃ, :ᵃ, :ᵃ]

        for loc in (:ᶜ, :ᶠ)
            code[d] = loc
            second_order_interp = Symbol(:ℑ, ξ, code...)
            interp = Symbol(bias, :_interpolate_, ξ, code...)
            alt_interp = Symbol(:_, interp)

            near_boundary = Symbol(:near_, ξ, :_boundary)

            # Conditional high-order interpolation in Bounded directions
            @eval begin
                import Oceananigans.Advection: $alt_interp
                using Oceananigans.Advection: $interp

                @inline $alt_interp(i, j, k, ibg::ImmersedBoundaryGrid, scheme, ψ) =
                    ifelse($near_boundary(i, j, k, ibg, scheme),
                           $second_order_interp(i, j, k, ibg.grid, ψ),
                           $interp(i, j, k, ibg.grid, scheme, ψ))

                # @inline $alt_interp(i, j, k, ibg::IBG, scheme, ψ) = $interp(i, j, k, ibg.grid, scheme, ψ)
            end
        end
    end
end

#####
##### Masking for GridFittedBoundary
#####

@inline function scalar_mask(i, j, k, grid, ::GridFittedBoundary, LX, LY, LZ, value, field)
    return @inbounds ifelse(solid_node(LX, LY, LZ, i, j, k, grid),
                            value,
                            field[i, j, k])
end

mask_immersed_velocities!(U, arch, grid::GFIBG) = Tuple(mask_immersed_field!(q) for q in U)


include("TwoPartCost.jl")
include("ThreePartCost.jl")
include("MultiStartCost.jl")
include("Area.jl")
include("LoadZone.jl")
include("Bus.jl")
include("Arc.jl")
include("Line.jl")
include("MonitoredLine.jl")
include("PhaseShiftingTransformer.jl")
include("TapTransformer.jl")
include("Transformer2W.jl")
include("HVDCLine.jl")
include("VSCDCLine.jl")
include("InterruptibleLoad.jl")
include("FixedAdmittance.jl")
include("PowerLoad.jl")
include("HydroEnergyReservoir.jl")
include("HydroDispatch.jl")
include("RenewableDispatch.jl")
include("RenewableFix.jl")
include("ThermalStandard.jl")
include("ThermalMultiStart.jl")
include("GenericBattery.jl")
include("StaticReserve.jl")
include("ReserveDemandCurve.jl")
include("VariableReserve.jl")
include("AGC.jl")
include("Transfer.jl")
include("AVRFixed.jl")
include("AVRSimple.jl")
include("AVRTypeI.jl")
include("AVRTypeII.jl")
include("BaseMachine.jl")
include("RoundRotorMachine.jl")
include("SalientPoleMachine.jl")
include("AndersonFouadMachine.jl")
include("FullMachine.jl")
include("MarconatoMachine.jl")
include("OneDOneQMachine.jl")
include("SimpleAFMachine.jl")
include("SimpleFullMachine.jl")
include("SimpleMarconatoMachine.jl")
include("PSSFixed.jl")
include("PSSSimple.jl")
include("SingleMass.jl")
include("FiveMassShaft.jl")
include("TGFixed.jl")
include("TGTypeI.jl")
include("TGTypeII.jl")
include("AverageConverter.jl")
include("FixedDCSource.jl")
include("LCLFilter.jl")
include("LCFilter.jl")
include("KauraPLL.jl")
include("VirtualInertia.jl")
include("ReactivePowerDroop.jl")
include("CurrentControl.jl")
include("Source.jl")

export get_Ae
export get_Be
export get_D
export get_D_12
export get_D_23
export get_D_34
export get_D_45
export get_D_ex
export get_D_hp
export get_D_ip
export get_D_lp
export get_H
export get_H_ex
export get_H_hp
export get_H_ip
export get_H_lp
export get_K0
export get_K_d
export get_K_ex
export get_K_hp
export get_K_i
export get_K_ip
export get_K_lp
export get_K_p
export get_K_ω
export get_Ka
export get_Ke
export get_Kf
export get_Kv
export get_L_1d
export get_L_1q
export get_L_ad
export get_L_aq
export get_L_d
export get_L_f1d
export get_L_ff
export get_L_q
export get_P_max
export get_P_min
export get_P_ref
export get_R
export get_R_1d
export get_R_1q
export get_R_f
export get_Se
export get_T1
export get_T2
export get_T3
export get_T4
export get_T5
export get_T_AA
export get_Ta
export get_Tc
export get_Td0_p
export get_Td0_pp
export get_Te
export get_Tf
export get_Tq0_p
export get_Tq0_pp
export get_Tr
export get_Ts
export get_V_pss
export get_V_ref
export get_Vf
export get_Vr_max
export get_Vr_min
export get_X_th
export get_Xd
export get_Xd_p
export get_Xd_pp
export get_Xl
export get_Xq
export get_Xq_p
export get_Xq_pp
export get_Y
export get_activepower
export get_activepower_flow
export get_activepowerlimits
export get_activepowerlimits_from
export get_activepowerlimits_to
export get_angle
export get_anglelimits
export get_arc
export get_area
export get_available
export get_b
export get_basepower
export get_basevoltage
export get_bias
export get_bus
export get_bustype
export get_capacity
export get_cf
export get_delta_t
export get_dynamic_injector
export get_efficiency
export get_energy
export get_eq_p
export get_ext
export get_fixed
export get_flowlimits
export get_from
export get_fuel
export get_inflow
export get_initial_ace
export get_initial_storage
export get_inputactivepowerlimits
export get_internal
export get_inv_d_fluxlink
export get_inv_q_fluxlink
export get_inverter_firing_angle
export get_inverter_taplimits
export get_inverter_xrc
export get_kad
export get_kd
export get_kffi
export get_kffv
export get_ki_pll
export get_kic
export get_kiv
export get_kp_pll
export get_kpc
export get_kpv
export get_kq
export get_kω
export get_lf
export get_lg
export get_load_response
export get_load_zone
export get_loss
export get_lv
export get_maxactivepower
export get_maxreactivepower
export get_model
export get_must_run
export get_n_states
export get_no_load
export get_number
export get_op_cost
export get_outputactivepowerlimits
export get_power_trajectory
export get_powerfactor
export get_primaryshunt
export get_primemover
export get_r
export get_ramplimits
export get_rate
export get_rating
export get_reactivepower
export get_reactivepower_flow
export get_reactivepowerlimits
export get_reactivepowerlimits_from
export get_reactivepowerlimits_to
export get_rectifier_firing_angle
export get_rectifier_taplimits
export get_rectifier_xrc
export get_requirement
export get_rf
export get_rg
export get_rv
export get_s_rated
export get_services
export get_shutdn
export get_start_time_limits
export get_start_types
export get_startup
export get_states
export get_status
export get_storage_capacity
export get_tap
export get_time_at_status
export get_timeframe
export get_timelimits
export get_to
export get_v_rated
export get_variable
export get_voltage
export get_voltagelimits
export get_x
export get_α
export get_γd
export get_γq
export get_τ_max
export get_τ_min
export get_ω_lp
export get_ωad
export get_ωf
export set_Ae!
export set_Be!
export set_D!
export set_D_12!
export set_D_23!
export set_D_34!
export set_D_45!
export set_D_ex!
export set_D_hp!
export set_D_ip!
export set_D_lp!
export set_H!
export set_H_ex!
export set_H_hp!
export set_H_ip!
export set_H_lp!
export set_K0!
export set_K_d!
export set_K_ex!
export set_K_hp!
export set_K_i!
export set_K_ip!
export set_K_lp!
export set_K_p!
export set_K_ω!
export set_Ka!
export set_Ke!
export set_Kf!
export set_Kv!
export set_L_1d!
export set_L_1q!
export set_L_ad!
export set_L_aq!
export set_L_d!
export set_L_f1d!
export set_L_ff!
export set_L_q!
export set_P_max!
export set_P_min!
export set_P_ref!
export set_R!
export set_R_1d!
export set_R_1q!
export set_R_f!
export set_Se!
export set_T1!
export set_T2!
export set_T3!
export set_T4!
export set_T5!
export set_T_AA!
export set_Ta!
export set_Tc!
export set_Td0_p!
export set_Td0_pp!
export set_Te!
export set_Tf!
export set_Tq0_p!
export set_Tq0_pp!
export set_Tr!
export set_Ts!
export set_V_pss!
export set_V_ref!
export set_Vf!
export set_Vr_max!
export set_Vr_min!
export set_X_th!
export set_Xd!
export set_Xd_p!
export set_Xd_pp!
export set_Xl!
export set_Xq!
export set_Xq_p!
export set_Xq_pp!
export set_Y!
export set_activepower!
export set_activepower_flow!
export set_activepowerlimits!
export set_activepowerlimits_from!
export set_activepowerlimits_to!
export set_angle!
export set_anglelimits!
export set_arc!
export set_area!
export set_available!
export set_b!
export set_basepower!
export set_basevoltage!
export set_bias!
export set_bus!
export set_bustype!
export set_capacity!
export set_cf!
export set_delta_t!
export set_dynamic_injector!
export set_efficiency!
export set_energy!
export set_eq_p!
export set_ext!
export set_fixed!
export set_flowlimits!
export set_from!
export set_fuel!
export set_inflow!
export set_initial_ace!
export set_initial_storage!
export set_inputactivepowerlimits!
export set_internal!
export set_inv_d_fluxlink!
export set_inv_q_fluxlink!
export set_inverter_firing_angle!
export set_inverter_taplimits!
export set_inverter_xrc!
export set_kad!
export set_kd!
export set_kffi!
export set_kffv!
export set_ki_pll!
export set_kic!
export set_kiv!
export set_kp_pll!
export set_kpc!
export set_kpv!
export set_kq!
export set_kω!
export set_lf!
export set_lg!
export set_load_response!
export set_load_zone!
export set_loss!
export set_lv!
export set_maxactivepower!
export set_maxreactivepower!
export set_model!
export set_must_run!
export set_n_states!
export set_no_load!
export set_number!
export set_op_cost!
export set_outputactivepowerlimits!
export set_power_trajectory!
export set_powerfactor!
export set_primaryshunt!
export set_primemover!
export set_r!
export set_ramplimits!
export set_rate!
export set_rating!
export set_reactivepower!
export set_reactivepower_flow!
export set_reactivepowerlimits!
export set_reactivepowerlimits_from!
export set_reactivepowerlimits_to!
export set_rectifier_firing_angle!
export set_rectifier_taplimits!
export set_rectifier_xrc!
export set_requirement!
export set_rf!
export set_rg!
export set_rv!
export set_s_rated!
export set_services!
export set_shutdn!
export set_start_time_limits!
export set_start_types!
export set_startup!
export set_states!
export set_status!
export set_storage_capacity!
export set_tap!
export set_time_at_status!
export set_timeframe!
export set_timelimits!
export set_to!
export set_v_rated!
export set_variable!
export set_voltage!
export set_voltagelimits!
export set_x!
export set_α!
export set_γd!
export set_γq!
export set_τ_max!
export set_τ_min!
export set_ω_lp!
export set_ωad!
export set_ωf!


using MPIReco

@testset "motion compensation reconstruction" begin

  ################
  ## Parameters ##
  ################
  alpha = 1.2

  # Choose the peak number according to the expected approximate value. Be careful, if there are peaks not corresponding to object motion, division by choosePeak can be false.
  # For experiments with different periodic motions it is used to pick one motion frequency (then don't divide by choosePeak)

  choosePeak = 4

  windowType = 1  # 1: Hann, 2: FT1A05, 3: Rectangle

  # Measurement data
  datadirMeas = "./data/mdf/"
  bMeas = MPIFile(datadirMeas*"measFast.mdf") # high frequency
  bBG = MPIFile(datadirMeas*"measBG.mdf") # background measurement

  # System matrices
  datadirSF = "./data/mdf/"
  SFall = ["SF1Small.mdf","SF2Small.mdf","SF3Small.mdf","SF4Small.mdf"]
  bSF = MultiMPIFile(datadirSF.*SFall)

  # Background frames
  bgFrames = [1:200, 201:400, 401:600 ,601:800]

  # Selected frames
  recoFrame = 1

  ####################
  ## Reconstruction ##
  ####################

  # Reconstruction parameters
  lambda = 0.01
  iterations = 1

  c = reconstruction(bSF, bMeas, periodicMotionCorrection = true,
                                bEmpty = bBG, bgFrames = bgFrames, #background measurement
                                alpha = alpha, choosePeak = choosePeak, recoFrame = recoFrame,
                                windowType=windowType, higherHarmonic=choosePeak,
                                lambda=lambda, iterations=iterations, # reconstruction parameter
                                SNRThresh=10, minFreq=80e3, recChannels=[1,2,3]) #frequency selection

  exportImage("./img/MotionComp.png", maximum(arraydata(c[1,:,:,:,1]),dims=3)[:,:,1])

end


using Flux
using Random

mutable struct GVF_DQNAgent{M, TN, O, LU, AP<:AbstractValuePolicy, Φ, ERP} <: AbstractAgent
    model::M
    target_network::TN
    opt::O
    lu::LU
    ap::AP
    er::ExperienceReplay{ERP}
    γ::Float32
    batch_size::Int64
    tn_counter_init::Int64
    target_network_counter::Int64
    action::Int64
    prev_s::Φ
end

GVF_DQNAgent(model, target_network, opt, lu, ap, size_buffer, γ, batch_size, tn_counter_init, s) =
    GVF_DQNAgent(model,
                 target_network,
                 opt,
                 lu,
                 ap,
                 ExperienceReplay(100000,
                                  (Array{Float32, 1}, Int64, Array{Float32, 1}, Float32, Bool),
                                  (:s, :a, :sp, :r, :t)),
                 γ,
                 batch_size,
                 tn_counter_init,
                 tn_counter_init,
                 0,
                 s)

function RLCore.start!(agent::DQNAgent, env_s_tp1, rng::AbstractRNG; kwargs...)
    # Start an Episode
    agent.action = sample(agent.ap,
                          agent.model(env_s_tp1),
                          rng)
    return agent.action
end

function RLCore.step!(agent::DQNAgent, env_s_tp1, r, terminal, rng::AbstractRNG; kwargs...)

    add!(agent.er, (Float32.(agent.prev_s), agent.action, copy(Float32.(env_s_tp1)), r, terminal))

    if size(agent.er)[1] > 1000
        e = sample(agent.er, agent.batch_size; rng=rng)
        update_params!(agent, e)
    end

    agent.prev_s .= env_s_tp1
    agent.action = sample(agent.ap,
                          agent.model(agent.prev_s),
                          rng)

    return agent.action
end

function update_params!(agent::DQNAgent, e)

    if agent.tn_counter_init > 0
        update!(agent.model, agent.lu, agent.opt, e.s, e.a, e.sp, e.r, e.t, agent.target_network)

        if agent.target_network_counter == 1
            agent.target_network_counter = agent.tn_counter_init
            agent.target_network = mapleaves(
                Flux.Tracker.data,
                deepcopy(agent.model))
        else
            agent.target_network_counter -= 1
        end
    else
        update!(agent.model, agent.lu, agent.opt, e.s, e.a, e.sp, e.r, e.t)
    end
    return nothing

end


#P6
#1920 1080
#255
#<frame RGB>
#
# ASCII dump:
# 0x50 0x36   0x0a
# 0x31 0x39 0x32 0x30   0x20   0x31 0x30 0x38 0x30   0x0a
# 0x32 0x35 0x35   0x0a
#
# lixun 2017/08/09
p6 = [0x50, 0x36, 0x0a]
horizontal = "480"
vertical = "640"
fps = 30
duration = 3600.7
start = 1
depth = [0x32, 0x35, 0x35, 0x0a]

noise_floor = "alexa-f0.ppm"
digest = "alexa.digest"

function delta(a, b)
    a > b && (return a-b)
    b-a
end

h = tryparse(Int64, horizontal)
h = isnull(h) ? 0 : get(h)
v = tryparse(Int64, vertical)
v = isnull(v) ? 0 : get(v)

header = zeros(UInt8, length(p6) + length(horizontal) + 1 + length(vertical) + 1 + length(depth))
f0 = zeros(UInt8, h*v*3)
fx = zeros(UInt8, h*v*3)
rgb = zeros(Float64, convert(Int64, ceil(duration * fps)), 3)

# capture the inactive frame as basis
fn = convert(Int64, ceil(start * fps))
for i = 1:fn
    read!(stdin, header)
    read!(stdin, f0)
end
open(noise_floor, "w") do f
    write(f, header)
    write(f, f0)
end

# dealing with the rest frames
i = 1
while !eof(stdin)
    read!(stdin, header)
    read!(stdin, fx)
    #version 1: signal[i] = sum(frame) - md
    for k = 1:3
        rgb[i,k] = sum(delta.(fx[k:3:end], f0[k:3:end])) / (h*v)
    end
    #version 3: signal[i] = delta(sum(frame), md)
    i += 1
end
open(digest, "w") do f
    for j = 1:i
        write(f, "$(rgb[j,1]), $(rgb[j,2]), $(rgb[j,3])\n")
    end
end


# using SuperResolution
using Distributions
using ProgressMeter
include("fctSR.jl")
include("segmentSR.jl")

###4.. Setup KLClustering

##	Generate all possible clustering models for a _specific segment_
#	For final use, create such ModelSR for every valid SEG.idx
mutable struct ModelSR
		#Segment-restricted features
	idx::Tuple{Int64,Int64}
	modus::Int64
	dim::Int64
	loc::Array{Array{Float64,2},1}	#localisations of cluster populations
	frame::Array{Int64,1}
	model::Array{MixtureModel,1}	#MixtureModel
	member::Array{Array{Int64,1},1}	#cluster ID for all localisations
	clus_ID::Array{Array{Int64,1},1}	#cluster ID for remaining populations (needed for dim2WKLDfull; might replace with unique() and findall() for indices)
	mol_ID::Array{Int64,1}		# ground truth member ID

	cov::Array{Array{Array{Float64,2},1},1}	#covariance matrices
	comp_mix::Array{Array{Float64,1},1}	#convex coefficient of all MixtureModel components
	wkld::Array{Array{Float64,2},1}	#distance matrices KLD & comp_mix
	weight::Array{Array{Float64,2},1}	#distance matrices KLD & comp_mix
	geom::Array{Array{Geometric{Float64},1},1}
	param::Array{Array{Float64,1},1}
	KLD::Array{Array{Float64,2},1}	#distance matrices KLD & comp_mix

	K::Array{Int64,1}	#number of molecules in segment (according to ground_truth)
##Function to initialize structure
	function ModelSR(SEG::SuperResolution.SegmentSR,idx::Tuple{Int64,Int64},WKLD_modINPUT=1) #idx from list SEG.idx[j]
		obj=new()
		obj.idx=idx
		obj.frame=SEG.frame[CartesianIndex(idx)]
		#obj.modus=["KLD","WKLD"][sum(WKLD_modINPUT==1)+1]	#WKLD_modINPUT==1.."WKLD", else "KLD"
		obj.modus=WKLD_modINPUT
		obj.dim=size(SEG.all_loc,2)
		#obj.dim=size(SEG.all_loc[:,1:(end-1)])[2]	#all_loc last col is framenumbers
		#obj.loc=Array{Array{Float64,2},1}(undef,SEG.N[CartesianIndex(idx)]) #
		obj.member=Array{Array{Int64,1},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.clus_ID=Array{Array{Int64,1},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.mol_ID=SEG.mol_ID[CartesianIndex(idx)]
		#obj.model=Array{MixtureModel,1}(undef,SEG.N[CartesianIndex(idx)])
		obj.cov=Array{Array{Array{Float64,2},1},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.comp_mix=Array{Array{Float64,1},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.wkld=Array{Array{Float64,2},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.KLD=Array{Array{Float64,2},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.weight=Array{Array{Float64,2},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.geom=Array{Array{Geometric{Float64},1},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.param=Array{Array{Float64,1},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.K=Array{Int64,1}(undef,SEG.N[CartesianIndex(idx)])

		obj.member[1]=collect(1:SEG.N[CartesianIndex(idx)])#Array{Array{Int64,1},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.clus_ID[1]=collect(1:SEG.N[CartesianIndex(idx)])#Array{Array{Int64,1},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.cov[1]=repeat([1.0*eye(obj.dim)],SEG.N[CartesianIndex(idx)])#Array{Array{Array{Float64,2},1},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.weight[1]=ones(SEG.N[CartesianIndex(idx)],SEG.N[CartesianIndex(idx)])
		obj.comp_mix[1]=1/(SEG.N[CartesianIndex(idx)]-1)*ones(1,SEG.N[CartesianIndex(idx)])[1,:]#Array{Array{Float64,2},1}(undef,SEG.N[CartesianIndex(idx)])
		obj.geom[1]=repeat([Geometric(0.999)],SEG.N[CartesianIndex(idx)])
		sKLD=[dim2adj(SEG.loc[CartesianIndex(idx)]),]	#dim2adj: 452×3 Array{Float64,2} --> 452-element Array{Array,1}
		#N=size(sKLD[1])[1]

		#global i_inc=1
		#sKLD[1],obj.comp_mix[1],obj.cov[1],obj.clus_ID[1],obj.member[1],obj.wkld[1],obj.weight[i_inc+1],obj.geom[1]=dim2WKLDfull(sKLD[i_inc],obj.cov[i_inc],obj.comp_mix[i_inc],SEG.frame[CartesianIndex(idx)],obj.clus_ID[i_inc],obj.member[i_inc])
		# sKLD[1],ωall[1],Σall[1],Aall[1],Acoll[1],KLDeval[1]=dim2WKLDfull(sKLD[i_inc],Σall[i_inc],ωall[i_inc],Aall[i_inc],Acoll[i_inc])
##1..Extract needed features
		global obj.loc=[dim2adj(dim2adj(SEG.loc[CartesianIndex(idx)])),] #x,y coordinates of all observations [MANDATORY FOR MODEL SELECTION/EVALUATION]
		obj.model=[dim2mix(SEG.loc[CartesianIndex(idx)][:,1:2]),]
		obj.wkld[1]=deepcopy(SEG.loc[CartesianIndex(idx)])

		###4.. KLClustering
		print("number of observations = $(SEG.N[CartesianIndex(idx)])\n")
		@time(
			@showprogress(for i_inc=2:SEG.N[CartesianIndex(idx)]#while size(obj.clus_ID[i_inc],1)>1
				tmpsKLD,obj.comp_mix[i_inc],obj.cov[i_inc],obj.clus_ID[i_inc],obj.member[i_inc],obj.wkld[i_inc],obj.weight[i_inc],obj.geom[i_inc]=dim2WKLDfull(
				# sKLD[i_inc-1],obj.cov[i_inc-1],obj.comp_mix[i_inc-1],SEG.frame[CartesianIndex(idx)],obj.clus_ID[i_inc-1],obj.member[i_inc-1],WKLD_modINPUT)
				sKLD[i_inc-1],obj.cov[i_inc-1],obj.comp_mix[i_inc-1],SEG.frame[CartesianIndex(idx)],obj.clus_ID[i_inc-1],obj.member[i_inc-1],WKLD_modINPUT)
				push!(sKLD,tmpsKLD)#x,y values + pdf value of Framenumber
				push!(obj.loc,dim2adj(sKLD[i_inc]))#x,y values from sKLD variable
				push!(obj.model,dim2mix(dim2adj(sKLD[i_inc]),obj.comp_mix[i_inc]))#MixtureModel (PDF)
				#global i_inc+=1
			end)
		)
		for j=1:(length(obj.geom))
			tete=map(i->params(obj.geom[j][i])[1],1:length(obj.geom[j]))
			obj.param[j]=tete
		end
		obj.K=map(i->length(unique(obj.member[i])),1:(length(obj.member)))
		obj
	end
end
export ModelSR
##
# #x,y coordinates of cluster centers, for each iteration
# obj.loc=map(i->obj.loc[i][:,1:2],1:length(obj.loc))
#
# ###5.. Cluster Evaluation
# #sMMsim=dim2mix(𝛍sim,𝛚sim)#GMM-Distribution based on mean ("RealLoc") mol_IDs, for comparison
#
# print("##########################\n")
# print("Compute Model Selection...\n")
# print("##########################\n")
#
# xRangeSel=collect(1:(i_inc))
# Knum=map(i->size(obj.loc[i],1),collect(1:size(obj.loc,1))) #number of clusters for each interation
#
# #compute number, index and locations of unique molecules
# Acoll_unique=map(i->length(unique(Acoll[i])),1:length(Acoll))
# #print("Acoll_unique:\n"); show(Acoll_unique); print("\n")
# #compute min|max|mean KLD values
# #2:length(obj.wkld) because the 1st entry is [Y,] !!!!
# minKLDeval=map(i->minimum(obj.wkld[i][1e-10 .<obj.wkld[i]]),1:length(obj.wkld)-1)
# push!(minKLDeval,sort(obj.wkld[end-2][minKLDeval[end-1].<obj.wkld[end-2]])[1]) #pseudo minKLD value for 1 Cluster
#
# #compute absolute minKLD changes
# abs_minKLDeval=abs.(minKLDeval[1:end-1]-minKLDeval[2:end])
# push!(abs_minKLDeval,0)
# abs_minKLDeval[2:end]=abs_minKLDeval[1:end-1]
# abs_minKLDeval[1]=0
# #print("abs_minKLDeval:\n"); show(abs_minKLDeval); print("\n")
#
# #obj.loc[end-solvIDXend] is the clustering solution
# global solvIDX=findall(abs_minKLDeval.==maximum(abs_minKLDeval))[1] #!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
# global solvNumClust=size(obj.loc[solvIDX],1)
# print("estimated cluster for partition $(partN)_$(partN2): $(solvNumClust)\n")
# # print("length(minKLDeval)=$(length(minKLDeval))\n")
# # print("length(abs_delta_minKLDeval)=$(length(abs_minKLDeval))\n")


struct NullInfoState <: CounterfactualRegret.AbstractInfoState end
abstract type AbstractDeepCFRSolver{G} <: CFR.AbstractCFRSolver{Nothing,G,NullInfoState} end
mutable struct DeepCFRSolver{
        GPU,
        AN,
        SN,
        INFO,
        REGRET,
        STRAT,
        G <: Game,
        T,
        ADV_OPT <: Flux.Optimise.AbstractOptimiser,
        STRAT_OPT <: Flux.Optimise.AbstractOptimiser
    } <: AbstractDeepCFRSolver{G}
    gpu::Val{GPU}

    "Advantage networks"
    V::NTuple{2,AN}

    "Advantage memory"
    Mv::NTuple{2,AdvantageMemory{INFO, REGRET}}

    "Strategy Network"
    Π::SN

    "Strategy Memory"
    Mπ::StrategyMemory{INFO, STRAT}

    value_batches::Int
    strategy_batches::Int
    buffer_size::Int
    batch_size::Int
    traversals::Int
    traverser::T
    advantage_opt::ADV_OPT
    strategy_opt::STRAT_OPT
    game::G
    T::Float32
end

on_gpu(::DeepCFRSolver{GPU}) where GPU = GPU
infotype(::DeepCFRSolver{GPU,A,S,I}) where {GPU,A,S,I} = I
regrettype(::DeepCFRSolver{GPU,A,S,I,R}) where {GPU,A,S,I,R} = R
strattype(::DeepCFRSolver{GPU,A,S,I,R,ST}) where {GPU,A,S,I,R,ST} = ST

function in_out_sizes(game::Game)
    h0 = initialhist(game)
    k = vectorized(game, infokey(game, h0))
    A = actions(game, h0)
    in_size = length(k)
    out_size = length(A)
    return in_size, out_size
end

"""
`game::CounterfactualRegret.Game`

`value_batches::Int = 100` - Number of epochs for which value/advantage/regret networks are trained

`strategy_batches::Int = 1000` - Number of epochs for which strategy networks are trained

`buffer_size::Int = 100_000` - Capacity of memory buffers (both strategy and value)

`batch_size::Int = 512` - Batch size for each gradient update step (both strategy and value)

`traversals::Int = 100` - Number of MCCFR tree traversals to make for regret data collection between training steps

`traverser = ExternalSample()` - Type of MCCFR sampling used for tree traversal

`value_optimizer::Flux.Optimise.AbstractOptimiser` - value network optimizer

`strategy_optimizer::Flux.Optimise.AbstractOptimiser` - strategy network optimizer

`values` - Tuple of Flux.jl value networks (one network for each player)

`strategy` - Single Flux.jl strategy network

`on_gpu::Bool = false` - Option to push network training to GPU
"""
function DeepCFRSolver(game::Game{H,K};
    value_batches::Int = 100,
    strategy_batches::Int = 1000,
    buffer_size::Int = 100*10^3,
    batch_size::Int = 512,
    traverser = ExternalSample(),
    traversals::Int = 50,
    value_optimizer = ADAM(5e-3),
    strategy_optimizer = ADAM(5e-3),
    strategy = nothing,
    values::Union{Nothing, Tuple} = nothing,
    on_gpu::Bool = false
    ) where {H,K}

    VK = first(Base.return_types(vectorized, (typeof(game),K)))
    @assert VK <: AbstractVector "`vectorized(::Game{H,K}, ::K)` should return vector"

    #= Leave to checks
    if promote_type(eltype(VK), Float32) !== Float32
        @warn "Float32 eltype preferred for vectorized information state key"
    end
    =#

    in_size, out_size = in_out_sizes(game)
    value_nets = if isnothing(values)
        value_net = Chain(Dense(in_size, 16, sigmoid), Dense(16,out_size))
        (value_net, deepcopy(value_net))
    else
        values
    end

    strategy_net = if isnothing(strategy)
        Chain(
            Dense(in_size, 16, sigmoid),
            Dense(16, 16, sigmoid),
            Dense(16, out_size, sigmoid),
            softmax)
    else
        strategy
    end

    return DeepCFRSolver(
        Val(on_gpu),
        value_nets,
        (AdvantageMemory{VK}(buffer_size), AdvantageMemory{VK}(buffer_size)),
        strategy_net,
        StrategyMemory{VK}(buffer_size),
        value_batches,
        strategy_batches,
        buffer_size,
        batch_size,
        traversals,
        traverser,
        value_optimizer,
        strategy_optimizer,
        game,
        1.0f0
    )
end

function CFR.strategy(sol::DeepCFRSolver, I)
    game = sol.game
    if typeof(I) === infokeytype(game)
        return sol.Π(vectorized(game, I))
    else
        return sol.Π(I)
    end
end

(sol::DeepCFRSolver)(I) = CFR.strategy(sol, I)

function Base.show(io::IO, mime::MIME"text/plain", sol::DeepCFRSolver)
    Lv = length(first(sol.Mv))
    Lπ = length(sol.Mπ)
    sz = sol.buffer_size
    println(io, "\n\t Deep CFR Solver")
    println(io, join(fill('_', 20)))
    println(io, "GPU                 | \t $(on_gpu(sol))")
    println(io, "Advantage Net       | \t $(string(first(sol.V)))")
    println(io, "Advantage Memory    | \t $Lv / $sz")
    println(io, "Strategy Net        | \t $(string(sol.Π))")
    println(io, "Strategy Memory     | \t $Lπ / $sz")
    println(io, "Batch Size          | \t $(sol.batch_size)")
    println(io, "Traversals          | \t $(sol.traversals)")
    println(io, "Advantage Optimizer | \t $(typeof(sol.advantage_opt))")
    println(io, "Strategy Optimizer  | \t $(typeof(sol.strategy_opt))")
    println(io, "Game                | \t $(typeof(sol.game))")

    nothing
end


include("Include.jl")

data_dictionary, TXTL_dictionary, species_list, Mean, Upper, Lower = LoadDictionaries()

t0_exp = 0
tf_exp = 3
tstep_exp = 0.01
experimental_time_exp = [t0_exp:tstep_exp:tf_exp;]

metabolite_list = data_dictionary["list_of_metabolite_symbols"]
rxn_list = data_dictionary["list_of_reaction_strings"]

(n_species,n_rxn) = size(data_dictionary["stoichiometric_matrix"])
number_of_fluxes = find(rxn_list.=="tRNA_charging_M_val_L_c_CAT::16.0*M_val_L_c+16.0*M_atp_c+16.0*tRNA+16.0*M_h2o_c --> 16.0*M_val_L_c_tRNA+16.0*M_amp_c+16.0*M_ppi_c")[1]

t0 = 0
tf = 3
tstep = 0.01
experimental_time = t0:tstep:tf
length_time = length(experimental_time)
data_dictionary["tstep"] = tstep

# this determines which parameters are fitted to data
dataset_index = [15; 18; 22; 24; 27; 29; 31; 34; 38; 41; 42; 59; 60; 61; 66; 69; 70; 76; 79; 83; 84; 86; 88; 89; 106; 110; 114; 121; 123; 126; 128; 130; 132; 133; 134; 135; 140]
non_dataset_index = collect(1:n_species)
deleteat!(non_dataset_index,dataset_index)

species_list = data_dictionary["list_of_metabolite_symbols"]
species_list_dataset = species_list[dataset_index]
species_list_non_dataset = species_list[non_dataset_index]

species_constraint_index = copy(dataset_index)

plot_color = "black"

num_constraint_sets = 1#size(species_constraint_array,1)

# not currently used
parameter = ones(202,2)
parameter[:,1] = parameter[:,1]*110
parameter[:,2] = parameter[:,2]*0.03

initial_conditions = data_dictionary["initial_conditions"]

# set up initial conditions (use kinetic model data)
time_state_array = zeros(n_species,length_time)
time_state_array[:,1] = initial_conditions
time_flux_array = zeros(n_rxn,length_time)
FBA = data_dictionary["default_flux_bounds_array"]
SBA = data_dictionary["species_bounds_array"]

EXIT_FLAG = zeros(length_time-1,num_constraint_sets)

overall_state_array = zeros()
# loop for all constraint subsets
FVA_run = true

constraint_index_array = collect(1:num_constraint_sets)

Exit_flag = Int64[]
uptake_array = 0
flux_array = 0

if !isdir("Ens")
	mkdir("Ens")
end
if !isdir("Ens/Best")
	mkdir("Ens/Best")
end

syn_data = Dict{AbstractString,Any}()

fva_time_array = collect(0.0:0.25:3.0)

fluxes_to_optimize = [[2 3], [23 24], 26, 37, [73 74], 21, 75]

species_to_vary = readdlm("species_to_vary")
fixed_indices = Int64[]
for i in 1:length(species_list)
	if !in(species_list[i],species_to_vary)
		push!(fixed_indices,i)
	end
end

species_boolean = zeros(size(species_list))
for i in 1:length(species_list)
	species_boolean[i] = in(i,dataset_index)
end

switch_prob = .25

if ~isfile("Ens/num_dir")
    writedlm("Ens/num_dir",0)
end

alpha = 1
num_to_reset = 50

if isfile("Ens/Best/error")
	error_best = readdlm("Ens/Best/error")[1]
	species_boolean = readdlm("Ens/Best/species_boolean")
else
	error_best = readdlm("error_best")[1]
end
species_boolean_best = copy(species_boolean)
error = copy(error_best)

error_bc = 3608115.9638814344
flux_uncertainty_bc = 4000 # based on branch points only 20932.739199515625

num_dir = convert(Int64,readdlm("Ens/num_dir")[1]) # Number of pre-existing directories

num_iter = 10000
for i = num_dir+1:num_dir+num_iter
	# Reset periodically
	if rem(i,num_to_reset) == 0
		species_boolean_new = copy(species_boolean_best)
	else
		species_boolean_new = copy(species_boolean)
	end

	# Perturb constraint array
	for j in 1:length(species_boolean_new)
#		if !in(j,fixed_indices)
			if rand() < switch_prob
				species_boolean_new[j] = 1-species_boolean_new[j]
			end
#		end
	end

	species_boolean_new[fixed_indices] = 0

	num_diff = Int64(sum(abs(species_boolean_new-species_boolean)))

	# discretized dfba
	state_array = deepcopy(initial_conditions)
	index = 1

	species_constraint_index_new = find(species_boolean_new)

	syn_data_upper = zeros(n_species,length_time)
	syn_data_lower = zeros(n_species,length_time)
	syn_mean = zeros(n_species,length_time)
	for syn_index in species_constraint_index_new
		itp = interpolate((experimental_time_exp,),Mean[metabolite_list[syn_index]],Gridded(Linear()))
		Mean_itp = itp[experimental_time]
		itp = interpolate((experimental_time_exp,),Lower[metabolite_list[syn_index]],Gridded(Linear()))
		Lower_itp = itp[experimental_time]
		itp = interpolate((experimental_time_exp,),Upper[metabolite_list[syn_index]],Gridded(Linear()))
		Upper_itp = itp[experimental_time]
		syn_data_upper[syn_index,:] = Upper_itp
		syn_data_lower[syn_index,:] = Lower_itp
		syn_mean[syn_index,:] = Mean_itp
	end
	syn_data["mean"] = syn_mean
	syn_data["upper"] = syn_data_upper
	syn_data["lower"] = syn_data_lower

	Exit_flag = Int64[]
	uptake_array = 0
	flux_array = 0

	FVA_min = Dict()
	FVA_max = Dict()
	Percentage_failed_tps_min = Dict()
	Percentage_failed_tps_max = Dict()
	for rxn_idx in fluxes_to_optimize
		key = split(rxn_list[rxn_idx[1]],':')[1]
		FVA_min[key] = zeros(length(fva_time_array),191) # Only record 191 fluxes
		FVA_max[key] = zeros(length(fva_time_array),191) # Only record 191 fluxes
	end

	#dfba
	fva_time = experimental_time[1:end-1]
	for j in 1:length(fva_time)
		time_index = fva_time[j]
		# constraint
		data_dictionary["objective_coefficient_array"] = zeros(number_of_fluxes)
		data_dictionary["objective_coefficient_array"][171] = -1
		data_dictionary = Bounds(data_dictionary,TXTL_dictionary,state_array)
		species_constraints = calculate_constraints(state_array, parameter, species_constraint_index_new, syn_data, index, tstep)
		data_dictionary["species_bounds_array"] = species_constraints
		#  data_dictionary["default_flux_bounds_array"] = flux_constraints
		data_dictionary["state_array"] = state_array

		# solve the lp problem -
		(objective_value, flux_array, dual_array, uptake_array, exit_flag) = FluxDriver(data_dictionary)
		push!(Exit_flag,exit_flag)

		# mass balance for concentration
		state_array = state_array + (uptake_array * tstep) # cell free mass balanace
		state_array[101] = initial_conditions[101]
		Z0 = objective_value
		if FVA_run && in(time_index,fva_time_array)
			for rxn_idx in fluxes_to_optimize
				key = split(rxn_list[rxn_idx[1]],':')[1]
				if length(rxn_idx) == 1
					#constraint objective flux to value determined
					data_dictionary["default_flux_bounds_array"][171] = Z0

					# minimize flux
					data_dictionary["objective_coefficient_array"] = zeros(number_of_fluxes)
					data_dictionary["objective_coefficient_array"][rxn_idx] = 1
					(objective_value, flux_array, dual_array, uptake_array, exit_flag) = FluxDriver(data_dictionary)
					FVA_min[key][Int64(time_index*4+1),:] = flux_array[1:191]

					# maximize flux
					data_dictionary["objective_coefficient_array"] = zeros(number_of_fluxes)
					data_dictionary["objective_coefficient_array"][rxn_idx] = -1
					(objective_value, flux_array, dual_array, uptake_array, exit_flag) = FluxDriver(data_dictionary)
					FVA_max[key][Int64(time_index*4+1),:] = flux_array[1:191]
				else
					#constraint objective flux to value determined
					data_dictionary["default_flux_bounds_array"][171] = Z0

					# maximize reverse flux
					data_dictionary["objective_coefficient_array"] = zeros(number_of_fluxes)
					data_dictionary["objective_coefficient_array"][rxn_idx[2]] = -1
					(objective_value, flux_array, dual_array, uptake_array, exit_flag) = FluxDriver(data_dictionary)
					FVA_min[key][Int64(time_index*4+1),:] = flux_array[1:191]

					# maximize forward flux
					data_dictionary["objective_coefficient_array"] = zeros(number_of_fluxes)
					data_dictionary["objective_coefficient_array"][rxn_idx[1]] = -1
					(objective_value, flux_array, dual_array, uptake_array, exit_flag) = FluxDriver(data_dictionary)
					FVA_max[key][Int64(time_index*4+1),:] = flux_array[1:191]
				end
			end
		end # if FVA_run

		index = index + 1
		time_state_array[:,index] = state_array
		time_flux_array[:,index] = flux_array

	end # for time_index in [t0:tstep:tf-tstep;]

	if sum(5-Exit_flag) == 0
		error_vector = CalcError(Upper,Lower,Mean,experimental_time,time_state_array,data_dictionary)

		FVA_MIN = zeros(length(fva_time_array),length(fluxes_to_optimize))
		for j in 1:length(fluxes_to_optimize)
			rxn_idx = fluxes_to_optimize[j]
			key = split(rxn_list[rxn_idx[1]],':')[1]
			if length(rxn_idx) == 1
				FVA_MIN[:,j] = FVA_min[key][:,rxn_idx]
			else
				FVA_MIN[:,j] = FVA_min[key][:,rxn_idx[1]]-FVA_min[key][:,rxn_idx[2]]
			end
		end
		FVA_MAX = zeros(length(fva_time_array),length(fluxes_to_optimize))
		for j in 1:length(fluxes_to_optimize)
			rxn_idx = fluxes_to_optimize[j]
			key = split(rxn_list[rxn_idx[1]],':')[1]
			if length(rxn_idx) == 1
				FVA_MAX[:,j] = FVA_max[key][:,rxn_idx]
			else
				FVA_MAX[:,j] = FVA_max[key][:,rxn_idx[1]]-FVA_max[key][:,rxn_idx[2]]
			end
		end
		flux_uncertainty = norm(FVA_MAX-FVA_MIN)

		error_new = sum(error_vector)*flux_uncertainty
#		error_new = length(species_constraint_index_new)

		if sum(error_vector) > error_bc
			error_new *= 1e9
		end
		if flux_uncertainty > flux_uncertainty_bc
			error_new *= 1e9
		end
		if sum(species_boolean_new) == 0
			error_new *= 1e9
		end

		acc_prob = exp(alpha*(error-error_new)/error)

		error_round = round(error/10^floor(log(10,error)),3)*10^floor(log(10,error))
		error_new_round = round(error_new/10^floor(log(10,error_new)),3)*10^floor(log(10,error_new))
		error_best_round = round(error_best/10^floor(log(10,error_best)),3)*10^floor(log(10,error_best))
		acc_prob_round = round(acc_prob,2)

		# If a new best is achieved, overwrite parameters and cost and save the relevant information to Best directory
		if error_new < error_best
		    # Save to Best directory
		    writedlm("Ens/Best/species_boolean",species_boolean_new)
		    writedlm("Ens/Best/time_state_array",time_state_array)
		    writedlm("Ens/Best/exit_flag",Exit_flag)
		    writedlm("Ens/Best/state_error",sum(error_vector))
		    writedlm("Ens/Best/flux_uncertainty",flux_uncertainty)
		    writedlm("Ens/Best/error",error_new)
		    species_boolean_best = copy(species_boolean_new)
		    error_best = copy(error_new)
		    println("$i: error_new = $error_new_round, error = $error_round, best = $error_best_round, acc_prob = $acc_prob_round, num_diff = $num_diff, NEW BEST")
		else
		    println("$i: error_new = $error_new_round, error = $error_round, best = $error_best_round, acc_prob = $acc_prob_round, num_diff = $num_diff")
		end
		# If new cost is lower than previous cost, choose new params as reference point for perturbation
		if rand() < acc_prob # Accept all better sets and some worse sets
		    # Create directory for next sample and save the relevant information
		    num_dir += 1
			writedlm("Ens/num_dir",i) # Record new number of directories
		    mkdir("Ens/$num_dir")
		    writedlm("Ens/$num_dir/species_boolean",species_boolean_new)
		    writedlm("Ens/$num_dir/time_state_array",time_state_array)
		    writedlm("Ens/$num_dir/exit_flag",Exit_flag)
		    writedlm("Ens/$num_dir/state_error",sum(error_vector))
		    writedlm("Ens/$num_dir/flux_uncertainty",flux_uncertainty)
		    writedlm("Ens/$num_dir/error",error_new)
		    species_boolean = copy(species_boolean_new)
		    error = copy(error_new)
			for rxn_idx in fluxes_to_optimize
				key = split(rxn_list[rxn_idx[1]],':')[1]
				mkdir("Ens/$num_dir/$key")
				writedlm("Ens/$num_dir/$key/FVA_min",FVA_min[key])
				writedlm("Ens/$num_dir/$key/FVA_max",FVA_max[key])
			end
		end
	end
end


# Each base as 'one hot' nucleotide encoding
#
# A   = 0b0001
# C   = 0b0010
# G   = 0b0100
# U   = 0b1000
# Gap = 0b0000

# Pairs encoded as adjacent sets of 'one hot' values
# so AA would be 0b00010001, and AU  0b00011000 and so forth.

abstract type NucleotidePair end

primitive type RNAPair<:NucleotidePair 8 end

const AU_PAIR = reinterpret(RNAPair, 0b00011000)
const UA_PAIR = reinterpret(RNAPair, 0b10000001)
const CG_PAIR = reinterpret(RNAPair, 0b00100100)
const GC_PAIR = reinterpret(RNAPair, 0b01000010)
const GU_PAIR = reinterpret(RNAPair, 0b01001000)
const UG_PAIR = reinterpret(RNAPair, 0b10000100)

primitive type RNAMismatch<:NucleotidePair 8 end

const AA_MISMATCH = reinterpret(RNAMismatch, 0b00010001)
const AC_MISMATCH = reinterpret(RNAMismatch, 0b00010010)
const AG_MISMATCH = reinterpret(RNAMismatch, 0b00010100)
const CA_MISMATCH = reinterpret(RNAMismatch, 0b00100001)
const CC_MISMATCH = reinterpret(RNAMismatch, 0b00100010)
const CU_MISMATCH = reinterpret(RNAMismatch, 0b00101000)
const GA_MISMATCH = reinterpret(RNAMismatch, 0b01000001)
const GG_MISMATCH = reinterpret(RNAMismatch, 0b01000100)
const UC_MISMATCH = reinterpret(RNAMismatch, 0b10000010)
const UU_MISMATCH = reinterpret(RNAMismatch, 0b10001000)

primitive type RNABulge<:NucleotidePair 8 end

const AB_BULGE = reinterpret(RNABulge, 0b00010000)
const CB_BULGE = reinterpret(RNABulge, 0b00100000)
const GB_BULGE = reinterpret(RNABulge, 0b01000000)
const UB_BULGE = reinterpret(RNABulge, 0b10000000)

const BA_BULGE = reinterpret(RNABulge, 0b00000001)
const BC_BULGE = reinterpret(RNABulge, 0b00000010)
const BG_BULGE = reinterpret(RNABulge, 0b00000100)
const BU_BULGE = reinterpret(RNABulge, 0b00001000)

primitive type InvalidPair<:NucleotidePair 8 end

const XX_INVALID = reinterpret(InvalidPair, 0b11111111)

const XA_INVALID = reinterpret(InvalidPair, 0b11110001)
const XC_INVALID = reinterpret(InvalidPair, 0b11110010)
const XG_INVALID = reinterpret(InvalidPair, 0b11110100)
const XU_INVALID = reinterpret(InvalidPair, 0b11111000)

const AX_INVALID = reinterpret(InvalidPair, 0b00011111)
const CX_INVALID = reinterpret(InvalidPair, 0b00101111)
const GX_INVALID = reinterpret(InvalidPair, 0b01001111)
const UX_INVALID = reinterpret(InvalidPair, 0b10001111)

primitive type InvalidBulge<:NucleotidePair 8 end

const BX_INVALID = reinterpret(InvalidBulge, 0b00001111)
const XB_INVALID = reinterpret(InvalidBulge, 0b11110000)

const BULGES = [AB_BULGE, CB_BULGE,
                GB_BULGE, UB_BULGE,
                BA_BULGE, BC_BULGE,
                BG_BULGE, BU_BULGE]

const MISMATCHES = [AA_MISMATCH, AC_MISMATCH,
                    AG_MISMATCH, CA_MISMATCH,
                    CC_MISMATCH, CU_MISMATCH,
                    GA_MISMATCH, GG_MISMATCH,
                    UC_MISMATCH, UU_MISMATCH]

const PAIRS = [AU_PAIR, UA_PAIR,
               CG_PAIR, GC_PAIR,
               GU_PAIR, UG_PAIR]

const INVALIDS = [XA_INVALID, XC_INVALID,
                  XG_INVALID, XU_INVALID,
                  AX_INVALID, CX_INVALID,
                  GX_INVALID, UX_INVALID,
                  XX_INVALID]

Base.convert{NP <: NucleotidePair}(::Type{NP}, x::UInt8) = reinterpret(NP, x)
Base.convert(::Type{UInt8}, x::NucleotidePair)           = reinterpret(UInt8, x)

function Base.convert{NP <: NucleotidePair, X <: NucleicAcid, Y <: NucleicAcid}(::Type{NP}, first::X,
                                                                                last::Y)
   enc  = reinterpret(UInt8, first) << 4
   enc |= reinterpret(UInt8, last)
   reinterpret(NP, enc)
end

function Base.convert{NP <: NucleotidePair}(::Type{NP}, first::UInt8, last::UInt8 )
   enc  = first << 4
   enc |= last
   reinterpret(NP, enc)
end

# convert two adjacent 'one hot' nucleotide 4-bit codes
# into two adjacent 2-bit encoding 0b00 = A, 0b01 = C, 0b10 = G, 0b11 = U
# for both left and right nucleotides, so 0b00010001 becomes 0b0000
# and 0b10001000 becomes 0b1111, then add 1 to bring to 1:16 range
function index(x::NucleotidePair)
   idx   = trailing_zeros(convert(UInt8, x) >> 4) << 2
   right = trailing_zeros(convert(UInt8, x) & 0x0F)
   (idx | right) + 1
end

function index(x::RNABulge)
   trailing_zeros(reinterpret(UInt8, x)) + 1
end

isgap{N <: NucleicAcid}( nuc::N ) = reinterpret(UInt8, nuc) == 0 ? true : false

isbulge(x::RNAPair)     = false
isbulge(x::RNAMismatch) = false
isbulge(x::RNABulge)    = true

ispair(x::RNAPair)     = true
ispair(x::RNAMismatch) = false
ispair(x::RNABulge)    = false

ismismatch(x::RNAPair)     = false
ismismatch(x::RNAMismatch) = true
ismismatch(x::RNABulge)    = false

function type_cnt{NP <: NucleotidePair}( v::Vector{NP}, func::Function )
   cnt = 0
   @inbounds for i in 1:length(v)
      func(v[i]) && (cnt += 1)
   end
   cnt
end

npairs{NP <: NucleotidePair}( v::Vector{NP} )      = type_cnt( v, ispair )
nmismatches{NP <: NucleotidePair}( v::Vector{NP} ) = type_cnt( v, ismismatch )
nbulges{NP <: NucleotidePair}( v::Vector{NP} )     = type_cnt( v, isbulge )

isfiveprime(x::RNABulge) = reinterpret(UInt8, x) & 0x0F == 0 ? true : false

isbulgefive(x::RNABulge)    = isfiveprime(x)
isbulgefive(x::RNAPair)     = false
isbulgefive(x::RNAMismatch) = false

isbulgethree(x::RNABulge)    = !isfiveprime(x)
isbulgethree(x::RNAPair)     = false
isbulgethree(x::RNAMismatch) = false

function flip{NP <: NucleotidePair}(x::NP)
   left   = convert(UInt8, x) >> 4
   retval = (convert(UInt8, x) & 0x0F) << 4
   reinterpret( NP, retval | left )
end

function Base.split(x::NucleotidePair)
   left  = trailing_zeros(convert(UInt8, x) >> 4) + 1
   right = trailing_zeros(convert(UInt8, x) & 0x0F) + 1
   (left, right)
end

function Base.split(x::RNABulge)
   x_int = reinterpret(UInt8, x)
   isfiveprime(x) ? trailing_zeros(x_int >> 4) + 1 : trailing_zeros(x_int) + 1
end

function Base.split(x::InvalidPair)
   left  = convert(UInt8, x) >> 4
   lret  = left == 15 ? 15 : trailing_zeros(left) + 1
   right = convert(UInt8, x) & 0x0F
   rret  = right == 15 ? 15 : trailing_zeros(right) + 1
   (lret, rret)
end

# this function returns the first, or last if flag is true
# five prime or three prime indexes (1:4) in a path of NucleotidePairs
function five_three{NP <: NucleotidePair}( path::Vector{NP}, range::UnitRange;
                                           last::Bool=false )
   five  = 0
   three = 0
   for i in (last ? reverse(range) : range)
      if isa( path[i], RNABulge )
         if isfiveprime( path[i] )
            five = split( path[i] )
         else
            three = split( path[i] )
         end
      else
         x,y   = split( path[i] )
         five  = five > 0 ? five : x
         three = three > 0 ? three : y
      end
      if five > 0 && three > 0
         break
      end
   end
   five, three
end

function is_purine_pyrimidine(x::RNAPair)
   if x == AU_PAIR || x == GC_PAIR || x == GU_PAIR
      return true
   else
      return false
   end
end

const PAIR_DICT = Dict{Tuple{Int,Int},NucleotidePair}()

for i in [PAIRS; MISMATCHES; INVALIDS]
   PAIR_DICT[split(i)] = i
end
for i in BULGES
   if isfiveprime(i)
      PAIR_DICT[(0,split(i))] = i
   else
      PAIR_DICT[(split(i),0)] = i
   end
end


############################################################################################
"""
Representation of a trajectory, ie a Hamiltonian with a discrete integrator that
also checks for divergence.
"""
struct TrajectoryNUTS{S<:Hamiltonian,F,C}
    "Hamiltonian"
    H::S
    "Log density of negative log energy at initial point."
    ℓH₀::F
    "Stepsize for leapfrog."
    ϵ::F
    "Maximum tree depth."
    max_depth::Int64
    "Smallest decrease allowed in the log density."
    min_Δ::Float64
    "Turn statistic configuration."
    turn_statistic_configuration::C
    function TrajectoryNUTS(
        H::S,
        ℓH₀::F,
        ϵ::F,
        max_depth=DEFAULT_MAX_TREE_DEPTH,
        min_Δ=-1000.0,
        turn_statistic_configuration=Val{:generalized}(),
    ) where {S<:Hamiltonian,F<:AbstractFloat}
        @argcheck 0 < max_depth ≤ MAX_DIRECTIONS_DEPTH
        @argcheck min_Δ < 0
        C = typeof(turn_statistic_configuration)
        return new{S,F,C}(H, ℓH₀, ϵ, max_depth, min_Δ, turn_statistic_configuration)
    end
end

"""
$(SIGNATURES)
Move along the trajectory in the specified direction. Return the new position.

# Examples
```julia
```

"""
function move(trajectory::T, phasepoint::PhasePoint, fwd::Bool) where {T<:TrajectoryNUTS}
    @unpack H, ϵ = trajectory
    return leapfrog(H, phasepoint, fwd ? ϵ : -ϵ)
end

############################################################################################
# Proposal
"""
$(SIGNATURES)
Given (relative) log probabilities `ω₁` and `ω₂`, return the log probabiliy of drawing a sample from the second (`logprob2`).
When `bias`, biases towards the second argument, introducing anti-correlations.

# Examples
```julia
```

"""
function biased_progressive_logprob2(
    bias::Bool, ω₁::T, ω₂::T, ω=BaytesCore.logaddexp(ω₁, ω₂)
) where {T<:Real}
    return ω₂ - (bias ? ω₁ : ω)
end

"""
$(SIGNATURES)
Random boolean which is `true` with the given probability `exp(logprob)`, which can be `≥ 1`
in which case no random value is drawn.

# Examples
```julia
```

"""
function rand_bool_logprob(_rng::Random.AbstractRNG, logprob::T) where {T<:Real}
    return logprob ≥ 0 || (randexp(_rng, T) > -logprob)
end

"""
$(SIGNATURES)
Calculate the log probability if selecting the subtree corresponding to `ω₂`. Being the log
of a probability, it is always `≤ 0`, but implementations are allowed to return and accept
values `> 0` and treat them as `0`. When `is_doubling`, the tree corresponding to `ω₂` was obtained from a doubling step (this
can be relevant eg for biased progressive sampling). The value `ω = logaddexp(ω₁, ω₂)` is provided for avoiding redundant calculations.
See [`biased_progressive_logprob2`](@ref) for an implementation.

# Examples
```julia
```

"""
function calculate_logprob2(::TrajectoryNUTS, is_doubling, ω₁, ω₂, ω)
    return biased_progressive_logprob2(is_doubling, ω₁, ω₂, ω)
end

"""
$(SIGNATURES)
Combine two proposals `ζ₁, ζ₂` on `trajectory`, with log probability `logprob2` for
selecting `ζ₂`.
 `ζ₁` is before `ζ₂` iff `is_forward`.

# Examples
```julia
```

"""
function combine_proposals(
    _rng::Random.AbstractRNG,
    trajectory::TrajectoryNUTS,
    z₁::PhasePoint,
    z₂::PhasePoint,
    logprob2::T,
    is_forward,
) where {T<:Real}
    return rand_bool_logprob(_rng, logprob2) ? z₂ : z₁
end

############################################################################################
# statistics for visited nodes
struct AcceptanceStatistic{F<:AbstractFloat}
    "
    Logarithm of the sum of metropolis acceptances probabilities over the whole trajectory
    (including invalid parts).
    "
    log_sum_α::F
    "Total number of leapfrog steps."
    steps::Int64
end

function combine_acceptance_statistics(A::AcceptanceStatistic, B::AcceptanceStatistic)
    return AcceptanceStatistic(
        BaytesCore.logaddexp(A.log_sum_α, B.log_sum_α), A.steps + B.steps
    )
end

"""
$(SIGNATURES)
Acceptance statistic for a leaf. The initial leaf is considered not to be visited.

# Examples
```julia
```

"""
function leaf_acceptance_statistic(Δ, is_initial)
    return if is_initial
        AcceptanceStatistic(oftype(Δ, -Inf), 0)
    else
        AcceptanceStatistic(min(Δ, 0), 1)
    end
end

"""
$(SIGNATURES)
Return the acceptance rate (a `Real` betwen `0` and `1`).

# Examples
```julia
```

"""
acceptance_rate(A::AcceptanceStatistic) = min(exp(A.log_sum_α) / A.steps, 1)

"""
$(SIGNATURES)
Combine visited node statistics for adjacent trees trajectory. Implementation should be
invariant to the ordering of `v₁` and `v₂` (ie the operation is commutative).

# Examples
```julia
```

"""
combine_visited_statistics(::TrajectoryNUTS, v, w) = combine_acceptance_statistics(v, w)

############################################################################################
# abstract trajectory interface
"""
$(TYPEDEF)
Information about an invalid (sub)tree, using positions relative to the starting node.
1. When `left < right`, this tree was *turning*.
2. When `left == right`, this is a *divergent* node.
3. `left == 1 && right == 0` is used as a sentinel value for reaching maximum depth without
encountering any invalid trees (see [`REACHED_MAX_DEPTH`](@ref). All other `left > right`
values are disallowed.

# Fields
$(TYPEDFIELDS)
"""
struct InvalidTree
    left::Int64
    right::Int64
end
InvalidTree(i::Integer) = InvalidTree(i, i)
is_divergent(invalid_tree::InvalidTree) = invalid_tree.left == invalid_tree.right

############################################################################################
# Directions

"""
$(TYPEDEF)
Internal type implementing random directions.

# Fields
$(TYPEDFIELDS)
"""
struct Directions
    flags::UInt32
end

Base.rand(_rng::Random.AbstractRNG, ::Type{Directions}) = Directions(rand(_rng, UInt32))

"""
$(SIGNATURES)
Return the next direction flag and the new state of directions. Results are undefined for
more than [`MAX_DIRECTIONS_DEPTH`](@ref) updates.

# Examples
```julia
```

"""
function next_direction(directions::Directions)
    @unpack flags = directions
    return Bool(flags & 0x01), Directions(flags >>> 1)
end

"Sentinel value for reaching maximum depth."
const REACHED_MAX_DEPTH = InvalidTree(1, 0)

################################################################################
# turn analysis

"""
$(TYPEDEF)
Statistics for the identification of turning points. See Betancourt (2017, appendix), and
subsequent discussion of improvements at
<https://discourse.mc-stan.org/t/nuts-misses-u-turns-runs-in-circles-until-max-treedepth/9727/>.
Momenta `p₋` and `p₊` are kept so that they can be added to `ρ` when combining turn
statistics.
Turn detection is always done by [`combine_turn_statistics`](@ref), which returns `nothing`
in case of turning. A `GeneralizedTurnStatistic` should always correspond to a trajectory
that is *not* turning (or a leaf node, where the concept does not apply).

# Fields
$(TYPEDFIELDS)
"""
struct GeneralizedTurnStatistic{T}
    "momentum at the left edge of the trajectory"
    ρ₋::T
    "p♯ at the left edge of the trajectory"
    ρ♯₋::T
    "momentum at the right edge of the trajectory"
    ρ₊::T
    "p♯ at the right edge of the trajectory"
    ρ♯₊::T
    "sum of momenta along trajectory"
    ρₛ::T
end

function leaf_turn_statistic(::Val{:generalized}, H::Hamiltonian, phasepoint::PhasePoint)
    @unpack ρ = phasepoint
    ρ♯ = calculate_ρ♯(H, phasepoint)
    return GeneralizedTurnStatistic(ρ, ρ♯, ρ, ρ♯, ρ)
end

"""
Internal test for turning. See Betancourt (2017, appendix).
"""
_is_turning(ρ♯₋, ρ♯₊, ρ) = dot(ρ♯₋, ρ) < 0 || dot(ρ♯₊, ρ) < 0

"""
$(SIGNATURES)
Combine turn statistics on trajectory. Implementation can assume that the trees that
correspond to the turn statistics have the same ordering.
When
```julia
τ = combine_turn_statistics(trajectory, τ₁, τ₂)
is_turning(trajectory, τ)
```
the combined turn statistic `τ` is guaranteed not to escape the caller, so it can eg change
type.

# Examples
```julia
```

"""
function combine_turn_statistics(
    trajectory::TrajectoryNUTS, x::GeneralizedTurnStatistic, y::GeneralizedTurnStatistic
)
    _is_turning(x.ρ♯₋, y.ρ♯₋, x.ρₛ + y.ρ₋) && return nothing
    _is_turning(x.ρ♯₊, y.ρ♯₊, x.ρ₊ + y.ρₛ) && return nothing
    ρₛᵤₘ = x.ρₛ + y.ρₛ
    #!NOTE: Most left and Right Momenta
    _is_turning(x.ρ♯₋, y.ρ♯₊, ρₛᵤₘ) && return nothing
    return GeneralizedTurnStatistic(x.ρ₋, x.ρ♯₋, y.ρ₊, y.ρ♯₊, ρₛᵤₘ)
end

"""
Internal test for turning. See Betancourt (2017, appendix).
"""
is_turning(::TrajectoryNUTS, ::GeneralizedTurnStatistic) = false
is_turning(::TrajectoryNUTS, ::Nothing) = true

############################################################################################
# utilities

"""
$(SIGNATURES)
Combine turn statistics with the given direction. When `is_forward`, `τ₁` is before `τ₂`,
otherwise after. Internal helper function.

# Examples
```julia
```

"""
@inline function combine_turn_statistics_in_direction(
    trajectory::TrajectoryNUTS,
    τ₁::GeneralizedTurnStatistic,
    τ₂::GeneralizedTurnStatistic,
    is_forward::Bool,
)
    if is_forward
        combine_turn_statistics(trajectory, τ₁, τ₂)
    else
        combine_turn_statistics(trajectory, τ₂, τ₁)
    end
end

function combine_proposals_and_logweights(
    _rng::Random.AbstractRNG,
    trajectory::TrajectoryNUTS,
    ζ₁::PhasePoint,
    ζ₂::PhasePoint,
    ω₁::R,
    ω₂::R,
    is_forward::Bool,
    is_doubling::Bool,
) where {R<:Real}
    ω = BaytesCore.logaddexp(ω₁, ω₂)
    logprob2 = calculate_logprob2(trajectory, is_doubling, ω₁, ω₂, ω)
    ζ = combine_proposals(_rng, trajectory, ζ₁, ζ₂, logprob2, is_forward)
    return ζ, ω
end

############################################################################################
# leafs

"""
$(SIGNATURES)
Information for a tree made of a single node. When `is_initial == true`, this is the first
node.
The first value is either
1. `nothing` for a divergent node,
2. a tuple containing the proposal `ζ`, the log weight (probability) of the node `ω`, the
turn statistics `τ` (never tested with `is_turning` for leaf nodes).
The second value is the visited node information.

# Examples
```julia
```

"""
function leaf(trajectory::TrajectoryNUTS, phasepoint::PhasePoint, is_initial)
    @unpack H, ℓH₀, min_Δ, turn_statistic_configuration = trajectory
    Δ = is_initial ? zero(ℓH₀) : ℓdensity(trajectory.H, phasepoint) - ℓH₀
    isdiv = Δ < min_Δ
    v = leaf_acceptance_statistic(Δ, is_initial)
    if isdiv
        nothing, v
    else
        τ = leaf_turn_statistic(turn_statistic_configuration, H, phasepoint)
        (phasepoint, Δ, τ), v
    end
end

############################################################################################
"""
$(SIGNATURES)
Traverse the tree of given `depth` adjacent to point `z` in `trajectory`.
`is_forward` specifies the direction, `rng` is used for random numbers in
[`combine_proposals`](@ref). `i` is an integer position relative to the initial node (`0`).
The *first value* is either
1. an `InvalidTree`, indicating the first divergent node or turning subtree that was
encounteted and invalidated this tree.
2. a tuple of `(ζ, ω, τ, z′, i′), with
    - `ζ`: the proposal from the tree.
    - `ω`: the log weight of the subtree that corresponds to the proposal
    - `τ`: turn statistics
    - `z′`: the last node of the tree
    - `i′`: the position of the last node relative to the initial node.
The *second value* is always the visited node statistic.

# Examples
```julia
```

"""
function adjacent_tree(
    _rng::Random.AbstractRNG,
    trajectory::TrajectoryNUTS,
    z::PhasePoint,
    i::Int64,
    depth::Int64,
    is_forward::Bool,
)
    i′ = i + (is_forward ? 1 : -1)
    if depth == 0
        z′ = move(trajectory, z, is_forward)
        ζωτ, v = leaf(trajectory, z′, false)
        if ζωτ ≡ nothing
            InvalidTree(i′), v
        else
            (ζωτ..., z′, i′), v
        end
    else
        ## “left” tree
        t₋, v₋ = adjacent_tree(_rng, trajectory, z, i, depth - 1, is_forward)
        t₋ isa InvalidTree && return t₋, v₋
        ζ₋, ω₋, τ₋, z₋, i₋ = t₋

        ## “right” tree — visited information from left is kept even if invalid
        t₊, v₊ = adjacent_tree(_rng, trajectory, z₋, i₋, depth - 1, is_forward)
        v = combine_visited_statistics(trajectory, v₋, v₊)
        t₊ isa InvalidTree && return t₊, v
        ζ₊, ω₊, τ₊, z₊, i₊ = t₊

        ## turning invalidates
        τ = combine_turn_statistics_in_direction(trajectory, τ₋, τ₊, is_forward)
        is_turning(trajectory, τ) && return InvalidTree(i′, i₊), v

        ## valid subtree, combine proposals
        ζ, ω = combine_proposals_and_logweights(
            _rng, trajectory, ζ₋, ζ₊, ω₋, ω₊, is_forward, false
        )
        (ζ, ω, τ, z₊, i₊), v
    end
end

"""
$(SIGNATURES)
Sample a `trajectory` starting at `z`, up to `max_depth`. `directions` determines the tree
expansion directions.
Return the following values
- `ζ`: proposal from the tree
- `v`: visited node statistics
- `termination`: an `InvalidTree` (this includes the last doubling step turning, which is
  technically a valid tree) or `REACHED_MAX_DEPTH` when all subtrees were valid and no
  turning happens.
- `depth`: the depth of the tree that was sampled from. Doubling steps that lead to an
  invalid adjacent tree do not contribute to `depth`.

# Examples
```julia
```

"""
function sample_trajectory(
    _rng::Random.AbstractRNG,
    trajectory::TrajectoryNUTS,
    z::PhasePoint,
    directions::Directions,
)
    @unpack max_depth = trajectory
    @argcheck max_depth ≤ MAX_DIRECTIONS_DEPTH
    (ζ, ω, τ), v = leaf(trajectory, z, true)
    z₋ = z₊ = z
    depth = 0
    termination = REACHED_MAX_DEPTH
    i₋ = i₊ = 0
    while depth < max_depth
        is_forward, directions = next_direction(directions)
        t′, v′ = adjacent_tree(
            _rng, trajectory, is_forward ? z₊ : z₋, is_forward ? i₊ : i₋, depth, is_forward
        )
        v = combine_visited_statistics(trajectory, v, v′)

        ## invalid adjacent tree: stop
        t′ isa InvalidTree && (termination = t′; break)

        ## extract information from adjacent tree
        ζ′, ω′, τ′, z′, i′ = t′

        ## update edges and combine proposals
        if is_forward
            z₊, i₊ = z′, i′
        else
            z₋, i₋ = z′, i′
        end

        ## tree has doubled successfully
        ζ, ω = combine_proposals_and_logweights(
            _rng, trajectory, ζ, ζ′, ω, ω′, is_forward, true
        )
        depth += 1

        ## when the combined tree is turning, stop
        τ = combine_turn_statistics_in_direction(trajectory, τ, τ′, is_forward)
        is_turning(trajectory, τ) && (termination = InvalidTree(i₋, i₊);
        break)
    end
    return ζ, v, termination, depth
end

############################################################################################
#export
export TrajectoryNUTS,
    biased_progressive_logprob2,
    rand_bool_logprob,
    calculate_logprob2,
    combine_proposals,
    AcceptanceStatistic,
    combine_acceptance_statistics,
    leaf_acceptance_statistic,
    acceptance_rate,
    combine_visited_statistics,
    InvalidTree,
    is_divergent,
    Directions,
    next_direction,
    REACHED_MAX_DEPTH,
    GeneralizedTurnStatistic,
    leaf_turn_statistic,
    _is_turning,
    combine_turn_statistics,
    combine_turn_statistics_in_direction,
    combine_proposals_and_logweights,
    leaf,
    adjacent_tree,
    sample_trajectory


function TopocentricFrame(arg0::BodyShape, arg1::GeodeticPoint, arg2::JString)
    return TopocentricFrame((BodyShape, GeodeticPoint, JString), arg0, arg1, arg2)
end

function compute_limit_visibility_point(obj::TopocentricFrame, arg0::jdouble, arg1::jdouble, arg2::jdouble)
    return jcall(obj, "computeLimitVisibilityPoint", GeodeticPoint, (jdouble, jdouble, jdouble), arg0, arg1, arg2)
end

function get_azimuth(obj::TopocentricFrame, arg0::FieldVector3D, arg1::Frame, arg2::FieldAbsoluteDate)
    return jcall(obj, "getAzimuth", RealFieldElement, (FieldVector3D, Frame, FieldAbsoluteDate), arg0, arg1, arg2)
end

function get_azimuth(obj::TopocentricFrame, arg0::Vector3D, arg1::Frame, arg2::AbsoluteDate)
    return jcall(obj, "getAzimuth", jdouble, (Vector3D, Frame, AbsoluteDate), arg0, arg1, arg2)
end

function get_east(obj::TopocentricFrame)
    return jcall(obj, "getEast", Vector3D, ())
end

function get_elevation(obj::TopocentricFrame, arg0::FieldVector3D, arg1::Frame, arg2::FieldAbsoluteDate)
    return jcall(obj, "getElevation", RealFieldElement, (FieldVector3D, Frame, FieldAbsoluteDate), arg0, arg1, arg2)
end

function get_elevation(obj::TopocentricFrame, arg0::Vector3D, arg1::Frame, arg2::AbsoluteDate)
    return jcall(obj, "getElevation", jdouble, (Vector3D, Frame, AbsoluteDate), arg0, arg1, arg2)
end

function get_nadir(obj::TopocentricFrame)
    return jcall(obj, "getNadir", Vector3D, ())
end

function get_north(obj::TopocentricFrame)
    return jcall(obj, "getNorth", Vector3D, ())
end

function get_parent_shape(obj::TopocentricFrame)
    return jcall(obj, "getParentShape", BodyShape, ())
end

function get_point(obj::TopocentricFrame)
    return jcall(obj, "getPoint", GeodeticPoint, ())
end

function get_point(obj::TopocentricFrame, arg0::Field)
    return jcall(obj, "getPoint", FieldGeodeticPoint, (Field,), arg0)
end

function get_pv_coordinates(obj::TopocentricFrame, arg0::AbsoluteDate, arg1::Frame)
    return jcall(obj, "getPVCoordinates", TimeStampedPVCoordinates, (AbsoluteDate, Frame), arg0, arg1)
end

function get_range(obj::TopocentricFrame, arg0::FieldVector3D, arg1::Frame, arg2::FieldAbsoluteDate)
    return jcall(obj, "getRange", RealFieldElement, (FieldVector3D, Frame, FieldAbsoluteDate), arg0, arg1, arg2)
end

function get_range(obj::TopocentricFrame, arg0::Vector3D, arg1::Frame, arg2::AbsoluteDate)
    return jcall(obj, "getRange", jdouble, (Vector3D, Frame, AbsoluteDate), arg0, arg1, arg2)
end

function get_range_rate(obj::TopocentricFrame, arg0::FieldPVCoordinates, arg1::Frame, arg2::FieldAbsoluteDate)
    return jcall(obj, "getRangeRate", RealFieldElement, (FieldPVCoordinates, Frame, FieldAbsoluteDate), arg0, arg1, arg2)
end

function get_range_rate(obj::TopocentricFrame, arg0::PVCoordinates, arg1::Frame, arg2::AbsoluteDate)
    return jcall(obj, "getRangeRate", jdouble, (PVCoordinates, Frame, AbsoluteDate), arg0, arg1, arg2)
end

function get_south(obj::TopocentricFrame)
    return jcall(obj, "getSouth", Vector3D, ())
end

function get_west(obj::TopocentricFrame)
    return jcall(obj, "getWest", Vector3D, ())
end

function get_zenith(obj::TopocentricFrame)
    return jcall(obj, "getZenith", Vector3D, ())
end

function point_at_distance(obj::TopocentricFrame, arg0::jdouble, arg1::jdouble, arg2::jdouble)
    return jcall(obj, "pointAtDistance", GeodeticPoint, (jdouble, jdouble, jdouble), arg0, arg1, arg2)
end


function MDP_fixture()
    dmodel = NoCrashIDMMOBILModel(3, PhysicalParam(3, lane_length=100.0))
    rmodel = NoCrashRewardModel()
    return NoCrashMDP{typeof(rmodel)}(dmodel, rmodel, 1.0)
end
function state_fixture()
    # XXX are we going to git rid of v_min, etc?
    lane_length=100.0
    pp = PhysicalParam(3, lane_length=lane_length)
    return MLState(0.0,0.0, CarState[CarState(lane_length/2.0, 2.0, pp.v_med, 0.0, Multilane.NORMAL,0),
                                CarState(lane_length*3/4, 1.0, pp.v_min, 0.0,
                                         IDMMOBILBehavior("normal", pp.v_min, 0.0, 1),0),
                                CarState(lane_length*1/4, 3.0, pp.v_max, 0.0,
                                         IDMMOBILBehavior("normal", pp.v_max, 0.0, 2),0)])
end

function test_occupation()
    println("\t\tTestng occupation_overlap")
    @test Multilane.occupation_overlap(1.0, 2.0) == false
    @test Multilane.occupation_overlap(1.0, 1.99) == true
    @test Multilane.occupation_overlap(0.9, 2.0) == false
    @test Multilane.occupation_overlap(0.9, 1.1) == true
    @test Multilane.occupation_overlap(0.9, 1.99) == true
end

# not done
# function test_e_brake()
#     mdp = MDP_fixture()
#     pp = mdp.dmodel.phys_param
#     s = MLState(false,0.0,0.0, CarState[CarState(0.0, 2.0, pp.v_med, 0.0, Multilane.NORMAL),
#                                 CarState(pp.l_car+0.1, 2.0, pp.v_min, 0.0,
#                                          BehaviorModel("normal"), pp.v_min, 0.0, 1)])
# end

"""
Test that cars are regenerated when there are some missing
"""
function test_encounter()
    println("\t\tTesting car regeneration for encounters")
    mdp = MDP_fixture()
    mdp.dmodel.p_appear = 1.0
    s = state_fixture()
    deleteat!(s.cars, 2)
    rng = MersenneTwister(2)
    sp, r = generate_sr(mdp, s, MLAction(0.0,0.0), rng)
    @test length(sp.cars) == 3
end

"""
Return a CarState for the ego vehicle with the specified y.
"""
ego_state_spec_y(y) = CarState(0.0, y, 30.0, 0.0, Multilane.NORMAL,1)

function test_snapback()
    println("\t\tTesting snapping back after lane change too far")
    mdp = MDP_fixture()
    rng = MersenneTwister(2)
    mdp.dmodel.phys_param.dt = 0.75
    s = state_fixture()
    s.cars[1] = ego_state_spec_y(2.0)
    a1 = MLAction(0,1.0)
    sp,r = generate_sr(mdp, s, a1, rng)
    @test sp.cars[1].y == 2.0 + 1.0*0.75
    a2 = MLAction(0,2.0)
    sp,r = generate_sr(mdp, s, a2, rng)
    @test sp.cars[1].y == 3.0
    s.cars[1] = ego_state_spec_y(1.99)
    a3 = MLAction(0,1.0)
    sp,r = generate_sr(mdp, s, a3, rng)
    @test sp.cars[1].y == 2.0
    s.cars[1] = ego_state_spec_y(2.01)
    a4 = MLAction(0,-1.0)
    sp,r = generate_sr(mdp, s, a4, rng)
    @test sp.cars[1].y == 2.0
    s.cars[1] = ego_state_spec_y(1.01)
    a5 = MLAction(0.,0.1)
    sp,r = generate_sr(mdp, s, a5, rng)
    @test sp.cars[1].y == 1.01 + 0.1*0.75
end

function test_model()
    println("\tTesting Model...")
    test_occupation()
    test_encounter()
    test_snapback()
end