text:
using JLD
using PyPlot
using PyCall
@pyimport matplotlib.animation as animation
@pyimport matplotlib.patches as patch
using JLD, ProfileView
# pos_info[1]  = s
# pos_info[2]  = eY
# pos_info[3]  = ePsi
# pos_info[4]  = v
# pos_info[5]  = s_start
# pos_info[6]  = x
# pos_info[7]  = y
# pos_info[8]  = v_x
# pos_info[9]  = v_y
# pos_info[10] = psi
# pos_info[11] = psiDot
# pos_info[12] = x_raw
# pos_info[13] = y_raw
# pos_info[14] = psi_raw
# pos_info[15] = v_raw
# pos_info[16] = psi_drift
# pos_info[17] = a_x
# pos_info[18] = a_y

include("../workspace/src/barc/src/barc_lib/classes.jl")

type Measurements{T}
    i::Int64                # measurement counter
    t::Array{Float64}       # time data (when it was received by this recorder)
    t_msg::Array{Float64}   # time that the message was sent
    z::Array{T}             # measurement values
end

# THIS FUNCTION EVALUATES DATA THAT WAS LOGGED BY THE SIMULATOR (INCLUDES "REAL" SIMULATION DATA)
# ***********************************************************************************************

function eval_sim(code::AbstractString)
    log_path_sim = "$(homedir())/simulations/output-SIM-$(code).jld"
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    d_sim = load(log_path_sim)
    d_rec = load(log_path_record)

    imu_meas    = d_sim["imu_meas"]
    gps_meas    = d_sim["gps_meas"]
    z           = d_sim["z"]
    cmd_log     = d_sim["cmd_log"]
    slip_a      = d_sim["slip_a"]
    pos_info    = d_rec["pos_info"]
    vel_est     = d_rec["vel_est"]

    t0 = pos_info.t[1]
    track = create_track(0.4)

    figure()
    ax1=subplot(311)
    plot(z.t-t0,z.z,"-*")
    title("Real states")
    grid()
    legend(["x","y","v_x","v_y","psi","psi_dot","a","d_f"])
    subplot(312,sharex=ax1)
    plot(cmd_log.t-t0,cmd_log.z,"-*")
    title("Inputs")
    grid()
    legend(["u","d_f"])
    subplot(313,sharex=ax1)
    plot(slip_a.t-t0,slip_a.z,"-*")
    title("Slip angles")
    grid()
    legend(["a_f","a_r"])

    figure()
    plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1],gps_meas.z[:,2],".",pos_info.z[:,6],pos_info.z[:,7],"-")
    plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")
    grid(1)
    title("x-y-view")
    axis("equal")
    legend(["Real state","GPS meas","Estimate"])

    figure()
    title("Comparison of psi")
    plot(imu_meas.t-t0,imu_meas.z,"-x",z.t-t0,z.z[:,5:6],pos_info.t-t0,pos_info.z[:,10:11],"-*")
    legend(["imu_psi","imu_psi_dot","real_psi","real_psi_dot","est_psi","est_psi_dot"])
    grid()

    figure()
    title("Comparison of v")
    plot(z.t-t0,z.z[:,3:4],z.t-t0,sqrt(z.z[:,3].^2+z.z[:,4].^2),pos_info.t-t0,pos_info.z[:,8:9],"-*",pos_info.t-t0,sqrt(pos_info.z[:,8].^2+pos_info.z[:,9].^2),"-*",vel_est.t-t0,vel_est.z)
    legend(["real_xDot","real_yDot","real_v","est_xDot","est_yDot","est_v","v_x_meas"])
    grid()

    figure()
    title("Comparison of x,y")
    plot(z.t-t0,z.z[:,1:2],pos_info.t-t0,pos_info.z[:,6:7],"-*",gps_meas.t-t0,gps_meas.z)
    legend(["real_x","real_y","est_x","est_y","meas_x","meas_x"])
    grid()
end

function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obstacle::Bool)

    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"

    Data      = load(log_path_LMPC)

    oldSS_xy       = Data["oldSS_xy"]
    oldSS          = Data["oldSS"]
    selectedStates = Data["selectedStates"]
    selStates      = Data["selStates"]
    statesCost     = Data["statesCost"]
    pred_sol       = Data["pred_sol"]
    pred_input     = Data["pred_input"]
    one_step_error = Data["one_step_error"]
    lapStatus      = Data["lapStatus"]
    posInfo        = Data["posInfo"]
    eps_alpha      = Data["eps_alpha"]
    cvx            = Data["cvx"]
    cvy            = Data["cvy"]
    cpsi           = Data["cpsi"]
    input          = Data["input"]
    cost           = Data["mpcCost"]
    costSlack      = Data["mpcCostSlack"]
    obs_log        = Data["obs_log"]
    sol_u          = Data["sol_u"]
    final_counter  = Data["final_counter"]
    status         = Data["sol_status"]

    Nl         = selectedStates.Nl
    Np         = selectedStates.Np
    buffersize = 5000
    currentIt  = 200#lapStatus.currentIt

    flag = zeros(2)

    track = create_track(0.4)

    pred_horizon = size(pred_sol)[1] - 1

    println("prediction horizon N= ", pred_horizon)

    ospe1 = []
        ospe2 = []
        ospe3 = []
        ospe4 = []
        ospe5 = []
        ospe6 = []

        slack1 = []
        slack2 = []
        slack3 = []
        slack4 = []
        slack5 = []
        slack6 = []

        for l = laps
            ospe1 = vcat(ospe1,one_step_error[1:currentIt,1,l])
            ospe2 = vcat(ospe2,one_step_error[1:currentIt,2,l])
            ospe3 = vcat(ospe3,one_step_error[1:currentIt,3,l])
            ospe4 = vcat(ospe4,one_step_error[1:currentIt,4,l])
            ospe5 = vcat(ospe5,one_step_error[1:currentIt,5,l])
            ospe6 = vcat(ospe6,one_step_error[1:currentIt,6,l])

            slack1 = vcat(slack1,costSlack[1:currentIt,1,l])
            slack2 = vcat(slack2,costSlack[1:currentIt,2,l])
            slack3 = vcat(slack3,costSlack[1:currentIt,3,l])
            slack4 = vcat(slack4,costSlack[1:currentIt,4,l])
            slack5 = vcat(slack5,costSlack[1:currentIt,5,l])
            slack6 = vcat(slack6,costSlack[1:currentIt,6,l])
        end

        t_ospe = linspace(1,length(ospe1),length(ospe1))

        figure()
        plot(t_ospe,ospe1,t_ospe,ospe2,t_ospe,ospe3,t_ospe,ospe4,t_ospe,ospe5,t_ospe,ospe6)
        legend(["vx","vy","psiDot","ePsi","eY","s"])
        grid("on")
        title("One step prediction errors")

        figure()
        plot(t_ospe,slack1,t_ospe,slack2,t_ospe,slack3,t_ospe,slack4,t_ospe,slack5,t_ospe,slack6)
        legend(["slack cost on vx","slack cost on vy","slack cost on psiDot","slack cost on ePsi","slack cost on eY","slack cost on s"])
        title("Slack costs")
        grid("on")

    for i = laps

        pred_sol_xy, xyPathAngle = xyObstacle(oldSS,obs_log,1,i,track)

        #println("pred sol= ",pred_sol_xy[:,1])
        # for index=1:buffersize
        #     if status[index,i] == :UserLimit
        #         flag[1]=index
        #         flag[2]= i

        #         break
        #     end
        # end

        vx_alpha     = eps_alpha[1,1:currentIt,i]
        vy_alpha     = eps_alpha[2,1:currentIt,i]
        psiDot_alpha = eps_alpha[3,1:currentIt,i]
        ePsi_alpha   = eps_alpha[4,1:currentIt,i]
        eY_alpha     = eps_alpha[5,1:currentIt,i]
        s_alpha      = eps_alpha[6,1:currentIt,i]

        cvx1         = cvx[1:currentIt,1,i]
        cvx2         = cvx[1:currentIt,2,i]
        cvx3         = cvx[1:currentIt,3,i]
        cvy1         = cvy[1:currentIt,1,i]
        cvy2         = cvy[1:currentIt,2,i]
        cvy3         = cvy[1:currentIt,3,i]
        cvy4         = cvy[1:currentIt,4,i]
        cpsi1        = cpsi[1:currentIt,1,i]
        cpsi2        = cpsi[1:currentIt,2,i]
        cpsi3        = cpsi[1:currentIt,3,i]

        if obstacle == false
            figure()
            plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.")
            #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob")
            plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.")

        elseif obstacle == true

            ellfig = figure(3)
            ax = ellfig[:add_subplot](1,1,1)
            ax[:set_aspect]("equal")
            plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.")
            # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob")
            plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.")

            angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1]))
            angle_deg = (angle_ell*180)/pi

            ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.2, 0.1, angle = 90)
            ax[:add_artist](ell1)
        end

        grid("on")
        title("X-Y view of Lap $i")

        t = linspace(1,currentIt,currentIt)

        figure()

        plot(t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+")
        title("Inputs in lap $i")
        grid("on")
        legend(["a","d_f"])

        #figure()

        # subplot(221)
        # plot(t,one_step_error[1:currentIt,1,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+")
        # # annotate("UserLimit",xy=[flag[1],one_step_error[flag[1],1,flag[2]]],xytext=[flag[1]+0.1,one_step_error[flag[1],1,flag[2]]+0.1],xycoords="data",arrowprops=["facecolor"=>"black"])
        # title("One step prediction error for v_x in lap $i")
        # #ylim(-0.0001,findmax(one_step_error[1:currentIt,1,i])[1])
        # legend(["ospe","a_x","d_f"])
        # grid("on")

        # subplot(222)
        # plot(t,one_step_error[1:currentIt,2,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+")
        # #ylim(-0.0001,findmax(one_step_error[1:currentIt,2,i])[1])
        # legend(["ospe","a_x","d_f"])
        # title("One step prediction error for v_y in lap $i")
        # grid("on")

        # subplot(223)
        # plot(t,one_step_error[1:currentIt,3,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+")
        # #ylim(-0.0001,findmax(one_step_error[1:currentIt,3,i])[1])
        # legend(["ospe","a_x","d_f"])
        # title("One step prediction error for psiDot in lap $i")
        # grid("on")

        # subplot(224)
        # plot(t,one_step_error[1:currentIt,4,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+")
        # #ylim(-0.0001,findmax(one_step_error[1:currentIt,4,i])[1])
        # legend(["ospe","a_x","d_f"])
        # title("One step prediction error for ePsi in lap $i")
        # grid("on")

        # figure()

        # subplot(221)
        # plot(t,one_step_error[1:currentIt,5,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+")
        # #ylim(-0.0001,findmax(one_step_error[1:currentIt,5,i])[1])
        # legend(["ospe","a_x","d_f"])
        # title("One step prediction error for eY in lap $i")
        # grid("on")

        # subplot(222)
        # plot(t,one_step_error[1:currentIt,6,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+")
        # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1])
        # legend(["ospe","a_x","d_f"])
        # title("One step prediction error for s in lap $i")
        # grid("on")

        # figure()
        # plot(t,costSlack[1:currentIt,1,i],t,costSlack[1:currentIt,2,i],t,costSlack[1:currentIt,3,i],t,costSlack[1:currentIt,4,i],t,costSlack[1:currentIt,5,i],t,costSlack[1:currentIt,6,i])
        # legend(["slack cost on vx","slack cost on vy","slack cost on psiDot","slack cost on ePsi","slack cost on eY","slack cost on s"])
        # title("Slack costs")
        # grid("on")

        # figure()
        # plot(t,one_step_error[1:currentIt,1,i],t,one_step_error[1:currentIt,2,i],t,one_step_error[1:currentIt,3,i],t,one_step_error[1:currentIt,4,i],t,one_step_error[1:currentIt,5,i],t,one_step_error[1:currentIt,6,i])
        # legend(["vx","vy","psiDot","ePsi","eY","s"])
        # grid("on")
        # title("One step prediction errors")

        # println("one step prediction error= ",one_step_error[1:30,:,i])

        figure()

        subplot(221)
        plot(t,oldSS.oldSS[1:currentIt,1,i],"-*")
        #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1])
        title("vx in lap $i")
        grid("on")

        subplot(222)
        plot(t,oldSS.oldSS[1:currentIt,2,i],"-*")
        #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1])
        title("vy in lap $i")
        grid("on")

        subplot(223)
        plot(t,oldSS.oldSS[1:currentIt,3,i],"-*")
        #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1])
        title("psiDot in lap $i")
        grid("on")

        subplot(224)
        plot(t,oldSS.oldSS[1:currentIt,4,i],"-*")
        #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1])
        title("ePsi in lap $i")
        grid("on")

        figure()
        subplot(221)
        plot(t,oldSS.oldSS[1:currentIt,5,i],"-*")
        #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1])
        title("eY in lap $i")
        grid("on")

        subplot(222)
        plot(t,oldSS.oldSS[1:currentIt,6,i],"-*")
        #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1])
        title("s in lap $i")
        grid("on")

        subplot(223)
        velocity= sqrt(oldSS.oldSS[1:currentIt,2,i].^2 + oldSS.oldSS[1:currentIt,1,i].^2)
        plot(t,velocity,"-*")
        title("Overall velocity in lap $i")
        grid("on")

        figure()

        subplot(221)
        plot(t,cvx1,t,cvx2,t,cvx3)
        legend(["cvx1","cvx2","cvx3"])
        title("C_Vx in lap $i")
        grid("on")

        subplot(222)
        plot(t,cvy1,t,cvy2,t,cvy3,t,cvy4)
        legend(["cvx1","cvx2","cvx3","cvy4"])
        title("C_Vy in lap $i")
        grid("on")

        subplot(223)
        plot(t,cpsi1,t,cpsi2,t,cpsi3)
        legend(["cpsi1","cpsi2","cpsi3"])
        title("C_Psi in lap $i")
        grid("on")

        figure()
        plot(t,cost[1:currentIt,2,i],t,cost[1:currentIt,3,i],t,cost[1:currentIt,4,i],t,cost[1:currentIt,6,i])
        legend(["terminal Cost","control Cost","derivative Cost","lane Cost"])
        title("Costs of the Mpc in lap $i")
        grid("on")

        if switch == true

            for j = 2:2000

                solution_status = status[final_counter[i-1]+j]

                vx_pred     = pred_sol[:,1,j,i]
                vy_pred     = pred_sol[:,2,j,i]
                psiDot_pred = pred_sol[:,3,j,i]
                ePsi_pred   = pred_sol[:,4,j,i]
                eY_pred     = pred_sol[:,5,j,i]
                s_pred      = pred_sol[:,6,j,i]

                oldvx       = selStates[1:Np,1,j,i]
                oldvx2      = selStates[Np+1:2*Np,1,j,i]
                #oldvx3      = selStates[2*Np+1:3*Np,1,j,i]
                oldvy       = selStates[1:Np,2,j,i]
                oldvy2      = selStates[Np+1:2*Np,2,j,i]
                #oldvy3      = selStates[2*Np+1:3*Np,2,j,i]
                oldpsiDot   = selStates[1:Np,3,j,i]
                oldpsiDot2  = selStates[Np+1:2*Np,3,j,i]
                #oldpsiDot3  = selStates[2*Np+1:3*Np,3,j,i]
                oldePsi     = selStates[1:Np,4,j,i]
                oldePsi2    = selStates[Np+1:2*Np,4,j,i]
                #oldePsi3    = selStates[2*Np+1:3*Np,4,j,i]
                oldeY       = selStates[1:Np,5,j,i]
                oldeY2      = selStates[Np+1:2*Np,5,j,i]
                #oldeY3      = selStates[2*Np+1:3*Np,5,j,i]
                olds        = selStates[1:Np,6,j,i]
                olds2       = selStates[Np+1:2*Np,6,j,i]
                #olds3       = selStates[2*Np+1:3*Np,6,j,i]

                t = linspace(1,j,j)

                if obstacle == false
                    figure(19)
                    clf()
                    plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"*")
                    #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob")
                    plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.")

                    #for i=1:4:size(x_est,1)
                    # for index=1:length(oldSS_xy[:,1,i])
                    # z_pred = zeros(size(pred_sol)[1],4)
                    #     #z_pred[1,:] = x_est[i,:]
                    # z_pred[1,:] = oldSS_xy[j,:,i]
                    # for j2=2:size(pred_sol)[1]
                    #     z_pred[j2,:] = simModel(z_pred[j2-1,:],pred_input[j2-1,:,j,i],0.1,0.125,0.125)
                    # end

                    plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og")
                    grid("on")
                    # title("Predicted solution in lap $i, iteration $j")
                    title("Position at iteration $j")
                    # end

                elseif obstacle == true
                    ellfig = figure(19)
                    clf()
                    # plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"*")
                    #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob")
                    plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.")

                    #for i=1:4:size(x_est,1)
                    # for index=1:length(oldSS_xy[:,1,i])
                    # z_pred = zeros(size(pred_sol)[1],4)
                    #     #z_pred[1,:] = x_est[i,:]
                    # z_pred[1,:] = oldSS_xy[j,:,i]
                    # for j2=2:size(pred_sol)[1]
                    #     z_pred[j2,:] = simModel(z_pred[j2-1,:],pred_input[j2-1,:,j,i],0.1,0.125,0.125)
                    # end

                    ax = ellfig[:add_subplot](1,1,1)
                    ax[:set_aspect]("equal")
                    plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.")
                    # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob")
                    plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.")

                    angle_ell = atan2(pred_sol_xy[2,j]-(pred_sol_xy[2,j-1]),pred_sol_xy[1,j]-(pred_sol_xy[1,j-1]))
                    angle_deg = (angle_ell*180)/pi

                    ell1 = patch.Ellipse([pred_sol_xy[1,j],pred_sol_xy[2,j]], 0.2, 0.1, angle = 0)#angle = angle_deg)
                    ax[:add_artist](ell1)

                    plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og")
                    grid("on")
                    # title("Predicted solution in lap $i, iteration $j")
                    title("Position at iteration $j")
                    # end
                end

                figure(15)
                clf()
                subplot(221)
                plot(s_pred,vx_pred,"or")
                plot(olds,oldvx,"b")
                plot(olds2,oldvx2,"b")
                #plot(olds3,oldvx3,"b")
                #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1])
                title("State vx in lap $i, iteration $j, status = $solution_status")
                grid("on")

                subplot(222)
                plot(s_pred,vy_pred,"or")
                plot(olds,oldvy,"b")
                plot(olds2,oldvy2,"b")
                #plot(olds3,oldvy3,"b")
                #ylim(findmin(oldTraj.z_pred_sol[:,3,:,i])[1],findmax(oldTraj.z_pred_sol[:,3,:,i])[1])
                title("State vy in lap $i, iteration $j, status = $solution_status ")
                grid("on")

                subplot(223)
                plot(s_pred,psiDot_pred,"or")
                plot(olds,oldpsiDot,"b")
                plot(olds2,oldpsiDot2,"b")
                #plot(olds3,oldpsiDot3,"b")
                #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1])
                title("State psiDot in lap $i , iteration $j, status = $solution_status")
                grid("on")

                subplot(224)
                plot(s_pred,ePsi_pred,"or")
                plot(olds,oldePsi,"b")
                plot(olds2,oldePsi2,"b")
                #plot(olds3,oldePsi3,"b")
                #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1])
                title("State ePsi in lap $i, iteration $j, status = $solution_status ")
                grid("on")

                figure(16)
                clf()
                subplot(221)
                plot(s_pred,eY_pred,"or")
                plot(olds,oldeY,"b")
                plot(olds2,oldeY2,"b")
                #plot(olds3,oldeY3,"b")
                #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1])
                title("State eY in lap $i, iteration $j, status $solution_status ")
                grid("on")

                figure(17)
                clf()

                subplot(121)
                velocity= sqrt(oldSS.oldSS[1:j,2,i].^2 + oldSS.oldSS[1:j,1,i].^2)
                plot(t,velocity,"-*")
                legend(["velocity"])

                subplot(122)
                #plot(linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,1,j,i],linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,2,j,i])
                plot(t,input[1:j,1,i],t,input[1:j,2,i])
                plot(linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,1,j,i],"-*",linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,2,j,i],"-*")
                legend(["a_x","d_f"])

                title("Comparison between velocity and inputs in lap $i, iteration $j ")
                grid("on")

                figure(18)
                clf()

                subplot(121)
                plot(t,oldSS.oldSS[1:j,5,i],"-*")
                legend(["e_y"])

                subplot(122)
                # plot(linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,1,j,i],linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,2,j,i])
                plot(t,input[1:j,1,i],t,input[1:j,2,i])
                plot(linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,1,j,i],"-*",linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,2,j,i],"-*")
                legend(["a_x","d_f"])

                title("Comparison between e_y and inputs in lap $i, iteration $j ")
                grid("on")

                figure(20)
                clf()
                subplot(211)
                plot(t,oldSS.oldSS[1:j,5,i],"-*")
                title("eY in lap $i, iteration $j")
                grid("on")
                plot(linspace(j,j+size(pred_sol)[1],size(pred_sol)[1]),pred_sol[:,5,j,i],"-+")

                subplot(212)
                plot(t,cost[1:j,6,i])
                title("lane cost in lap $i, iteration $j")
                grid("on")

                 sleep(1)
            end
        end
    end

end

# THIS FUNCTION EVALUATES DATA THAT WAS RECORDED BY BARC_RECORD.JL
# ****************************************************************
function eval_run(code::AbstractString)
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    #log_path_record = "$(homedir())/open_loop/output-record-0ed5.jld"
    d_rec = load(log_path_record)

    imu_meas    = d_rec["imu_meas"]
    gps_meas    = d_rec["gps_meas"]
    cmd_log     = d_rec["cmd_log"]
    cmd_pwm_log = d_rec["cmd_pwm_log"]
    vel_est     = d_rec["vel_est"]
    pos_info    = d_rec["pos_info"]

    t0      = pos_info.t[1]
    track   = create_track(0.4)

    # Calculate accelerations
    acc = smooth(diff(smooth(vel_est.z[:,1],10))./diff(vel_est.t),10)
    figure(10)
    plot(vel_est.t[1:end-1]-t0,acc,cmd_log.t-t0,cmd_log.z[:,1])
    grid("on")
    legend(["Acc","u_a"])

    figure()
    plot(gps_meas.z[:,1],gps_meas.z[:,2],"-.",pos_info.z[:,6],pos_info.z[:,7],"-*")
    plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")
    grid(1)
    title("x-y-view")
    axis("equal")
    legend(["GPS meas","estimate"])

    figure()
    #plot(imu_meas.t-t0,imu_meas.z[:,7:9])
    plot(pos_info.t-t0,pos_info.z[:,19:20])
    grid("on")
    legend(["a_x","a_y"])
    ylabel("a [m/s^2]")
    xlabel("t [s]")
    title("Measured accelerations")

    figure()
    plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,6:7],"-",gps_meas.t_msg-t0,gps_meas.z,"--")
    grid(1)
    title("GPS comparison")
    xlabel("t [s]")
    ylabel("Position [m]")
    legend(["x_meas","y_meas","x_est","y_est"])

    figure()
    ax2=subplot(211)
    title("Commands")
    plot(cmd_log.t-t0,cmd_log.z,"-*",cmd_log.t_msg-t0,cmd_log.z,"-x")
    grid("on")
    xlabel("t [s]")
    subplot(212,sharex=ax2)
    plot(cmd_pwm_log.t-t0,cmd_pwm_log.z,"-*")
    grid("on")
    xlabel("t [s]")

    figure()
    title("Comparison of psi")
    plot(imu_meas.t-t0,imu_meas.z[:,6],imu_meas.t-t0,imu_meas.z[:,3],"-x",pos_info.t-t0,pos_info.z[:,10:11],"-*",pos_info.t-t0,pos_info.z[:,16],"-*",pos_info.t-t0,pos_info.z[:,14])
    legend(["imu_psi","imu_psi_dot","est_psi","est_psi_dot","psi_drift","psi_raw"])
    grid()

    figure()
    plot(pos_info.t-t0,pos_info.z[:,2:3])
    legend(["e_y","e_psi"])
    title("Deviations from reference")
    grid("on")
    xlabel("t [s]")
    ylabel("eY [m], ePsi [rad]")

    # figure()
    # title("Raw IMU orientation data")
    # plot(imu_meas.t-t0,imu_meas.z[:,1:3],"--",imu_meas.t-t0,imu_meas.z[:,4:6],imu_meas.t_msg-t0,imu_meas.z[:,4:6])
    # grid("on")
    # legend(["w_x","w_y","w_z","roll","pitch","yaw"])

    figure()
    title("v measurements and estimate")
    plot(vel_est.t-t0,vel_est.z[:,1],"-x",pos_info.t-t0,pos_info.z[:,[8:9,4]],"-+")
    legend(["v_raw","est_xDot","est_yDot","est_v"])
    grid()

    # figure()
    # title("s, eY and inputs")
    # ax1=subplot(211)
    # plot(pos_info.t-t0,pos_info.z[:,2:3],"-*")
    # grid("on")
    # legend(["eY","ePsi"])
    # subplot(212,sharex=ax1)
    # plot(cmd_log.t-t0,cmd_log.z,"-*")
    # grid("on")
    # legend(["u","d_f"])
    nothing
end

function plot_friction_circle(code::AbstractString,lap::Int64)
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"
    d_rec       = load(log_path_record)
    d_lmpc      = load(log_path_LMPC)

    oldTraj     = d_lmpc["oldTraj"]
    pos_info    = d_rec["pos_info"]

    # Find timing of selected lap:
    t_start = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].>=0,lap][1]
    t_end = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].<=19.11,lap][end]
    idx = (pos_info.t.>t_start) & (pos_info.t.<t_end)
    plot(pos_info.z[idx,17],pos_info.z[idx,18])
    grid("on")
    xlabel("a_x [m/s^2]")
    ylabel("a_y [m/s^2]")
    axis("equal")
    title("")
end

function plot_v_ey_over_s(code::AbstractString,laps::Array{Int64})
    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"
    d_lmpc      = load(log_path_LMPC)

    oldTraj     = d_lmpc["oldTraj"]
    n_laps = size(laps,1)

    println("OldCost: ",oldTraj.oldCost)
    # plot v_x over s
    figure(1)
    for i=1:n_laps
        idx = (oldTraj.oldTraj[:,6,laps[i]] .>= 0.0) & (oldTraj.oldTraj[:,6,laps[i]] .<= 19.11)
        plot(smooth(oldTraj.oldTraj[idx,6,laps[i]],5),oldTraj.oldTraj[idx,1,laps[i]],label="Lap $(laps[i])")
    end
    legend()
    grid("on")
    xlabel("s [m]")
    ylabel("v [m/s]")
    xlim([0,19.11])
    ylim([0,3.2])

    # plot e_y over s
    figure(2)
    for i=1:n_laps
        idx = (oldTraj.oldTraj[:,6,laps[i]] .>= 0.0) & (oldTraj.oldTraj[:,6,laps[i]] .<= 19.11)
        plot(oldTraj.oldTraj[idx,6,laps[i]],oldTraj.oldTraj[idx,5,laps[i]],label="Lap $(laps[i])")
    end
    legend()
    grid("on")
    xlabel("s [m]")
    ylabel("e_Y [m]")
    xlim([0,19.11])
    ylim([-0.5,0.5])

    # plot lap time over iterations
    laps_run = size(oldTraj.oldCost[oldTraj.oldCost.>1],1)
    lap_times = zeros(laps_run)
    for i=1:laps_run
        t_start = oldTraj.oldTimes[oldTraj.oldTraj[:,6,i].>=0,i][1]
        t_end = oldTraj.oldTimes[oldTraj.oldTraj[:,6,i].<=19.11,i][end]
        lap_times[i] = t_end-t_start
    end
    figure(3)
    plot(1:size(lap_times,1),lap_times,"-o")
    grid("on")
    xlabel("Lap number")
    ylabel("Lap time [s]")
    ylim([0,25])
end

function plot_v_over_xy(code::AbstractString,lap::Int64)
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"
    d_rec       = load(log_path_record)
    d_lmpc      = load(log_path_LMPC)

    oldTraj     = d_lmpc["oldTraj"]
    pos_info    = d_rec["pos_info"]

    # Find timing of selected lap:
    t_start = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].>=0,lap][1]
    t_end = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].<=19.11,lap][end]
    println("Laptime = $(t_end-t_start) s")

    idx = (pos_info.t.>=t_start) & (pos_info.t.<=t_end)

    track   = create_track(0.4)

    figure()
    plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")
    scatter(pos_info.z[idx,6],pos_info.z[idx,7],c=pos_info.z[idx,8],cmap=ColorMap("jet"),edgecolors="face",vmin=0.5,vmax=2.0)
    grid(1)
    title("x-y-view")
    axis("equal")
    cb = colorbar()
    cb[:set_label]("Velocity [m/s]")
    println("Average v_x = ",mean(pos_info.z[idx,8])," m/s")

end

function eval_open_loop(code::AbstractString)
    log_path_record = "$(homedir())/open_loop/output-record-$(code).jld"
    d_rec = load(log_path_record)

    imu_meas    = d_rec["imu_meas"]
    gps_meas    = d_rec["gps_meas"]
    cmd_log     = d_rec["cmd_log"]
    cmd_pwm_log = d_rec["cmd_pwm_log"]
    vel_est     = d_rec["vel_est"]
    pos_info    = d_rec["pos_info"]

    t0      = pos_info.t[1]

    figure()
    ax3=subplot(211)
    title("Comparison speed and input")
    plot(vel_est.t-t0,vel_est.z,"-*",vel_est.t_msg-t0,vel_est.z,"-x")
    legend(["vel_est"])
    grid("on")
    subplot(212,sharex=ax3)
    plot(cmd_pwm_log.t-t0,cmd_pwm_log.z[:,1],cmd_pwm_log.t_msg-t0,cmd_pwm_log.z[:,1])
    grid("on")

    gps_speed_raw = diff(gps_meas.z)./diff(gps_meas.t)
    gps_speed = [0;sqrt(gps_speed_raw[:,1].^2+gps_speed_raw[:,2].^2)]
    figure()
    title("Comparison GPS and encoder speed")
    plot(vel_est.t-t0,vel_est.z,"-*",gps_meas.t-t0,gps_speed,"-x",pos_info.t_msg-t0,pos_info.z[:,4],"-+")
    grid("on")
    legend(["encoder","gps","estimator"])

    figure()
    title("Acceleration data")
    plot(imu_meas.t-t0,imu_meas.z[:,7:9],"-x",imu_meas.t_msg-t0,imu_meas.z[:,7:9],"-*")
    legend(["a_x","a_y","a_z"])
    grid()

    figure()
    plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,12:13],"-x",pos_info.t-t0,pos_info.z[:,6:7])
    legend(["x_gps","y_gps","x_filtered","y_filtered","x_est","y_est"])
    grid("on")

    figure()
    plot(gps_meas.t-t0,gps_meas.z,"-*")
    legend(["x_gps","y_gps"])
    xlabel("t [s]")
    ylabel("Position [m]")
    grid("on")
end

# THIS FUNCTION EVALUATES MPC-SPECIFIC DATA
# *****************************************
function eval_LMPC(code::AbstractString)
    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    d_rec       = load(log_path_record)
    d_lmpc      = load(log_path_LMPC)

    oldTraj     = d_lmpc["oldTraj"]
    t           = d_lmpc["t"]
    state       = d_lmpc["state"]
    sol_z       = d_lmpc["sol_z"]
    sol_u       = d_lmpc["sol_u"]
    cost        = d_lmpc["cost"]
    curv        = d_lmpc["curv"]
    c_Vx        = d_lmpc["c_Vx"]
    c_Vy        = d_lmpc["c_Vy"]
    c_Psi       = d_lmpc["c_Psi"]
    cmd         = d_lmpc["cmd"]                 # this is the command how it was sent by the MPC
    step_diff   = d_lmpc["step_diff"]           # this is the command how it was sent by the MPC

    x_est       = d_lmpc["x_est"]
    coeffX      = d_lmpc["coeffX"]
    coeffY      = d_lmpc["coeffY"]
    imu_meas    = d_rec["imu_meas"]
    gps_meas    = d_rec["gps_meas"]
    cmd_log     = d_rec["cmd_log"]              # this is the command how it was received by the simulator
    pos_info    = d_rec["pos_info"]

    t0 = t[1]

    figure(2)
    ax1=subplot(311)
    plot(pos_info.t-t0,pos_info.z[:,8],".",t-t0,state[:,1],"-*")
    title("Comparison of publishing and receiving time")
    legend(["x_dot_estimate","x_dot_MPC"])
    grid("on")
    xlabel("t [s]")
    ylabel("v_x [m/s]")
    subplot(312,sharex=ax1)
    plot(cmd_log.t-t0,cmd_log.z,"-o",t-t0,cmd[1:length(t),:],"-*",t-t0,state[:,7])
    legend(["a_rec","d_f_rec","a_MPC","d_f_MPC","acc_filter"])
    grid("on")
    subplot(313,sharex=ax1)
    plot(t-t0,c_Vx)
    title("System ID xDot coefficients")
    grid("on")
    legend(["1","2","3"])

    figure(3)
    plot(t,state)
    grid("on")
    legend(["v_x","v_y","psiDot","ePsi","eY","s"])

    figure()
    subplot(311)
    title("c_Vx")
    plot(t-t0,c_Vx)
    legend(["1","2","3"])
    grid("on")
    subplot(312)
    title("c_Vy")
    plot(t-t0,c_Vy)
    legend(["1","2","3","4"])
    grid("on")
    subplot(313)
    title("c_Psi")
    plot(t-t0,c_Psi)
    legend(["1","2","3"])
    grid("on")

    # *********** CURVATURE *********************
    figure()
    c = zeros(size(curv,1),1)
    for i=1:size(curv,1)
        s = state[i,6]
        c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1]
        #c[i] = ([s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1]
        #c[i] = ([s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1]
    end
    plot(state[:,6],c,"-o")
    for i=1:5:size(curv,1)
        if isnan(sol_z[1,6,i])
            s = sol_z[:,1,i]
        else
            s = sol_z[:,6,i]
        end
        c = zeros(size(curv,1),1)
        c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]'
        #c = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]'
        #c = [s.^3 s.^2 s.^1 s.^0] * curv[i,:]'
        plot(s,c,"-*")
    end
    title("Curvature over path")
    xlabel("Curvilinear abscissa [m]")
    ylabel("Curvature")
    grid()

    track = create_track(0.4)
    figure()
    hold(1)
    plot(x_est[:,1],x_est[:,2],"-*")
    title("Estimated position")
    plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")
    axis("equal")
    grid(1)
    # HERE YOU CAN CHOOSE TO PLOT DIFFERENT DATA:
    # CURRENT HEADING (PLOTTED BY A LINE)
    # for i=1:10:size(pos_info.t,1)
    #     dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])]
    #     lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir]
    #     plot(lin[:,1],lin[:,2],"-+")
    # end

    # PREDICTED PATH
    for i=1:4:size(x_est,1)
            z_pred = zeros(11,4)
            z_pred[1,:] = x_est[i,:]
            for j=2:11
                z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125)
            end
            plot(z_pred[:,1],z_pred[:,2],"-*")
    end

    # PREDICTED REFERENCE PATH (DEFINED BY POLYNOM)
    # for i=1:size(x_est,1)
    #     s = 0.4:.1:2.5
    #     ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0]
    #     x = ss*coeffX[i,:]'
    #     y = ss*coeffY[i,:]'
    #     plot(x,y)
    # end

    # rg = 100:500
    # figure()
    # plot(s_start[rg]+state[rg,1],state[rg,2:4],"-o")
    # title("Comparison states and prediction")
    # legend(["ey","epsi","v"])
    # grid(1)
    # for i=100:5:500
    #     plot(s_start[i]+sol_z[:,1,i],sol_z[:,2:4,i],"-*")
    # end

    # figure()
    # plot(oldTraj[:,6,1,2],oldTraj[:,1:5,1,2],"-x")
    # title("Old Trajectory")
    # legend(["v_x","v_y","psiDot","ePsi","eY"])
    # xlabel("s")
    # grid(1)

    figure()
    ax1=subplot(211)
    title("MPC states and cost")
    plot(t-t0,state)
    legend(["v_x","v_y","psiDot","ePsi","eY","s"])
    grid(1)
    subplot(212,sharex = ax1)
    plot(t-t0,cost)
    grid(1)
    legend(["costZ","costZTerm","constZTerm","derivCost","controlCost","laneCost"])

    figure()
    ax1=subplot(211)
    title("States and inputs")
    plot(t-t0,state[:,1:5])
    legend(["v_x","v_y","psiDot","ePsi","eY"])
    grid(1)
    subplot(212,sharex = ax1)
    plot(cmd_log.t-t0,cmd_log.z)
    grid(1)
    legend(["u","d_f"])
end

function eval_predictions_kin(code::AbstractString)
    # This function helps to evaluate predictions of the *kinematic* model
    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    d_rec       = load(log_path_record)
    d_lmpc      = load(log_path_LMPC)

    t           = d_lmpc["t"]
    z           = d_lmpc["state"]
    sol_z       = d_lmpc["sol_z"]
    sol_u       = d_lmpc["sol_u"]
    cmd_lmpc    = d_lmpc["cmd"]
    cmd_log     = d_rec["cmd_log"]              # this is the command how it was received by the simulator
    pos_info    = d_rec["pos_info"]
    step_diff   = d_lmpc["step_diff"]

    t0 = t[1]

    sz = size(z,1)
    N = size(sol_z,1)-1     # number of steps (prediction horizon)
    figure(1)
    plot(pos_info.t-t0,pos_info.z[:,2],"--g")
    plot(t-t0,z[:,5],"-b")
    for i=1:sz
        plot((t[i]-t0):.1:(t[i]-t0+0.1*N),sol_z[:,2,i],"--xr")
    end
    grid("on")
    xlabel("t [s]")
    legend(["Estimate","LMPC estimate","Prediction"])
    title("e_y")

    figure(2)
    plot(pos_info.t-t0,pos_info.z[:,3],"--g")
    plot(t-t0,z[:,4],"-b")
    for i=1:sz
        plot((t[i]-t0):.1:(t[i]-t0+0.1*N),sol_z[:,3,i],"--xr")
    end
    grid("on")
    xlabel("t [s]")
    legend(["Estimate","LMPC estimate","Prediction"])
    title("e_psi")

    figure(3)
    plot(pos_info.t-t0,(pos_info.z[:,8].^2+pos_info.z[:,9].^2).^0.5,"--g")
    plot(t-t0,(z[:,1].^2+z[:,2].^2).^0.5,"-b")
    for i=1:sz
        plot(linspace(t[i]-t0,t[i]-t0+0.1*N,N+1),sol_z[:,4,i],"--xr")
    end
    grid("on")
    xlabel("t [s]")
    legend(["Estimate","LMPC estimate","Prediction"])
    title("v")

    figure(4)
    plot(cmd_log.t-t0,cmd_log.z,"--g")
    plot(t-t0,cmd_lmpc,"-b")
    for i=1:sz
        plot(linspace(t[i]-t0,t[i]-t0+0.1*(N-2),N-1),sol_u[1:N-1,1,i],"--xr")
        plot(linspace(t[i]-t0,t[i]-t0+0.1*(N-3),N-2),sol_u[1:N-2,2,i],"--xr")
    end
    grid("on")

    # Calculate one-step-errors:
    one_step_err = zeros(sz,4)
    for i=1:sz-1
        one_step_err[i,:] = sol_z[2,1:4,i] - [z[i+1,6] z[i+1,5] z[i+1,4] norm(z[i+1,1:2])]
    end

    # Calculate 'real' d_f
    L_b = 0.125
    v_x = real(sqrt(complex((pos_info.z[:,8].^2+pos_info.z[:,9].^2)-pos_info.z[:,11].^2*L_b^2)))
    delta = atan2(pos_info.z[:,11]*0.25,v_x)

    figure(5)
    ax1=subplot(211)
    plot(t-t0,one_step_err)
    grid("on")
    legend(["s","ey","epsi","v"])
    subplot(212,sharex=ax1)
    plot(t-t0,cmd_lmpc)
    plot(cmd_log.t_msg-t0,cmd_log.z)
    plot(cmd_log.t-t0,cmd_log.z,"--")
    plot(pos_info.t-t0,delta)
    grid("on")

end
function eval_predictions(code::AbstractString)
    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    d_rec       = load(log_path_record)
    d_lmpc      = load(log_path_LMPC)

    t           = d_lmpc["t"]
    z           = d_lmpc["state"]
    sol_z       = d_lmpc["sol_z"]
    sol_u       = d_lmpc["sol_u"]
    cmd_log     = d_rec["cmd_log"]              # this is the command how it was received by the simulator
    pos_info    = d_rec["pos_info"]
    step_diff   = d_lmpc["step_diff"]

    t0 = t[1]

    N = size(sol_z,1)-1     # number of steps (prediction horizon)

    #N = 7
    sz = size(t,1)
    # Calculate one-step-errors:
    ###########
    # s
    # eY
    # ePsi
    # v
    ###########
    # v_x
    # v_y
    # psiDot
    # ePsi
    # eY
    # s
    ###########
    one_step_err = NaN*ones(sz,6)
    for i=1:sz-1
        if isnan(sol_z[1,5,i])          # if we are in path following mode
            one_step_err[i,1:4] = sol_z[2,1:4,i] - [z[i+1,[6,5,4]] norm(z[i+1,1:2])]
        else
            one_step_err[i,:] = sol_z[2,[6,5,4,1,2,3],i] - z[i+1,[6,5,4,1,2,3]]
        end
    end
    i=1
    while isnan(sol_z[1,5,i])
        i = i+1
    end
    figure(1)
    plot(t-t0,step_diff)
    grid("on")
    title("One-step-errors")
    legend(["xDot","yDot","psiDot","ePsi","eY"])

    figure(3)
    plot(pos_info.t-t0,pos_info.z[:,11],"-o")
    title("Open loop predictions psidot")
    for i=1:1:size(t,1)-N
        if sol_z[1,5,i]==NaN
            #plot(t[i:i+N]-t0,sol_z[:,2,i],"+")
        else
            plot(t[i:i+N]-t0,sol_z[:,3,i],"-x")
        end
    end
    grid("on")

    figure(4)
    plot(pos_info.t-t0,pos_info.z[:,9],"-o")
    title("Open loop predictions v_y")
    for i=1:1:size(t,1)-N
        if sol_z[1,5,i]==NaN
            #plot(t[i:i+N]-t0,sol_z[:,2,i],"+")
        else
            plot(t[i:i+N]-t0,sol_z[:,2,i],"-x")
        end
    end
    grid("on")

    figure(2)
    ax1=subplot(411)
    title("Open loop predictions e_y")
    plot(pos_info.t-t0,pos_info.z[:,2],"-o")
    for i=1:2:size(t,1)-N
        if sol_z[1,5,i]==NaN
            plot(t[i:i+N]-t0,sol_z[:,2,i],"-+")
        else
            plot(t[i:i+N]-t0,sol_z[:,5,i])
        end
    end
    grid("on")

    subplot(412,sharex=ax1)
    title("Open loop predictions e_psi")
    plot(pos_info.t-t0,pos_info.z[:,3],"-o")
    for i=1:2:size(t,1)-N
        if sol_z[1,5,i]==NaN
            plot(t[i:i+N]-t0,sol_z[:,3,i],"-+")
        else
            plot(t[i:i+N]-t0,sol_z[:,4,i])
        end
    end
    grid("on")

    subplot(413,sharex=ax1)
    title("Open loop predictions v")
    plot(pos_info.t-t0,pos_info.z[:,8],"-o")
    for i=1:2:size(t,1)-N
        if sol_z[1,5,i]==NaN
            plot(t[i:i+N]-t0,sol_z[:,4,i],"-+")
        else
            plot(t[i:i+N]-t0,sol_z[:,1,i])
        end
    end
    grid("on")

    N = 7
    subplot(414,sharex=ax1)
    title("Open loop inputs")
    for i=1:2:size(t,1)-N
        plot(t[i:i+N-3]-t0,sol_u[1:N-2,2,i],t[i:i+N-1]-t0,sol_u[1:N,1,i])
    end
    grid("on")
end

function eval_sysID(code::AbstractString)
    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    d_rec       = load(log_path_record)
    d_lmpc      = load(log_path_LMPC)

    oldTraj     = d_lmpc["oldTraj"]
    t           = d_lmpc["t"]
    state       = d_lmpc["state"]
    sol_z       = d_lmpc["sol_z"]
    sol_u       = d_lmpc["sol_u"]
    cost        = d_lmpc["cost"]
    curv        = d_lmpc["curv"]
    c_Vx        = d_lmpc["c_Vx"]
    c_Vy        = d_lmpc["c_Vy"]
    c_Psi       = d_lmpc["c_Psi"]
    cmd         = d_lmpc["cmd"]                 # this is the command how it was sent by the MPC
    step_diff   = d_lmpc["step_diff"]           # this is the command how it was sent by the MPC

    x_est       = d_lmpc["x_est"]
    coeffX      = d_lmpc["coeffX"]
    coeffY      = d_lmpc["coeffY"]
    imu_meas    = d_rec["imu_meas"]
    gps_meas    = d_rec["gps_meas"]
    cmd_log     = d_rec["cmd_log"]              # this is the command how it was received by the simulator
    pos_info    = d_rec["pos_info"]

    t0 = imu_meas.t[1]

    figure(1)       # longitudinal (xDot)
    ax1=subplot(211)
    title("Vx")
    plot(t-t0,c_Vx)
    legend(["c1","c2","c3"])
    grid("on")
    subplot(212,sharex=ax1)
    plot(cmd_log.t-t0,cmd_log.z[:,1])
    grid("on")

    figure(2)       # longitudinal (xDot)
    ax2=subplot(211)
    title("Psi")
    plot(t-t0,c_Psi)
    legend(["c1","c2","c3"])
    grid("on")
    subplot(212,sharex=ax2)
    plot(cmd_log.t-t0,cmd_log.z[:,2])
    grid("on")

    figure(3)       # longitudinal (xDot)
    ax3=subplot(211)
    title("Vy")
    plot(t-t0,c_Vy)
    legend(["c1","c2","c3","c4"])
    grid("on")
    subplot(212,sharex=ax3)
    plot(cmd_log.t-t0,cmd_log.z[:,2])
    grid("on")
end

function eval_oldTraj(code::AbstractString,i::Int64)
    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"
    d = load(log_path_LMPC)
    oldTraj = d["oldTraj"]
    #t       = d["t"]
    plot(oldTraj.oldTimes[:,i],oldTraj.oldTraj[:,:,i],"-x")
    grid("on")
    legend(["v_x","v_x","psiDot","ePsi","eY","s"])
    figure()
    plot(oldTraj.oldTimes[:,i],oldTraj.oldInput[:,:,i],"-x")
    grid("on")
    legend(["a","d_f"])
    figure()
    plot(oldTraj.oldTraj[:,6,i],oldTraj.oldTraj[:,1:5,i],"-x")
    grid("on")
    legend(["v_x","v_x","psiDot","ePsi","eY"])
end

function eval_LMPC_coeff(code::AbstractString,k::Int64)
    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"
    d           = load(log_path_LMPC)
    oldTraj     = d["oldTraj"]
    sol_z       = d["sol_z"]
    sol_u       = d["sol_u"]
    coeffCost   = d["coeffCost"]
    coeffConst  = d["coeffConst"]
    cost        = d["cost"]

    s   = sol_z[:,6,k]
    ss  = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0]
    subplot(311)
    plot(s,sol_z[:,5,k],"-o",s,ss*coeffConst[:,1,5,k],s,ss*coeffConst[:,2,5,k])
    grid()
    title("Position = $(s[1]), k = $k")
    xlabel("s")
    ylabel("e_Y")
    subplot(312)
    plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,4,k],s,ss*coeffConst[:,2,4,k])
    grid()
    xlabel("s")
    ylabel("e_Psi")
    subplot(313)
    plot(s,sol_z[:,1,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k])
    grid()
    xlabel("s")
    ylabel("v_x")
    println("Cost = $(cost[k,3])")
end

function anim_LMPC_coeff(code::AbstractString)
    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"
    d           = load(log_path_LMPC)
    oldTraj     = d["oldTraj"]
    sol_z       = d["sol_z"]
    sol_u       = d["sol_u"]
    coeffCost   = d["coeffCost"]
    coeffConst  = d["coeffConst"]
    cost        = d["cost"]
    sol_status  = d["sol_status"]

    i=1
    while isnan(sol_z[1,6,i])
        i = i+1
    end
    for k=i:size(cost,1)
        clf()
        s   = sol_z[:,6,k]
        ss  = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0]
        subplot(511)
        plot(s,sol_z[:,5,k],"-o",s,ss*coeffConst[:,1,5,k],s,ss*coeffConst[:,2,5,k])
        ylim([-0.4,0.4])
        grid()
        title("Position = $(s[1]), k = $k, status = $(sol_status[k])")
        xlabel("s")
        ylabel("e_Y")
        subplot(512)
        plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,4,k],s,ss*coeffConst[:,2,4,k])
        ylim([-0.5,0.5])
        grid()
        xlabel("s")
        ylabel("e_Psi")
        subplot(513)
        plot(s,sol_z[:,3,k],"-o",s,ss*coeffConst[:,1,3,k],s,ss*coeffConst[:,2,3,k])
        ylim([-0.5,0.5])
        grid()
        xlabel("s")
        ylabel("psiDot")
        subplot(514)
        plot(s,sol_z[:,1,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k])
        ylim([0.6,1.5])
        grid()
        xlabel("s")
        ylabel("v_x")
        subplot(515)
        plot(s,sol_z[:,2,k],"-o",s,ss*coeffConst[:,1,2,k],s,ss*coeffConst[:,2,2,k])
        ylim([-0.5,0.5])
        grid()
        xlabel("s")
        ylabel("v_y")
        println("Cost = $(cost[k,3])")
    end
end

function visualize_tdiff(code::AbstractString)
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    d_rec       = load(log_path_record)
    pos_info    = d_rec["pos_info"]
    t_diff = diff(pos_info.t_msg)
    t_diff = t_diff[t_diff .< 0.08]
    plt[:hist](t_diff,100)
    grid("on")
    xlabel("t_diff")
    nothing
end

function anim_run(code::AbstractString)
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    d_rec       = load(log_path_record)
    pos_info    = d_rec["pos_info"]
    L_a = 0.125
    w = 0.15
    alpha = atan(w/2/L_a)
    l = sqrt(L_a^2+(w/2)^2)
    t0 = pos_info.t[1]
    #Construct Figure and Plot Data
    fig = figure(figsize=(10,10))
    ax = axes(xlim = (-3,3),ylim=(-5,1))

    track = create_track(0.4)
    plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")
    grid("on")
    car = ax[:plot]([],[],"r-+")[1]
    gps = ax[:plot]([],[],"g*")[1]
    h_ti = ax[:title]("abc")
    function init()
        car[:set_data]([],[])
        return (car,None)
    end
    function animate(k)
        i = k + 2000
        car_c = [pos_info.z[i,6]+cos(pos_info.z[i,10]-alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]-alpha)*l;
                pos_info.z[i,6]+cos(pos_info.z[i,10]+alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]+alpha)*l;
                pos_info.z[i,6]+cos(pos_info.z[i,10]+pi-alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]+pi-alpha)*l;
                pos_info.z[i,6]+cos(pos_info.z[i,10]+pi+alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]+pi+alpha)*l;
                pos_info.z[i,6]+cos(pos_info.z[i,10]-alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]-alpha)*l]
        car[:set_data]([car_c[:,1]],[car_c[:,2]])
        gps[:set_data]([pos_info.z[max(1,i-100):i,12]],[pos_info.z[max(1,i-100):i,13]])
        ax[:set_title]("t = $(pos_info.t[i]-t0)")
        #title(pos_info.t[i]-t0)
        return (car,gps,None)
    end
    t=0:30
    anim = animation.FuncAnimation(fig, animate, frames=1000, interval=50)
    anim[:save]("test2.mp4", bitrate=-1, extra_args=["-vcodec", "libx264", "-pix_fmt", "yuv420p"]);
end

function anim_constraints(code::AbstractString)
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"
    d_rec       = load(log_path_record)
    d_lmpc      = load(log_path_LMPC)

    t           = d_lmpc["t"]
    sol_z       = d_lmpc["sol_z"]
    sol_u       = d_lmpc["sol_u"]
    coeffCost   = d_lmpc["coeffCost"]
    coeffConst  = d_lmpc["coeffConst"]

    #Construct Figure and Plot Data
    fig = figure(figsize=(10,10))
    ax = axes()

    pred_vx = ax[:plot]([],[],"r-+")[1]
    poly_vx1 = ax[:plot]([],[],"--")[1]
    poly_vx2 = ax[:plot]([],[],"--")[1]

    function init()
        #car[:set_data]([],[])
        return (None)
    end
    function animate(k)
        i = k + 1000
        s = sol_z[:,6,i]
        ss  = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0]
        pred_vx[:set_data]([s],[sol_z[:,1,i]])
        poly_vx1[:set_data]([s],[ss*coeffConst[:,1,1,i]])
        poly_vx2[:set_data]([s],[ss*coeffConst[:,2,1,i]])

        return (pred_vx,poly_vx1,poly_vx2,None)
    end
    t=0:30
    anim = animation.FuncAnimation(fig, animate, frames=100, interval=50)
    anim[:save]("test2.mp4", bitrate=-1, extra_args=["-vcodec", "libx264", "-pix_fmt", "yuv420p"]);
end

# *****************************************************************
# ****** HELPER FUNCTIONS *****************************************
# *****************************************************************

function anim_MPC(z)
    figure()
    hold(0)
    grid(1)
    for i=1:size(z,3)
        plot(z[:,:,i])
    xlim([1,11])
    ylim([-2,2])
        sleep(0.01)
    end
end

function anim_curv(curv)
    s = 0.0:.05:2.0
    figure()
    hold(0)
    #ss = [s.^10 s.^9 s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0]
    ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0]
    for i=1:size(curv,1)
        c = ss*curv[i,:]'
        plot(s,c)
        xlim([0,2])
        ylim([-1.5,1.5])
        sleep(0.1)
    end
end

# function eval_prof()
#     Profile.clear()
#     @load "$(homedir())/simulations/profile.jlprof"
#     ProfileView.view(li, lidict=lidict)
# end

function create_track(w)
    x = [0.0]           # starting point
    y = [0.0]
    x_l = [0.0]           # starting point
    y_l = [w]
    x_r = [0.0]           # starting point
    y_r = [-w]
    ds = 0.06

    theta = [0.0]

    # SOPHISTICATED TRACK
    # add_curve(theta,30,0.0)
    # add_curve(theta,60,-2*pi/3)
    # add_curve(theta,90,pi)
    # add_curve(theta,80,-5*pi/6)
    # add_curve(theta,10,0.0)
    # add_curve(theta,50,-pi/2)
    # add_curve(theta,50,0.0)
    # add_curve(theta,40,-pi/4)
    # add_curve(theta,30,pi/4)
    # add_curve(theta,20,0.0)
    # add_curve(theta,50,-pi/2)
    # add_curve(theta,25,0.0)
    # add_curve(theta,50,-pi/2)
    # add_curve(theta,28,0.0)

    # # SIMPLE track
    # add_curve(theta,50,0)
    # add_curve(theta,100,-pi)
    # add_curve(theta,100,0)
    # add_curve(theta,100,-pi)
    # add_curve(theta,49,0)

    # GOGGLE TRACK
    # add_curve(theta,30,0)
    # add_curve(theta,40,-pi/2)
    # add_curve(theta,40,-pi/2)
    # add_curve(theta,20,-pi/6)
    # add_curve(theta,30,pi/3)
    # add_curve(theta,20,-pi/6)
    # add_curve(theta,40,-pi/2)
    # add_curve(theta,40,-pi/2)
    # add_curve(theta,35,0)

    # SIMPLE GOGGLE TRACK

    add_curve(theta,30,0)
    add_curve(theta,40,-pi/2)
    add_curve(theta,10,0)
    add_curve(theta,40,-pi/2)
    add_curve(theta,20,pi/10)
    add_curve(theta,30,-pi/5)
    add_curve(theta,20,pi/10)
    add_curve(theta,40,-pi/2)
    add_curve(theta,10,0)
    add_curve(theta,40,-pi/2)
    add_curve(theta,35,0)

    # SIMPLE GOOGLE TRACK FOR 3110

    # add_curve(theta,25,0)
    # add_curve(theta,40,-pi/2)
    # add_curve(theta,10,0)
    # add_curve(theta,40,-pi/2)
    # add_curve(theta,17,-pi/10)
    # add_curve(theta,25,pi/5)
    # add_curve(theta,17,-pi/10)
    # add_curve(theta,40,-pi/2)
    # add_curve(theta,10,0)
    # add_curve(theta,40,-pi/2)
    # add_curve(theta,32,0)

    # OVAL TRACK FOR TESTS IN VSD

    # add_curve(theta,80,0)
    # add_curve(theta,110,-pi)
    # add_curve(theta,160,0)
    # add_curve(theta,110,-pi)
    # add_curve(theta,80,0)

    # add_curve(theta,53,0)
    # add_curve(theta,73,-pi)
    # add_curve(theta,106,0)
    # add_curve(theta,73,-pi)
    # add_curve(theta,53,0)

    #  # SHORT SIMPLE track
    # add_curve(theta,10,0)
    # add_curve(theta,80,-pi)
    # add_curve(theta,20,0)
    # add_curve(theta,80,-pi)
    # add_curve(theta,9,0)

    for i=1:length(theta)
            push!(x, x[end] + cos(theta[i])*ds)
            push!(y, y[end] + sin(theta[i])*ds)
            push!(x_l, x[end-1] + cos(theta[i]+pi/2)*w)
            push!(y_l, y[end-1] + sin(theta[i]+pi/2)*w)
            push!(x_r, x[end-1] + cos(theta[i]-pi/2)*w)
            push!(y_r, y[end-1] + sin(theta[i]-pi/2)*w)
    end
    track = cat(2, x, y, x_l, y_l, x_r, y_r)
    #plot(track[:,1],track[:,2])
    return track
    #plot(x,y,x_l,y_l,x_r,y_r)
end

function add_curve(theta::Array{Float64}, length::Int64, angle)
    d_theta = 0
    curve = 2*sum(1:length/2)+length/2
    for i=0:length-1
        if i < length/2+1
            d_theta = d_theta + angle / curve
        else
            d_theta = d_theta - angle / curve
        end
        push!(theta, theta[end] + d_theta)
    end
end

function initPlot()
    linewidth = 0.4
    rc("axes", linewidth=linewidth)
    rc("lines", linewidth=linewidth, markersize=2)
    #rc("font", family="")
    rc("axes", titlesize="small", labelsize="small")        # can be set in Latex
    rc("xtick", labelsize="x-small")
    rc("xtick.major", width=linewidth/2)
    rc("ytick", labelsize="x-small")
    rc("ytick.major", width=linewidth/2)
    rc("legend", fontsize="small")
    rc("font",family="serif")
    rc("font",size=8)
    rc("figure",figsize=[4.5,3])
    #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""")
end

function simModel(z,u,dt,l_A,l_B)

   # kinematic bicycle model
   # u[1] = acceleration
   # u[2] = steering angle

    bta = atan(l_A/(l_A+l_B)*tan(u[2]))

    zNext = copy(z)
    zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta))                 # x
    zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta))                 # y
    zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta))                    # psi
    zNext[4] = z[4] + dt*(u[1] - 0.5 * z[4])                    # v

    return zNext
end

function checkTimes(code::AbstractString)
    log_path_LMPC   = "$(homedir())/simulations/output-LMPC-$(code).jld"
    d_lmpc      = load(log_path_LMPC)

    t_solv     = d_lmpc["t_solv"]
    sol_status = d_lmpc["sol_status"]
    cmd        = d_lmpc["cmd"]                 # this is the command how it was sent by the MPC
    sol_status_int = zeros(size(t_solv,1))
    t = d_lmpc["t"]
    t0 = t[1]

    for i=1:size(sol_status,1)
        if sol_status[i]==:Optimal
            sol_status_int[i] = 1
        elseif sol_status[i]==:Infeasible
            sol_status_int[i] = 2
        elseif sol_status[i]==:UserLimit
            sol_status_int[i] = 3
        end
    end
    ax1=subplot(211)
    plot(t,t_solv)
    plot(t,sol_status_int,"*")
    grid("on")
    subplot(212,sharex=ax1)
    plot(t,cmd,"-x")
    grid("on")

    figure(2)
    plot(t-t0,t_solv)
    title("Solver time")
    grid("on")
    xlabel("t [s]")
    ylabel("Solver time [s]")
end

function checkConnectivity(code::AbstractString)
    log_path_record = "$(homedir())/open_loop/output-record-$code.jld"
    d_rec = load(log_path_record)

    cmd_pwm_log = d_rec["cmd_pwm_log"]
    vel_est     = d_rec["vel_est"]
    pos_info    = d_rec["pos_info"]
    imu_meas    = d_rec["imu_meas"]
    gps_meas    = d_rec["gps_meas"]

    plot(gps_meas.t,gps_meas.z,"-x",gps_meas.t_msg,gps_meas.z,"--x",pos_info.t,pos_info.z[:,12:13],"-*",pos_info.t_msg,pos_info.z[:,12:13],"--*")
    grid("on")
end

function smooth(x,n)
    y = zeros(size(x))
    for i=1:size(x,1)
        start = max(1,i-n)
        fin = min(size(x,1),start + 2*n)
        y[i,:] = mean(x[start:fin,:],1)
    end
    return y
end

function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,track::Array{Float64})

    obs   = obs_log[:,:,obstacle,lap]

    buffersize = size(obs)[1]

    # println("obs= ",obs)

    OrderXY        = 10
    OrderThetaCurv = 8

    ds = 0.06

    s_vec = zeros(OrderXY+1)

    pred_sol_xy = zeros(2,buffersize,1)
    xyPathAngle = []

    x_track = track[:,1]

    y_track = track[:,2]

    #println("x_track= ",x_track)
    # println("nodes= ",size([x_track'; y_track']))
    for i = 1:buffersize

            nodes          = [x_track'; y_track']
            n_nodes        = size(x_track)[1]
            #println("number of nodes= ",n_nodes)
            s_start   = (obs[i,1] - 2)
            #println("s_start= ",s_start)
            s_end     = (obs[i,1] + 10)
            #println("s_end= ",s_end)
            s_nearest = obs[i,1]

            idx_start = round(s_start / ds)
            #println("idx_start= ",idx_start)
            idx_end   = round(s_end / ds)
            #println("idx_end= ",idx_end)

            # if idx_start>n_nodes
            #   idx_start=idx_start%n_nodes
            #   idx_end=idx_end%n_nodes
            # end

            if idx_start<=0
                 nodes_XY = hcat(nodes[:,n_nodes+idx_start:n_nodes],nodes[:,1:idx_end])       # then stack the end and beginning of a lap together
            #     #nodes_Y = hcat(nodes[2,n_nodes+idx_start:n_nodes],nodes[2,1:idx_end])

                 #idx_start = n_nodes+idx_start
            elseif idx_end>=n_nodes                   # if the end is behind the finish line
                 nodes_XY = hcat(nodes[:,idx_start:n_nodes],nodes[:,1:idx_end-n_nodes])       # then stack the end and beginning of the lap together
                 #nodes_Y = hcat(nodes[2,idx_start:n_nodes],nodes[2,1:idx_end-n_nodes])
            else                               # if we are somewhere in the middle of the track
                nodes_XY = nodes[:,idx_start:idx_end]     # then just use the nodes from idx_start to end for interpolation
             #nodes_Y = nodes[2,idx_start:idx_end]
            end

            nodes_X = vec(nodes_XY[1,:])
            nodes_Y = vec(nodes_XY[2,:])
            #println("length node xy= ",length(nodes_XY[1,:]))

            n_poly = round(Int,length(nodes_XY[1,:]))
            #println("n_poly= ",n_poly)

            itp_matrix = zeros(n_poly,OrderXY+1)

            for ind=1:n_poly
                for k=0:OrderXY

                    itp_matrix[ind,OrderXY+1-k] = (s_start + (ind-1)*ds)^k
                end
            end

            itp_matrix_curv = zeros(n_poly,OrderThetaCurv+1)

            for ind=1:n_poly
                for k=0:OrderThetaCurv

                    itp_matrix_curv[ind,OrderThetaCurv+1-k] = (s_start + (ind-1)*ds)^k
                end
            end

           # println("size of nodes x= ",size(nodes_X))
           # println("size of itpmatrix= ",size(itp_matrix))
           # println("s start= ",s_start)
           # println("s end= ",s_end)

            coeffY = itp_matrix\nodes_Y
            coeffX = itp_matrix\nodes_X

            b_curvature_vector = zeros(n_poly)

            Counter = 1

            for ind = 0:n_poly-1
                s_expression_der  = zeros(OrderXY+1)
                s_expression_2der = zeros(OrderXY+1)
                s_poly       = s_start + ind*ds
                for k=0:OrderXY-1
                    s_expression_der[OrderXY-k] = (k+1)*s_poly^k
                end
                for k=0:OrderXY-2
                    s_expression_2der[OrderXY-1-k] = (2+k*(3+k))*s_poly^k
                end

                dX  = dot(coeffX,s_expression_der)
                dY  = dot(coeffY,s_expression_der)
                ddX = dot(coeffX,s_expression_2der)
                ddY = dot(coeffY,s_expression_2der)

                curvature = (dX*ddY-dY*ddX)/((dX^2+dY^2)^(3/2)) #standard curvature formula

                b_curvature_vector[Counter] = curvature

                Counter = Counter + 1
            end

            coeffCurv  = itp_matrix_curv\b_curvature_vector

            s0 =  obs[i,1]+0.001

            s_vec = zeros(OrderXY+1)::Array{Float64}
            sdot_vec = zeros(OrderXY+1)::Array{Float64}

            for k = 1:OrderXY+1
                    s_vec[k] = obs[i,1]^(OrderXY-k+1)

            end
            for k = 1:OrderXY
                    sdot_vec[k] = (OrderXY+1-k)* obs[i,1]^(OrderXY-k)
            end

            XCurve  = dot(coeffX,s_vec)
            YCurve  = dot(coeffY,s_vec)

            dX = dot(coeffX,sdot_vec)
            dY = dot(coeffY,sdot_vec)

            xyPathAngle = atan2(dY,dX)

            pred_sol_xy[2,i] = YCurve + obs[i,2]*cos(xyPathAngle)
            pred_sol_xy[1,i] = XCurve - obs[i,2]*sin(xyPathAngle)

    end

    return pred_sol_xy, xyPathAngle
end

max_stars_repo_name: RoyaFiroozi/barc

max_issues_repo_name: RoyaFiroozi/barc

max_forks_repo_name: RoyaFiroozi/barc


text:
function readconf(file)
    vars = Dict()
    for line in eachline(file)
        line = strip(line)
        if !isempty(line) && !startswith(line, '#') && !startswith(line, ';')
            fspace  = searchindex(line, " ")
            if fspace == 0
                vars[Symbol(lowercase(line))] = true
            else
                vname, line = Symbol(lowercase(line[1:fspace-1])), line[fspace+1:end]
                value = ',' ∈ line ? strip.(split(line, ',')) : line
                vars[vname] = value
            end
        end
    end
    for (vname, value) in vars
        eval(:($vname = $value))
    end
    return vars
end

readconf("test.conf")

@show fullname favouritefruit needspeeling otherfamily

ext: jl

max_stars_repo_name: ethansaxenian/RosettaDecode

max_issues_repo_name: ethansaxenian/RosettaDecode

max_forks_repo_name: ethansaxenian/RosettaDecode


text:
"""
    lattice = BravaisLattice(sim::Simulation)

Rectangular lattice with same size as defined in `sim.bnd` along periodic directions.
"""
# Bravais.BravaisLattice(sim::Simulation) = BravaisLattice(sim.bnd)

"""
    lattice = BravaisLattice(bnd::Boundary)

Rectangular lattice with same size as defined in `bnd` along periodic directions.
"""
function Bravais.BravaisLattice(bnd::Boundary)
    if typeof(bnd.bc[1][1])<:FloquetBC && typeof(bnd.bc[1][2])<:FloquetBC
        a = bnd.∂Ω[1][2]-bnd.∂Ω[1][1]
    elseif (typeof(bnd.bc[1][1])<:FloquetBC && !(typeof(bnd.bc[1][2])<:FloquetBC)) || (typeof(bnd.bc[1][2])<:FloquetBC && !(typeof(bnd.bc[1][1])<:FloquetBC))
        throw(ArgumentError("only one boundary condition along dimension 1 is periodic."))
    else
        a = Inf
    end
    if typeof(bnd.bc[2][1])<:FloquetBC && typeof(bnd.bc[2][2])<:FloquetBC
        b = bnd.∂Ω[2][2]-bnd.∂Ω[2][1]
    elseif (typeof(bnd.bc[2][1])<:FloquetBC && !(typeof(bnd.bc[2][2])<:FloquetBC)) || (typeof(bnd.bc[2][2])<:FloquetBC && !(typeof(bnd.bc[2][1])<:FloquetBC))
        throw(ArgumentError("only one boundary condition along dimension 2 is periodic."))
    else
        b = Inf
    end
    return BravaisLattice(a=a, b=b)
end

"""
    xb, yb = bravais_coordinates_unit_cell(x, y, domain_index, sys)

maps cartesian (`x`,`y`) into cartesian (`xb`,`yb`) unit cell specified by
sys.domains[domain_index].lattice
"""
# Bravais.bravais_coordinates_unit_cell(x, y, domain::Int, system::System) =
    # bravais_coordinates_unit_cell(x, y, system.domains[domain])

"""
    xb, yb = bravais_coordinates_unit_cell(x, y, domain)

maps cartesian (`x`,`y`) into cartesian (`xb`,`yb`) unit cell specified by
domain.lattice
"""
# Bravais.bravais_coordinates_unit_cell(x, y, domain::Domain) =
#     bravais_coordinates_unit_cell(x, y, domain.lattice)
# function Bravais.bravais_coordinates_unit_cell(x, y, domains::Array)
#     lattices = [domains[i].lattice for i ∈ eachindex(domains)]
#     #lattices = Array{BravaisLattice}(undef,size(x,1),size(y,2))
#     # for i ∈ CartesianIndices(domains)
#         # lattices[i] = domains[i].lattice
#     # end
#     return bravais_coordinates_unit_cell(x, y, lattices)
# end

"""
    xb, yb = bravais_coordinates_unit_cell(x, y, sim)

maps cartesian (`x`,`y`) into cartesian (`xb`,`yb`) unit cell specified by
sim.lat
"""
# Bravais.bravais_coordinates_unit_cell(x, y, sim::Simulation) =
#     bravais_coordinates_unit_cell(x,y,sim.lat)

"""
    bravais_coordinates_unit_cell!(xb, yb, x, y, domain_index, sys)

maps cartesian (`x`,`y`) into pre-allocated cartesian (`xb`,`yb`) unit cell specified by
sys.domains[domain_index].lattice
"""
# Bravais.bravais_coordinates_unit_cell!(xb::Array, yb::Array, x::AbstractArray, y::AbstractArray, domains::Array, system::System) =
    # bravais_coordinates_unit_cell!(xb, yb, x, y, system.domains[domains])

"""
    bravais_coordinates_unit_cell(xb, yb, x, y, domain)

maps cartesian (`x`,`y`) into pre-allocated cartesian (`xb`,`yb`) unit cell specified by
domain.lattice
"""
function Bravais.bravais_coordinates_unit_cell!(xb::AbstractArray, yb::AbstractArray, x::AbstractArray, y::AbstractArray, domains::AbstractArray, sys::System)
    for i ∈ eachindex(xb)
        xb[i],yb[i] = bravais_coordinates_unit_cell(x[i],y[i],sys.domains[domains[i]].lattice)
    end
    return nothing
end

"""
    bravais_coordinates_unit_cell!(xb, yb, x, y, sim)

maps cartesian (`x`,`y`) into pre-allocated cartesian (`xb`,`yb`) unit cell specified by
sim.lat
"""
# Bravais.bravais_coordinates_unit_cell!(xb, yb, x, y, sim::Simulation) =
#     bravais_coordinates_unit_cell!(xb, yb, x,y,sim.lat)

"""
    p1, p2 = bravais_coordinates(x, y, domain_index, sys)

coordinates in bravais frame specified by sys.domains[domain_index].lattice
(i.e. (x,y) = p1*v1 + p2*v2)
"""
# Bravais.bravais_coordinates(x, y, domain, system::System) =
#     bravais_coordinates(x, y, system.domains[domain])
"""
    p1, p2 = bravais_coordinates(x, y, domain)

coordinates in bravais frame specified by domain.lattice
(i.e. (x,y) = p1*v1 + p2*v2)
"""
# Bravais.bravais_coordinates(x, y, domain::Domain) =
#     bravais_coordinates(x, y, domain.lattice)

"""
    p1, p2 = bravais_coordinates(x, y, sim)

coordinates in bravais frame specified by sim.lat
(i.e. (x,y) = p1*v1 + p2*v2)
"""
# Bravais.bravais_coordinates(x, y, sim::Simulation) =
#     bravais_coordinates(x, y, sim.lat)


text:
# Load and activate packages
using Pkg
Pkg.activate(".")
Pkg.instantiate()
using LinearAlgebra, CSV, Statistics, MAT, JLD

################################
################################
# load other jl files
################################
################################

include("custom_types.jl")  # parametric types specific to this project
include("solvers_tvf.jl")       # inner solver functions for time varying fundamentals
include("solvers_cf.jl")       # inner solver functions for constant fundamentals
include("aux_functions.jl") # auxiliary functions
include("baseline.jl")      # baseline equilibrium-specific functions
include("counterfactual.jl")      # counterfactual equilibrium-specific functions

################################
################################
# construct datasets
################################
################################

################################
################################
# initialize parameter type
################################
################################

# model parameters
mp_cf = modelParams(22, 87, 50, 200, 0.99, 5.3436)
mp_tvf = modelParams(22, 87, 50, 29, 0.99, 5.3436)
mp_counterfactual = modelParams(22, 87, 50, 200, 0.99, 5.3436)

# algorithm parameters
ap_cf = algoParams(1e-3, 1e-2, 1e5, 1., 1, .05, Inf, 15)
ap_cf = algoParams(1e-3, 1e-7, 1e5, 1., 1, .05, Inf, 15)
ap_tvf = algoParams(1e-3, 1e-7, 1e5, 1., 1, .05, Inf, 10)
ap_counterfactual = algoParams(1e-3, 1e-7, 1e5, 1., 1, .05, Inf, 15)

################################
################################
# solve baseline model
################################
################################

# time varying fundamentals for years where we have data
@time solve_baseline_tvf(ap_tvf, mp_tvf)

# constant fundamentals past the last year of our data
@time solve_baseline_cf(ap_cf, mp_cf)

# combine the two baseline economies into one
@time combine_baseline(mp_tvf, mp_cf)

################################
################################
# solve counterfactual model
################################
################################

# compute the counterfactual outcome in changes relative to baseline
@time solve_counterfactual_tvf(ap_counterfactual, mp_counterfactual)

################################
################################
# compute results
################################
################################

max_stars_repo_name: rafserqui/class-repo

max_issues_repo_name: rafserqui/class-repo

max_forks_repo_name: rafserqui/class-repo


text:
function rrg_to_mcgraph(g::Network)
    # For some weird reason RRRMC stores
    #couplings and fields with opposite sign.

    A = [tuple(a...) for a in adjacency_list(g)]
    z = length(first(A))
    @assert all(length.(A) .== z)
    J = [([-eprop(g, e)["J"] for e in edges(g, i)]...,) for i=1:nv(g)]
    @assert all(length.(J) .== z)
    H = -vprop(g, "H").data

    LEV = tuple(Set(Iterators.flatten(J))...)
    ET = eltype(first(J))

    X = RRRMC.RRG.GraphRRG{ET,LEV,z}(A, J)
    X = RRRMC.AddFields.GraphAddFields(H, X)
    return X
end

function run_monte_carlo(X::RRRMC.Interface.AbstractGraph;
                    β = 2.0,
                    sweeps = 10^7,
                    infotime = 10,
                    seed = -1,
                    force = false,
                    filesave = false,
                    verbose = false,
                    t_limit = 40.0,
                    alg = :met, # [:met, :bkl, :rrr, :wtm]
                    )

    # @assert alg ∈ [:met, :bkl, :rrr, :wtm]
    if alg == :rrr
        runMC = RRRMC.rrrMC
    elseif alg == :met
        runMC = RRRMC.standardMC
    elseif alg == :met
        @error "unsupported algorithms :$alg"
    end
    N = RRRMC.Interface.getN(X)

    dirname = "output_mc_beta$(β)_tmax$(t_limit)_tstep$(infotime)"
    gen_fname(alg, seed) = joinpath(dirname, "output_$(alg)_s$(seed).txt")
    gen_Cfname(alg, seed) = joinpath(dirname, "Cs_$(alg)_s$(seed).jld2")

    function gen_hook(alg, seed)
        if filesave
            isdir(dirname) || mkdir(dirname)
            fn = gen_fname(alg, seed)
            !force && isfile(fn) && error("file $fn exists")
            f = open(fn, "w")
        else
            f = stdout
        end
        # Cv = Vector{BitVector}() #BitArray(N, samples)
        μ = [OnlineStats.Mean() for _=1:N]
        t0 = time()
        verbose && println(f, "#mctime acc E clocktime")

        function hook(mct, X, C, acc, E)
            t = time() - t0
            σ = 2 .* C.s .- 1
            # @show μ
            OnlineStats.fit!.(μ, σ)
            # push!(Cv, copy(C.s))
            verbose && println(f, "$mct $acc $E $t")
            return t < t_limit
        end

        cleanup() = filesave && close(f)

        return hook, cleanup, μ  #, Cv
    end

    @info "### SEED = $seed"

    iters = sweeps * N
    step = infotime * N
    samples = iters ÷ step
    # @show iterate step samples infotime

    hook, cleanup, μ = gen_hook(alg, seed)
    @time Es, Clast = runMC(X, β, iters, step=step, seed=seed, hook=hook)
    cleanup()
    σlast = 2 .* Clast.s .- 1
    μ = OnlineStats.value.(μ)
    return Es, σlast, μ  #, Chistory
end


text:
using FEMfunctions
using LinearAlgebra

x,eConn,iB = twodMesh(0.0,1.0,0.0,1.0,"quadratic",5,5)

eAdjacency = TriMesh_ElementAdjacency(eConn)

xInterp = rand(20,2)

ff,eList = TriMesh_Interpolate(x,eConn,eAdjacency,x,xInterp)

@test norm(ff-xInterp)<1e-14


text:
mutable struct ZipCodeStr
    val::String
end

"""
```julia
mutable struct ZipCodeCleaner
    name::AbstractString
    pattern::Regex
    cleanfun::Function
end
```

Holds a textual identifier for clarity purposes, a regex
pattern to match a potential ZIP code to, and a function to
apply when a match is found.
"""
struct ZipCodeCleaner
    name::String
    pattern::Regex
    cleanfun::Function
end

"Returns a ZipCodeCleaner for stripping whitespace"
removewhitespace() = ZipCodeCleaner("Whitespace", r"[ ]*", x -> strip(x))

"Returns a ZipCodeCleaner for removing the suffix on ZIP+4 patterns"
removezipsuffix() = ZipCodeCleaner("Z+4 Suffix", r"-[0-9]{1,4}", x -> string(split(x,"-")[1]))

"Returns a ZipCodeCleaner for padding left zeros to ensure ZIP code is 5 digits"
padleftzeros() = ZipCodeCleaner("Left Zeros", r"^[0-9]{1,4}$", x -> lpad(x, 5, "0"))

"""
```julia
cleanzipcode(
  zip::ZipCodeStr,
  cleaner::ZipCodeCleaner
)
```

Executes `cleaner.cleanfun` on `zip` when `zip` matches `cleaner.pattern`.
"""
function cleanzipcode!(zip::ZipCodeStr, cleaner::ZipCodeCleaner)
    occursin(cleaner.pattern, zip.val) && (zip.val = cleaner.cleanfun(zip.val))
    zip
end

"""
```julia
cleanzipcode(
    zip::AbstractString;
    whitespace::Bool=true,      # Remove leading/trailing whitespace
    suffix::Bool=true,          # Remove "Z-{4}" suffix
    padzeros::Bool=true,        # Pad left zeros
    returnNA::Bool=true,        # Return unrecognized values as NA
    enforcestring::Bool=false   # Ensure return values are Strings
)
```

Description
===========
Returns `zip` as an AbstractString with the following default corrections:
* Leading and trailing whitespace removed
* Z+4 suffix removed (e.g. 99999-1234 → 99999)
* Padded for left zeros(e.g. 245 → 00245)

If `zip` does not match the "12345" pattern even after the above
corrections an NA value will be returned by default.

Usage
=====
```julia
cleanzipcode("01234 ")
cleanzipcode("1234")
cleanzipcode("1234-0000")
cleanzipcode("notaZip")
cleanzipcode("notaZip", enforcestring=true)
```
"""
function cleanzipcode(str::AbstractString;
                      whitespace::Bool=true,      # Remove leading/trailing whitespace
                      suffix::Bool=true,          # Remove "Z-{4}" suffix
                      padzeros::Bool=true,        # Pad left zeros
                      returnNA::Bool=true,        # Return unrecognized values as NA
                      enforcestring::Bool=false)  # Ensure return type is a String

    occursin(PATTERN_ZIPCODE, str) && return str

    # Clean routine
    zip = ZipCodeStr(str)
    whitespace && cleanzipcode!(zip, removewhitespace())
    suffix && cleanzipcode!(zip, removezipsuffix())
    padzeros && cleanzipcode!(zip, padleftzeros())

    # Return routine
    ((occursin(PATTERN_ZIPCODE, zip.val) || !returnNA)
     ? zip.val
     : (enforcestring ? "NA" : missing))
end

cleanzipcode(zip::Any; kwargs...) = cleanzipcode(string.(zip); kwargs...)

cleanzipcode(zip::AbstractVector; kwargs...) = StringNA[cleanzipcode(x; kwargs...) for x in zip]


text:
""" Linear Kalman Filter Structure """

abstract type AbstractFilter end

mutable struct KalmanFilter{a<:AbstractMatrix, b<:AbstractMatrix, q<:Symmetric, h<:AbstractMatrix, r<:Symmetric} <: AbstractFilter
    A::a # process matrix
    B::b # control matrix
    Q::q # process zero mean noise covariance
    H::h # measurement matrix
    R::r # measurement zero mean noise covariance
end

function KalmanFilter(A::AbstractMatrix, B::AbstractMatrix, Q::AbstractMatrix,
    H::AbstractMatrix, R::AbstractMatrix)
    return KalmanFilter(A, B, Symmetric(Q), H, Symmetric(R))
end

function KalmanFilter(A::AbstractMatrix, Q::AbstractMatrix,
    H::AbstractMatrix, R::AbstractMatrix)
    return KalmanFilter(A, zeros(size(A)), Symmetric(Q), H, Symmetric(R))
end

""" Extended Kalman Filter Structure """

mutable struct ExtendedKalmanFilter{a<:Function, q<:Symmetric, b<:Function, r<:Symmetric} <: AbstractFilter
    f::a # non linear process function f(x, u)
    Q::q # process zero mean noise covariance
    h::b # non linear observation function h(x)
    R::r# measurement zero mean noise covariance
end

function ExtendedKalmanFilter(f::Function, Q::AbstractMatrix, h::Function, R::AbstractMatrix)
    return ExtendedKalmanFilter(f, Symmetric(Q), h, Symmetric(R))
end

""" Parametrized Kalman and Extended Kalman Filters """

abstract type AbstractParamFilter end

mutable struct ParamKalmanFilter{a, b, q, h, r} <: AbstractParamFilter
    # parametrized kalman filter where each matrix is a function of θ
    A::a # process matrix A(θ)
    B::b # control matrix B(θ)
    Q::q # process zero mean noise covariance Q(θ)
    H::h # measurement matrix H(θ)
    R::r # measurement zero mean noise covariance R(θ)
end

mutable struct ExtendedParamKalmanFilter{a, q, b, r} <: AbstractParamFilter
    f::a # non linear process function f(x, u)
    Q::q # process zero mean noise covariance
    h::b # non linear observation function h(x)
    R::r# measurement zero mean noise covariance
end

""" State: mean and cov """
struct State{a<:Number, b<:Number}
    x::AbstractVector{a}
    P::Symmetric{b}
end

function State(x::AbstractVector, P::AbstractMatrix)
    return State(x, Symmetric(P))
end


text:
mutable struct Game{T <: Vector{<:AbstractPlayer}} <: AbstractGame
  # no parent
  scope::Scope
  players::T
  in_focus::Bool
  board::Board
end

function Game(cur_scope::Scope=Scope())
  cur_game = Game(
    cur_scope,
    AbstractPlayer[],
    false,
    Board()
  )

  cur_game
end


text:
# Note: we use the 100s place of the enum to represent the number of dimensions
# And the 10s place to represent which dimension is the observation (N), 0 for no observation dimension
@enum ImageFormat begin
    L=100;
    CL=200; LC; HW; WH; NL=210; LN=220;
    HWC=300; WHC; CHW; CWH; NCL=310; NHW; NWH; NLC; CLN=330; LCN; HWN; WHN;
    NHWC=410; NWHC; NCHW; NCWH; HWCN=440; WHCN; CHWN; CWHN;
end

expected_ndims(x::ImageFormat) = Int(x) ÷ 100 # get the 100's digit
obs_dim(x::ImageFormat) = (Int(x) % 100) ÷ 10 # get the 10's digit
const strip_obs = Dict(
    NL=>L, LN=>L, NCL=>CL, CLN=>CL, NLC=>LC, LCN=>LC,
    NHW=>HW, HWN=>HW, NWH=>WH, WHN=>WH,
    NHWC=>HWC, NWHC=>WHC, NCHW=>CHW, NCWH=>CWH, HWCN=>HWC, WHCN=>WHC, CHWN=>CHW, CWHN=>CWH
)


text:
export Platte

struct Platte{T<:AbstractFloat, Ti<:Integer}
    xL::T
    yL::T
    nx::Ti
    ny::Ti
    rho::Matrix{T}
    lambda::Matrix{T}
    cp::Matrix{T}
    temp::Matrix{T}
end

function Platte(xL::T, yL::T, nx::Ti, ny::Ti, dens, heat, cap, tv::Union{Number,Function}) where {T,Ti}
    if nx <= 0
        throw(ArgumentError("nx muss > 0 sein, ist aber $nx"))
    end

        function setgrid(f::Number)
            return [f for ix in 1:nx, iy in 1:ny]
        end

        function setgrid(f::Function)
            return [f((iy - 0.5) * yL / ny, (ix - 0.5) * xL / nx) for ix in 1:nx, iy in 1:ny]
        end

    rho = setgrid(dens)
    temp = setgrid(tv)
    lambda= setgrid(heat)
    cp = setgrid(cap)

    return Platte{T, Ti}(xL, yL, nx, ny, rho, lambda, cp, temp)
end

function tempiter(p::Platte, deltat::T) where {T}

    dtemp = heattransfer(p)
    tempn = dtemp .* deltat .+ p.temp

    return Platte(p.xL, p.yL, p.nx, p.ny, p.rho, p.lambda, p.cp, tempn)
end

function heattransfer(p::Platte)

    produkt = p.lambda .* nabla(p.temp, p)
    tempdiff = nabla(produkt, p) ./ (p.cp .* p.rho)

    return tempdiff
end

"""
    nabla(∇m::Matrix{Vector}, p::Platte)

Calculate new matrix containing `∇(∇m)` given `∇m` at all positions of `p`.
"""
function nabla(vm::Matrix{<:Vector}, p::Platte)
    vmx = [vm[ix, iy][1] for ix in 1:p.nx, iy in 1:p.ny]
    vmy = [vm[ix, iy][2] for ix in 1:p.nx, iy in 1:p.ny]
    [nablax(vmx, ix, iy, p) + nablay(vmy, ix, iy, p) for ix in 1:p.nx, iy in 1:p.ny]
end

"""
    nabla(m::Matrix, p::Platte)

Calculate new matrix of vectors containing `∇m` at all postions of `p`.
"""
function nabla(m::Matrix{<:Number}, p::Platte)
    [[nablax(m, ix, iy, p), nablay(m, ix, iy, p)] for ix in 1:p.nx, iy in 1:p.ny]
end

"""
    nablax(m::Matrix, ix, iy, p::Platte)

Calculate finite x-difference at position `[ix,iy]` of matrix `p`.
Matrix dimensions must coincide with Platte.
"""
function nablax(m::Matrix, ix, iy, p::Platte)
    dx = p.xL / p.nx
    size(m) == (p.nx, p.ny) || throw(ArgumentException("wrong matrix size"))
    ix == 1 ? (m[ix + 1, iy] - m[ix, iy]) / dx :
    ix == p.nx ? (m[ix, iy] - m[ix - 1, iy]) / dx :
    (m[ix + 1, iy] - m[ix - 1, iy]) / (2 * dx)
end

"""
    nablay(m::Matrix, ix, iy, p::Platte)

Calculate finite y-difference at position `[ix,iy]` of matrix `m`.
Matrix dimensions must coincide with Platte.
"""
function nablay(m::Matrix, ix, iy, p::Platte)
    dy = p.yL / p.ny
    size(m) == (p.nx, p.ny) || throw(ArgumentException("wrong matrix size"))
    iy == 1 ? (m[ix, iy + 1] - m[ix, iy]) / dy :
    iy == p.ny ? (m[ix, iy] - m[ix, iy - 1]) / dy :
    (m[ix, iy + 1] - m[ix, iy - 1]) / (2 * dy)
end

nothing


text:
function Historic.events(recordmodule::Module)
    # TODO: recursively discover recorder modules
    records = recordmodule.get_records()::EventRecord
    table = append_events!(empty_eventtable(), records)
    Threads.nthreads() == 1 && return table
    return sort_eventtable(table)
end

function Historic.events()
    table = empty_eventtable()
    for recordmodule in RECORDERS
        records = recordmodule.get_records()::EventRecord
        append_events!(table, records)
    end
    return sort_eventtable(table)
end

function empty_eventtable()
    return (
        name = Symbol[],
        data = NamedTuple[],
        location = UUID[],
        __source__ = Union{LineNumberNode,Nothing}[],
        __module__ = Union{Module,Nothing}[],
        time_ns = TimeNS[],
        threadid = Int[],
    )
end

function append_events!(table, records)
    for (threadid, shard) in pairs(records.shards)
        a = length(table.name)
        append_sharded_events!(table, shard)

        b = length(table.name)
        resize!(table.threadid, b)
        fill!(view(table.threadid, a+1:b), threadid)
    end
    return table
end

function append_sharded_events!(table, shard)
    for blk in blocks(shard)
        for i in eachindex(blk)
            event = @inbounds blk[i]
            ntuple(Val(nfields(event))) do j
                @_inline_meta
                push!(getfield(table, j), getfield(event, j))
                nothing
            end
        end
    end
end

function sort_eventtable(table)
    # TODO: since each shard is sorted, it'd be much more efficient to use "mergesorted!"
    idx = sortperm(table.time_ns)
    cols = ntuple(Val(nfields(table))) do j
        @_inline_meta
        (; fieldname(typeof(table), j) => getfield(table, j)[idx])
    end
    return foldl(merge, cols)
end

Historic.flattable(recordmodule::Module) = Historic.flattable(Historic.events(recordmodule))
Historic.flattable() = Historic.flattable(Historic.events())

function Historic.flattable(eventtable::NamedTuple)
    table = merge(_flattable(eventtable.data), eventtable)
    ks = filter(!=(:data), keys(table))
    return NamedTuple{(ks...,)}(table)
end

function _flattable(rows)
    columnnames = Symbol[]
    columndict = Dict{Symbol,Vector}()
    nrows = 0
    for nt in rows
        for (k, v) in pairs(nt)
            c = get(columndict, k, nothing)
            if c === nothing
                if nrows > 0
                    c = Vector{Union{Missing,typeof(v)}}(undef, nrows + 1)
                    fill!(c, missing)
                    c[end] = v
                else
                    c = [v]
                end
                columndict[k] = c
                push!(columnnames, k)
            else
                T = promote_type(typeof(v), eltype(c))
                n = length(c)
                if nrows > n
                    T = Union{T,Missing}
                end
                if !(T <: eltype(c))
                    columndict[k] = c = collect(T, c)
                end
                resize!(c, nrows + 1)
                if nrows > n
                    fill!(view(c, n+1:nrows), missing)
                end
                c[end] = v
            end
        end
        nrows += 1
    end
    for (k, v) in pairs(columndict)
        columndict[k] = grow_with_missing!(v, nrows)
    end
    return (; (k => columndict[k] for k in columnnames)...)
end

function grow_with_missing!(v::AbstractVector, n)
    @assert firstindex(v) == 1
    T = Union{Missing,eltype(v)}
    m = length(v)
    m == n && return v
    @assert m < n
    if !(T <: eltype(v))
        v = collect(T, v)
    end
    resize!(v, n)
    fill!(view(v, m+1:n), missing)
    return v
end


text:
include("iron_line_heatmap.jl")
include("../temperature_render.jl")
include("../iron_line_profile.jl")
using Plots
gr()

hmp=iron_line_heatmap()
x_data=0:100
y_data=repeat(x_data, 100)
plot!(x_data, y_data)


text:
#=
parser3:
- Julia version: 1.1.0
- Author: bramb
- Date: 2019-03-05
=#

using LightGraphs,MetaGraphs,LibExpat

abstract type XMLToken end

struct XMLStartElement <: XMLToken
    name::String
    attrs::Dict{String,String}
end

struct XMLEndElement <: XMLToken
    name::String
end

mutable struct TextToken <: XMLToken
    text::String
end

mutable struct GraphBuildContext
    divergencenodes::Vector{Integer}
    open_tags::Vector{String}
    last_unconnected_node::Integer
    branch_ends::Dict{Integer,Vector{Integer}}

    GraphBuildContext() = new([],[],0,Dict{Integer,Vector{Integer}}())
end

struct GraphBuilder
  metagraph::MetaDiGraph
  context::GraphBuildContext

  GraphBuilder() = new(MetaDiGraph(SimpleDiGraph()),GraphBuildContext())
end

@enum(VertexType,TEXTNODE,DIVERGENCE,CONVERGENCE)

is_divergence_element(name::String) = name in ["subst", "choice", "app"]
is_optional_element(name::String) = name in ["del", "add"]

function add_divergence_node!(gb::GraphBuilder)
    add_vertices!(gb.metagraph.graph,1)
    v = nv(gb.metagraph.graph)
    set_props!(gb.metagraph,v,Dict(:type => DIVERGENCE, :text => "<"))
    return v
end

function add_convergence_node!(gb::GraphBuilder)
    add_vertices!(gb.metagraph.graph,1)
    v = nv(gb.metagraph.graph)
    set_props!(gb.metagraph,v,Dict(:type => CONVERGENCE, :text => ">"))
    return v
end

function grow_graph!(gb::GraphBuilder, startelement::XMLStartElement)
#     @show(gb)
#     @show(startelement)
#     @show(startelement.name,is_optional_element(startelement.name),!parent_is_divergence_element(gb))
    if (is_divergence_element(startelement.name)) || (is_optional_element(startelement.name) && !parent_is_divergence_element(gb))
        v = add_divergence_node!(gb)
        push!(gb.context.divergencenodes,v)
        add_edge!(gb.metagraph.graph,gb.context.last_unconnected_node,v)
        gb.context.last_unconnected_node = v
        gb.context.branch_ends[v] = []
    end
    push!(gb.context.open_tags,startelement.name)

#     println()
    return gb
end

parent_is_divergence_element(gb::GraphBuilder) = !isempty(gb.context.open_tags) && is_divergence_element(gb.context.open_tags[end])

function grow_graph!(gb::GraphBuilder, endelement::XMLEndElement)
#     @show(gb)
#     @show(endelement)

    pop!(gb.context.open_tags)
    if (is_divergence_element(endelement.name)) || (is_optional_element(endelement.name) && !parent_is_divergence_element(gb))
        v = add_convergence_node!(gb)
        variation = pop!(gb.context.divergencenodes)
        for n in gb.context.branch_ends[variation]
            add_edge!(gb.metagraph.graph,n,v)
        end
        if is_optional_element(endelement.name)
            add_edge!(gb.metagraph.graph,gb.context.last_unconnected_node,v)
            add_edge!(gb.metagraph.graph,variation,v)
        end
        gb.context.last_unconnected_node = v

    elseif (parent_is_divergence_element(gb))
        variation = gb.context.divergencenodes[end]
        branch_ends = gb.context.branch_ends[variation]
        push!(branch_ends,gb.context.last_unconnected_node)
        gb.context.branch_ends[variation] = branch_ends
        gb.context.last_unconnected_node = variation
    end

#     println()
    return gb
end

function grow_graph!(gb::GraphBuilder, texttoken::TextToken)
#     @show(gb)
#     @show(texttoken)

    add_vertices!(gb.metagraph.graph,1)
    v = nv(gb.metagraph.graph)
    set_props!(gb.metagraph,v,Dict(:type => TEXTNODE,:text => texttoken.text))
    add_edge!(gb.metagraph.graph,gb.context.last_unconnected_node,v)
    gb.context.last_unconnected_node = v

#     println()
    return gb
end

function tokenize(xml::AbstractString)::Vector{XMLToken}
    tokens = []
    cbs = XPCallbacks()
    cbs.start_element = function(h, name, attrs)
        push!(h.data, XMLStartElement(name,attrs))
    end
    cbs.end_element = function(h, name)
        push!(h.data, XMLEndElement(name))
    end
    cbs.character_data = function(h, txt)
        if (isa(h.data[end],TextToken))
            h.data[end].text *= txt
        else
            push!(h.data, TextToken(txt))
        end
    end
    parse(xml,cbs,data = tokens)
    return tokens
end

to_graph(xml::String) = reduce(grow_graph!, tokenize(xml); init = GraphBuilder()).metagraph

function string_value(t::XMLStartElement)
    attribs = isempty(t.attrs) ? "" : " " * join( ["$k=\"$(t.attrs[k])\"" for k in keys(t.attrs)], " ")
    return "<$(t.name)$attribs>"
end

string_value(t::XMLEndElement) = "</$(t.name)>"

string_value(t::TextToken) = "$(t.text)"

function to_dot(mg::MetaDiGraph)
    digraph = _metagraph_as_dot(mg)

    dot = """
    digraph VariantGraph {
        rankdir=LR
        labelloc=b
        color=white
        edge [arrowsize=0.5]
        $digraph
    }
    """
    return dot
end

function _metagraph_as_dot(mg::MetaDiGraph)
    nodes_buf = IOBuffer()
    for n in 1:nv(mg.graph)
        type = get_prop(mg,n,:type)
        if (type == TEXTNODE)
            text = get_prop(mg,n,:text)
            node_def = """v$n[shape=box;label="$text"]"""
        else
            node_def = """v$n[shape=circle;width=0.05;label=""]"""
        end
        println(nodes_buf,node_def)
    end
    nodes = String(take!(nodes_buf))

    edges_buf = IOBuffer()
    for e in edges(mg.graph)
        edge_def = """v$(e.src) -> v$(e.dst)"""
        println(edges_buf,edge_def)
    end
    edgesstring = String(take!(edges_buf))

    dot = """
    $nodes
    $edgesstring
    """
    return dot
end


text:
"""
    ShiftableLoad

ShiftableLoad Load
"""
struct ShiftableLoad <: Resource
    index::Int  # Unique index of device
    num_timesteps::Int  # Number of time-steps in horizon

    cycle_begin_min::Int  # Beginning of each cycle
    cycle_begin_max::Int    # End of each cycle
    cycle_length::Int  # Length of a cycle
    cycle_load::Vector{Float64}  # Load level at each time of the cycle

    function ShiftableLoad(;
        index::Integer=0,
        T::Integer=0,
        cycle_begin_min::Int=1,
        cycle_begin_max::Int=0,
        cycle_length::Int=0,
        cycle_load::Vector{Float64}=Float64[]
    )
        # Dimension checks
        T >= cycle_length || throw(error(
            "T=$T but cycle has length $cycle_length"
        ))
        cycle_begin_min >= 1 || throw(error(
            "Earliest start time is too early: $cycle_begin_min"
        ))
        cycle_begin_max <= T - cycle_length + 1 || throw(error(
            "Latest start time is too late: $cycle_begin_max"
        ))
        cycle_length == length(cycle_load) || throw(DimensionMismatch(
            "L=$cycle_length but load has length $(length(cycle_load))"
        ))

        return new(
            index, T,
            cycle_begin_min, cycle_begin_max,
            cycle_length, cycle_load
        )
    end
end

function add_resource_to_model!(
    m::MOI.ModelLike,
    h::House,
    l::ShiftableLoad,
    var2idx, con2idx
)
    T = l.num_timesteps
    T0 = l.cycle_begin_min
    T1 = l.cycle_begin_max

    # ==========================================
    #    I. Add local variables
    # ==========================================
    pnet = MOI.add_variables(m, T)  # Net load
    L = T1 - T0 + 1
    ustart = MOI.add_variables(m, L)  # Start indicator

    # Variable bounds
    for k in 1:L
        MOI.add_constraint(m, MOI.SingleVariable(ustart[k]), MOI.ZeroOne())
    end

    # Update variable indices
    for t in 1:T
        var2idx[(:shift, l.index, :pnet, t)] = pnet[t]
    end
    for k in 1:L
        var2idx[(:shift, l.index, :pnet, k + T0 - 1)] = ustart[k]
    end

    # ==========================================
    #    II. Update linking constraints
    # ==========================================
    for t in 1:T
        cidx = con2idx[(:house, h.index, :link, t)]
        MOI.modify(m, cidx, MOI.ScalarCoefficientChange(pnet[t], -1.0))
    end

    # ==========================================
    #    III. Add local constraints
    # ==========================================
    # Net load
    for t in 1:T
        MOI.add_constraint(m,
            MOI.ScalarAffineFunction([
                [
                    MOI.ScalarAffineTerm(1.0, pnet[t])
                ]; [
                    MOI.ScalarAffineTerm(-l.cycle_load[t - T0 - k + 2], ustart[k])
                    for k in 1:L
                    if (T0 <= t - k + 1 <= T1) && (1 <= t - T0 - k + 2 <= l.cycle_length)
                ]
            ], 0.0),
            MOI.EqualTo(0.0)
        )
    end

    MOI.add_constraint(m, MOI.ScalarAffineFunction([
            MOI.ScalarAffineTerm(1.0, ustart[t])
            for t in 1:L
        ], 0.0),
        MOI.EqualTo(1.0)
    )

    # ==========================================
    #    IV. Add sub-resources to current model
    # ==========================================
    # (None here)

    return nothing
end


text:
"""
    mode(nums)

Finds mode of a vector of numbers

# Example

```julia
mode([2, 3, 4, 5, 3, 4, 2, 5, 2, 2, 4, 2, 2, 2])        # returns [2]
mode([3, 4, 5, 3, 4, 2, 5, 2, 2, 4, 4, 2, 2, 2])        # returns [2]
mode([3, 4, 5, 3, 4, 2, 5, 2, 2, 4, 4, 4, 2, 2, 4, 2])  # returns [2, 4]
mode(["x", "y", "y", "z"])                              # returns ["y"]
mode(["x", "x" , "y", "y", "z"])                       # returns ["x", "y"]
```

Contributed By:- [Ashwani Rathee](https://github.com/ashwani-rathee)
"""
function mode(nums)
    dict = Dict() # nums => Number of repetitions
    result = [] # Array of the modes so far
    max = 0 # Max of repetitions so far

    for i in nums
        # Add one to the dict[i] entry (create one if none)
        if i in keys(dict)
            dict[i] += 1
        else
            dict[i] = 1
        end
        # Result updated if no of repetitions of i >= max
        if dict[i] >= max
            if dict[i] > max
                empty!(result)
                max += 1
            end
            append!(result, [i])
        end
    end

    return result
end
println(mode([1, 3, 6, 6, 6, 6, 7, 7, 12, 12, 17]))
println(mode((1, 1, 2, 4, 4)))

max_stars_repo_name: arubhardwaj/Julia

max_issues_repo_name: arubhardwaj/Julia

max_forks_repo_name: arubhardwaj/Julia