using ModelingToolkit
using DifferentialEquations
using PowerDynamics
using FFTW
using LinearAlgebra
using PlotlyJS
using DataFrames
using Polynomials

function getStateIndex(pg::PowerGrid,node_str::String,sym::Symbol)
    ind = 0
    node = pg.nodes[node_str]
    for (ind_tmp,val) in enumerate(pg.nodes)
        if val[2] == node
            ind = ind + indexin([sym],symbolsof(node))[1]
            break
        end
       ind += length(symbolsof(val[2]))
    end
    return ind
end

function getPreFaultVoltages(pg::PowerGrid,ic_prefault::Array{Float64,1},ic_endfault::Array{Float64,1})
    ind = getVoltageSymbolPositions(pg)
    ic_endfault[ind] = ic_prefault[ind]
    return ic_endfault
end

function getPreFaultVoltages(pg::PowerGrid,ic_prefault,ic_endfault)
    ind = getVoltageSymbolPositions(pg)
    ic_endfault[ind] = ic_prefault[ind]
    return ic_endfault
end

function getPreFaultAlgebraicStates(pg::PowerGrid,ic_prefault::Array{Float64,1},ic_endfault::Array{Float64,1})
    prob = ODEFunction(rhs(pg))
    ind = findall(x-> iszero(x),diag(prob.mass_matrix))
    ic_endfault[ind] = ic_prefault[ind]
    return ic_endfault
end

function getVoltageSymbolPositions(pg::PowerGrid)
    n = map(x->PowerDynamics.variable_index(pg.nodes,x,:u_r),collect(keys(pg.nodes)))
    append!(n,map(x->PowerDynamics.variable_index(pg.nodes,x,:u_i),collect(keys(pg.nodes))))
end

function getComplexBusVoltage(pg::PowerGrid,ic::Array{Float64,1})
        ind = sort(getVoltageSymbolPositions(pg))
        Uc = Complex.(zeros(Int(length(ind)/2)))
        for (i,val) in enumerate(ind[1:2:length(ind)])
            Uc[i] = ic[val] + 1im*ic[val+1]
        end
        return Uc
end

function getSymbolPosition(pg::PowerGrid,syms::Symbol)
    ind =  indexin([syms],rhs(pg).syms)[1]
    if isnothing(ind)
        error(syms," is not a valid symbol")
    end
    return ind
end

function getSymbolPosition(pg::PowerGrid,syms::Array{Symbol,1})
    return sort(indexin(syms,rhs(pg).syms))
end

function AddNaNsIntoSolution(pg1::PowerGrid,pg2::PowerGrid,sol)
    key1 = collect(keys(pg1.nodes))
    key2 = collect(keys(pg2.nodes))
    n = findfirst(x->x==nothing,indexin(key1,key2))
    if ~isnothing(n)
        len_bus = length(symbolsof(pg1.nodes[key1[n]]))
        first_ind = PowerDynamics.variable_index(pg1.nodes,key1[n],:u_r)
        start_u = findfirst(x-> x== length(sol.u[end]),length.(sol.u))
        for i in start_u:length(sol.u)
            splice!(sol.u[i],first_ind:first_ind-1,NaN*zeros(len_bus))
            for j in sol.k[i]
                splice!(j,first_ind:first_ind-1,NaN*zeros(len_bus))
            end
        end
    end
    return sol
end

# The following PiModel function is copied from the actual PowerDynamics package, Michael
function PiModel(y, y_shunt_km, y_shunt_mk, t_km, t_mk)
    Π = Matrix{Any}(undef,2,2)#zeros(Complex{Float64}, 2, 2)
    Π[1, 1] = - abs2(t_km) * (y + y_shunt_km) # Our sign convention is opposite for the source of the edge
    Π[1, 2] = conj(t_km) * t_mk * y # Our sign convention is opposite for the source of the edge
    Π[2, 1] = - conj(t_mk) * t_km * y
    Π[2, 2] = abs2(t_mk) * (y + y_shunt_mk)
    Π
end

function PiModel(y::Complex{Num}, y_shunt_km, y_shunt_mk, t_km, t_mk)
    Π = Array{Union{Complex{Float64},Complex{Num}}}(undef,2,2)
    Π[1, 1] = - abs2(t_km) * (y + y_shunt_km) # Our sign convention is opposite for the source of the edge
    Π[1, 2] = conj(t_km) * t_mk * y # Our sign convention is opposite for the source of the edge
    Π[2, 1] = - conj(t_mk) * t_km * y
    Π[2, 2] = abs2(t_mk) * (y + y_shunt_mk)
    Π
end

function PiModel(y, y_shunt_km, y_shunt_mk, t_km, t_mk::Num)
    Π = Array{Union{Complex{Float64},Complex{Num}}}(undef,2,2)
    Π[1, 1] = - abs2(t_km) * (y + y_shunt_km) # Our sign convention is opposite for the source of the edge
    Π[1, 2] = conj(t_km) * t_mk * y # Our sign convention is opposite for the source of the edge
    Π[2, 1] = - conj(t_mk) * t_km * y
    Π[2, 2] = abs2(t_mk) * (y + y_shunt_mk)
    Π
end

function PiModel(y, y_shunt_km, y_shunt_mk, t_km::Num, t_mk)
    Π = Array{Union{Complex{Float64},Complex{Num}}}(undef,2,2)
    Π[1, 1] = - abs2(t_km) * (y + y_shunt_km) # Our sign convention is opposite for the source of the edge
    Π[1, 2] = conj(t_km) * t_mk * y # Our sign convention is opposite for the source of the edge
    Π[2, 1] = - conj(t_mk) * t_km * y
    Π[2, 2] = abs2(t_mk) * (y + y_shunt_mk)
    Π
end

function CalcEigenValues(pg::PowerGrid, p)
    mtsys = GetMTKSystem(pg, (0.0, 1.0), p)
    CalcEigenValues(mtsys)
end

function CalcEigenValues(mtsys::ODESystem; output::Bool = false, plot::Bool = false)
  Fx, Fy, Gx, Gy = GetSymbolicFactorizedJacobian(mtsys)
  Fxf, Fyf, Gxf, Gyf = [
    substitute(f, mtsys.defaults,fold=true) for
    f in [Fx, Fy, Gx, Gy]
  ]
  Af = Fxf - Fyf * inv(Gyf) * Gxf
  Af = substitute.(Symbolics.value.(Af),([],)) # um alle Ausdrücke innerhalb der Matrix einmal auszuwerten!
  EW = eigvals(Symbolics.value.(Af))

  eqs,aeqs,x,y = GetSymbolicEquationsAndStates(mtsys)
  index = indexin(x, states(mtsys))
  syms = states(mtsys)[index]
  if output
    display("|ID | Real-part | Imag-part | Frequency | Damping Time Constant |")
    for (ind, ew) in enumerate(EW)
      display(
        "| $(syms[ind])) | $(round(real(ew),digits =3)) | $(round(imag(ew),digits = 3)) | $(round(abs(imag(ew))/2/pi,digits =3)) | $(round(1.0/abs(real(ew)),digits =3)) |",
      )
    end
  end
  if plot
      Plots.scatter([real(EW)[1]],[imag(EW)[1]], legend = :outertopright) #,label = String(syms[1])
      for i in 2:length(EW)
         Plots.scatter!([real(EW)[i]],[imag(EW)[i]]) #,label = String(syms[i])
      end
      ylims!((-maximum(imag.(EW))*1.1,maximum(imag.(EW))*1.1))
      min_real_ew = minimum(real.(EW))
      Plots.plot!([min_real_ew;0.0],[min_real_ew*20.0;0.0], linestyle=:dash,linecolor = :red, label = "5 % damping")
      Plots.plot!([min_real_ew;0.0],[-min_real_ew*20.0;0.0], linestyle=:dash,linecolor = :red, label = nothing)
      Plots.plot!([min_real_ew;0.0],[min_real_ew*10.0;0.0], linestyle=:dash,linecolor = :blue, label = "10 % damping")
      Plots.plot!([min_real_ew;0.0],[-min_real_ew*10.0;0.0], linestyle=:dash,linecolor = :blue, label = nothing)
      Plots.plot!([min_real_ew;0.0],[min_real_ew*5.0;0.0], linestyle=:dash,linecolor = :green, label = "20 % damping")
      display(Plots.plot!([min_real_ew;0.0],[-min_real_ew*5.0;0.0], linestyle=:dash,linecolor = :green, label = nothing));
  end
  return EW
end

function DFTplot(signal,t,fmax;norm=true)
    F,freqs = DFT(signal,t,norm_to_fundamental=norm)
    display(bar(freqs,abs.(F),xlims=(0,fmax)))
end

function DFT(signal,t;norm_to_fundamental=false)
    N = length(t)
    Ts = t[end]/N  #it is assumed that measured point are equally distributed
    # Fourier Transform of it
    F = fft(signal) |> fftshift
    freqs = fftfreq(N, 1.0/Ts) |> fftshift
    if norm_to_fundamental
        ind = indexin([0.0],freqs)[1]+1
        F ./= abs.(F[ind])
    end
    return F, freqs
end

function RMS(signal,time;fund = 50)
    RMS = zeros(length(signal))
    dt = time[end]/length(time) # fixed step-size assumed
    L = Int(floor(1/fund/dt))
    zaehler = Int(floor(1/fund/dt))
    ind = 0;
    for i=0:1:length(signal)-L
        s = signal[i+1:i+L]
        t = time[i+1:i+L]
        t = t.-t[1]
        val, freq = DFT(s,t)
        ind = i==0 ? findmin(abs.(freq.-fund))[2] : ind
        RMS[zaehler] = abs(val[ind]);
        zaehler = zaehler + 1;
    end
    RMS./(sqrt(2)*L/2)
end

function ExtractResult(pgsol::PowerGridSolution, sym::Symbol)
    sol = pgsol.dqsol
    ind = indexin([sym],rhs(pgsol.powergrid).syms)[1]
    return pgsol.dqsol[ind,:]
end

function ExtractResult(pgsol::PowerGridSolution, bus::String, sym::Symbol)
    keys_node = collect(keys(pgsol.powergrid.nodes))
    ind = indexin([bus],keys_node)[1]
    if isnothing(ind)
        error("$bus is not a valid bus key")
    else
        sym = Symbol(string(sym,"_",ind))
        return ExtractResult(pgsol,sym)
    end
end

function SavePlotApprTrajectories(pg_tmp,sol_or,sol_per,sens,par_num,or_value,new_value,labels_p; state_sym = :i_abs)
    sol_appr = deepcopy(sol_or.dqsol)
    for (ind,val) in enumerate(collect(eachcol(sens[par_num])))
        sol_appr.u[ind+1] .+= val*(new_value-or_value)
    end
    pgsol_tmp = PowerGridSolution(sol_appr,pg_tmp)
    title_str  = labels_p[par_num] * ": from " * string(or_value) * " to " * string(new_value)
    plot(sol_or,"bus3",state_sym, label = "Original - " * string(state_sym), title = title_str)
    plot!(sol_per,"bus3",state_sym, label = "Real perturbed - " * string(state_sym))
    display(plot!(pgsol_tmp,"bus3",state_sym, label = "Approximated - " * string(state_sym),linestyle = :dash))
    savefig("C:\\Users\\liemann\\Desktop\\Julia_figures\\"*labels_p[par_num] *".svg")
end

function PlotApproTrajectories(
        pg::PowerGrid,
        sol_or::PowerGridSolution,
        sol_per::PowerGridSolution,
        sensis::Vector{Array{Float64}},
        par_num::Int64,
        or_value::Float64,
        new_value::Float64,
        labels_p::Vector{String},
        state::Symbol;
        bus = "bus4"
    )
    sol_appr = CalcApprSolution(sol_or,sensis,par_num,or_value,new_value)
    title_str  = labels_p[par_num] * ": from " * string(or_value) * " to " * string(new_value)
    plot(sol_or,bus,state, label = "Original - " * string(state), title = title_str)
    plot!(sol_per,bus,state, label = "Real perturbed - " * string(state))
    display(plot!(sol_appr,bus,state, label = "Approximated - " * string(state),linestyle = :dash))
end

function CalcApprSolution(sol_or::PowerGridSolution,sensis::Vector{Array{Float64}},par_num::Int64,or_value::Float64,new_value::Float64)
    sol_appr = deepcopy(sol_or)
    for (ind,val) in enumerate(collect(eachcol(sensis[par_num])))
        sol_appr.dqsol.u[ind+1] .+= val*(new_value-or_value)
    end
    return sol_appr
end

function SaveComparingTrajectoryPlots(pg_tmp,ic_or,pgsol_or,sens,labels_p,delta)
    params_tmp = GFC_params()
    for (ind,val) in enumerate(params_tmp)
        params_new = deepcopy(params_tmp)
        params_new[ind] += delta*params_new[ind]
        prob_tmp = ODEProblem(rhs(pg_tmp),ic_or,(0.0,3.0),params_new)
        try
            pgsol_per,evr = simGFC(prob_tmp)
            SavePlotApprTrajectories(pg_tmp,pgsol_or,pgsol_per,sens,ind,params[ind],params_new[ind],labels_p)
        catch
            @warn "Could not calc: " *labels_p[ind]
        end
    end
end

function GetVoltageSensis(sensis,ind_ur_state,ind_ui_state)
    sensi_ur = Array{Float64}(undef,size(sensis[1])[2],length(sensis));
    sensi_ui = Array{Float64}(undef,size(sensis[1])[2],length(sensis));

    for (ind,val) in enumerate(sensis)
        sensi_ur[:,ind] = val[ind_ur_state,:]
        sensi_ui[:,ind] = val[ind_ui_state,:]
    end
    return sensi_ur, sensi_ui
end

function GetAbsVoltageSensis(pg_sol::PowerGridSolution,ur::Symbol,ui::Symbol,sensis::Vector{Array{Float64}},param::Vector{Int64},Δp::Vector{Float64})
    #Idea is: |U_appr| = |U_or| + Z   with |U_or| = sqrt(Ur_or^2 + Ui_or^2)
    #         U_r_appr = Ur_or  + xx0*Δp (same for U_i_appr)
    #         |U_appr| = sqrt(U_r_appr^2 + U_r_appr^2)
    #next    |U_appr|^2 = (|U_or| + Z)^2 solve for Z

    ur_or = ExtractResult(pg_sol,ur)[1:end-1]
    ui_or = ExtractResult(pg_sol,ui)[1:end-1]

    ind = indexin([ur,ui],rhs(pg_sol.powergrid).syms)
    sens_ur,sens_ui = GetVoltageSensis(toll_tap,ind[1],ind[2])
    z1 = Array{Float64}(undef,length(pg_sol.dqsol.t[1:end-1]),length(param))
    z2 = Array{Float64}(undef,length(pg_sol.dqsol.t[1:end-1]),length(param))
    for (ind,val) in enumerate(param)
        z1[:,ind],z2[:,ind] = CalcAbsVoltageSens(ur_or,ui_or,sens_ur,sens_ui,val,Δp[ind])
    end
    return z1,z2
end

function CalcAbsVoltageSens(ur_or,ui_or,sens_ur,sens_ui,param,Δp)
    s_ur = sens_ur[:,param]*Δp
    s_ui = sens_ur[:,param]*Δp

    p = 2*sqrt.(ur_or.^2.0 .+ ui_or.^2.0)
    q = -(2 .*(ur_or.*s_ur + ui_or.*s_ui) .+ s_ur.^2 .+ s_ui.^2)

    z1 = -p./2 + sqrt.((p./2).^2 .- q)
    z2 = -p./2 - sqrt.((p./2).^2 .- q)
    return z1,z2
end

function CalcAbsVoltageSens_v2(ur_or,ui_or,sens_ur,sens_ui;Δp=1.0)
    s_ur = sens_ur*Δp
    s_ui = sens_ur*Δp

    p = 2*sqrt.(ur_or.^2.0 .+ ui_or.^2.0)
    q = -(2 .*(ur_or.*s_ur + ui_or.*s_ui) .+ s_ur.^2 .+ s_ui.^2)

    z1 = -p./2 + sqrt.((p./2).^2 .- q)
    z2 = -p./2 - sqrt.((p./2).^2 .- q)
    return z1,z2
end

function getallParameters(node)
    cond = true
    i = 1
    param = Vector{Float64}()
    while cond
        try
            tmp = getfield(node,i)
            append!(param,tmp)
            i += 1
        catch
            cond = false
        end
    end
    param[1:end-1] #last entry is Y_n from PowerDynamics
end

function Sol2DF(pgsol::PowerGridSolution)
    sol = DataFrame(pgsol.dqsol)

    for (ind,val) in enumerate(pgsol.powergrid.nodes)
        ur = ExtractResult(pgsol,Symbol("u_r_"*string(ind)))
        ui = ExtractResult(pgsol,Symbol("u_i_"*string(ind)))
        u = sqrt.(ur.^2 + ui.^2)
        sol[!,Symbol("uabs_"*string(ind))] = u;
    end
    return sol
end

function Sol2DFonlyVoltages(pgsol::PowerGridSolution)
    sol = DataFrame()

    for (ind,val) in enumerate(pgsol.powergrid.nodes)
        ur = ExtractResult(pgsol,Symbol("u_r_"*string(ind)))
        ui = ExtractResult(pgsol,Symbol("u_i_"*string(ind)))
        u = sqrt.(ur.^2 + ui.^2)
        sol[!,Symbol("uabs_"*string(ind))] = u;
    end
    sol[!,Symbol("time")] = pgsol.dqsol.t;
    return sol
end

function plotallvoltages(pgsol::PowerGridSolution)
    newdf = Sol2DF(pgsol)
    p = Vector{GenericTrace}()
    for (ind,val) in enumerate(pgsol.powergrid.nodes)
        tmp = PlotlyJS.scatter(x=newdf.timestamp,y=newdf[:,"uabs_"*string(ind)],name=val[1])
        push!(p,tmp)
    end
    display(PlotlyJS.plot(p))
    return p
end

function myplot(pgsol::PowerGridSolution,sym::Symbol;y_norm=1.0,y_bias = 0.0)
    ind = findfirst(x->x==sym,collect(rhs(pgsol.powergrid).syms))
    t = pgsol.dqsol.t
    y =  pgsol.dqsol[ind,:]./y_norm .+ y_bias
    sc = PlotlyJS.scatter(x=t,y=y,name=String(sym))
    display(PlotlyJS.plot(sc))
    return sc
end

function myplot(pgsol::PowerGridSolution,bus::String,sym::Symbol;y_norm=1.0,y_bias = 0.0)
    ind_bus = findfirst(x->x==bus,collect(keys(pgsol.powergrid.nodes)))
    sym = Symbol(string(sym)*"_"*string(ind_bus))
    myplot(pgsol,sym,y_norm=y_norm,y_bias=y_bias)
end

function myplot(pgsol::PowerGridSolution,bus::String,symv::Vector{Symbol};y_norm=1.0,y_bias = 0.0)
    ind_bus = findfirst(x->x==bus,collect(keys(pgsol.powergrid.nodes)))
    p = Vector{GenericTrace}()
    for sym in symv
        sym = Symbol(string(sym)*"_"*string(ind_bus))
        ind = findfirst(x->x==sym,collect(rhs(pgsol.powergrid).syms))
        t = pgsol.dqsol.t
        y =  pgsol.dqsol[ind,:]./y_norm .+ y_bias
        sc = PlotlyJS.scatter(x=t,y=y,name=String(sym))
        push!(p,sc)
    end
    display(PlotlyJS.plot(p))
    return p
end


function myplot(pgsol::Vector{PowerGridSolution},bus::String,sym::Symbol;y_norm=1.0,y_bias = 0.0)
    p = Vector{GenericTrace}()
    for i in pgsol
        tmp  =myplot(i,bus,sym,y_norm=y_norm,y_bias=y_bias)
        push!(p,tmp)
    end
    display(plot(p))
    return p
end

function plotv(pgsol,bus::String;y_norm=1.0,y_bias = 0.0)
    ind = findfirst(x->x==bus,collect(keys(pgsol.powergrid.nodes)))
    ur = ExtractResult(pgsol,Symbol("u_r_"*string(ind)))
    ui = ExtractResult(pgsol,Symbol("u_i_"*string(ind)))
    t = pgsol.dqsol.t
    y =  sqrt.(ur.^2 + ui.^2)./y_norm .+ y_bias
    sc = PlotlyJS.scatter(x=t,y=y,name=bus)
    display(PlotlyJS.plot(sc))
    return sc
end

function plotv(pgsol,bus::Vector{String})
    p = Vector{GenericTrace}()
    for (ind,val) in enumerate(bus)
        tmp = plotv(pgsol,val)
        push!(p,tmp)
    end
    display(plot(p))
    return p
end

function plotv(pgsol::Vector{PowerGridSolution},bus::String)
    p = Vector{GenericTrace}()
    for i in pgsol
        tmp  = plotv(i,bus)
        push!(p,tmp)
    end
    display(plot(p))
    return p
end

function DetermineBoundary(XR,Rv,Xv)
    r = Vector{Float64}()
    x = Vector{Float64}()
    flag_dir = "left"
    flag_end = false
    ind = [(-1,0);(0,1);(1,0);(0,-1)]

    XR = reverse(XR)'
    Rv = reverse(Rv)
    Xv = reverse(Xv)

    lenx = size(XR)[1]
    lenr = size(XR)[2]

    #starting indices
    indr = 1
    indx = findfirst(k->k==1,XR[:,1])

    #saving first entry
    push!(r,Rv[indr])
    push!(x,Xv[indx])

    function next_ind(start_ind,r,x)
        for i=start_ind:start_ind+3
            if mod(i,4) == 0
                dx = ind[4][1]
                dr = ind[4][2]
            else
                dx = ind[mod(i,4)][1]
                dr = ind[mod(i,4)][2]
            end

            if indr+dr == 0 || indr+dr > lenr || indx+dx > lenx 
                #skip boundarys
            elseif indx + dx == 1  && XR[indx,indr] == 1 
                indr = indr + dr
                indx = indx + dx
                push!(r,Rv[indr])
                push!(x,Xv[indx])
                indr = indr -1
                
                while indr != 0 && XR[indx,indr] == 1  # go x-axis to zero 
                    push!(r,Rv[indr])
                    push!(x,Xv[indx])
                    indr = indr -1
                end
                flag_end = true
                break;
            elseif XR[indx+dx,indr+dr] != 1 
                #skip unstable case
            elseif XR[indx + dx,indr + dr] == 1
                indr = indr + dr
                indx = indx + dx
                push!(r,Rv[indr])
                push!(x,Xv[indx])

                if mod(i,4) == 1
                   flag_dir = "bottom"
                   break;
                elseif mod(i,4) == 2
                   flag_dir = "left"
                   break;
                elseif mod(i,4) == 3
                    flag_dir = "top"
                    break;
                elseif mod(i,4) == 0
                    flag_dir = "right"
                    break;
                else
                    error("error2")
                end
            end 
        end
        return flag_end,flag_dir,r,x
    end    
    counter = 0;

    while flag_end == false && counter < 1e4
        if flag_dir == "top"
            flag_end,flag_dir,r,x = next_ind(2,r,x)
        elseif flag_dir == "left"
            flag_end,flag_dir,r,x = next_ind(1,r,x)
        elseif flag_dir == "bottom"
            flag_end,flag_dir,r,x = next_ind(4,r,x)
        elseif flag_dir == "right"
            flag_end,flag_dir,r,x = next_ind(3,r,x)
        else
            error("error")
        end
        counter = counter + 1
    end
    if counter == 1e4
        error("Counter reached 1e4")
    end
    return r,x
end

function CompareXRResults(obj::Array{String})
    sc = Vector{GenericTrace}()
    for i in obj
        data = load(i)
        Rv= data["Rverlauf"];
        Xv = data["Xverlauf"];
        XRv= data["XR"];
        x,y = DetermineBoundary(XRv,Rv,Xv)
        tmp_sc = scatter(x=x,y=y,name=i)
        push!(sc,tmp_sc)
    end
    display(plot(sc))
end

function CardanosFormular(A::Float64,B::Float64,C::Float64,D::Float64)
    p = (9*A*C-3*B^2)./(9*A^2);
    q = (2*B^3 -9*A*B*C + 27*D*A^2)./(27*A^3)
    delta = (q/2)^2 +  (p/3)^3

    roots_ = -1.0
    if delta >0
        #display("a")
        u = Complex((-q/2+sqrt(delta)+0im)^(1/3))
        v = Complex((-q/2-sqrt(delta)+0im)^(1/3))
        x1 = u+v- B/(3*A) 
        x2 = -(u+v)/2 - B/(3*A) + 1im*(u-v)/2*sqrt(3)
        x3 = -(u+v)/2 - B/(3*A) - 1im*(u-v)/2*sqrt(3)
        roots_ = real(x1) #auch nur das wird in PowerFactory genommen
        if roots_ > 0.005
            roots_= -0.00001
        end
        #display(roots_)
    elseif delta == 0 && p == 0
        #display("b")
        x2 = - B/(3*A)
        roots_ = real(x2)
        #display(roots_)
    elseif delta == 0 && p != 0
        #display("c")
        x1 = 3*q/p- B/(3*A)
        x23 = -3*q/(2*p)- B/(3*A)
        roots_ = real(x23)
        #display(roots_)
    elseif delta < 0
        #display("d")
        x1 = -sqrt(-4/3*p).*cos(1/3.0*acos(-q/2.0*sqrt(-27.0/(p.^3)))+pi/3)-B/(3*A)
        x2 =  sqrt(-4/3*p).*cos(1/3.0*acos(-q/2.0*sqrt(-27.0/(p.^3))))     -B./(3*A)
        x3 = -sqrt(-4/3*p).*cos(1/3.0*acos(-q/2.0*sqrt(-27.0/(p.^3)))-pi/3)-B./(3*A)
        roots_ =  real(x1)
        if roots_ > 0.005
            roots_= -0.00001
        end
        #display(roots_)
    end
    roots_
end

function CardanosFormular(A,B,C,D)
    p = (9*A*C-3*B^2)./(9*A^2);
    q = (2*B^3 -9*A*B*C + 27*D*A^2)./(27*A^3)
    delta = (q/2)^2 +  (p/3)^3

    roots_ = -0.00001
    if delta >0
        u = Complex((-q/2+sqrt(delta)+0im)^(1/3))
        v = Complex((-q/2-sqrt(delta)+0im)^(1/3))
        x1 = u+v- B/(3*A) 
        x2 = -(u+v)/2 - B/(3*A) + 1im*(u-v)/2*sqrt(3)
        x3 = -(u+v)/2 - B/(3*A) - 1im*(u-v)/2*sqrt(3)
        roots_ = real(x1) #auch nur das wird in PowerFactory genommen
        if roots_ > 0.005
            roots_= -0.00001
        end
    elseif delta == 0 && p == 0
        x2 = - B/(3*A)
        roots_ = real(x2)
    elseif delta == 0 && p != 0
        x1 = 3*q/p- B/(3*A)
        x23 = -3*q/(2*p)- B/(3*A)
        roots_ = real(x23)
    elseif delta < 0
        x1 = -sqrt(-4/3*p).*cos(1/3.0*acos(-q/2.0*sqrt(-27.0/(p.^3)))+pi/3)-B/(3*A)
        x2 =  sqrt(-4/3*p).*cos(1/3.0*acos(-q/2.0*sqrt(-27.0/(p.^3))))     -B./(3*A)
        x3 = -sqrt(-4/3*p).*cos(1/3.0*acos(-q/2.0*sqrt(-27.0/(p.^3)))-pi/3)-B./(3*A)
        roots_ =  real(x1)
        if roots_ > 0.005
            roots_= -0.00001
        end
    end
    roots_
end


function CalcnPFCtoff(V0::Float64,Pdc::Float64,Cd::Float64;ω0=100*pi,t=-1.0)
    if 2*Pdc/(ω0*Cd*V0^2) > 1.0
        toff =  0.01
    else
        toff = π/(2*ω0) + asin(2*Pdc/(ω0*Cd*V0^2))/(2*ω0)
    end
    return toff
end

function CalcnPFCtoff(V0,Pdc,Cd;ω0=100*pi)
    if 2*Pdc/(ω0*Cd*V0^2) > 1.0
        toff =  0.01
    else
        toff = π/(2*ω0) + asin(2*Pdc/(ω0*Cd*V0^2))/(2*ω0)
    end
    return toff
end

function CalfnPFCVoffT2(Voff::Float64,Pdc::Float64,Cd::Float64,dt::Float64)
    VoffT2 = max(real(sqrt(Complex(Voff^2 - 2*Pdc*(dt)/Cd))))
end

function CalfnPFCVoffT2(Voff,Pdc,Cd,dt)
    VoffT2 = max(real(sqrt(Complex(Voff^2 - 2*Pdc*(dt)/Cd))))
end

function CalfnPFCton(V0::Float64,Pdc::Float64,Cd::Float64,VoffT2::Float64;ω0=100*pi)    
    x1 = 0.004517042542168
    x2 = -0.084973441092720
    x3 =  1.367157627046003
    x4 = -0.580239811988436

    A = V0^2*ω0^3*x4
    B = V0^2*ω0^2*x3
    C = V0^2*ω0*x2 + 2*Pdc/Cd
    D = V0^2*x1 - VoffT2^2  

    ton = CardanosFormular(A,B,C,D)
end

function CalfnPFCton(V0,Pdc,Cd,VoffT2;ω0=100*pi)    
    x1 = 0.004517042542168
    x2 = -0.084973441092720
    x3 =  1.367157627046003
    x4 = -0.580239811988436

    A = V0^2*ω0^3*x4
    B = V0^2*ω0^2*x3
    C = V0^2*ω0*x2 + 2*Pdc/Cd
    D = V0^2*x1 - VoffT2^2  

    ton = CardanosFormular(A,B,C,D)
end

function CalcnPFCP1Q1(V0::Float64,Pdc::Float64,Cd::Float64,ton::Float64,toff::Float64;ω0=100*pi,T=0.02)
    tmp1_a1 = V0*(1/2)*ω0*Cd*(toff-ton+1/(2*ω0)*(sin(2*ω0*toff)-sin(2*ω0*ton)))
    tmp2_a1 = Pdc/(V0*ω0)*(log(abs(sin(ω0*toff)))-log(abs(sin(ω0*ton))))
    a1 = 4/T*(tmp1_a1+tmp2_a1)

    tmp1_b1 = V0*(1/4)*Cd*(cos(2*ω0*ton)-cos(2*ω0*toff))
    tmp2_b1 = Pdc/V0*(toff-ton)
    b1 = 4/T*(tmp1_b1+tmp2_b1)

    q1 = a1*V0/2  # positive = capacitive reactive power (for PowerDynamics)
    p1 = -b1*V0/2 # negative = load active power (for PowerDynamics)

    return p1, q1
end

function CalcnPFCPower(V0::Float64,Pdc::Float64,Cd::Float64;ω0=100*pi,T=0.02,init=false)
    toff = CalcnPFCtoff(V0,Pdc,Cd)
    voff = V0*sin(ω0*toff)
    VoffT2 = CalfnPFCVoffT2(voff,Pdc,Cd,(T/2-toff))
    ton = CalfnPFCton(V0,Pdc,Cd,VoffT2)  
    p1,q1 = CalcnPFCP1Q1(V0,Pdc,Cd,ton,toff)

    if init
        [VoffT2, ton, toff, p1, q1]
    else
        return [p1, q1]
    end
end