text:
using FymEnvs
using LinearAlgebra

using Plots
ENV["GKSwstype"]="nul"  # do not show plot
# using Debugger

if !isdefined(Main, :TestEnvs)
    # using Revise; includet("custom_env.jl")  # to avoid conflict
    include("custom_env.jl")  # to avoid conflict
    using .TestEnvs
end

function print_msg(test_name)
    println(">"^6*" "*test_name*" "*"<"^6)
end

function test_Fym()
    print_msg("FymEnvs")
    function set_dyn(env, t)
        # corresponding to `set_dot` of the original fym
        # you can use any names in this package
        sys = env.systems["sys"]
        x = sys.state
        A = Matrix(I, 3, 3)
        sys.dot = -A * x
    end
    function step(env)
        t = time(env.clock)
        sys = env.systems["sys"]
        x = sys.state
        update!(env)
        next_obs = sys.state
        reward = zeros(1)
        done = time_over(env.clock)
        info = Dict(
                    "time" => t,
                    "state" => x,
                   )
        return next_obs, reward, done, info
    end

    x0 = collect(1:3)
    systems = Dict("sys" => BaseSystem(initial_state=x0, name="3d_sys"))
    log_dir = "data/test"
    file_name = "fym.h5"
    logger = Logger(log_dir=log_dir, file_name=file_name)
    env = BaseEnv(max_t=100.00, logger=logger, name="test_env",)
    systems!(env, systems)  # set systems; required
    dyn!(env, set_dyn)  # set dynamics; required
    step!(env, step)  # set step; required

    reset!(env)  # reset env; required before propagation
    obs = observe_flat(env)
    i = 0
    @time while true
        render(env)  # not mendatory; would make simulator slow
        next_obs, reward, done, info = env.step()
        obs = next_obs
        i += 1
        if done
            break
        end
    end
    close!(env)
    data = load(env.logger.path)
    show(env)
    show(size(data["state"]["sys"]))
end

function test_largescale_env()
    print_msg("large-scale envs (nested env)")
    function set_dyn(env, t, p=0.0)
        x = env.systems["sys"].state
        y = env.systems["sys2"].state
        A = Matrix(I, 25, 25)
        B = Matrix(I, 25, 25)
        env.systems["sys"].dot = -p * A * x
        env.systems["sys2"].dot = -B * y
        env.systems["env0"].systems["sys"].dot = -p * A * x
        env.systems["env0"].systems["sys2"].dot = -B * y
    end
    function set_dyn0(env, t, p=0.0)  # extend
        x = env.systems["sys"].state
        y = env.systems["sys2"].state
        A = Matrix(I, 25, 25)
        B = Matrix(I, 25, 25)
        env.systems["sys"].dot = -p * A * x
        env.systems["sys2"].dot = -B * y
    end
    function step(env, action=nothing)
        t = time(env.clock)
        x = env.systems["sys"].state
        y = env.systems["sys2"].state
        env0x = env.systems["env0"].systems["sys"].state
        env0y = env.systems["env0"].systems["sys2"].state
        update!(env)
        next_obs = env.systems["sys"].state
        reward = zeros(1)
        done = time_over(env.clock)
        info = Dict("time" => t, "state" => x, "state2" => y,
                    "env0state" => env0x, "env0state2" => env0y)
        return next_obs, reward, done, info
    end

    x0 = collect(1:25)
    y0 = ones(25)
    systems0 = Dict(
                   "sys" => BaseSystem(initial_state=x0),
                   "sys2" => BaseSystem(initial_state=y0),
                  )
    env0 = BaseEnv(systems=systems0, dyn=set_dyn0, name="env0")
    systems = Dict(
                   "sys" => BaseSystem(initial_state=x0),
                   "sys2" => BaseSystem(initial_state=y0),
                   "env0" => env0,
                  )
    log_dir = "data/test"
    file_name = "largescale.h5"
    logger = Logger(log_dir=log_dir, file_name=file_name)
    env = BaseEnv(max_t=10.00, logger=logger, name="env")
    systems!(env, systems)
    dyn!(env, set_dyn)
    step!(env, step)
    # functions
    reset!(env)  # reset!
    # simulation
    obs = observe_flat(env)
    i = 0
    @time while true
        next_obs, reward, done, info = env.step()
        obs = next_obs
        i += 1
        if done
            break
        end
    end
    close!(env)
    data = load(env.logger.path)
    state = data["state"]["sys"]
    state2 = data["state"]["sys2"]
    env0state = data["state"]["env0"]["sys"]
    env0state2 = data["state"]["env0"]["sys2"]
    t = data["time"]
    p = plot(t, state)
    p2 = plot(t, state2)
    env0p = plot(t, env0state)
    env0p2 = plot(t, env0state2)
    savefig(p, joinpath(log_dir, "state.pdf"))
    savefig(p2, joinpath(log_dir, "state2.pdf"))
    savefig(env0p, joinpath(log_dir, "env0state.pdf"))
    savefig(env0p2, joinpath(log_dir, "env0state2.pdf"))
    show(env)
end

function test_custom_env()
    env = test_env()
    log_dir = "data/test"
    file_name = "custom.h5"
    reset!(env)
    _sample(env, nothing, log_dir, file_name)
    data = load(joinpath(log_dir, file_name))
    for name in ["state", "input"]
        p = plot(data["time"], data[name])
        savefig(p, joinpath(log_dir, "custom_"*name*".pdf"))
    end
end

function _sample(env::BaseEnv, agent, log_dir, file_name)
    logger = Logger(log_dir=log_dir, file_name=file_name)
    obs = observe_flat(env)
    i = 0
    @time while true
        if agent == nothing
            action = nothing
        end
        next_obs, reward, done, info = env.step(action=action)
        record(logger, info)
        obs = next_obs
        i += 1
        if done
            break
        end
    end
    close!(env)
    close!(logger)
end

function test_all()
    test_Fym()
    test_largescale_env()
    test_custom_env()
end

test_all()


text:
module Measurements
    using LinearAlgebra
    using SparseArrays
    import ..LTK_universe:Universe,make_WdagWmatrix,calc_IntegratedFermionAction
    import ..Gaugefields:GaugeFields,set_wing!,substitute!,
            make_staple!,calc_Plaq!,SU3GaugeFields,
            SU2GaugeFields,SU3GaugeFields_1d,SU2GaugeFields_1d,
            GaugeFields_1d,calc_Polyakov,calc_Plaq,calc_Plaq_notrace_1d,SUn,SU2,SU3,TA,add!,
            SUNGaugeFields,SUNGaugeFields_1d,
            Loops,evaluate_loops!,evaluate_loops,
            U1GaugeFields,U1GaugeFields_1d,calc_smearingU
    import ..Fermionfields:clear!,FermionFields,WilsonFermion
    import ..Fermionfields:Z4_distribution_fermi!,gauss_distribution_fermi!,set_wing_fermi!
    import ..CGmethods:bicg
    #import ..CGfermion:cg0!
    import ..System_parameters:Params
    import ..Actions:FermiActionParam_Wilson,FermiActionParam_Staggered,FermiActionParam_WilsonClover
    import ..Diracoperators:Dirac_operator
    import ..Verbose_print:Verbose_level,Verbose_3,Verbose_2,Verbose_1,println_verbose3,println_verbose2,println_verbose1,
            print_verbose1,print_verbose2,print_verbose3
    import ..Smearing:gradientflow!,calc_stout!,calc_fatlink_APE!,calc_stout,calc_fatlink_APE,calc_multihit!
    import ..Wilsonloops:Wilson_loop,Wilson_loop_set
    import ..System_parameters:set_params

    #=
    abstract type MeasureMethod end

    Base.@kwdef mutable struct Pion_correlator <: MeasureMethod
        measure_every::Int64 = 10
        Dirac_operator::String = "Wilson"
    end
    =#

    struct Measurement{Fermi,GaugeP,FermiP,Gauge_temp}
        gparam::GaugeP
        fparam::Union{Nothing,FermiP}

        _temporal_gauge::Array{Gauge_temp,1}
        _temporal_fermi1::Union{Nothing,Array{Fermi,1}}
        _temporal_fermi2::Union{Nothing,Array{Fermi,1}}

        function Measurement(p::Params,univ::Universe)
            if p.Dirac_operator == p.Dirac_operator_measurement
                return Measurement(univ,Dirac_operator=univ.Dirac_operator)
            else
                if p.Dirac_operator_measurement == nothing
                    fparam = nothing
                else
                    if p.Dirac_operator_measurement == "Wilson"
                        fparam = FermiActionParam_Wilson(p.hop_measurement,p.r_measurement,p.eps_measurement,
                                                            p.Dirac_operator_measurement,p.MaxCGstep_measurement,p.quench)
                    elseif p.Dirac_operator_measurement == "Staggered"
                        fparam = FermiActionParam_Staggered(p.mass_measurement,p.eps_measurement,
                                                            p.Dirac_operator_measurement,p.MaxCGstep_measurement,p.quench,p.Nf)
                    end
                end
                return Measurement(univ,fparam;Dirac_operator=p.Dirac_operator_measurement)
            end
        end

        function Measurement(univ::Universe,gparam,fparam;Dirac_operator=nothing)
            NX = univ.NX
            NY = univ.NY
            NZ = univ.NZ
            NT = univ.NT
            NC = univ.NC

            if NC == 3
                U = Array{SU3GaugeFields,1}(undef,4)
                _temporal_gauge = Array{SU3GaugeFields_1d,1}(undef,4)
            elseif NC == 2
                U = Array{SU2GaugeFields,1}(undef,4)
                _temporal_gauge = Array{SU2GaugeFields_1d,1}(undef,4)
            elseif NC ≥ 4
                U = Array{SUNGaugeFields,1}(undef,4)
                _temporal_gauge = Array{SUNGaugeFields_1d,1}(undef,4)
            elseif NC == 1
                U = Array{U1GaugeFields,1}(undef,4)
                _temporal_gauge = Array{U1GaugeFields_1d,1}(undef,4)
            end

            for i=1:length(_temporal_gauge)
                _temporal_gauge[i] = GaugeFields_1d(NC,NX,NY,NZ,NT) #similar(U[1])
            end

            if Dirac_operator == nothing
                _temporal_fermi1 = nothing
                _temporal_fermi2 = nothing
                φ = nothing
            else
                φ = FermionFields(NC,NX,NY,NZ,NT,fparam,univ.BoundaryCondition)

                _temporal_fermi1 = Array{FermionFields,1}(undef,4)
                _temporal_fermi2 = Array{FermionFields,1}(undef,4)
                for i=1:length(_temporal_fermi1)
                    _temporal_fermi1[i] = similar(φ)
                end
                for i=1:length(_temporal_fermi2)
                    _temporal_fermi2[i] = similar(φ)
                end
            end

            Fermi = typeof(φ)
            GaugeP = typeof(gparam)
            FermiP = typeof(fparam)
            Gauge_temp = eltype(_temporal_gauge)

            return new{Fermi,GaugeP,FermiP,Gauge_temp}(gparam,fparam,_temporal_gauge,_temporal_fermi1,_temporal_fermi2)

        end

        function Measurement(univ::Universe;Dirac_operator=nothing)
            return Measurement(univ::Universe,univ.gparam,univ.fparam,Dirac_operator=Dirac_operator)
        end

        function Measurement(univ::Universe,fparam;Dirac_operator=nothing)
            return Measurement(univ::Universe,univ.gparam,fparam,Dirac_operator=Dirac_operator)
        end
    end

    defaultmeasures = Array{Dict,1}(undef,2)
    for i=1:length(defaultmeasures)
        defaultmeasures[i] = Dict()
    end
    defaultmeasures[1]["methodname"] = "Plaquette"
    defaultmeasures[1]["measure_every"] = 1
    defaultmeasures[1]["fermiontype"] = nothing
    defaultmeasures[2]["methodname"] = "Polyakov_loop"
    defaultmeasures[2]["measure_every"] = 1
    defaultmeasures[2]["fermiontype"] = nothing
    #=
    defaultmeasures[3]["methodname"] = "Chiral_cond"
    defaultmeasures[3]["measure_every"] = 10
    defaultmeasures[3]["fermiontype"] = "Staggered"

    defaultmeasures[4]["methodname"] = "Pion_correlator"
    defaultmeasures[4]["measure_every"] = 20
    defaultmeasures[4]["fermiontype"] = "Wilson"

    defaultmeasures[5]["methodname"] = "Topologicalcharge"
    defaultmeasures[5]["measure_every"] = 5
    defaultmeasures[5]["fermiontype"] = nothing
    defaultmeasures[5]["numflow"]  = 10
    =#

    struct Measurement_set
        nummeasurement::Int64
        fermions::Array{Measurement,1}
        measurement_methods::Array{Dict,1}
        measurementfps::Array{IOStream,1}

        function Measurement_set(univ::Universe,measurement_dir;measurement_methods=defaultmeasures)
            nummeasurement = length(measurement_methods)
            fermions = Array{Measurement,1}(undef,nummeasurement)
            measurementfps = Array{IOStream,1}(undef,nummeasurement)
            for i=1:nummeasurement

                method = measurement_methods[i]
                if haskey(method,"fermiontype")
                    fermiontype = method["fermiontype"]
                else
                    fermiontype = nothing
                end

                measure_overwrite = "w"
                if method["methodname"] == "Plaquette"
                    measurementfps[i] = open(measurement_dir*"/Plaquette.txt",measure_overwrite)
                elseif method["methodname"] == "Polyakov_loop"
                    measurementfps[i] = open(measurement_dir*"/Polyakov_loop.txt",measure_overwrite)
                elseif method["methodname"] == "Topological_charge"
                    measurementfps[i] = open(measurement_dir*"/Topological_charge.txt",measure_overwrite)
                elseif method["methodname"] == "Chiral_condensate"
                    measurementfps[i] = open(measurement_dir*"/Chiral_condensate.txt",measure_overwrite)
                    @assert fermiontype != nothing "fermiontype should be set in Chiral_condensate measurement"
                elseif method["methodname"] == "Pion_correlator"
                    measurementfps[i] = open(measurement_dir*"/Pion_correlator.txt",measure_overwrite)
                    @assert fermiontype != nothing "fermiontype should be set in Pion_correlator measurement"
                elseif method["methodname"] == "Polyakov_loop_correlator"
                    measurementfps[i] = open(measurement_dir*"/Polyakov_loop_correlator.txt",measure_overwrite)
                elseif method["methodname"] == "Wilson_loop"
                    measurementfps[i] = open(measurement_dir*"/Wilson_loop.txt",measure_overwrite)
                elseif method["methodname"] == "smeared_Wilson_loop"
                    measurementfps[i] = open(measurement_dir*"/smeared_Wilson_loop.txt",measure_overwrite)
                elseif method["methodname"] == "integrated_fermion_action"
                    measurementfps[i] = open(measurement_dir*"/Fermion_actions.txt","w")
                else
                    error("$(method["methodname"]) is not supported in measurement functions")
                end

                if haskey(method,"eps")
                    eps = method["eps"]
                else
                    eps = 1e-16
                end

                if haskey(method,"MaxCGstep")
                    MaxCGstep = method["MaxCGstep"]
                else
                    MaxCGstep = 5000
                end

                if fermiontype  == "Wilson"

                    if haskey(method,"hop")
                        hop = method["hop"]
                    else
                        hop = 0.141139
                        println("Warning. hop = $hop, Default value is used in measurement $(method["methodname"])")
                    end

                    if haskey(method,"r")
                        r = method["r"]
                    else
                        r = 1
                    end

                    quench = false

                    smearing_for_fermion = set_params(method,"smearing_for_fermion","nothing")
                    stout_numlayers = set_params(method,"stout_numlayers",nothing)
                    stout_ρ = set_params(method,"stout_ρ",nothing)
                    stout_loops = set_params(method,"stout_loops",nothing)

                    #fparam = FermiActionParam_Wilson(hop,r,eps,fermiontype,MaxCGstep,quench)

                    if smearing_for_fermion == "nothing"
                        fparam = FermiActionParam_Wilson(hop,r,eps,fermiontype,MaxCGstep,quench)
                    else

                        L = (univ.NX,univ.NY,univ.NZ,univ.NT)
                        fparam = FermiActionParam_Wilson(hop,r,eps,fermiontype,MaxCGstep,quench,
                                                            smearingparameters = "stout",
                                                            loops_list = stout_loops,
                                                            coefficients  = stout_ρ,
                                                            numlayers = stout_numlayers,
                                                            L = L)
                    end

                elseif fermiontype == "WilsonClover"
                    if haskey(method,"hop")
                        hop = method["hop"]
                    else
                        hop = 0.141139
                        println("Warning. hop = $hop, Default value is used in measurement $(method["methodname"])")
                    end

                    if haskey(method,"r")
                        r = method["r"]
                    else
                        r = 1
                    end

                    if haskey(method,"Clover_coefficient")
                        Clover_coefficient = method["Clover_coefficient"]
                    else
                        Clover_coefficient = 1.5612
                    end

                    NV = univ.NV
                    inn_table= zeros(Int64,NV,4,2)
                    internal_flags = zeros(Bool,2)
                    _ftmp_vectors = Array{Array{ComplexF64,3},1}(undef,6)
                    for i=1:6
                        _ftmp_vectors[i] = zeros(ComplexF64,univ.NC,NV,4)
                    end

                    _is1 = zeros(Int64,NV)
                    _is2 = zeros(Int64,NV)

                    quench = false
                    fparam = FermiActionParam_WilsonClover(hop,r,eps,fermiontype,MaxCGstep,Clover_coefficient,
                                    internal_flags,inn_table,_ftmp_vectors,_is1,_is2,
                                    quench)

                    smearing_for_fermion = set_params(method,"smearing_for_fermion","nothing")
                    stout_numlayers = set_params(method,"stout_numlayers",nothing)
                    stout_ρ = set_params(method,"stout_ρ",nothing)
                    stout_loops = set_params(method,"stout_loops",nothing)

                    if smearing_for_fermion == "nothing"
                        FermiActionParam_WilsonClover(hop,r,eps,fermiontype,MaxCGstep,Clover_coefficient,
                                    internal_flags,inn_table,_ftmp_vectors,_is1,_is2,
                                    quench)
                    else
                        error("stout for WilsonClover is not supported yet!")
                        L = (univ.NX,univ.NY,univ.NZ,univ.NT)
                        FermiActionParam_WilsonClover(hop,r,eps,fermiontype,MaxCGstep,Clover_coefficient,
                                    internal_flags,inn_table,_ftmp_vectors,_is1,_is2,
                                    quench,
                                    smearingparameters = "stout",
                                    loops_list = stout_loops,
                                    coefficients  = stout_ρ,
                                    numlayers = stout_numlayers,
                                    L = L)
                    end

                elseif fermiontype == "Staggered"
                    if haskey(method,"mass")
                        mass = method["mass"]
                    else
                        mass = 0.5
                        println("Warning. mass = $mass, Default value is used in measurement $(method["methodname"])")
                    end

                    if haskey(method,"Nf")
                        Nf = method["Nf"]
                    else
                        error("Nf should be set if you want to use the staggered fermion in measurements")
                        #println("Warning. mass = $hop, Default value is used")
                    end

                    quench = false

                    smearing_for_fermion = set_params(method,"smearing_for_fermion","nothing")
                    stout_numlayers = set_params(method,"stout_numlayers",nothing)
                    stout_ρ = set_params(method,"stout_ρ",nothing)
                    stout_loops = set_params(method,"stout_loops",nothing)

                    if smearing_for_fermion == "nothing"
                        fparam = FermiActionParam_Staggered(mass,eps,fermiontype,MaxCGstep,quench,Nf)
                    else
                        L = (univ.NX,univ.NY,univ.NZ,univ.NT)
                        fparam = FermiActionParam_Staggered(mass,eps,fermiontype,MaxCGstep,quench,Nf,
                                    smearingparameters = "stout",
                                    loops_list = stout_loops,
                                    coefficients  = stout_ρ,
                                    numlayers = stout_numlayers,
                                    L = L)
                    end

                    #println("Measurement_set::mass_measurement = $(p.mass_measurement)")
                    #fparam = FermiActionParam_Staggered(mass,eps,fermiontype,MaxCGstep,quench,Nf)

                elseif fermiontype == nothing
                    fparam = nothing
                else
                    error("$fermiontype is not supported. use Wilson or Staggered")
                end

                fermions[i] = Measurement(univ,fparam;Dirac_operator=fermiontype )
            end
            return new(nummeasurement,fermions,measurement_methods,measurementfps)
        end
    end

    function measurements(itrj,U,univ,measset::Measurement_set;verbose = Verbose_2())
        plaq = 0.0
        poly = 0.0 + 0.0im
        for i = 1:measset.nummeasurement
            method = measset.measurement_methods[i]
            measfp = measset.measurementfps[i]
            #println(method)
            #println(method["measure_every"])
            if itrj % method["measure_every"] == 0
                println_verbose1(verbose,"-----------------")
                if method["methodname"] == "Plaquette"
                    plaq = calc_plaquette(U)
                    println_verbose1(verbose,"$itrj $plaq # plaq")
                    println(measfp,"$itrj $plaq # plaq")
                elseif method["methodname"] == "integrated_fermion_action"
                    Sfexact,Sfapprox = calc_fermionaction(univ,measset.fermions[i],method)
                    println_verbose1(verbose,"$itrj $(real(Sfexact)) $(real(Sfapprox)) # fermion action")
                    println(measfp,"$itrj $(real(Sfexact)) $(real(Sfapprox)) # fermion action")
                elseif method["methodname"] == "Wilson_loop"
                    for T=1:method["Tmax"]
                        for R=1:method["Rmax"]
                            WL = calc_Wilson_loop(U,T,R) # calculate RxT Wilson loop
                            println_verbose1(verbose,"$itrj $T $R $WL # WL # itrj T R W(T,R)")
                            println(measfp,"$itrj $T $R $WL # WL # itrj T R W(T,R)")
                        end
                    end
                elseif method["methodname"] == "smeared_Wilson_loop"
                    Usmr = deepcopy(U)
                    # arXiv:hep-lat/0107006
                    α = 0.5
                    for iflow = 1:5 #method["numflow"]
                        calc_fatlink_APE!(Usmr,α,α,normalize_method="special unitary", temporal_dir_smear=false)
                    end
                    for T=1:method["Tmax"]
                        for R=1:method["Rmax"]
                            WL = calc_Wilson_loop(Usmr,T,R) # calculate RxT Wilson loop
                            println_verbose1(verbose,"$itrj $T $R $WL # sWL # itrj T R W(T,R)")
                            println(measfp,"$itrj $T $R $WL # sWL # itrj T R W(T,R)")
                        end
                    end
                elseif method["methodname"] == "Polyakov_loop"
                    poly = calc_Polyakov(U)
                    println_verbose1(verbose,"$itrj $(real(poly)) $(imag(poly)) # poly")
                    println(measfp,"$itrj $(real(poly)) $(imag(poly)) # poly")
                elseif method["methodname"] == "Polyakov_loop_correlator"
                    Usmr = deepcopy(U)
                    β=univ.gparam.β
                    calc_multihit!(Usmr,U,β)
                    #α = 0.5
                    #calc_fatlink_APE!(Usmr,α,α,normalize_method="special unitary", temporal_dir_smear=false)
                    for R=1:method["Rmax"]
                        PP=calc_Polyakov_loop_correlator(Usmr, R)
                        println_verbose1(verbose,"$itrj $R $PP # PP # itrj R PP(R)")
                        println(measfp,"$itrj $R $PP # PP # itrj R PP(R)")
                    end
                elseif method["methodname"] == "Topological_charge"
                    Nflowsteps = method["Nflowsteps"]#50
                    eps_flow =  method["eps_flow"] #0.01

                    println_verbose2(verbose,"# epsilon for the Wilson flow is $eps_flow")
                    Usmr = deepcopy(U)
                    W1 = deepcopy(univ.U)
                    W2 = deepcopy(univ.U)

                    temp_UμνTA = Array{GaugeFields_1d,2}(undef,4,4)

                    τ = 0.0
                    plaq = calc_plaquette(Usmr)
                    #Q = calc_topological_charge(Usmr)
                    # sign of topological charge defined to be positive for one-instanton.
                    Qplaq = calc_topological_charge_plaq(Usmr,temp_UμνTA)
                    Qclover= calc_topological_charge_clover(Usmr)
                    Qimproved= calc_topological_charge_improved(Usmr,temp_UμνTA,Qclover)
                    clov = calc_energy_density(Usmr)
                    #println_verbose1(verbose,"$itrj $τ $plaq $(real(Qplaq)) $(real(Qclover)) #flow itrj flowtime plaq Qplaq Qclov")
                    #println(measfp,"$itrj $τ $plaq $(real(Qplaq)) $(real(Qclover)) #flow itrj flowtime plaq Qplaq Qclov")
                    println_verbose1(verbose,"$itrj $τ $plaq $clov $(real(Qplaq)) $(real(Qclover)) $(real(Qimproved)) #flow itrj flowtime plaq E Qplaq Qclov Qimproved")
                    println(measfp,"$itrj $τ $plaq $clov $(real(Qplaq)) $(real(Qclover)) $(real(Qimproved)) #flow itrj flowtime plaq E Qplaq Qclov Qimproved")
                    flush(stdout)
                    smearing_type = "gradient_flow"
                    #smearing_type = "APE"
                    #smearing_type = "stout"
                    if smearing_type == "gradient_flow"
                        for iflow = 1:method["numflow"]#5000 # eps=0.01: t_flow = 50
                            gradientflow!(Usmr,univ,W1,W2,Nflowsteps,eps_flow)
                            plaq = calc_plaquette(Usmr)
                            Qplaq = calc_topological_charge_plaq(Usmr,temp_UμνTA)
                            Qclover= calc_topological_charge_clover(Usmr,temp_UμνTA)
                            Qimproved= calc_topological_charge_improved(Usmr,temp_UμνTA,Qclover)
                            clov = calc_energy_density(Usmr)
                            #@time Q = calc_topological_charge(Usmr)
                            τ = iflow*eps_flow*Nflowsteps
                            println_verbose1(verbose,"$itrj $(round(τ, digits=3)) $plaq $clov $(real(Qplaq)) $(real(Qclover)) $(real(Qimproved)) #flow itrj flowtime plaq E Qplaq Qclov Qimproved")
                            println(measfp,"$itrj $(round(τ, digits=3)) $plaq $clov $(real(Qplaq)) $(real(Qclover)) $(real(Qimproved)) #flow itrj flowtime plaq E Qplaq Qclov Qimproved")
                            #if iflow%10 == 0
                            flush(stdout)
                            #end
                        end
                    elseif smearing_type == "APE" # TODO this should be commonized to gradient flow
                        Usmr = deepcopy(U)
                        α = 1/(7/6+1) # magic number from hep-lat/9907019 for reproduction. This should be an input parameter
                        for iflow = 1:method["numflow"]
                            Usmr = calc_fatlink_APE(Usmr,α,α,normalize_method="special unitary")
                            #calc_fatlink_APE!(Usmr,α,α,normalize_method="special unitary")
                            plaq = calc_plaquette(Usmr)
                            Qplaq = calc_topological_charge_plaq(Usmr,temp_UμνTA)
                            Qclover= calc_topological_charge_clover(Usmr,temp_UμνTA)
                            Qimproved= calc_topological_charge_improved(Usmr,temp_UμνTA,Qclover)
                            τ = iflow
                            println_verbose1(verbose,"$itrj $τ $plaq $(real(Qplaq)) $(real(Qclover)) $(real(Qimproved)) #ape itrj flowtime plaq Qplaq Qclov Qimproved")
                            println(measfp,"$itrj $τ $plaq $(real(Qplaq)) $(real(Qclover)) $(real(Qimproved)) #ape itrj flowtime plaq Qplaq Qclov Qimproved")
                            #if iflow%10 == 0
                            flush(stdout)
                            #end
                        end
                    elseif smearing_type == "stout" # TODO this should be commonized to gradient flow
                        Usmr = deepcopy(U)
                        ρ = 1/(7/6+1)/6
                        for iflow = 1:method["numflow"]
                            Usmr = calc_stout(Usmr,ρ)
                            #calc_stout!(Usmr,Usmr,ρ)
                            plaq = calc_plaquette(Usmr)
                            Qplaq = calc_topological_charge_plaq(Usmr,temp_UμνTA)
                            Qclover= calc_topological_charge_clover(Usmr,temp_UμνTA)
                            Qimproved= calc_topological_charge_improved(Usmr,temp_UμνTA,Qclover)
                            τ = iflow
                            println_verbose1(verbose,"$itrj $τ $plaq $(real(Qplaq)) $(real(Qclover)) $(real(Qimproved)) #stout itrj flowtime plaq Qplaq Qclov Qimproved")
                            println(measfp,"$itrj $τ $plaq $(real(Qplaq)) $(real(Qclover)) $(real(Qimproved)) #stout itrj flowtime plaq Qplaq Qclov Qimproved")
                            #if iflow%10 == 0
                            flush(stdout)
                            #end
                        end
                    else
                        error("Invalid smearing_type = $smearing_type")
                    end
                elseif method["methodname"] == "Chiral_condensate"
                    #fermiontype = method["fermiontype"]
                    measure_chiral_cond(univ,measset.fermions[i],itrj,measfp,verbose)
                elseif method["methodname"] == "Pion_correlator"
                    #fermiontype = method["fermiontype"]
                    #calc_pion_correlator(univ,measset.fermions[i])
                    measure_correlator(univ,measset.fermions[i],itrj,measfp,verbose)
                else
                    error("$(method["methodname"]) is not supported")
                end
                println_verbose1(verbose,"-----------------")
                flush(measfp)
            end
        end
        return plaq,poly
    end

    function calc_polyakovloop(univ::Universe)
        poly = calc_Polyakov(univ.U)

        return poly

    end

    function calc_factor_plaq(U)
        factor = 2/(U[1].NV*4*3*U[1].NC)
    end

# - - - Polyakov loop correlator
function calc_Polyakov_loop_correlator(U::Array{T,1}, R) where T <: GaugeFields
    WL = 0.0+0.0im
    NV = U[1].NV
    NC = U[1].NC
    Wmat = Array{GaugeFields_1d,2}(undef,4,4)
    #
    construct_Polyakov_correlator!(Wmat,U) # make wilon loop operator and evaluate as a field, not traced.
    WL = calc_Polyakov_loop_correlator_core(Wmat,U,R) # tracing over color and average over spacetime and x,y,z.
    NDir = 3.0 # in 4 diemension, 3 associated staples. t-x plane, t-y plane, t-z plane
    return real(WL)/NV/NDir/NC
end
function construct_Polyakov_correlator!(Wmat,U)
    NT = U[1].NT
    W_operator = make_2Polyakov_loop(NT)
    calc_2Polyakov_loop!(Wmat,W_operator,U)
    return
end
function calc_Polyakov_loop_correlator_core(Wmat, U::Array{GaugeFields{S},1} ,R) where S <: SUn
    if S == SU3
        NC = 3
    elseif S == SU2
        NC = 2
    else
        NC = U[1].NC
        #error("NC != 2,3 is not supported")
    end
    W = 0.0 + 0.0im
    NX = U[1].NX
    NY = U[1].NY
    NZ = U[1].NZ
    it=1
    for ix=1:NX
    for iy=1:NY
    for iz=1:NZ
        i = (((it-1)NZ+iz-1)NY+iy-1)*NX + ix
        #
        μ=1
        ix2=(ix+R-1)%NX+1
        i2= (((it-1)NZ+iz-1)NY+iy-1)*NX + ix2
        W += tr(Wmat[μ][:,:,i]')*tr(Wmat[μ][:,:,i2])
        #
        μ=2
        iy2=(iy+R-1)%NY+1
        i2= (((it-1)NZ+iz-1)NY+iy2-1)*NX + ix
        W += tr(Wmat[μ][:,:,i]')*tr(Wmat[μ][:,:,i2])
        #
        μ=3
        iz2=(iz+R-1)%NZ+1
        i2= (((it-1)NZ+iz2-1)NY+iy-1)*NX + ix
        W += tr(Wmat[μ][:,:,i]')*tr(Wmat[μ][:,:,i2])
    end
    end
    end
    return W
end
function make_2Polyakov_loop(NT)
    #= Making a Wilson loop operator for potential calculations
        Ls × Lt
        ν=4
        ↑
        +
        |
        |
        |
        +   → μ=1,2,3 (averaged)
    =#
    Wmatset= Array{Wilson_loop_set,1}(undef,3)
    for μ=1:3 #
        t=4
        loops = Wilson_loop_set()
        loop = Wilson_loop([(t,NT)]) #plakov loop    # This part is under construction.
        push!(loops,loop)
        Wmatset[μ] = loops
    end
    return Wmatset
end
function calc_2Polyakov_loop!(temp_Wmat,loops_μν,U)
    W = temp_Wmat
    for μ=1:3 # spatial directions
        loopset = Loops(U,loops_μν[μ])
        W[μ] = evaluate_loops(loopset,U)
    end
    return
end
# - - - end Polyakov loop correlator

    function calc_Wilson_loop(U::Array{T,1},Lt,Ls) where T <: GaugeFields
        # Making a ( Ls × Lt) Wilson loop operator for potential calculations
        WL = 0.0+0.0im
        NV = U[1].NV
        NC = U[1].NC
        Wmat = Array{GaugeFields_1d,2}(undef,4,4)
        #
        calc_large_wiloson_loop!(Wmat,Lt,Ls,U) # make wilon loop operator and evaluate as a field, not traced.
        WL = calc_Wilson_loop_core(Wmat,U,NV) # tracing over color and average over spacetime and x,y,z.
        NDir = 3.0 # in 4 diemension, 3 associated staples. t-x plane, t-y plane, t-z plane
        return real(WL)/NV/NDir/NC
    end
    function calc_Wilson_loop_core(Wmat, U::Array{GaugeFields{S},1} ,NV) where S <: SUn
        if S == SU3
            NC = 3
        elseif S == SU2
            NC = 2
        else
            NC = U[1].NC
            #error("NC != 2,3 is not supported")
        end
        W = 0.0 + 0.0im
        for n=1:NV
            for μ=1:3 # spatial directions
                ν=4  # T-direction is not summed over
                W += tr(Wmat[μ,ν][:,:,n])
            end
        end
        return W
    end
    function calc_large_wiloson_loop!(Wmat,Lt,Ls,U)
        W_operator = make_Wilson_loop(Lt,Ls)
        calc_large_wiloson_loop!(Wmat,W_operator,U)
        return
    end
    function make_Wilson_loop(Lt,Ls)
        #= Making a Wilson loop operator for potential calculations
            Ls × Lt
            ν=4
            ↑
            +--+
            |  |
            |  |
            |  |
            +--+ → μ=1,2,3 (averaged)
        =#
        Wmatset= Array{Wilson_loop_set,2}(undef,4,4)
        for μ=1:3 # spatial directions
            ν=4 # T-direction is not summed over
            loops = Wilson_loop_set()
            loop = Wilson_loop([(μ,Ls),(ν,Lt),(μ,-Ls),(ν,-Lt)])
            push!(loops,loop)
            Wmatset[μ,ν] = loops
        end
        return Wmatset
    end
    function calc_large_wiloson_loop!(temp_Wmat,loops_μν,U)
        W = temp_Wmat
        for μ=1:3 # spatial directions
            ν=4 # T-direction is not summed over
            loopset = Loops(U,loops_μν[μ,ν])
            W[μ,ν] = evaluate_loops(loopset,U)
        end
        return
    end
# = = = calc energy density = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
function calc_energy_density(U::Array{T,1}) where T <: GaugeFields
    # Making a ( Ls × Lt) Wilson loop operator for potential calculations
    WL = 0.0+0.0im
    NV = U[1].NV
    NC = U[1].NC
    Wmat = Array{GaugeFields_1d,2}(undef,4,4)
    #
    make_energy_density!(Wmat,U) # make wilon loop operator and evaluate as a field, not traced.
    WL =  make_energy_density_core(Wmat,U,NV) # tracing over color and average over spacetime and x,y,z.
    NDir = 4.0*3.0/2 # choice of 2 axis from 4.
    return real(WL)/NV/NDir/NC/8
end
function  make_energy_density_core(Wmat, U::Array{GaugeFields{S},1} ,NV) where S <: SUn
    if S == SU3
        NC = 3
    elseif S == SU2
        NC = 2
    else
        NC = U[1].NC
        #error("NC != 2,3 is not supported")
    end
    W = 0.0 + 0.0im
    for n=1:NV
        for μ=1:4 # all directions
            for ν=1:4
                if μ == ν
                    continue
                end
                W += tr(Wmat[μ,ν][:,:,n]*Wmat[μ,ν][:,:,n])/4
            end
        end
    end
    return W
end
function make_energy_density!(Wmat,U)
    W_operator,numofloops = calc_loopset_μν("clover")#make_Wilson_loop(Lt,Ls)
    calc_large_wiloson_loop4d!(Wmat,W_operator,U)
    return
end
function calc_large_wiloson_loop4d!(temp_Wmat,W_operator,U)
    W = temp_Wmat
    for μ=1:4
        for ν=1:4
            if μ == ν
                continue
            end
            #println(typeof(μ)," ",typeof(ν))
            #exit()
            loopset = Loops(U,W_operator[μ,ν])
            W[μ,ν] = evaluate_loops(loopset,U)
        end
    end
    return
end
# = = = end of energy density = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
    function calc_plaquette(U::Array{T,1}) where T <: GaugeFields
        plaq = 0
        factor = calc_factor_plaq(U)
        plaq = calc_Plaq(U)*factor
        return real(plaq)
    end

    function calc_plaquette(univ::Universe,U::Array{T,1}) where T <: GaugeFields
        plaq = 0
        temps = univ._temporal_gauge
        factor = calc_factor_plaq(U)
        plaq = calc_Plaq!(U,temps)*factor
        return real(plaq)
    end

    function calc_topological_charge(U::Array{GaugeFields{S},1}) where S <: SUn
        return calc_topological_charge_plaq(U)
    end

    function calc_topological_charge_clover(U::Array{GaugeFields{S},1}) where S <: SUn
        UμνTA = Array{GaugeFields_1d,2}(undef,4,4)
        Q = calc_topological_charge_clover(U,UμνTA)
        return Q
    end

    function calc_topological_charge_clover(U::Array{GaugeFields{S},1},temp_UμνTA) where S <: SUn
        UμνTA = temp_UμνTA
        numofloops = calc_UμνTA!(UμνTA,"clover",U)
        Q = calc_Q(UμνTA,numofloops,U)
        return Q
    end

    function calc_topological_charge_improved(U::Array{GaugeFields{S},1},temp_UμνTA) where S <: SUn
        UμνTA = temp_UμνTA
        numofloops = calc_UμνTA!(UμνTA,"clover",U)
        Qclover = calc_Q(UμνTA,numofloops,U)
        Q = calc_topological_charge_improved(U,UμνTA,Qclover)
        return Q
    end

    function calc_topological_charge_improved(U::Array{GaugeFields{S},1},temp_UμνTA,Qclover) where S <: SUn
        UμνTA = temp_UμνTA
        #numofloops = calc_UμνTA!(UμνTA,"clover",U)
        #Qclover = calc_Q(UμνTA,numofloops,U)

        numofloops = calc_UμνTA!(UμνTA,"rect",U)
        Qrect = 2*calc_Q(UμνTA,numofloops,U)
        c1 = -1/12
        c0 = 5/3
        Q = c0*Qclover + c1*Qrect
        return Q
    end

    function calc_topological_charge_plaq(U::Array{GaugeFields{S},1},temp_UμνTA) where S <: SUn
        UμνTA = temp_UμνTA
        numofloops = calc_UμνTA!(UμνTA,"plaq",U)
        Q = calc_Q(UμνTA,numofloops,U)
        return Q
    end

    function calc_topological_charge_plaq(U::Array{GaugeFields{S},1}) where S <: SUn
        UμνTA = Array{GaugeFields_1d,2}(undef,4,4)
        return calc_topological_charge_plaq(U,UμνTA )
    end

    function calc_loopset_μν(name)
        loops_μν= Array{Wilson_loop_set,2}(undef,4,4)
        if name == "plaq"
            numofloops = 1
            for μ=1:4
                for ν=1:4
                    if ν == μ
                        continue
                    end
                    loops = Wilson_loop_set()
                    loop = Wilson_loop([(μ,1),(ν,1),(μ,-1),(ν,-1)])
                    push!(loops,loop)
                    loops_μν[μ,ν] = loops
                end
            end
        elseif name == "clover"
            numofloops = 4
            for μ=1:4
                for ν=1:4
                    if ν == μ
                        continue
                    end
                    loops = Wilson_loop_set()

                    loop_righttop = Wilson_loop([(μ,1),(ν,1),(μ,-1),(ν,-1)])
                    loop_lefttop = Wilson_loop([(ν,1),(μ,-1),(ν,-1),(μ,1)])
                    loop_rightbottom = Wilson_loop([(ν,-1),(μ,1),(ν,1),(μ,-1)])
                    loop_leftbottom= Wilson_loop([(μ,-1),(ν,-1),(μ,1),(ν,1)])
                    push!(loops,loop_righttop)
                    push!(loops,loop_lefttop)
                    push!(loops,loop_rightbottom)
                    push!(loops,loop_leftbottom)

                    loops_μν[μ,ν] = loops
                end
            end
        elseif name == "rect"
            numofloops = 8
            for μ=1:4
                for ν=1:4
                    if ν == μ
                        continue
                    end
                    loops = Wilson_loop_set()

                    loop_righttop = Wilson_loop([(μ,2),(ν,1),(μ,-2),(ν,-1)])
                    loop_lefttop = Wilson_loop([(ν,1),(μ,-2),(ν,-1),(μ,2)])
                    loop_rightbottom = Wilson_loop([(ν,-1),(μ,2),(ν,1),(μ,-2)])
                    loop_leftbottom= Wilson_loop([(μ,-2),(ν,-1),(μ,2),(ν,1)])
                    push!(loops,loop_righttop)
                    push!(loops,loop_lefttop)
                    push!(loops,loop_rightbottom)
                    push!(loops,loop_leftbottom)

                    loop_righttop = Wilson_loop([(μ,1),(ν,2),(μ,-1),(ν,-2)])
                    loop_lefttop = Wilson_loop([(ν,2),(μ,-1),(ν,-2),(μ,1)])
                    loop_rightbottom = Wilson_loop([(ν,-2),(μ,1),(ν,2),(μ,-1)])
                    loop_leftbottom= Wilson_loop([(μ,-1),(ν,-2),(μ,1),(ν,2)])
                    push!(loops,loop_righttop)
                    push!(loops,loop_lefttop)
                    push!(loops,loop_rightbottom)
                    push!(loops,loop_leftbottom)

                    loops_μν[μ,ν] = loops
                end
            end
        else
            error("$name is not supported")
        end
        return loops_μν,numofloops
    end

    function calc_UμνTA!(temp_UμνTA,loops_μν,U)
        UμνTA = temp_UμνTA
        for μ=1:4
            for ν=1:4
                if ν == μ
                    continue
                end
                loopset = Loops(U,loops_μν[μ,ν])
                UμνTA[μ,ν] = evaluate_loops(loopset,U)
                UμνTA[μ,ν] = TA(UμνTA[μ,ν])
            end
        end
        return
    end

    function calc_UμνTA!(temp_UμνTA,name::String,U)
        loops_μν,numofloops = calc_loopset_μν(name)
        calc_UμνTA!(temp_UμνTA,loops_μν,U)
        return numofloops
    end

    #=
    implementation of topological charge is based on
    https://arxiv.org/abs/1509.04259
    =#
    function calc_Q(UμνTA,numofloops,U::Array{GaugeFields{S},1}) where S <: SUn
        if S == SU3
            NC = 3
        elseif S == SU2
            NC = 2
        else
            NC = U[1].NC
            #error("NC != 2,3 is not supported")
        end

        Q = 0.0

        ε(μ,ν,ρ,σ) = epsilon_tensor(μ,ν,ρ,σ)
        NV=UμνTA[1,2].NV
        for n=1:NV
            for μ=1:4
                for ν=1:4
                    if ν == μ
                        continue
                    end
                    for ρ =1:4
                        for σ=1:4
                            if ρ == σ
                                continue
                            end

                            s = 0im
                            for i=1:NC
                                for j=1:NC
                                    s += UμνTA[μ,ν][i,j,n]*UμνTA[ρ,σ][j,i,n]
                                end
                            end

                            Q += ε(μ,ν,ρ,σ)*s/numofloops^2 #*tr(tmp1*tmp2)
                        end
                    end
                end
            end
        end
        return -Q/(32*(π^2))
    end

    function TAm(M)
        NC = size(M)[1]
        AM = (M - M')/2
        t = tr(AM)/NC
        for i=1:NC
            AM[i,i] -= t
        end
        return AM
    end

    function calc_plaquette(univ::Universe)
        return calc_plaquette(univ,univ.U)
    end

    function spincolor(ic,is,univ::Universe)
        return ic-1 + (is-1)*univ.NC + 1
    end

    function calc_chiral_cond(univ::Universe,meas,measfp,itrj, Nr = 10, verbose = Verbose_2())
        # pbp = (1/Nr) Σ_i p_i
        # p_i = r_i^\dag xi_i
        # xi_i = D^{-1} r_i   # D xi = r : r is a random veccor
        #
        Nfbase = ifelse( meas.fparam.Dirac_operator == "Staggered",4,1)
        Nf = meas.fparam.Nf
        factor = Nf/Nfbase
        #
        println_verbose2(verbose,"Chiral condensate for Nf = $(Nf), Dirac_operator = $(meas.fparam.Dirac_operator), factor=$factor is multiplied.")
        #
        pbp = 0.0
        # setup a massive Dirac operator
        U,_... = calc_smearingU(univ.U,meas.fparam.smearing)
        M = Dirac_operator(U,meas._temporal_fermi2[1],meas.fparam)
        #M = Dirac_operator(univ.U,meas._temporal_fermi2[1],meas.fparam)
        for ir=1:Nr
            r = similar(meas._temporal_fermi2[1])
            p = similar(r)
            clear!(p)
            #gauss_distribution_fermi!(r,univ.ranf)
            Z4_distribution_fermi!(r)
            #set_wing_fermi!(r)
            bicg(p,M,r,eps=meas.fparam.eps,maxsteps = meas.fparam.MaxCGstep,verbose = verbose) # solve Mp=b, we get p=M^{-1}b
            tmp = r*p # hemitian inner product
            println_verbose2(verbose,"# $itrj $ir $(real(tmp)/univ.NV) # itrj irand chiralcond")
            println(measfp,"# $itrj $ir $(real(tmp)/univ.NV) # itrj irand chiralcond")
            pbp+=tmp
        end
        return real(pbp/Nr)/univ.NV * factor
    end

    function measure_chiral_cond(univ::Universe,meas::Measurement,itrj,measfp,verbose = Verbose_2())
        Nr = 10
        pbp = calc_chiral_cond(univ,meas,measfp,itrj,Nr,verbose)
        println_verbose1(verbose,"$itrj $pbp # pbp Nr=$Nr")
        println(measfp,"$itrj $pbp # pbp Nr=$Nr")
        flush(stdout)
    end

    function calc_fermionaction(univ::Universe,meas::Measurement,method)
        println("Making W^+W matrix...")
        @time WdagW = make_WdagWmatrix(univ.U,meas._temporal_fermi2[1],meas._temporal_fermi2[2],meas.fparam)

        if haskey(method,"nc")
            nc = method["nc"]
        else
            nc = 20
        end

        if haskey(method,"M")
            M = method["M"]
        else
            M = 128
        end

        if haskey(method,"nonc")
            nonc = method["nonc"]
        else
            nonc = false
        end

        if haskey(method,"m")
            m = method["m"]
        else
            m = 100
        end

        Sfexact = calc_IntegratedFermionAction(univ,WdagW;debug = false)

        Sfapprox = calc_IntegratedFermionAction(univ,WdagW;debug = true,M=M,m=m,nc=nc,nonc=nonc)
        return Sfexact,Sfapprox

    end

    function calc_quark_propagators_point_source_each(M,meas,i,NC,verbose)
        # calculate D^{-1} for a given source at the origin.
        # Nc*Ns (Ns: dim of spinor, Wilson=4, ks=1) elements has to be gathered.
        # staggered Pion correlator relies on https://itp.uni-frankfurt.de/~philipsen/theses/breitenfelder_ba.pdf (3.33)
        b = similar(meas._temporal_fermi2[1]) # source is allocated
        p = similar(b) # sink is allocated (propagator to sink position)
        #k = meas._temporal_fermi2[2]
        clear!(b)
        Nspinor = ifelse( meas.fparam.Dirac_operator == "Staggered" ,1,4)
        is = ((i-1) % Nspinor)+1 # spin index
        ic = ((i-is) ÷ Nspinor)+ 1 # color index
        println_verbose1(verbose,"$ic $is")
        #println("$ic $is")
        b[ic,1,1,1,1,is]=1 # source at the origin

        @time bicg(p,M,b,eps=meas.fparam.eps,maxsteps = meas.fparam.MaxCGstep,verbose = verbose) # solve Mp=b, we get p=M^{-1}b

        #@time cg0!(k,b,1, univ.U, meas._temporal_gauge, meas._temporal_fermi1, meas.fparam) # k[x] = M^{-1}b[0]
        println_verbose1(verbose,"Hadron spectrum: Inversion $(i)/$(NC*Nspinor) is done")
        #println("Hadron spectrum: Inversion $(i)/$(NC*4) is done")
        flush(stdout)
        return p
    end

    function calc_quark_propagators_point_source(D,meas,NC,verbose)
        # D^{-1} for each spin x color element
        Nspinor = ifelse( meas.fparam.Dirac_operator == "Staggered" ,1,4)
        propagators = map(i -> calc_quark_propagators_point_source_each(D,meas,i,NC,verbose),1:NC*Nspinor)
        return propagators
    end

    function calc_pion_correlator(univ::Universe,meas::Measurement,verbose)
        println_verbose2(verbose,"Hadron spectrum started")
        #println("Hadron spectrum started")
        #U = univ._temporal_gauge
        #if univ.NC != 3
        #    error("not implemented yet for calc_pion_correlator")
        #end
        #b = meas._temporal_fermi2[1] # source allocate
        #k = meas._temporal_fermi2[2] # sink allocate

        # setup a massive Dirac operator
        U,_... = calc_smearingU(univ.U,meas.fparam.smearing)
        M = Dirac_operator(U,meas._temporal_fermi2[1],meas.fparam)
        #M = Dirac_operator(univ.U,meas._temporal_fermi2[1],meas.fparam)

        # Allocate the Wilson matrix = S
        Nspinor = ifelse( meas.fparam.Dirac_operator == "Staggered" ,1,4)
        S = zeros(ComplexF64, (univ.NX,univ.NY,univ.NZ,univ.NT, Nspinor*univ.NC, Nspinor*univ.NC) )

        # calculate quark propagators from a point source at he origin
        propagators = calc_quark_propagators_point_source(M,meas,univ.NC,verbose)

        #ctr = 0 # a counter
        for ic=1:univ.NC
            for is=1:Nspinor
                icum = (ic-1)*Nspinor+is

                #=
                clear!(b)
                b[ic,1,1,1,1,is]=1 # source at the origin
                @time cg0!(k,b,1, univ.U, meas._temporal_gauge, meas._temporal_fermi1, meas.fparam) # k[x] = M^{-1}b[0]
                =#
                propagator = propagators[icum]
                α0=spincolor(ic,is,univ) # source(color-spinor) index
                #

                # reconstruction
                for t=1:univ.NT
                    for z=1:univ.NZ
                        for y=1:univ.NY
                            for x=1:univ.NX
                                for ic2=1:univ.NC
                                    for is2=1:Nspinor # Nspinor is the number of spinor index in 4d.
                                        β=spincolor(ic2,is2,univ)
                                        S[x,y,z,t,α0,β]+= propagator[ic,x,y,z,t,is]
                                    end
                                end
                            end
                        end
                    end
                end
                # end for the substitution

                #ctr+=1
            end
        end
        # contruction end.

        println_verbose2(verbose,"Hadron spectrum: Reconstruction")
        #println("Hadron spectrum: Reconstruction")
        Cpi = zeros( univ.NT )
        # Construct Pion propagator
        for t=1:univ.NT
            tmp = 0.0+0.0im
            for z=1:univ.NZ
                for y=1:univ.NY
                    for x=1:univ.NX
                        for ic=1:univ.NC
                            for is=1:Nspinor # Nspinor is the number of spinor index in 4d.
                                α=spincolor(ic,is,univ)
                                for ic2=1:univ.NC
                                    for is2=1:Nspinor # Nspinor is the number of spinor index in 4d.
                                        β=spincolor(ic2,is2,univ)
                                        tmp += S[x,y,z,t,α,β] * S[x,y,z,t,α,β]'#inner product.
                                        # complex conjugate = g5 S g5.
                                    end
                                end
                                # complex conjugate = g5 S g5.
                            end
                        end
                    end
                end
            end
            # staggered Pion correlator relies on https://itp.uni-frankfurt.de/~philipsen/theses/breitenfelder_ba.pdf (3.33)
            # we adopt ignoreing the staggering factor. See detail above reference.
            ksfact = 1.0 #ifelse( meas.fparam.Dirac_operator == "Staggered" , (-1)^(t-1) * 64, 1)
            Cpi[t] = real(tmp)*ksfact
        end
        #println(typeof(verbose),"\t",verbose)
        println_verbose2(verbose,"Hadron spectrum end")
        #println("Hadron spectrum end")
        return Cpi
    end
    function measure_correlator(univ::Universe,meas::Measurement,itrj,measfp,verbose)
        C = calc_pion_correlator(univ,meas,verbose)
        print_verbose1(verbose,"$itrj ")
        print(measfp,"$itrj ")
        for it=1:length(C)
            cc = C[it]
            print_verbose1(verbose,"$cc ")
            print(measfp,"$cc ")
        end
        println_verbose1(verbose,"#pioncorrelator")
        println(measfp,"#pioncorrelator")
    end
    #topological charge
    function epsilon_tensor(mu::Int,nu::Int,rho::Int,sigma::Int)
        sign=1 # (3) 1710.09474 extended epsilon tensor
        if mu < 0
            sign*=-1
            mu=-mu
        end
        if nu < 0
            sign*=-1
            nu=-nu
        end
        if rho < 0
            sign*=-1
            rh=-rho
        end
        if sigma < 0
            sign*=-1
            sigma=-sigma
        end
        epsilon = zeros(Int,4,4,4,4)
        epsilon[ 1, 2, 3, 4 ] = 1
        epsilon[ 1, 2, 4, 3 ] = -1
        epsilon[ 1, 3, 2, 4 ] = -1
        epsilon[ 1, 3, 4, 2 ] = 1
        epsilon[ 1, 4, 2, 3 ] = 1
        epsilon[ 1, 4, 3, 2 ] = -1
        epsilon[ 2, 1, 3, 4 ] = -1
        epsilon[ 2, 1, 4, 3 ] = 1
        epsilon[ 2, 3, 1, 4 ] = 1
        epsilon[ 2, 3, 4, 1 ] = -1
        epsilon[ 2, 4, 1, 3 ] = -1
        epsilon[ 2, 4, 3, 1 ] = 1
        epsilon[ 3, 1, 2, 4 ] = 1
        epsilon[ 3, 1, 4, 2 ] = -1
        epsilon[ 3, 2, 1, 4 ] = -1
        epsilon[ 3, 2, 4, 1 ] = 1
        epsilon[ 3, 4, 1, 2 ] = 1
        epsilon[ 3, 4, 2, 1 ] = -1
        epsilon[ 4, 1, 2, 3 ] = -1
        epsilon[ 4, 1, 3, 2 ] = 1
        epsilon[ 4, 2, 1, 3 ] = 1
        epsilon[ 4, 2, 3, 1 ] = -1
        epsilon[ 4, 3, 1, 2 ] = -1
        epsilon[ 4, 3, 2, 1 ] = 1
        return epsilon[mu,nu,rho,sigma]*sign
    end

end

ext: jl


text:
# Author: Ritchie Lee, ritchie.lee@sv.cmu.edu
# Date: 11/11/2014

#Piggyback and add coordination to SimpleTCAS

module CoordSimpleTCASImpl

export
    addObserver,

    initialize,
    update,

    testThreat,
    selectRA,

    CoordSimpleTCAS,
    CoordSimpleTCASInput,
    CoordSimpleTCASResolutionAdvisory

using AbstractCollisionAvoidanceSystemImpl
using AbstractCollisionAvoidanceSystemInterfaces
using CommonInterfaces

using Util
using RLESUtils, Observers

using AbstractCASCoordImpl

using CASCoordination
using SimpleTCASImpl

import CommonInterfaces.addObserver
import CommonInterfaces.initialize
import CommonInterfaces.update
import AbstractCollisionAvoidanceSystemInterfaces.testThreat
import AbstractCollisionAvoidanceSystemInterfaces.selectRA

type CoordSimpleTCAS <: AbstractCollisionAvoidanceSystem
  aircraft_number::Int64
  simpleTCAS::SimpleTCAS
  coordination::AbstractCASCoord

  CoordSimpleTCAS(aircraft_number::Int,coordination::AbstractCASCoord) = new(aircraft_number,SimpleTCAS(),coordination)
end

addObserver(cas::CoordSimpleTCAS, f::Function) = add_observer(cas.observer, f)
addObserver(cas::CoordSimpleTCAS, tag::AbstractString, f::Function) = add_observer(cas.observer, tag, f)

testThreat(cas::CoordSimpleTCAS, input::SimpleTCASInput) = testThreat(cas.simpleTCAS, input)

function selectRA(cas::CoordSimpleTCAS, input::SimpleTCASInput)

  #Make a two aircraft assumption for now
  if cas.aircraft_number == 1
    intruder_number = 2
  else
    intruder_number = 1
  end

  intruderRA = getRecord(cas.coordination,intruder_number) #The coordination object is ownship's RA

  if intruderRA != nothing
    h_d = intruderRA.h_d < 0 ? 1500 / 60 : -1500 / 60  #Do opposite sense, default strength
  else
    h_d = selectRA(cas.simpleTCAS, input)
  end

  return h_d
end

update(cas::CoordSimpleTCAS, input) = update(cas,convert(SimpleTCASInput, input))

function update(cas::CoordSimpleTCAS, input::SimpleTCASInput)

  if cas.simpleTCAS.b_TCAS_activated == false
    cas.simpleTCAS.b_TCAS_activated = testThreat(cas, input)

    if cas.simpleTCAS.b_TCAS_activated
      cas.simpleTCAS.RA = SimpleTCASResolutionAdvisory(selectRA(cas, input))
      setRecord(cas.coordination,cas.aircraft_number,cas.simpleTCAS.RA)
    end
  end

  return cas.simpleTCAS.RA
end

function initialize(cas::CoordSimpleTCAS)
    initialize(cas.simpleTCAS)
end

end

max_stars_repo_name: sisl/RLES-SISLES

max_issues_repo_name: sisl/RLES-SISLES

max_forks_repo_name: sisl/RLES-SISLES


text:
context("test OID") do
    # Test constructor
    HEX1 = "15b648aec6ed045b5ca6f57f8b7831a8b4757298"
    HEX2 = "15b648aec6ed045b5ca6f57f8b7831a8b4757299"

    # test string constructor
    oid1 = Oid(HEX1)

    # Test equality
    oid2 = Oid(HEX1)
    @test oid1 == oid2

    oid2 = Oid(HEX2)
    @test oid1 != oid2

    # test array constructor
    @test Oid(hex2bytes(HEX1)) == Oid(HEX1)

    # test pointer constructor
    @test Oid(pointer(hex2bytes(HEX1))) == Oid(HEX1)

    # constructors throw argument errors on invalid inputs
    @test_throws ArgumentError Oid(HEX1[2:end-1])
    @test_throws ArgumentError Oid(hex2bytes(HEX1)[1:end-1])
    @test_throws ArgumentError Oid(convert(Ptr{UInt8}, C_NULL))

    # test raw
    @test bytes2hex(raw(Oid(HEX1))) == HEX1

    # Test comparisons
    @test is(oid1 < oid2, true)
    @test is(oid1 <= oid2, true)
    @test is(oid1 == oid2, false)
    @test is(oid1 > oid2, false)
    @test is(oid1 >= oid2, false)

    # Test hash
    oid1 = Oid(HEX1)
    oid2 = Oid(HEX1)

    s = Set{Oid}([oid1, oid2])
    @test length(s) == 1

    push!(s, Oid("0000000000000000000000000000000000000000"))
    push!(s, Oid("0000000000000000000000000000000000000001"))
    @test length(s) == 3

    # Test iszero
    @test iszero(Oid()) == true
    @test iszero(oid1) == false
    @test iszero(Oid(zeros(UInt8, 20))) == true
end

context("test SHA1") do
    sha = sha1"15b648aec6ed045b5ca6f57f8b7831a8b4757298"
    psha = sha1"15b648aec6ed"

    #test sha1 constructors
    @test isa(sha, SHA1)
    @test isa(psha, PartialSHA1)

    # invalid string length
    @test_throws ArgumentError SHA1("15b648aec6ed045b5ca6f57f8b7831a8b475729")

end


text:
add_assert_scope(:CSAMaxOrd)

###############################################################################
#
#  Types
#
###############################################################################

mutable struct AlgAssOrd
  A::AlgAss{fmpq}                  # CSA containing it
  dim::Int
  basis_alg::Vector{AlgAssElem{fmpq}}    # Basis as array of elements of the algebra
  basis_mat::FakeFmpqMat           # Basis matrix of order wrt basis of the algebra
  basis_mat_inv::FakeFmpqMat       # Inverse of basis matrix
  gen_index::fmpq                  # The det of basis_mat_inv as fmpq
  index::fmpz                      # The det of basis_mat_inv
                                   # (this is the index of the equation order
                                   #  in the given order)
  disc::fmpz                       # Discriminant

  ismaximal::Int                   # 0 Not known
                                   # 1 Known to be maximal
                                   # 2 Known to not be maximal

  trace_mat::fmpz_mat              # The reduced trace matrix (if known)

  function AlgAssOrd(a::AlgAss{fmpq}, basis::Array{AlgAssElem{fmpq},1})
    # "Default" constructor with default values.
    r = new()
    r.A=a
    r.dim=size(a.mult_table,1)
    @assert length(basis)==r.dim
    r.basis_alg=basis
    r.basis_mat=basis_mat(basis)
    r.basis_mat_inv=inv(r.basis_mat)
    r.ismaximal = 0
    return r
  end

  function AlgAssOrd(a::AlgAss{fmpq}, mat::FakeFmpqMat)
    r = new()
    r.A=a
    r.dim=size(a.mult_table,1)
    r.basis_alg=Array{AlgAssElem{fmpq},1}(rows(mat))
    for i=1:length(r.basis_alg)
      r.basis_alg[i]=elem_from_mat_row(a,mat.num, i, mat.den)
    end
    r.basis_mat=mat
    r.ismaximal = 0
    return r
  end

end

mutable struct AlgAssOrdElem
  elem_in_algebra::AlgAssElem{fmpq}
  elem_in_basis::Vector{fmpz}
  parent::AlgAssOrd

  function AlgAssOrdElem(O::AlgAssOrd, a::AlgAssElem)
    z = new()
    z.elem_in_algebra = a
    z.parent = O
    return z
  end

  function AlgAssOrdElem(O::AlgAssOrd, a::AlgAssElem, arr::Array{fmpz, 1})
    z = new()
    z.parent = O
    z.elem_in_algebra = a
    z.elem_in_basis = arr
    return z
  end

  function AlgAssOrdElem(O::AlgAssOrd, arr::Array{fmpz, 1})
    z = new()
    z.elem_in_algebra = dot(O.basis_alg, arr)
    z.elem_in_basis = arr
    z.parent = O
    return z
  end

end

mutable struct AlgAssOrdIdl
  O::AlgAssOrd                     # Order containing it
  basis_alg::Vector{AlgAssOrdElem} # Basis of the ideal as array of elements of the order
  basis_mat::fmpz_mat              # Basis matrix of ideal wrt basis of the order
  gens::Vector{AlgAssOrdElem}      # Generators of the ideal

  function AlgAssOrdIdl(a::AlgAssOrd, basis::Array{AlgAssOrdElem,1})
    # "Default" constructor with default values.
    r = new()
    r.O=a
    r.basis_alg=basis
    r.basis_mat=zero_matrix(FlintZZ, a.dim, a.dim)
    for i=1:a.dim
      for j=1:a.dim
        r.basis_mat[i,j]=basis[i].elem_in_basis[j]
      end
    end
    return r
  end

  function AlgAssOrdIdl(a::AlgAssOrd, M::fmpz_mat)
    r = new()
    r.O=a
    r.basis_alg=Array{AlgAssOrdElem,1}(a.dim)
    for i=1:a.dim
      r.basis_alg[i]=elem_from_mat_row(a,M,i)
    end
    r.basis_mat=M
    return r
  end

end

###############################################################################
#
#  Some auxiliary functions
#
###############################################################################

function lift(M::fq_nmod_mat)

  N=zero_matrix(FlintZZ, rows(M), cols(M))
  for i=1:rows(M)
    for j=1:cols(M)
      N[i,j]=FlintZZ(coeff(M[i,j],0))
    end
  end
  return N
end

function FakeFmpqMat(x::Array{fmpq,1})
  dens=[denominator(x[i]) for i=1:length(x)]
  den=lcm(dens)
  M=zero_matrix(FlintZZ, 1, length(x))
  for i=1:length(x)
    M[1,i]=numerator(x[i])*divexact(den, dens[i])
  end
  return FakeFmpqMat(M,den)
end

function find_elem(G::Array{T,1}, el::T) where T
  i=1
  while true
    if el.prim_img==G[i].prim_img
      return i
    end
    i+=1
  end
end

###############################################################################
#
#  Functions for ideals
#
###############################################################################

function ideal(O::AlgAssOrd, mat::fmpz_mat)
  return AlgAssOrdIdl(O,mat)
end

function in(x::AlgAssOrdElem, I::AlgAssOrdIdl)

  y=matrix(FlintZZ, 1, length(I.basis_alg), elem_in_basis(x))
  M1, d =pseudo_inv(I.basis_mat)
  if FakeFmpqMat(y*M1, d).den==1
    return true
  else
    return false
  end

end

###############################################################################
#
#  Functions for elements of order
#
###############################################################################

@inline parent(x::AlgAssOrdElem)=x.parent

(O::AlgAssOrd)(a::Array{fmpz,1}) = AlgAssOrdElem(O,a)

function elem_in_basis(x::AlgAssOrdElem, copy::Type{Val{T}} = Val{true}) where T
  if isdefined(x, :elem_in_basis)
    if copy==Val{true}
      return deepcopy(x.elem_in_basis)
    else
      return x.elem_in_basis
    end
  else
    M=FakeFmpqMat(x.elem_in_algebra.coeffs)*x.parent.basis_mat_inv
    x.elem_in_basis=Array{fmpz,1}(M.c)
    for i=1:M.c
      x.elem_in_basis[i]=M.num[1,i]
    end
  end
  if copy==Val{true}
    return deepcopy(x.elem_in_basis)
  else
    return x.elem_in_basis
  end
end

function *(x::AlgAssOrdElem, y::AlgAssOrdElem)
  @assert parent(x)==parent(y)
  O=parent(x)
  return O(x.elem_in_algebra*y.elem_in_algebra)
end

function *(n::Int, x::AlgAssOrdElem)

  O=x.parent
  y=Array{fmpz,1}(O.dim)
  z=elem_in_basis(x, Val{false})
  for i=1:O.dim
    mul!(y[i], x.elem_in_basis[i], n)
  end
  return AlgAssOrdElem(O,y)

end

function in(x::AlgAssElem, O::AlgAssOrd)

  y=FakeFmpqMat(x.coeffs)
  if isdefined(O, :basis_mat_inv)
    M1=O.basis_mat_inv
  else
    M1=inv(O.basis_mat)
    O.basis_mat_inv=M1
  end
  if (y*M1).den==1
    return true
  else
    return false
  end

end

(O::AlgAssOrd)(a::AlgAssElem) = begin
  if !isdefined(O, :basis_mat_inv)
    O.basis_mat_inv=inv(O.basis_mat)
  end
  x=FakeFmpqMat(a.coeffs)*O.basis_mat_inv
  @assert denominator(x)==1
  return AlgAssOrdElem(O,vec(Array(x.num)))
end

function elem_from_mat_row(O::AlgAssOrd, M::fmpz_mat, i::Int)
  return AlgAssOrdElem(O, [M[i,j] for j=1:O.dim])
end

###############################################################################
#
#  Functions for orders
#
###############################################################################

function basis_mat(A::Array{AlgAssElem{fmpq}, 1})
  @assert length(A) > 0
  n = length(A)
  d = size(parent(A[1]).mult_table,1)

  M = zero_matrix(FlintZZ, n, d)

  dens = [lcm([denominator(A[i].coeffs[j]) for j=1:d]) for i=1:n]
  deno = lcm(dens)

  for i in 1:n
    temp_den = divexact(deno, dens[i])
    for j in 1:d
      M[i, j] = numerator(A[i].coeffs[j]*temp_den)
    end
  end
  return FakeFmpqMat(M, deno)
end

function elem_from_mat_row(A::AlgAss, M::fmpz_mat, i::Int, d::fmpz=fmpz(1))
  return A([M[i,j]//d for j=1:cols(M)])
end

function order_gen(O::AlgAssOrd)

  M=O.basis_mat
  if isdefined(O, :basis_mat_inv)
    M1=O.basis_mat_inv
  else
    M1=inv(M)
  end
  for x in O.basis_alg
    for y in O.basis_alg
      if !(x*y in O)
        a=FakeFmpqMat((x*y).coeffs)
        N=vcat(M,a)
        return AlgAssOrd(O.A, hnf(N))
      end
    end
  end

end

###############################################################################
#
#  Sum of orders
#
###############################################################################

# Be careful!
# To be used only in the case of the construction of a maximal order!
function +(a::AlgAssOrd, b::AlgAssOrd)
  aB = a.basis_mat
  bB = b.basis_mat
  d = a.dim
  c = sub(_hnf(vcat(bB.den*aB.num, aB.den*bB.num), :lowerleft), d + 1:2*d, 1:d)
  return AlgAssOrd(a.A, FakeFmpqMat(c, aB.den*bB.den))
end

###############################################################################
#
#  Print
#
###############################################################################

function show(io::IO, O::AlgAssOrd)
  print(io, "Order of ")
  println(io, O.A)
end

function show(io::IO, a::AlgAssOrdElem)
  print(io, a.elem_in_algebra.coeffs)
end

function show(io::IO, a::AlgAssOrdIdl)
  print(io, "Ideal of ")
  print(io, a.O)
  println(io, "with basis matrix")
  print(io, a.basis_mat)
end

###############################################################################
#
#  Construction of a crossed product algebra
#
###############################################################################

#K/Q is a Galois extension.
function CrossedProductAlgebra(K::AnticNumberField, G::Array{T,1}, cocval::Array{nf_elem, 2}) where T

  n=degree(K)

  #=
  Multiplication table
  I order the basis in this way:
  First, I put the basis of the Galois Group, then the product of them with the first
  element of basis of the order and so on...
  =#

  M=Array{fmpq,3}(n^2, n^2, n^2)
  for i=1:n^2
    for j=1:n^2
      for s=1:n^2
        M[i,j,s]=fmpq(0)
      end
    end
  end
  B=basis(K)
  for i=1:n
    for j=1:n
      #I have the element B[i]*G[j]
      for k=1:n
        for h=1:n
          # I take the element B[k]*G[h]
          # and I take the product
          # B[i]*G[j]* B[k]*G[h]=B[i]*G[j](B[k])*c[j,h]*(G[j]*G[h])
          ind=find_elem(G,G[j]*G[h])
          x=B[i]*G[j](B[k])*cocval[j,h]
          #@show i, j, k,h,  ind,B[i],G[j](B[k]),cocval[j,h],  x
          for s=0:n-1
            M[j+(i-1)*n, h+(k-1)*n, ind+s*n]=coeff(x,s)
          end
          #@show M
        end
      end
    end
  end
  return AlgAss(FlintQQ, M)

end

function CrossedProductAlgebraWithMaxOrd(O::NfOrd, G::Array{T,1}, cocval::Array{nf_elem, 2}) where T

  n=degree(O)
  K=nf(O)
  #=
  Multiplication table
  I order the basis in this way:
  First, I put the basis of the Galois Group, then the product of them with the first
  element of basis of the order and so on...
  =#

  M=Array{fmpq,3}(n^2, n^2, n^2)
  for i=1:n^2
    for j=1:n^2
      for s=1:n^2
        M[i,j,s]=fmpq(0)
      end
    end
  end
  B=basis(O)
  for i=1:n
    for j=1:n
      #I have the element B[i]*G[j]
      for k=1:n
        for h=1:n
          # I take the element B[k]*G[h]
          # and I take the product
          # B[i]*G[j]* B[k]*G[h]=B[i]*G[j](B[k])*c[j,h]*(G[j]*G[h])
          ind=find_elem(G,G[j]*G[h])
          x=B[i]*O(G[j](K(B[k])))*O(cocval[j,h])
          y=elem_in_basis(x)
          for s=0:n-1
            M[j+(i-1)*n, h+(k-1)*n, ind+s*n]=y[s+1]
          end
        end
      end
    end
  end
  return AlgAss(FlintQQ, M)

end

###############################################################################
#
#  Quaternion algebras
#
###############################################################################

function quaternion_algebra(a::Int, b::Int)

  M=Array{fmpq,3}(4,4,4)
  for i=1:4
    for j=1:4
      for k=1:4
        M[i,j,k]=0
      end
    end
  end
  M[1,1,1]=1 # 1*1=1
  M[1,2,2]=1 # 1*i=i
  M[1,3,3]=1 # 1*j=j
  M[1,4,4]=1 # 1*ij=1

  M[2,1,2]=1
  M[2,2,1]=a
  M[2,3,4]=1
  M[2,4,3]=a

  M[3,1,3]=1
  M[3,2,4]=-1
  M[3,3,1]=b
  M[3,4,2]=-b

  M[4,1,4]=1
  M[4,2,3]=-a
  M[4,3,2]=b
  M[4,4,1]=-a*b
  return AlgAss(FlintQQ, M)

end

###############################################################################
#
#  Quotient
#
###############################################################################

function quo(O::AlgAssOrd, p::Int)

  F,a=FiniteField(p,1,"a")
  M=Array{fq_nmod, 3}(O.dim, O.dim, O.dim)
  x=fmpz[0 for i=1:O.dim]
  for i=1:O.dim
    x[i]=1
    N=representation_matrix(O(x))
    for j=1:O.dim
      for k=1:O.dim
        M[i,j,k]=F(N[j,k])
      end
    end
    x[i]=0
  end
  oneO=elem_in_basis(O(one(O.A)))
  oneQ=fq_nmod[F(s) for s in oneO]
  return AlgAss(F,M, oneQ)

end

function _mod(x::fmpz_mat, y::fmpz_mat, pivots::Array{Int,1})
   for i=1:length(pivots)
     for k=1:cols(x)
       z=div(x[pivots[i],k], y[k,k])
       if z!=0
         for j=k:cols(x)
           x[pivots[i],j]-=z*y[k,j]
         end
       end
     end
   end
   return nothing
end

function quo(O::AlgAssOrd, I::AlgAssOrdIdl, p::Int)
  pivots=Int[]
  for i=1:O.dim
    if I.basis_mat[i,i]==p
      push!(pivots, i)
    end
  end
  @hassert :CSAMaxOrd 1 check_ideal(I)
  F,a=FlintFiniteField(p,1,"a")
  M=Array{fq_nmod, 3}(length(pivots), length(pivots), length(pivots))
  x=fmpz[0 for s=1:O.dim]
  for i=1:length(pivots)
    x[pivots[i]]=1
    y=O(x)
    N=representation_matrix(y)
    _mod(N, I.basis_mat, pivots)
    for j=1:length(pivots)
      #reduce the vector with respect to the ideal.
      #I assume that the basis of the ideal is in upper triangular HNF
      for k=1:length(pivots)
        M[i,j,k]=F(N[pivots[j],pivots[k]])
      end
    end
    x[pivots[i]]=0
  end
  A=AlgAss(F,M)
  function AtoO(a::AlgAssElem)
    x=fmpz[0 for i=1:O.dim]
    for i=1:length(pivots)
      x[pivots[i]]=FlintZZ(coeff(a.coeffs[i],0))
    end
    return O(x)
  end

  return A, AtoO

end

###############################################################################
#
#  Center
#
###############################################################################

function _rep_for_center(M::MatElem, A::AlgAss)

  n=dim(A)
  for i=1:n
    for j = 1:n
      for k = 1:n
        M[k+(i-1)*n, j] = A.mult_table[i, j, k]-A.mult_table[j, i, k]
      end
    end
  end
  return nothing
end

function center(A::AlgAss{T}) where {T}

  if iscommutative_known(A) && A.iscommutative==1
    return A, AlgAssMor(A, A, identity_matrix(base_ring(A), dim(A)))
  end
  n=dim(A)
  M=zero_matrix(base_ring(A), n^2, n)
  # I concatenate the difference between the right and left representation matrices.
  _rep_for_center(M,A)
  k,B=nullspace(M)
  res=Array{AlgAssElem{T},1}(k)
  for i=1:k
    res[i]= A(T[B[j,i] for j=1:n])
  end
  return subalgebra(A, res)
end

###############################################################################
#
#  Some tests
#
###############################################################################

function check_ideal(I::AlgAssOrdIdl)

  O=I.O
  x=fmpz[0 for i=1:O.dim]
  for i=1:O.dim
    x[i]=1
    y=O(x)
    for j=1:O.dim
      @assert y*I.basis_alg[j] in I
      @assert I.basis_alg[j]*y in I
    end
    x[i]=0
  end
  return true

end

function check_order(O::AlgAssOrd)

  for x in O.basis_alg
    @assert denominator(minpoly(x))==1
    for y in O.basis_alg
      if !(x*y in O)
        @show x,y
        error("not in the order!")
      end
    end
  end
  return true

end

function check_pradical(I::AlgAssOrdIdl, p::Int)

  O=I.O
  for i=1:O.dim
    x=elem_from_mat_row(O,I.basis_mat, i)
    for j=1:O.dim
      @assert divisible(numerator(trace(x.elem_in_algebra*O.basis_alg[j])), p)
    end
  end
  #=
  if p==2
    for i=1:O.dim
      x=elem_from_mat_row(O,I.basis_mat, i)
      for j=1:O.dim
        for k=1:clog(fmpz(O.dim), p)
          @assert divisible(numerator(trace((x.elem_in_algebra*O.basis_alg[j])^(p^k))), p^(k+1))
        end
      end
    end
  end
  =#
  return true
end

###############################################################################
#
#  ring of multipliers
#
###############################################################################

doc"""
***
    ring_of_multipliers(I::AlgAssOrdIdl)

> Given an ideal I, it returns the ring (I : I)
"""

function ring_of_multipliers(I::AlgAssOrdIdl, p::fmpz=fmpz(1))

  O = I.O
  @hassert :CSAMaxOrd 1 Hecke.check_associativity(O.A)
  @hassert :CSAMaxOrd 1 Hecke.check_distributivity(O.A)
  @hassert :CSAMaxOrd 1 check_ideal(I)
  bmatinv, deno =pseudo_inv(I.basis_mat)
  if isdefined(I, :gens) && length(I.gens)<O.dim
    m=zero_matrix(FlintZZ, O.dim*length(I.gens), O.dim)
    for i=1:length(I.gens)
      M=representation_matrix(I.gens[i])
      mul!(M, M, bmatinv)
      if deno==1
        for s=1:O.dim
          for t=1:O.dim
            m[t+(i-1)*(O.dim),s]=M[s,t]
          end
        end
      else
        for s=1:O.dim
          for t=1:O.dim
            @hassert :CSAMaxOrd 1 divisible(M[s,t], deno)
            m[t+(i-1)*(O.dim),s]=divexact(M[s,t], deno)
          end
        end
      end
    end
  else
    B= I.basis_alg
    m=zero_matrix(FlintZZ, O.dim^2, O.dim)
    for i=1:O.dim
      M=representation_matrix(B[i])
      mul!(M, M, bmatinv)
      if deno==1
        for s=1:O.dim
          for t=1:O.dim
            m[t+(i-1)*(O.dim),s]=M[s,t]
          end
        end
      else
        for s=1:O.dim
          for t=1:O.dim
            @hassert :CSAMaxOrd 1 divisible(M[s,t], deno)
            m[t+(i-1)*(O.dim),s]=divexact(M[s,t], deno)
          end
        end
      end
    end
  end
  #In the case of the p-radical, it is important to do this modulo p
  if p==1
    m = hnf(m)
  else
    hnf_modular_eldiv!(m, p)
  end
  s = prod(m[i,i] for i=1:cols(m))
  if s==1
    return AlgAssOrd(O.A, deepcopy(O.basis_mat))
  end
  # n is upper right HNF
  n=transpose(sub(m, 1:O.dim, 1:O.dim))
  b= FakeFmpqMat(pseudo_inv(n))
  O1=AlgAssOrd(O.A, mul!(b,b,O.basis_mat))
  O1.disc=divexact(O.disc, s^2)
  @hassert :CSAMaxOrd 1 check_order(O1)
  return O1
end

###############################################################################
#
#  p-radical
#
###############################################################################

function assure_basis_mat_inv(O::AlgAssOrd)
  if !isdefined(O, :basis_mat_inv)
    O.basis_mat_inv=inv(O.basis_mat)
  end
  return nothing
end

function pradical_meataxe(O::AlgAssOrd, p::Int)

  A1 = quo(O, p)
  lM = fq_nmod_mat[transpose(representation_matrix(A1[i])) for i=1:O.dim]
  M = FqGModule(lM)
  ls = minimal_submodules(M)
  l = sum(rows(x) for x in ls)
  M1 = zero_matrix(base_ring(A1), l, O.dim)
  i=1
  for x in ls
    for j=1:rows(x)
      for k=1:O.dim
        M1[i,k]=x[j,k]
      end
      i+=1
    end
  end
  r = rref!(M1)
  if r == O.dim
    J= AlgAssOrdIdl(O,MatrixSpace(FlintZZ, O.dim, O.dim, false)(p))
    J.gens=AlgAssOrdElem[O(p*one(O.A))]
    return J
  end
  M1 = sub(M1, 1:r, 1:O.dim)
  dM = transpose(nullspace(M1)[2])
  gens=Vector{AlgAssOrdElem}(rows(dM)+1)
  m = zero_matrix(FlintZZ, O.dim, O.dim)
  for i=1:rows(dM)
    for j=1:cols(dM)
      m[i,j]=FlintZZ(coeff(dM[i,j],0))
    end
    gens[i]= elem_from_mat_row(O,m,i)
  end
  hnf_modular_eldiv!(m, fmpz(p))
  gens[rows(dM)+1]=O(p*one(O.A))
  J=AlgAssOrdIdl(O,m)
  J.gens=gens
  return J

end

doc"""
***
    pradical(O::AlgAssOrd, p::Int)

> Given an order O and a prime p, it returns the radical of the ideal generated by p
"""

function pradical(O::AlgAssOrd, p::Int)

  #See the paper from Ronyai, Structure of finite algebra
  l=clog(fmpz(O.dim),p)
  if l>1
    return pradical_meataxe(O,p)
  end
  n=root(O.dim,2)

  F=ResidueRing(FlintZZ, p, cached=false)

  #TrA=trace_matrix(O.A)
  #First step: kernel of the trace matrix mod p
  W=MatrixSpace(F,O.dim,O.dim, false)
  #if isdefined(O, :trace_mat)
  #  I=W(n*O.trace_mat)
  #else
  #  assure_basis_mat_inv(O)
  #  I=W((O.basis_mat*FakeFmpqMat(TrA)*O.basis_mat_inv).num)
  #end
  I=W(n*redtrace_mat(O))
  B,k=nullspace(I)
  # The columns of B give the coordinates of the elements in the order.
  if k==0
    J= AlgAssOrdIdl(O,MatrixSpace(FlintZZ, O.dim, O.dim, false)(p))
    J.gens=AlgAssOrdElem[O(p*one(O.A))]
    return J
  end

  if l==1
    #In this case, we can output I: it is the standard p-trace method.
    M=zero_matrix(FlintZZ, O.dim, O.dim)
    for i=1:cols(B)
      for j=1:O.dim
        M[i,j]=B[j,i].data
      end
    end
    M1=hnf_modular_eldiv!(M, fmpz(p))
    res=AlgAssOrdIdl(O,sub(M1,1:O.dim,1:O.dim))
    B1=lift(B')
    res.gens=Array{AlgAssOrdElem, 1}(k+1)
    for i=1:k
      res.gens[i]= elem_from_mat_row(O,B1,i)
    end
    res.gens[k+1]= O(p*one(O.A))
    @hassert :CSAMaxOrd 1 check_pradical(res,p)
    return res
  end

  Ide=transpose(lift(B))
  #Now, iterate: we need to find the kernel of tr((xy)^(p^i))/p^i mod p
  #on the subspace generated by I
  #Hard to believe, but this is linear on I!!!!
  for i=1:l
    N=zero_matrix(F, O.dim, rows(Ide))
    a=O.A()
    for t=1:rows(Ide)
      elm=elem_from_mat_row(O,Ide,t)
      for s=1:O.dim
        mul!(a, elm.elem_in_algebra, O.basis_alg[s])
        bbb = (a^(p^i))
        trel=trace(bbb, TrA)
        el=divexact(numerator(trel),p^i)
        N[s,t]=F(el)
      end
    end
    B2,k=nullspace(N)
    if k==0
      J= AlgAssOrdIdl(O,MatrixSpace(FlintZZ, O.dim, O.dim, false)(p))
      J.gens=AlgAssOrdElem[O(p*one(O.A))]
      return J
    end
    Ide=lift(transpose(B2))*Ide
  end
  gens = Vector{AlgAssOrdElem}(rows(Ide)+1)
  for i in 1:rows(Ide)
    gens[i] = elem_from_mat_row(O, Ide, i)
  end
  gens[rows(Ide)+1]= O(p*one(O.A))
  #Now, I have to give a basis for the ideal.
  m=zero_matrix(FlintZZ, O.dim, O.dim)
  for i=1:rows(Ide)
    for j=1:cols(Ide)
      m[i,j]=Ide[i,j]
    end
  end
  hnf_modular_eldiv!(m,fmpz(p))
  res=AlgAssOrdIdl(O,m)
  res.gens=gens
  @hassert :CSAMaxOrd 1 check_pradical(res,p)
  return res

end

function trace(x::AlgAssElem, M::fmpq_mat)
  M1=matrix(FlintQQ, 1, rows(M), x.coeffs)
  M2=matrix(FlintQQ, rows(M), 1, x.coeffs)
  return (M1*M*M2)[1,1]
end

###############################################################################
#
#  Trace, Discriminant and Reduced Trace Matrix
#
###############################################################################

function representation_matrix(x::AlgAssOrdElem)

  O = parent(x)
  M = O.basis_mat
  if isdefined(O, :basis_mat_inv)
    M1 = O.basis_mat_inv
  else
    M1 = inv(O.basis_mat)
    O.basis_mat_inv=M1
  end
  B = FakeFmpqMat(representation_matrix(x.elem_in_algebra))
  mul!(B, M, B)
  mul!(B, B, M1)

  @assert B.den==1
  return B.num
end

function trace(x::AlgAssElem)
  M=representation_matrix(x)
  return trace(M)
end

function trace(x::AlgAssOrdElem)
  if isdefined(parent(x), :trace_mat)
    O=parent(x)
    n=root(O.dim,2)
    return n*(matrix(FlintZZ, 1, O.dim, elem_in_basis(x))*O.trace_mat*matrix(FlintZZ, O.dim, 1, elem_in_basis(x)))[1,1]
  else
    return trace(x.elem_in_algebra)
  end
end

function redtrace_mat(O::AlgAssOrd)

  if isdefined(O, :trace_mat)
    return O.trace_mat
  end
  x=O.basis_alg
  m=length(x)
  n=root(O.dim,2)
  M=zero_matrix(FlintZZ, m, m)
  for i=1:m
    M[i,i]=divexact(numerator(trace(x[i]^2)),n)
  end
  for i=1:m
    for j=i+1:m
      a=divexact(numerator(trace(x[i]*x[j])),n)
      M[i,j]=a
      M[j,i]=a
    end
  end
  O.trace_mat=M
  return M

end

function discriminant(O::AlgAssOrd)

  if isdefined(O, :disc)
    return O.disc
  end
  M=redtrace_mat(O)
  O.disc=det(M)
  return O.disc

end

###############################################################################
#
#  Schur Index at Infinity
#
###############################################################################

#Steel Nebe paper
doc"""
***
    schur_index_at_real_plc(O::AlgAssOrd)

> Given an order O, this function returns the schur index
> of the algebra over the field of real numbers
"""
function schur_index_at_real_plc(O::AlgAssOrd)

  x=trace_signature(O)
  n=root(O.dim,2)
  if x[1]==divexact(n*(n+1),2)
    return 1
  else
    return 2
  end

end

function trace_signature(O::AlgAssOrd)

  M=redtrace_mat(O)
  # This can be improved using Sturm sequences
  Zx,x=PolynomialRing(FlintZZ, "x")
  f=charpoly(Zx,M)
  #if issquarefree(f)
  #  a=number_positive_roots(f)
  #  b=degree(f)-a
  #  return (a,b)
  #else
    ff=factor(f)
    sgtpos=0
    sgtneg=0
    for (h,v) in ff.fac
      if degree(h)==1
        if coeff(h,0)>0
          sgtneg+=v
        else
          sgtpos+=v
        end
        continue
      end
      p=64
      while p<1024
        sgtposf=0
        sgtnegf=0
        R=AcbField(p, false)
        Rx=AcbPolyRing(R, Symbol("x"), false)
        g=Rx(h)
        l=roots(g)
        for i=1:length(l)
          y=real(l[i])
          if ispositive(y)
            sgtposf+=1
          end
          if isnegative(y)
            sgtnegf+=1
          end
        end
        if sgtposf+sgtnegf==degree(h)
          sgtpos+=sgtposf*v
          sgtneg+=sgtnegf*v
          break
        else
          p*=2
        end
      end
      if p>1024
        error("Precision issue")
      end
    end
    return (sgtpos, sgtneg)
  #end

end

###############################################################################
#
#  Schur Index at p
#
###############################################################################

doc"""
***
    schur_index_at_p(O::AlgAssOrd, p::fmpz)

> Given a maximal order O and a prime p, this function returns the schur index
> of the completion of the algebra at p
"""
function schur_index_at_p(O::AlgAssOrd, p::fmpz)
  @assert O.ismaximal==1
  d=discriminant(O)
  v=valuation(d,p)
  if v==0
    return 1
  end
  n=root(O.dim,2)
  t = n - divexact(v,n)
  return divexact(n,t)
end

###############################################################################
#
#  p-maximal overorder
#
###############################################################################

function _maximal_ideals(O::AlgAssOrd, p::Int)

  A1 = quo(O, p)
  lM = fq_nmod_mat[representation_matrix(A1[i]) for i=1:O.dim]
  append!(lM, fq_nmod_mat[representation_matrix(A1[i], :right) for i=1:O.dim])
  M = FqGModule(lM)
  ls = maximal_submodules(M)
  ideals=AlgAssOrdIdl[]
  poneO=O(p*one(O.A))
  for x in ls
    @hassert :CSAMaxOrd 1 closure(x, M.G)==rref(x)[2]
    m = zero_matrix(FlintZZ, O.dim, O.dim)
    gens=Vector{AlgAssOrdElem}(rows(x)+1)
    for i=1:rows(x)
      for j=1:cols(x)
        m[i,j]=FlintZZ(coeff(x[i,j],0))
      end
      gens[i]= elem_from_mat_row(O,m,i)
    end
    hnf_modular_eldiv!(m, fmpz(p))
    gens[rows(x)+1]=poneO
    J=AlgAssOrdIdl(O,m)
    J.gens=gens
    push!(ideals, J)
  end
  return ideals

end

function _maximal_ideals(O::AlgAssOrd, I::AlgAssOrdIdl, p::Int)

  A1 , A1toO= quo(O, I, p)
  lM = fq_nmod_mat[representation_matrix(A1[i]) for i=1:O.dim]
  append!(lM, fq_nmod_mat[representation_matrix(A1[i], :right) for i=1:O.dim])
  M = FqGModule(lM)
  ls = maximal_submodules(M)
  ideals=AlgAssOrdIdl[]
  poneO=O(p*one(O.A))
  for x in ls
    @hassert :CSAMaxOrd 1 closure(x, M.G)==rref(x)[2]
    m = zero_matrix(FlintZZ, O.dim, O.dim)
    gens=Vector{AlgAssOrdElem}(rows(x)+1)
    for i=1:rows(x)
      for j=1:cols(x)
        m[i,j]=FlintZZ(coeff(x[i,j],0))
      end
      gens[i]= elem_from_mat_row(O,m,i)
    end
    hnf_modular_eldiv!(m, fmpz(p))
    gens[rows(x)+1]=poneO
    J=AlgAssOrdIdl(O,m)
    J.gens=gens
    push!(ideals, J)
  end
  return ideals

end

function pmaximal_overorder(O::AlgAssOrd, p::Int)

  d=discriminant(O)
  if rem(d, p^2) != 0
    return O
  end

  if p>O.dim
    return pmaximal_overorder_trace(O,p)
  else
    return pmaximal_overorder_meataxe(O,p)
  end
end

function pmaximal_overorder_meataxe(O::AlgAssOrd, p::Int)

  extend = false
  d=discriminant(O)
  while true
    dd = fmpz(1)
    max_id =_maximal_ideals(O, p)
    for i = 1:length(max_id)
      OO = ring_of_multipliers(max_id[i], fmpz(p))
      dd = discriminant(OO)
      if d != dd
        extend = true
        O = OO
        d = dd
        break
      end
    end

    if extend
      v = valuation(dd, p)
      if v == 0 || v == 1
        break
      end
      extend = false
      continue
    else
      break
    end

  end
  return O
end

function pmaximal_overorder_trace(O::AlgAssOrd, p::Int)

  #First, the head order by computing the pradical and its ring of multipliers
  d=discriminant(O)
  I=pradical(O,p)
  OO=ring_of_multipliers(I, fmpz(p))
  dd=discriminant(OO)
  while dd!= d
    O=OO
    I=pradical(O,p)
    OO=ring_of_multipliers(I, fmpz(p))
    dd=discriminant(OO)
  end
  #Now, we have to check the maximal ideals.
  extend = false
  while true
    dd = fmpz(1)
    max_id =_maximal_ideals(O, p)
    for i = 1:length(max_id)
      OO = ring_of_multipliers(max_id[i], fmpz(p))
      dd = discriminant(OO)
      if d != dd
        extend = true
        O = OO
        d = dd
        break
      end
    end

    if extend
      v = valuation(dd, p)
      if v == 0 || v == 1
        break
      end
      extend = false
      continue
    else
      break
    end

  end
  return O
end

###############################################################################
#
#  Maximal Order
#
###############################################################################

doc"""
***
    MaximalOrder(O::AlgAssOrd)

> Given an order O, this function returns a maximal order containing O
"""

function MaximalOrder(O::AlgAssOrd)
  @vtime :NfOrd fac = factor(root(abs(discriminant(O)),2))
  OO=O
  for (p,j) in fac
    OO = pmaximal_overorder(OO, Int(p))
  end
  OO.ismaximal=1
  return OO
end

###############################################################################
#
#  IsSplit
#
###############################################################################

doc"""
***
    issplit(A::AlgAss)

> Given a Q-algebra A, this function returns true if A splits over Q, false otherwise
"""

function issplit(A::AlgAss)
  O=Hecke.AlgAssOrd(A, [A[i] for i=1:dim(A)])
  i=schur_index_at_real_plc(O)
  if i==2
    return false
  end
  fac = factor(root(abs(discriminant(O)),2))
  for (p,j) in fac
    O = pmaximal_overorder(O, Int(p))
    if valuation(O.disc, Int(p))!=0
      return false
    end
  end
  return true

end

###############################################################################
#
#  Sturm sequence
#
#########à#####################################################################

function sturm_sequence(f::fmpq_poly)

  g=deepcopy(f)
  h=derivative(g)
  seq=fmpz_poly[g,h]
  while true
    q, r=divrem(g,h)
    if r!=0
      push!(seq, r)
      h=g
      g=r
    else
      break
    end
  end
  return seq

end

function _number_changes(a::Array{fmpz,1})
  nc=0
  filter!(x -> x!=0, a)
  for i=2:length(a)
    if sign(a[i])!=sign(a[i-1])
      nc+=1
    end
  end
  return nc
end

function number_positive_roots(f::fmpz_poly)

  s = sturm_sequence(f)
  b = maximum([coeff(f,i) for i=0:degree(f)-1])
  evb=fmpz[x(b) for x in s]
  ev0=fmpz[x(0) for x in s]
  return _number_changes(ev0)-evb
end


text:
module Playground

using Reexport
include("geodesiccomplex.jl")
@reexport using Playground.GeodesicComplexes
include("equilateraliterator.jl")
@reexport using Playground.EquilateralIterators
include("persistence.jl")

export persistence, cleanup

end


text:
import FLOWFarm; const ff = FLOWFarm

rotor_diameter = 80.0
hub_height = 70.0
yaw = 0.0

cut_in_speed = 4.  # m/s
cut_out_speed = 25.  # m/s
rated_speed = 16.  # m/s
rated_power = 2.0E6  # W
generator_efficiency = 0.944

ai = 1.0/3.0
ct = 0.689
wind_speed = 8.0
air_density = 1.1716  # kg/m^3
ambient_ti = 0.077

data = readdlm("inputfiles/horns_rev_locations.txt",  ',', skipstart=1)
turbine_x = data[:, 1].*diam
nturbines = length(turbine_x)
turbine_y = data[:, 2].*diam
turbine_z = zeros(nturbines)

rotor_diameter = zeros(nturbines) .+ 80.0
hub_height = zeros(nturbines) .+ 70.0
sorted_turbine_index = sortperm(turbine_x)
turbine_ct = zeros(nturbines)
turbine_ai = zeros(nturbines)
turbine_inflow_velcities = zeros(nturbines)
turbine_local_ti = zeros(nturbines)
turbine_yaw = zeros(nturbines)
ambient_ti = 0.077
ai = 1.0/3.0
turbine_ai = zeros(nturbines) .+ ai

localtimodel = ff.LocalTIModelMaxTI()

# load thrust curve
ctdata = readdlm("inputfiles/predicted_ct_vestas_v80_niayifar2016.txt",  ',', skipstart=1)
velpoints = ctdata[:,1]
ctpoints = ctdata[:,2]

# initialize thurst model
ct_model1 = ff.ThrustModelCtPoints(velpoints, ctpoints)
ct_model = Vector{typeof(ct_model1)}(undef, nturbines)
for i = 1:nturbines
    ct_model[i] = ct_model1
end

rotor_sample_points_y = [0.0]
rotor_sample_points_z = [0.0]

winddirections = [270.0*pi/180.0]
wind_speed = 8.0
windspeeds = [wind_speed]
windprobabilities = [1.0]
measurementheight = [hub_height[1]]
air_density = 1.1716  # kg/m^3
ambient_tis = [ambient_ti]
shearexponent = 0.15
# initialize wind shear model
wind_shear_model = ff.PowerLawWindShear(shearexponent)

windresource = ff.DiscretizedWindResource(winddirections, windspeeds, windprobabilities, measurementheight, air_density, ambient_tis, wind_shear_model)

wakedeficitmodel = ff.GaussYaw(0.022, 0.022, 2.32, 0.154, 1.0)
wakedeflectionmodel = ff.GaussYawDeflection()
wakecombinationmodel = ff.LinearLocalVelocitySuperposition()
localtimodel = ff.LocalTIModelMaxTI()
model_set = ff.WindFarmModelSet(wakedeficitmodel, wakedeflectionmodel, wakecombinationmodel, localtimodel)

# yaw = 0.0
#
# cut_in_speed = 4.  # m/s
# cut_out_speed = 25.  # m/s
# rated_speed = 16.  # m/s
# rated_power = 2.0E6  # W
# generator_efficiency = 0.944
#
#
#
#
# ambient_ti = 0.077
#
# turbine_z = zeros(nturbines)
# turbine_yaw = zeros(nturbines)
# turbine_ct = zeros(nturbines) .+ ct
#
#
#
# turbine_inflow_velcities = zeros(nturbines) .+ wind_speed
#
# # rotor sample points
# rotor_points_y = [0.0]
# rotor_points_z = [0.0]
#
# # load power curve
# powerdata = readdlm("inputfiles/niayifar_vestas_v80_power_curve_observed.txt",  ',', skipstart=1)
# velpoints = powerdata[:,1]
# powerpoints = powerdata[:,2]*1E6
#
# # initialize power model
# power_model = ff.PowerModelPowerPoints(velpoints, powerpoints)
#
#
#
#
#
# # initialize turbine definition
# turbine1 = ff.TurbineDefinition(1, [rotor_diameter], [hub_height], [cut_in_speed], [rated_speed], [cut_out_speed], [rated_power], [generator_efficiency], ct_model, power_model)
#
# turbine_definitions = [turbine1 for i in 1:nturbines]
# sorted_turbine_index = sortperm(turbine_x)
# turbine_definition_ids = ones(Int, nturbines)
#
# windfarm = ff.WindFarm(turbine_x, turbine_y, turbine_z, turbine_definition_ids, turbine_definitions)
# windfarmstate = ff.SingleWindFarmState(1, turbine_x, turbine_y, turbine_z, turbine_yaw, turbine_ct, turbine_ai, turbine_inflow_velcities, zeros(nturbines), (zeros(nturbines).+ambient_ti),sorted_turbine_index)
#
#
#
#
#
# pd4 = ff.WindFarmProblemDescription(windfarm, windresource, [windfarmstate])


text:
# Prepare a solvehook to solve via a Lagrangian relaxation in SDDP

# Overload how stage problems are solved in the backward pass
function SDDP.JuMPsolve(::Type{SDDP.BackwardPass}, m::SDDPModel, sp::JuMP.Model)
    if sp.solvehook == nothing
        JuMP.solve(sp)
    else
        JuMP.solve(sp, require_duals=true, iteration=length(m.log))
    end
end

lagrangian(m::JuMP.Model) = m.ext[:Lagrangian]

struct MixedSolvers
    LP::JuMP.MathProgBase.AbstractMathProgSolver
    MIP::JuMP.MathProgBase.AbstractMathProgSolver
end

# Just handy caching
struct IncreasingPattern
    benders::Int
    strengthenedbenders::Int
    integeroptimality::Int
    lagrangian::Int
end
"""
    Pattern(;benders=0, strengthened_benders=0, integer_optimality=0, lagrangian=1)

Construct a cut pattern.

# Example:
Pattern(benders=0, strengthened_benders=1, integer_optimality=0, lagrangian=4)
means: in every cycle of 5 iterations, add 1 strengthened benders cut and 4
lagrangian cuts.
"""
function Pattern(;benders=0, strengthened_benders=0, integer_optimality=0, lagrangian=1)
    @assert benders >= 0 && strengthened_benders >= 0 && integer_optimality >= 0 && lagrangian >= 0
    strengthened_benders += benders
    integer_optimality += strengthened_benders
    lagrangian += integer_optimality
    @assert lagrangian > 0
    IncreasingPattern(benders, strengthened_benders, integer_optimality, lagrangian)
end

function getcuttype(iteration::Int, p::IncreasingPattern)
    if mod(iteration, p.lagrangian) + 1 <= p.benders
        return :benders
    elseif mod(iteration, p.lagrangian) + 1 <= p.strengthenedbenders
        return :strengthened_benders
    elseif mod(iteration, p.lagrangian) + 1 <= p.integeroptimality
        return :integer_opt
    else
        return :lagrangian
    end
end

# The solvehook
function SDDiPsolve!(sp::JuMP.Model; require_duals::Bool=false, iteration::Int=-1, kwargs...)
    @assert !(require_duals && iteration == -1)
    solvers = sp.ext[:solvers]
    if require_duals && SDDP.ext(sp).stage > 1
        # Update the objective we cache in case the objective has noises
        l = lagrangian(sp)
        l.obj = getobjective(sp)
        cuttype = getcuttype(iteration, sp.ext[:pattern])
        if cuttype == :benders
            # Solve linear relaxation
            setsolver(sp, solvers.LP)
            status = JuMP.solve(sp, ignore_solve_hook=true, relaxation=true)
        elseif cuttype == :strengthened_benders
            # Get the LP duals
            setsolver(sp, solvers.LP)
            @assert JuMP.solve(sp, ignore_solve_hook=true, relaxation=true) == :Optimal
            π = -SDDP.getdual.(SDDP.states(sp))
            # Update slacks because RHSs change each iteration
            l.slacks = getslack.(l.constraints)
            # Relax bounds to formulate Lagrangian
            Lagrangian.relaxandcache!(l, sp)
            # Change the MIP objective
            Lagrangian.setlagrangianobjective!(sp, l, π)
            # Solve the Lagrangian, with LP πs chosen and fixed
            setsolver(sp, solvers.MIP)
            status = solve(sp, ignore_solve_hook=true)
            # Undo changes
            sp.obj = l.obj
            Lagrangian.recover!(l, sp, π)
        elseif cuttype == :integer_opt
            status = JuMP.solve(sp, ignore_solve_hook=true)
            Q = getobjectivevalue(sp)
            # Need a bound on the future cost at every point other than the
            # current state. The cached problem bound will do.
            L = SDDP.ext(sp).problembound
            for s in SDDP.states(sp)
                x0 = sp.linconstr[s.constraint.idx].terms.vars[1]
                idx = s.constraint.idx
                if getvalue(x0) > 0.5
                    sp.linconstrDuals[idx] = Q - L
                else
                    sp.linconstrDuals[idx] = L - Q
                end
            end
        else
            # Update initial bound of the dual problem
            @assert solve(sp) == :Optimal
            l.method.initialbound = getobjectivevalue(sp)
            # Slacks have a new RHS each iteration, so update them
            l.slacks = getslack.(l.constraints)
            # Somehow choose duals to start with
            π0 = zeros(length(l.constraints)) # or rand, or ones
            # Lagrangian objective and duals
            setsolver(sp, solvers.MIP)
            status, _ = lagrangiansolve!(l, sp, π0)
        end
        sp.obj = l.obj
    else
        # We are in the forward pass, or we are in stage 1
        setsolver(sp, solvers.MIP)
        status = JuMP.solve(sp, ignore_solve_hook=true)
    end
    status
end

"""
    setSDDiPsolver!(sp::JuMP.Model; method=Subgradient(0.0), pattern=Pattern(), MIPsolver=sp.solver, LPsolver=MIPsolver)

Sets a JuMP solvehook for integer SDDP to stage problem `sp` that will call a
a Lagrangian solver of type `method.` Argument `pattern` can be used to specify
a pattern of different cut types.

You should specify an LP/MIP solver if you are using different cut types in a cut
pattern, and you are not using a solver that can solve both MIPs and LPs.
"""
function setSDDiPsolver!(sp::JuMP.Model; method=Subgradient(0.0), pattern=Pattern(), MIPsolver=sp.solver, LPsolver=MIPsolver)

    constraints = SDDP.LinearConstraint[]
    for s in SDDP.states(sp)
        # xinₜ = xoutₜ₋₁ is being relaxed
        push!(constraints, s.constraint)
        # xout has to be binary
        setcategory(s.variable, :Bin)
        # Explicitly add bounds in case we relax integrality on some iterations
        setlowerbound(s.variable, 0.0)
        setupperbound(s.variable, 1.0)
        # Set upper/lower bounds on the incoming state
        x0 = sp.linconstr[s.constraint.idx].terms.vars[1]
        setlowerbound(x0, 0.0)
        setupperbound(x0, 1.0)
    end

    relaxed_bounds = ones(length(constraints))
    sp.ext[:Lagrangian] = LinearProgramData(QuadExpr(),         # objective
                                           constraints,         # relaxed constraints
                                           relaxed_bounds,      # RHS of relaxed constraints
                                           method=method)       # method to solve
    # Store pattern and solvers
    sp.ext[:pattern] = pattern
    sp.ext[:solvers] = MixedSolvers(LPsolver, MIPsolver)

    JuMP.setsolvehook(sp, SDDiPsolve!)
end