text:
using Test
using DataFrames
using XLSX
using UiPathOrchestratorJobSchedulingPlanCreate

@testset "UiPathOrchestratorJobSchedulingPlanCreate.jl" begin
    # Write your own tests here.
    schedulcolumn=6
    jobname=Array{Any}(["スケジュール1","スケジュール2","スケジュール3","スケジュール4","スケジュール5","スケジュール6","スケジュール7","スケジュール8","スケジュール9","スケジュール10"])
    runtime=Array{Any}([30,60,15,30,60,45,30,15,60,30])
    Specifiedtime=Array{Any}([true,true,true,true,true,false,false,false,false,false])
    JobStartTime=Array{Any}(["0:30","0:45","0:15","0:00","1:00",missing,missing,missing,missing,missing])
    JobEndTime=Array{Any}(["1:00","1:45","0:30","0:30","2:00",missing,missing,missing,missing,missing])
#   0:00
    col1=Array{Any}([missing,missing,missing,1,missing,missing,missing,missing,missing,missing])
#    0:15
    col2=Array{Any}([missing,missing,1,1,missing,missing,missing,missing,missing,missing])
#    0:30
    col3=Array{Any}([1,missing,missing,missing,missing,missing,missing,missing,missing,missing])
#    0:45
    col4=Array{Any}([1,1,missing,missing,missing,missing,missing,missing,missing,missing])
#    1:00
    col5=Array{Any}([missing,1,missing,missing,1,missing,missing,missing,missing,missing])
#    1:15
    col6=Array{Any}([missing,1,missing,missing,1,missing,missing,missing,missing,missing])
#    1:30
    col7=Array{Any}([missing,1,missing,missing,1,missing,missing,missing,missing,missing])
#    1:45
    col8=Array{Any}([missing,missing,missing,missing,1,missing,missing,missing,missing,missing])
#    2:00
    col9=Array{Any}([missing,missing,missing,missing,missing,missing,missing,missing,missing,missing])
#    DataFrame作成
    schedule=DataFrame(jobname=jobname,runtime=runtime,Specifiedtime=Specifiedtime,JobStartTime=JobStartTime,JobEndTime=JobEndTime,col1=col1,col2=col2,col3=col3,col4=col4,col5=col5,col6=col6,col7=col7,col8=col8,col9=col9)
    rename!(schedule,[:jobname,:runtime,:Specifiedtime,:JobStartTime,:JobEndTime,Symbol("0:00"),Symbol("0:15"),Symbol("0:30"),Symbol("0:45"),Symbol("1:00"),Symbol("1:15"),Symbol("1:30"),Symbol("1:45"),Symbol("2:00")])

    InputFilePath=joinpath(@__DIR__, "schedule.xlsx")
    OutputFilePath=joinpath(@__DIR__, "scheduleoutput.xlsx")

    scheduleplan,robotn,run_unit_time,jobn,timen=UiPathOrchestratorJobSchedulingPlanCreate.readprerequisite(InputFilePath,"parameters","schedule")

    rename!(schedule,names(scheduleplan))
    @test scheduleplan[1:schedulcolumn-1,1:schedulcolumn-1] == schedule[1:schedulcolumn-1,1:schedulcolumn-1]
    @test scheduleplan[1,8:9] == schedule[1,8:9]
    @test (robotn,run_unit_time,jobn,timen) == (6,15,10,8)

    output=DataFrames.DataFrame(XLSX.readtable(OutputFilePath, "v0.19.2")...)
    plan,runtime=uipathorchestratorschedulreadjustment(scheduleplan,robotn,run_unit_time,jobn,timen)
    plan=adjustedresultcheck(plan,runtime,scheduleplan,robotn,jobn,timen)
    @test sum(convert(Matrix,plan[:,schedulcolumn:end-1])) == sum(runtime)
#    @test convert(Matrix,plan[:,schedulcolumn:end-1]) == convert(Matrix,output[:,schedulcolumn:end-1])
    @test convert(Matrix,plan[1:7,schedulcolumn:end-1]) == convert(Matrix,output[1:7,schedulcolumn:end-1])
    @test convert(Matrix,plan[9:10,schedulcolumn:end-1]) == convert(Matrix,output[9:10,schedulcolumn:end-1])
    @test sum(convert(Matrix,uipathorchestratorschedulrecreate(InputFilePath,"parameters","schedule",plotengine="off")[:,schedulcolumn:end-1])) == sum(runtime)
    @test sum(convert(Matrix,uipathorchestratorschedulrecreate(InputFilePath,"parameters","schedule",plotengine="GR")[:,schedulcolumn:end-1])) == sum(runtime)
#    @test sum(convert(Matrix,uipathorchestratorschedulrecreate(InputFilePath,"parameters","schedule",plotengine="PlotlyJS")[:,schedulcolumn:end-1])) == sum(runtime)
    @test sum(convert(Matrix,uipathorchestratorschedulrecreate(InputFilePath,"parameters","schedule",plotengine="それ以外")[:,schedulcolumn:end-1])) == sum(runtime)
    @test uipathorchestratorschedulrecreate(InputFilePath,"parameters","schedule",plotengine="それ以外",checkreturn=true)[2]
    @test createtimeset()[1][1]=="00:00"
    if Sys.isapple()
        sum(convert(Matrix,uipathorchestratorschedulrecreate(InputFilePath,"parameters","schedule",plotengine="PlotlyJS")[:,schedulcolumn:end-1])) == sum(runtime)
    end

    OutputTestFilePath=joinpath(@__DIR__, "TestUiPathOrchestratorJobSchedulingPlan.xlsx")
    uipathorchestratorschedulrecreate(InputFilePath,"parameters","schedule",plotengine="off",planexport=true,ExportExcelFilePath=OutputTestFilePath)
    @test isfile(OutputTestFilePath)
    @test DataFrames.DataFrame(XLSX.readtable(OutputTestFilePath, "REPORT_jobplan")...)[1:5,1:5] == schedule[1:5,1:5]
    rm(OutputTestFilePath)

    exportplan(plan,robotn,run_unit_time,ExcelFilePath=OutputTestFilePath)
    @test isfile(OutputTestFilePath)
    @test DataFrames.DataFrame(XLSX.readtable(OutputTestFilePath, "REPORT_parameters")...)[2,2] == robotn
    @test DataFrames.DataFrame(XLSX.readtable(OutputTestFilePath, "REPORT_jobplan")...)[1:5,1:5] == schedule[1:5,1:5]
    rm(OutputTestFilePath)

    #ジョブスケジュール作成失敗の場合のテスト
    robotn=1
    plan,runtime=uipathorchestratorschedulreadjustment(scheduleplan,robotn,run_unit_time,jobn,timen)
    @test convert(Matrix,adjustedresultcheck(plan,runtime,scheduleplan,robotn,jobn,timen)[:,schedulcolumn:end-1] ) == zeros(Int,jobn,timen)

    # 特定範囲内に収める
    scheduleplan,robotn,run_unit_time,jobn,timen=UiPathOrchestratorJobSchedulingPlanCreate.readprerequisite(InputFilePath,"parameters","schedule2")
    robotn=1
    plan,runtime=uipathorchestratorschedulreadjustment(scheduleplan,robotn,run_unit_time,jobn,timen)
    @test sum(convert(Matrix,adjustedresultcheck(plan,runtime,scheduleplan,robotn,jobn,timen)[:,schedulcolumn:end-1] ) )== sum(runtime)
end


text:
module WignerDMatrices

using LinearAlgebra
using HalfIntegers
using HalfIntegerArrays
import HalfIntegerArrays: AbstractHalfIntegerMatrix

import Base: @propagate_inbounds

export WignerDMatrix
export WignerdMatrix

# Dictionary to cache the eigenvectors of Jy
const JyEigenDict = Dict{HalfInt,Matrix{ComplexF64}}()

include("specialpoints.jl")
include("wignerdtypes.jl")
include("wignerdeval.jl")
include("linearalgebra.jl")

end # module

ext: jl


text:
include("./chapter_1/bisect.jl")
include("./chapter_1/fpi.jl")

f(x::Float64) = x^3-9

x = f(2.0)
println(x)

xc = bisect(f, 2.0, 3.0, 0.000005)
println("xc = ", xc)

f(x::Float64) = x + cos(x) -sin(x)
xc_fpi = fpi(f, 0, 20)

println("xc_fpi = ", xc_fpi)

max_stars_repo_path: julia/Numerical Analysis/run.jl

max_issues_repo_path: julia/Numerical Analysis/run.jl

max_forks_repo_path: julia/Numerical Analysis/run.jl


text:
# custom functions used in corresponding jupyter notebook

unzip(a) = map(x->getfield.(a, x), [1, 2]);

function convert_positions_to_labels(positions; grid_size=64, image_size=256, classes=false)
    """ Returns 64x64xchannelsxn_images label tensors computed from list of vortex positions """
    n_images = size(positions, 3)
    grid_cell = image_size ÷ grid_size
    max_objects = size(positions, 1)
    if classes channels=4 else channels=3 end
    labels = zeros(grid_size, grid_size, channels, n_images)
    for n = 1:n_images
        for i = 1:max_objects
            if positions[i,1,n] > 0 # check first if object exists
                x = Int((positions[i,1,n] - 1) ÷ grid_cell + 1)
                y = Int((positions[i,2,n] - 1) ÷ grid_cell + 1)

                x_offset = (positions[i,1,n] - 1) / grid_cell + 1 - x
                y_offset = (positions[i,2,n] - 1) / grid_cell + 1 - y

                if classes
                    if positions[i,3,n] == 1.0
                        labels[x, y, 1, n] = 1.0
                    elseif positions[i,3,n] == -1.0
                        labels[x, y, 2, n] = 1.0
                    else
                        error("class does not exist")
                    end
                    labels[x, y, 3, n] = x_offset
                    labels[x, y, 4, n] = y_offset
                else
                    labels[x, y, 1, n] = 1.0
                    labels[x, y, 2, n] = x_offset
                    labels[x, y, 3, n] = y_offset
                end
            end
        end
    end
    return labels
end;

function convert_label_to_position(label; threshold=0.5, grid_cell=4, classes=false)
    """ Returns list of vortex positions computed from single 64x64xchannel label tensor """
    positions, confidence = [], []
    if classes
        thres_min = minimum(threshold)
        combined = max.(label[:,:,1], label[:,:,2])
        grid_idx = findall(x->x>=thres_min, combined[:,:])
        if length(threshold) == 1
            threshold = [threshold, threshold]
        end
    else
        grid_idx = findall(x->x>=threshold, label[:,:,1])
    end
    for (i, id) in enumerate(grid_idx)
        conf = label[id[1],id[2],1]
        grid_offset_x = grid_cell * (id[1] - 1)
        grid_offset_y = grid_cell * (id[2] - 1)
        if classes
            if label[id[1],id[2],1] > label[id[1],id[2],2]
                if label[id[1],id[2],1] >= threshold[1]
                    class = 1.0
                    conf = label[id[1],id[2],1]
                else
                    continue
                end
            else
                if label[id[1],id[2],2] >= threshold[2]
                    class = -1.0
                    conf = label[id[1],id[2],2]
                else
                    continue
                end
            end
            x = grid_offset_x + grid_cell * label[id[1],id[2],3] + 1
            y = grid_offset_y + grid_cell * label[id[1],id[2],4] + 1
            append!(positions, [[round(x), round(y), class]])
        else
            x = grid_offset_x + grid_cell * label[id[1],id[2],2] + 1
            y = grid_offset_y + grid_cell * label[id[1],id[2],3] + 1
            append!(positions, [[round(x), round(y)]])
        end
        append!(confidence, conf)
    end
    return round.(Int, hcat(positions...)'), confidence
end;

function convert_labels_to_positions(labels; threshold=0.5, grid_cell=4, classes=false)
    """ Returns list of vortex positions computed from a batch of 64x64xchannelsxn_images label tensors """
    return unzip(convert_label_to_position.(eachslice(labels, dims=4), threshold=threshold, grid_cell=grid_cell, classes=classes))
end;

function split_data(images, labels; train_test_ratio=0.8, seed=1234)
    """ Returns training and test set by randomly splitting images into two subsets """
    Random.seed!(seed)
    n_training = Int(n_images*train_test_ratio);

    # shuffle images first before splitting into train and test data
    id = randperm(n_images)
    images = images[:,:,:,id]
    labels = labels[:,:,:,id]

    train_images = images[:,:,:,1:n_training];
    test_images = images[:,:,:,(n_training+1):end];
    train_labels = labels[:,:,:,1:n_training];
    test_labels = labels[:,:,:,(n_training+1):end];

    return train_images, train_labels, test_images, test_labels
end;

function make_minibatch(x, y, idxs)
    """ Returns minibatches by splitting training data according to a list of indices """
    x_batch = Array{Float32}(undef, size(x[:,:,:,1])..., length(idxs))
    y_batch = Array{Float32}(undef, size(y[:,:,:,1])..., length(idxs))
    for i in 1:length(idxs)
        x_batch[:, :, :, i] = Float32.(x[:,:,:,idxs[i]]);
        y_batch[:, :, :, i] = Float32.(y[:,:,:,idxs[i]]);
    end
    return (x_batch, y_batch)
end;

function nms(probability_map; kernel=3, classes=false)
    """ Non-max suppression to eliminate multiple detections of the same vortex """
    pad = (kernel - 1) ÷ 2
    if classes
        pmax = NNlib.maxpool(probability_map[:,:,1:2,:], (kernel, kernel), pad=pad, stride=1)|>gpu
        keep = ones(size(probability_map))|>gpu
        keep[:,:,1:2,:] = float.((pmax .== probability_map[:,:,1:2,:]))
    else
        pmax = NNlib.maxpool(probability_map[:,:,1:1,:], (kernel, kernel), pad=pad, stride=1)|>gpu
        keep = float.((pmax .== probability_map[:,:,1:1,:]))
    end
    return probability_map .* keep
end;

function train(loss, opt, epochs, model, train_set, test_images, test_labels)
    """ Train model for number of epochs, returns trained model and list of test losses """
    losses = []
    test_loss = loss(model, test_images, test_labels)
    println("Loss: $(round(test_loss, digits=2))")
    for epoch = 1:epochs
        for d in train_set
            gs = gradient(Flux.params(model)) do
                l = loss(model, d...)
            end
            update!(opt, Flux.params(model), gs)
        end
        test_loss = loss(model, test_images, test_labels)
        append!(losses, test_loss)
        println("$(epoch), Loss: $(round(test_loss, digits=2))")
    end
    return model, losses
end;

distance(xA, xB, yA, yB) = sqrt((xA - xB)^2 + (yA - yB)^2)

""" Returns F1 score: harmonic mean of precision and recall """
f1score(precision, recall) = 2 .* precision .* recall ./ (precision .+ recall)

function precision_and_recall(y_hat, y; confidence_threshold=0.5, distance_threshold=2.3)
    """ Returns precision and recall for a single image """
    predicted_pos, confidence = convert_label_to_position(y_hat, threshold=confidence_threshold)
    true_pos, _ = convert_label_to_position(y)

    if size(predicted_pos,2) == 0# case 1: no predictions
        precision = 1.0
        if size(true_pos,2) == 0 # case 1a: no objects
            recall = 1.0
        else                     # case 1b: there are objects
            recall = 0.0
        end

    else                         # case 2: there are predictions
        if size(true_pos,2) == 0 # case 2a: no objects
            recall = 1.0
            precision = 0.0
        else                     # case 2b: there are objects
            already_detected = zeros(size(true_pos, 1))
            id = sortperm(-confidence)
            predicted_pos = predicted_pos[id,:]

            true_positives = 0
            false_positives = 0
            for i = 1:size(predicted_pos, 1)
                xB = predicted_pos[i,1]
                yB = predicted_pos[i,2]
                distance_list = zeros(size(true_pos, 1))

                for j = 1:size(true_pos, 1)
                    xA = true_pos[j,1]
                    yA = true_pos[j,2]
                    distance_list[j] = distance(xA, xB, yA, yB)
                end

                jmin = argmin(distance_list)
                dismin = distance_list[jmin]
                if dismin <= distance_threshold # true positives
                    if already_detected[jmin] == 0
                        true_positives += 1
                        already_detected[jmin] = 1
                    else
                        false_positives += 1
                    end
                else
                    false_positives += 1
                end
            end
            total_positives = size(predicted_pos, 1) # same as: true_positives + false_positives
            total_objects = size(true_pos, 1)

            precision = true_positives / total_positives
            recall = true_positives / total_objects
        end
    end
    return precision, recall
end;

function precision_and_recall_classes(y_hat, y; confidence_threshold=0.5, distance_threshold=2.3)
    """ Returns precision and recall for each different class for a single image """
    predicted_pos, confidence = convert_label_to_position(y_hat, threshold=confidence_threshold, classes=true)
    true_pos, _ = convert_label_to_position(y, classes=true)

    if size(predicted_pos,2) == 0 # case 1: no predictions
        precision = ones(2)

        if size(true_pos,2) == 0  # case 1a: no objects
            recall = ones(2)
        else                      # case 1b: there are objects
            recall = zeros(2)
            id1 = findall(x->x==1.0, true_pos[:,3])
            id2 = findall(x->x==-1.0, true_pos[:,3])

            if size(id1, 1) == 0 # case 1b1: no vortex objects
                recall[1] = 1.0
            end
            if size(id2, 1) == 0 # case 1b2: no antivortex objects
                recall[2] = 1.0
            end
        end

    else                         # case 2: there are predictions
        if size(true_pos,2) == 0 # case 2a: no objects
            id1 = findall(x->x==1.0, predicted_pos[:,3])
            id2 = findall(x->x==-1.0, predicted_pos[:,3])

            recall = ones(2)
            precision = zeros(2)
            if size(id1, 1) == 0 # case 2a1: no vortex predictions
                precision[1] = 1.0
            end
            if size(id2, 1) == 0 # case 2a2: no antivortex predictions
                precision[2] = 1.0
            end
        else                     # case 2b: there are objects
            precision = zeros(2)
            recall = zeros(2)

            idp1 = findall(x->x==1.0, predicted_pos[:,3])
            predicted_pos1 = predicted_pos[idp1,:]
            confidence1 = confidence[idp1]

            idt1 = findall(x->x==1.0, true_pos[:,3])
            true_pos1 = true_pos[idt1,:]

            idp2 = findall(x->x==-1.0, predicted_pos[:,3])
            predicted_pos2 = predicted_pos[idp2,:]
            confidence2 = confidence[idp2]

            idt2 = findall(x->x==-1.0, true_pos[:,3])
            true_pos2 = true_pos[idt2,:]

            predicted_poss = [predicted_pos1, predicted_pos2]
            confidences = [confidence1, confidence2]
            true_poss = [true_pos1, true_pos2]

            for (l, predicted_pos) in enumerate(predicted_poss)
                confidence = confidences[l]
                true_pos = true_poss[l]

                if size(true_pos, 1) == 0 # no object
                    recall[l] = 1.0
                    if size(predicted_pos, 1) == 0 # no prediction
                        precision[l] = 1.0
                    else                           # there is prediction prediction
                        precision[l] = 0.0
                    end
                elseif size(predicted_pos, 1) == 0 # there is object but no prediction
                    precision[l] = 1.0
                    recall[l] = 0.0
                else                               # there is object and prediction
                    already_detected = zeros(size(true_pos, 1))
                    id = sortperm(-confidence)
                    predicted_pos = predicted_pos[id,:]

                    true_positives = 0
                    false_positives = 0
                    for i = 1:size(predicted_pos, 1)
                        xB = predicted_pos[i,1]
                        yB = predicted_pos[i,2]
                        distance_list = zeros(size(true_pos, 1))

                        for j = 1:size(true_pos, 1)
                            xA = true_pos[j,1]
                            yA = true_pos[j,2]

                            distance_list[j] = distance(xA, xB, yA, yB)
                        end
                        jmin = argmin(distance_list)
                        dismin = distance_list[jmin]

                        if dismin <= distance_threshold
                            if already_detected[jmin] == 0
                                true_positives += 1
                                already_detected[jmin] = 1
                            else
                                false_positives += 1
                            end
                        else
                            false_positives += 1
                        end
                    end
                    total_positives = size(predicted_pos, 1) # same as: true_positives + false_positives
                    total_objects = size(true_pos, 1)

                    precision[l] = true_positives / total_positives
                    recall[l] = true_positives / total_objects
                end
            end
        end
    end
    return precision, recall
end;

function mean_precision_and_recall(model, x, y; confidence_threshold=0.5, distance_threshold=2.3, classes=false)
    """ Returns mean precision and recall over the whole data set """
    y_hat = nms(model(x), classes=classes)
    if classes
        precision, recall = unzip(precision_and_recall_classes.(eachslice(y_hat, dims=4), eachslice(y, dims=4); confidence_threshold=confidence_threshold, distance_threshold=distance_threshold))
    else
        precision, recall = unzip(precision_and_recall.(eachslice(y_hat, dims=4), eachslice(y, dims=4); confidence_threshold=confidence_threshold, distance_threshold=distance_threshold))
    end
    return mean(precision), mean(recall)
end;

function precision_recall_curve(model, test_images, test_labels; distance_threshold=2.3, classes=false)
    """ Returns precision and recall for 10 different confidence thresholds, the optimal confidence threshold, and the maximum F1 score """
    if classes
        return precision_recall_curve_classes(model, test_images, test_labels; distance_threshold=2.3)
    else
        conf_thres = range(0.05, stop=0.95, length=10)
        precisions, recalls = [], []
        for i in conf_thres
            prec, recall = mean_precision_and_recall(model, test_images, test_labels; confidence_threshold=i, distance_threshold=distance_threshold)
            append!(precisions, prec)
            append!(recalls, recall)
        end
        score = f1score.(precisions, recalls)
        max_id = argmax(score)
        return precisions, recalls, conf_thres[max_id], score[max_id]
    end
end;

function precision_recall_curve_classes(model, test_images, test_labels; distance_threshold=2.3)
    """ Returns precision and recall for 10 different confidence thresholds, the optimal confidence threshold,
        and the maximum F1 score separately for each class """
    conf_thres = range(0.05, stop=0.95, length=10)
    precs1, recalls1, precs2, recalls2 = [], [], [], []
    for i in conf_thres
        prec, recall = mean_precision_and_recall(model, test_images, test_labels; confidence_threshold=i, distance_threshold=distance_threshold, classes=true)
        append!(precs1, prec[1])
        append!(recalls1, recall[1])
        append!(precs2, prec[2])
        append!(recalls2, recall[2])
    end
    f11 = f1score.(precs1, recalls1)
    f12 = f1score.(precs2, recalls2)
    max_id1 = argmax(f11)
    max_id2 = argmax(f12)
    opt_conf_thres = [conf_thres[max_id1], conf_thres[max_id2]]
    f1_max = [maximum(f11), maximum(f12)]
    return [precs1, precs2], [recalls1, recalls2], opt_conf_thres, f1_max, [precs1[max_id1], precs2[max_id2]], [recalls1[max_id1], recalls2[max_id2]]
end;


text:
################################
# Coded by Julien Pascal
# Last modification : 16/04/2016
# Replication of the the following paper:

####################################################################################
# "On the Dynamics of Unemployment and Wage Distributions"
# Paper from Jean Marc Robin (2011)
# Available here: https://sites.google.com/site/jmarcrobin/research/publications
####################################################################################

###############################################################################################
# Module that calculates the wages value functions based on the paramaters estimated by the MSM
module calculate_wages

	using PyPlot, Distributions, StatsFuns, JLD, DataFrames, Copulas

	export Wages

	function Wages()

		####################################################
		#A. Define functions necessary to compute the wages:
		####################################################

	    ##################
	    # Function that returns the inverse of lognormal distribution
	    function logninv(p,mu,sigma)

	        #output: scalar x
	        logx0 = -sqrt(2)*erfcinv(2*p);
	        x = exp(sigma*logx0 + mu);

	        return x
	    end
	    #################

	    ##########################
	    #define match productivity:
	    function yi_m(y_index,x_index, ygrid, xgrid)
	        match = ygrid[y_index,1]*xgrid[x_index,1]
	        return match
	    end
	    ##########################

	    ##########################
	    # define opportunity cost of employment:
	    function zi_m(y_index,x_index, ygrid, xgrid, z0, alpha)

	        op_cost = z0 + alpha*(yi_m(y_index,x_index, ygrid, xgrid) - z0) #call the function defined above

	        return op_cost
	    end

	    #################
	    # Load Parameters
	    #################
		println("loading estimated parameters")
		coeff = readtable(string("tables/estimated_parameters.csv"))

		"""
		z0 = coeff[:Value][1]
		sigma = coeff[:Value][2]
		pho = coeff[:Value][3]
		lambda0 = coeff[:Value][4]
		eta = coeff[:Value][5]
		mu = coeff[:Value][6]
		x_lower_bound = parameters[7]
		delta = coeff[:Value][8] #exogenous layoff rate

		k = 0.12
		lambda1  = k*lambda0
		#x_lower_bound = 0.73
		alpha = 0.64
		tau = 0.5
		#delta = 0.043 #exogenous layoff rate
		r = 0.05/4 #quarterly interest rate
		discount = (1 - delta)/(1 + r)
		epsilon = 0.002
		"""

		"""
		z0 = coeff[:Value][1];
		sigma = coeff[:Value][2];
		lambda0 = coeff[:Value][3];
		eta =  coeff[:Value][4];
		mu =  coeff[:Value][5];
		x_lower_bound = coeff[:Value][6] #lower bound of the ability level
		alpha = coeff[:Value][7]
		delta = coeff[:Value][8]

		pho = 0.94
		k = 0.12;
		lambda1  = k*lambda0;
		#x_lower_bound = 0.73
		#alpha = 0.64
		tau = 0.5
		#delta = 0.043 # exogenous layoff rate

		r = 0.05/4 # interest rate
		discount = (1 - delta)/(1 + r)
		epsilon = 0.002
		"""

		z0 = coeff[:Value][1];
	    sigma = coeff[:Value][2];
	    lambda0 = coeff[:Value][3];
	    eta =  coeff[:Value][4];
	    mu =  coeff[:Value][5];
	    x_lower_bound = coeff[:Value][6] #lower bound of the ability level

	    pho = 0.94
	    k = 0.12;
	    lambda1  = k*lambda0;
	    #x_lower_bound = 0.73
	    alpha = 0.64
	    tau = 0.5
	    delta = 0.042 # exogenous layoff rate

	    r = 0.05/4 # interest rate
	    discount = (1 - delta)/(1 + r)
	    epsilon = 0.002

	    N = 100 #number of states
		M = 500 #number of ability levels

	    # Define the grid for ability x
	    xgrid_space = linspace(x_lower_bound,x_lower_bound+1,M); # column vector with M rows

	    xgrid = Array{Float64}(M,1);
	    for i=1:M
	    xgrid[i,1] = xgrid_space[i];
	    end

	    # Define the grid for match ability yi:
	        # 1. a:
	        agrid = linspace((0+epsilon),(1-epsilon),N);

	        # 2. yi:
	        #intialization:
	        ygrid = Array{Float64}(N,1);

	        for i=1:N
	            ygrid[i] = logninv(agrid[i],0,sigma); #calculate the inverse of the lognormal
	        end

	    #Markov transition matrix:
	    Markov = zeros(N,N);

	    # number of dimension
	    # AR1 parameter
	    cop = Copulas.NormalCopula(2,pho)

	    #Gaussian copula pdf:
	    for j=1:N #move along the column:
	        for i=1:N #move along the lines:
	            Markov[i,j] = Copulas.dnormCopula([agrid[i] agrid[j]], cop)[];
	        end
	    end

	    #normalize so that each row sum to 1:
	    for i = 1:N
	        Markov[i,:] = Markov[i,:]./sum(Markov[i,:]);
	    end

	    # distribution of workers on the grid x
	    lm = Array{Float64}(M,1);

	    # load the beta pdf
	    b = Beta(eta, mu)

	    for i=1:M
	        lm[i,1] = pdf(b, xgrid[i]-x_lower_bound); #evaluation of the beta pdf at a given point
	    end

	    #normalization so that mass of workers sum up to 1:
	    lm[:,1] = lm[:,1]./sum(lm[:,1]);

	    # Pre calculate the opportunity cost along the grid of ability:
	    # N times M matrix
	    # rows = aggregate staes
	    # columns = ability level
	    zi_m_grid = zeros(N,M);
	    for i=1:N
	        for m=1:M
	            zi_m_grid[i,m] = zi_m(i,m, ygrid, xgrid, z0, alpha);
	        end
	    end

	    ##########################################################
	    # Calculate the match surplus by value function iteration:
	    # N times M matrices
	    # N = aggregate level
	    # M = ability level

	    G = zeros(N,M) #intialization
	    p = zeros(N,M)

	    # Pre calculate values:
	    for i = 1:N
	        for m = 1:M
	            G[i,m] = yi_m(i,m, ygrid, xgrid) - zi_m_grid[i,m];
	            p[i,m] = yi_m(i,m, ygrid, xgrid);
	        end
	    end

	    # N times M matrix
	    # column index indicates the workers' type
	    # row index indicates the shock index

	    Si_m = zeros(N,M);

	    #tol = 0.0001;
	    tol = 0.01;
	    maxits = 300;
	    dif = tol+tol;
	    its = 1;

	    #initialization:
	    up = ones(N,M);
	    up_plus1 = zeros(N,M);
	    compare = zeros(N,M);

	    while dif>tol
	        up_plus1 =  G + discount*Markov*max(up,compare)
	        dif = norm(up_plus1 - up)
	        up = up_plus1

	        its = its + 1
	        print(its)

	        if its > maxits
	            break
	        end
	    end

	    Si_m = up;

		##############################################
		# Create the wages by value function iteration
		##############################################
		# 1. Wage low
		W_low = zeros(N,N,M); #Use mutlidimensional arrays
		W_low_star = zeros(N,N,M);

		up_ini = zeros(N,N,M);
		down_ini = zeros(N,N,M);

		###########################
		# Pre caculates some values:
		Z = zeros(N,N,M);

	    for m=1:M
	        for i = 1:N
	            for k = 1:N

	            Z[k,i,m] = zi_m_grid[i,m] - zi_m_grid[k,m]

	            #initialization
	            up_ini[k,i,m] = Z[k,i,m];

	                a = max(up_ini[k,i,m],0)
	                down_ini[k,i,m] = min(a,Si_m[i,m])

	            end
	        end
	    end

		function calculate_W_low(m, N, M, discount, Markov, up_ini, down_ini, Z, Si_m)

		        #########################
		        tol = 0.1;
		        maxits = 100;
		        compare = zeros(N,1);
		        a = 0

		        #move along workers' types

		            its = 1
		            dif = tol+tol

		            up = up_ini #initial value
		            up_plus1 = zeros(N,N)

		            down = down_ini;
		            down_star = down_ini;

		            while dif>tol

		                #move along the columns
		                for i=1:N
		                    for k = 1:N

		                        a = max(up[k,i],0)
		                        down[k,i] = min(a,Si_m[i,m])

		                        sum = 0

		                        for j = 1:N
		                            if Si_m[j,m] > 0
		                                sum = sum + (Markov[k,j] - Markov[i,j])*(lambda1*Si_m[j,m]+(1-lambda1)*(down[j,i]))
		                            end
		                        end

		                        up_plus1[k,i] = (Z[k,i,m])  + discount*sum

		                        a = max(up_plus1[k,i],0)
		                        down_star[k,i] = min(a, Si_m[i,m])
		                    end
		                end

		                dif = vecnorm(up_plus1[:,:]- up[:,:],2)

		                up[:,:] = up_plus1[:,:]

		                its = its + 1 #show the iteration number

		                if its > maxits
		                    break
		                end
		            end

		            #W_low[:,:,m] = up[:,:,m]

		    #println(dif) #show the current worker type
		    return [up; down_star] # concatenate vertically and return the result
		end

		##############
		# 2. Wage high
		##############
		up_high_ini = zeros(N,N,M);
		down2_ini = zeros(N,N,M);

		# Initialization:
		W_high = zeros(N,N,M);
		W_high_star = zeros(N,N,M);

		###########################
		# Pre caculates some values:
		Q = zeros(N,N,M);

		    for m=1:M
		        for i = 1:N
		            for k = 1:N
		                Q[k,i,m] = Z[k,i,m] + Si_m[i,m]

		                #initialization
		                up_high_ini[k,i,m] = Q[k,i,m];

		                    a = max(up_high_ini[k,i,m],0);
		                    down2_ini[k,i,m] = min(a, Si_m[i,m]);
		            end
		        end
		    end

		function calculate_W_high(m, N, M, discount, Markov, up_high_ini, down2_ini, Q, Si_m)

		    #initialization:
		    tol = 0.1;
		    maxits = 100;
		    its = 1;
		    dif = tol+tol;

		    up_high = up_high_ini;
		    up_plus1_high = zeros(N,N);

		    down = down2_ini;
		    down_star = down2_ini;
		    a = 0

		    while dif>tol

		        #move along the columns
		        for i=1:N
		            for k=1:N

		                a = max(up_high[k,i],0);
		                down[k,i] = min(a, Si_m[i,m]);

		                sum = 0;
		                for j = 1:N
		                    if Si_m[j,m] > 0
		                     sum = sum + (Markov[k,j] - Markov[i,j])*(lambda1*Si_m[j,m]+(1-lambda1)*(down[j,i])) ;
		                    end
		                 end

		                up_plus1_high[k,i] =  Q[k,i,m] + discount*sum;

		                a = max(up_plus1_high[k,i], 0);

		                down_star[k,i] = min(a, Si_m[i,m]);
		            end
		        end

		            dif = vecnorm(up_plus1_high[:,:] - up_high[:,:],2)

		            up_high[:,:] = up_plus1_high[:,:];

		            its = its + 1

		            if its > maxits
		                break
		            end
		    end

		    return [up_high; down_star] # concatenate vertically and return the result
		end

		##############
		#B. Iteration:
		##############
		tic()
		transition = zeros(2*N,N);
		for a = 1:M
			println(a)

			transition = calculate_W_low(a, N, M, discount, Markov, up_ini[:,:,a], down_ini[:,:,a], Z, Si_m);

		    W_low[:,:,a] = transition[1:N,:];
			W_low_star[:,:,a] = transition[(N+1):(2*N),:];
		end

		jldopen("surpluses/W_low_star.jld", "w") do file
		    write(file, "W_low_star", W_low_star)
		end

		jldopen("surpluses/W_low.jld", "w") do file
		    write(file, "W_low", W_low)
		end

		transition2 = zeros(2*N,N);
		for a = 1:M
			println(a)

			transition2 = calculate_W_high(a, N, M, discount, Markov, up_high_ini[:,:,a], down2_ini[:,:,a], Q, Si_m)

		    W_high[:,:,a] = transition2[1:N,:];
		    W_high_star[:,:,a] = transition2[(N+1):(2*N),:];

		end
		toc() #Takes 23 mn to run on my computer

		jldopen("surpluses/W_high_star.jld", "w") do file
		        write(file, "W_high_star", W_high_star)
		end

		jldopen("surpluses/W_high.jld", "w") do file
		       write(file, "W_high", W_high)
		end

		xgrid_plot = repmat(ygrid',N,1);
		ygrid_plot = repmat(ygrid,1,N);

		fig = figure("pyplot_surfaceplot",figsize=(10,10))
		ax = fig[:add_subplot](2,1,1)
		cp = ax[:contour](xgrid_plot, ygrid_plot, W_low_star[:,:,100], colors="black", linewidth=2.0)
		ax[:clabel](cp, inline=1, fontsize=10)
		xlabel("X")
		ylabel("Y")
		title("Contour Plot")

		subplot(212)
		ax = fig[:add_subplot](2,1,2)
		cp = ax[:contour](xgrid_plot, ygrid_plot, W_high_star[:,:,100], colors="black", linewidth=2.0)
		ax[:clabel](cp, inline=1, fontsize=10)
		xlabel("X")
		ylabel("Y")
		title("Contour Plot")
	end
end


text:
# stop gap to allow AWS requests which have a non-empty payload...

function aws_string(dt::DateTime)
    y,m,d = Dates.yearmonthday(dt)
    h,mi,s = Dates.hour(dt),Dates.minute(dt),Dates.second(dt)
    yy = y < 0 ? @sprintf("%05i",y) : lpad(y,4,"0")
    mm = lpad(m,2,"0")
    dd = lpad(d,2,"0")
    hh = lpad(h,2,"0")
    mii = lpad(mi,2,"0")
    ss = lpad(s,2,"0")
    return "$yy-$mm-$(dd)T$hh:$mii:$ss"
end

aws_string(v::Bool) = v ? "True" : "False"
aws_string(v::Any) = string(v)

function get_utc_timestamp(addsecs=0;basic=false)
    dt = Dates.unix2datetime(Dates.datetime2unix(now(Dates.UTC)) + addsecs)
    dstr = aws_string(dt)
    if basic
        dstr = replace(dstr, Set(":-"), "")
    end
    return string(dstr, "Z")
end

sign(key, msg) = Crypto.hmacsha256_digest(msg, key)

function get_signature_key(key, datestamp, region, service)
    kDate = sign("AWS4" * key, datestamp)
    kRegion = sign(kDate, region)
    kService = sign(kRegion, service)
    kSigning = sign(kService, "aws4_request")
    return kSigning
end

function signature_version_4(env, service, method, host, action, payload)
    # inputs to request
    amzdate = get_utc_timestamp(; basic=true)
    datestamp = amzdate[1:searchindex(amzdate, "T")-1]

    # Task 1: canonical request
    canonical_uri = "/"
    canonical_querystring = ""
    canonical_headers = "host:" * host * "\n" * "x-amz-date:" * amzdate * "\n"
    signed_headers = "host;x-amz-date"
    if env.aws_token != ""
        canonical_headers = canonical_headers * "x-amz-security-token:" * env.aws_token * "\n"
        signed_headers = signed_headers * ";x-amz-security-token"
    end
    canonical_headers = canonical_headers * "x-amz-target:DynamoDB_20120810.$action" * "\n"
    signed_headers = signed_headers * ";x-amz-target"
    payload_hash = bytes2hex(Crypto.sha256(payload))

    canonical_request = method * "\n" * canonical_uri * "\n" * "" * "\n" *
        canonical_headers * "\n" * signed_headers * "\n" * payload_hash

    # Task 2: string to sign
    algorithm = "AWS4-HMAC-SHA256"
    region = replace(replace("ec2.us-east-1.amazonaws.com", r".amazonaws.com$", ""), r"^ec2.", "")
    credential_scope = datestamp * "/" * region * "/" * "dynamodb" * "/" * "aws4_request"
    string_to_sign = algorithm * "\n" * amzdate * "\n" * credential_scope * "\n" * bytes2hex(Crypto.sha256(canonical_request))

    # Task 3: calculate the signature
    signing_key = get_signature_key(env.aws_seckey, datestamp, region, service)
    signature = bytes2hex(sign(signing_key, string_to_sign))

    # Task 4: add signing information to request ##### how to return header to caller?
    authorization_header = algorithm * " " * "Credential=" * env.aws_id * "/" * credential_scope * ", " *
        "SignedHeaders=" * signed_headers * ", " * "Signature=" * signature

    headers = Dict("Authorization" => authorization_header,
               "X-Amz-Date" => amzdate,
               "Host" => host,
               "Content-Type" => "application/x-amz-json-1.0",
               "X-Amz-Target" => "DynamoDB_20120810.$action")
    if env.aws_token != ""
        headers["x-amz-security-token"] = env.aws_token
    end

    return headers
end

function dynamo_execute(env, action, json_data; current_retry=0)
    host = "dynamodb.$(env.region).amazonaws.com"

    body = JSON.json(json_data)
    amz_headers = signature_version_4(env, "dynamodb", "POST", host, action, body)

    function retry(msg :: AbstractString)
        if current_retry > 9
            error("Request failed after 10 retries: $msg")
        end

        sleep(2^current_retry * 0.05)
        return dynamo_execute(env, action, json_data; current_retry=current_retry+1)
    end

    resp = nothing
    try
        resp = Requests.post("https://$host/"; data=body, headers=amz_headers, timeout=env.timeout)
    catch e
        if isa(e, AbstractString)
            if ismatch(r"((Couldn't connect to server)|(Couldn't resolve host name))", e)
                @show :retry
                return retry(e)
            end
        end
        throw(e)
    end

    status = resp.status
    value = JSON.parse(bytestring(resp.data))

    if status == 400
        if haskey(value, "__type") && ismatch(r"ProvisionedThroughputExceededException$", value["__type"])
            return retry("ProvisionedThroughputExceededException")
        end
    end

    return (status, value)
end


text:
function deltasumkAll(p0, ps, k, T,Npi,IRScale, UVScale, mfun, lamfun)
    if p0>=0.0
        deltasumkfix(p0, ps, k, T,Npi,IRScale, UVScale, mfun, lamfun)
    elseif p0<0.0
        -deltasumkfix(-p0, ps, k, T,Npi,IRScale, UVScale, mfun, lamfun)
    end
end

function deltasumkAllSmooth(p0, ps, k, T,Npi,IRScale, UVScale, mfun, lamfun)
    if p0>=0.0
        deltasumkfixSmooth(p0, ps, k, T,Npi,IRScale, UVScale, mfun, lamfun)
    elseif p0<0.0
        -deltasumkfixSmooth(-p0, ps, k, T,Npi,IRScale, UVScale, mfun, lamfun)
    end
end

function deltasumkAll_dp0(p0, ps, k, T,Npi,IRScale, UVScale, mfun, lamfun)
    if p0>=0.0
        deltasumkfix_dp0(p0, ps, k, T,Npi,IRScale, UVScale, mfun, lamfun)
    elseif p0<0.0
        deltasumkfix_dp0(-p0, ps, k, T,Npi,IRScale, UVScale, mfun, lamfun)
    end
end

@doc raw"""
    F1All(p0, ps, k, m, T)

 $F_1$ is an odd function about $p_0$

when `p0>0`,`F1All=loopfunpp`

when `p0<0`,`dkF1All=-loopfunpp(-p0)`

`F1All` doesn't contains the type-2 delta function
"""
function F1All(p0, ps, k, m, T)
    if p0>=0.0
        loopfunpp(p0, ps, k, m, T)
    elseif p0<0.0
        -loopfunpp(-p0, ps, k, m, T)
    end
end

@doc raw"""
    F2All(p0, ps, k, m, T)

 $F_2$ is an odd function about $p_0$

when `p0>0`,`F2All=loopfunpm`

when `p0<0`,`F2All=-loopfunpm(-p0)`
"""
function F2All(p0, ps, k, m, T)
    if p0>=0.0
        loopfunpm(p0, ps, k, m, T)
    elseif p0<0.0
        -loopfunpm(-p0, ps, k, m, T)
    end
end

@doc raw"""
    dkF1All(p0, ps, k, m, T)

 $F_1$ is an odd function about $p_0$

when `p0>0`,`dkF1All=flowpp`

when `p0<0`,`dkF1All=-flowpp(-p0)`

`dkF1All` doesn't contains the type-2 delta function
"""
function dkF1All(p0, ps, k, m, T,δ=0.02)
    if p0>=0.0
        flowpp(p0, ps, k, m, T,δ)
    elseif p0<0.0
        -flowpp(-p0, ps, k, m, T,δ)
    end
end

function dkF1All(k, m, T)
    return 0.0
end

@doc raw"""
    dkF2All(p0, ps, k, m, T)

 $F_2$ is an odd function about $p_0$

when `p0>0`,`dkF2All=flowpm`

when `p0<0`,`dkF2All=-flowpm(-p0)`
"""
function dkF2All(p0, ps, k, m, T)
    if p0>=0.0
        flowpm(p0, ps, k, m, T)
    elseif p0<0.0
        -flowpm(-p0, ps, k, m, T)
    end
end

function dkF2All(k, m, T)
    if m > 0.0
        return 0.0
    elseif m <= 0.0
        Ek = Epi(k, m)
        return k / (16 * Ek * pi^2 * T - 16 * Ek * pi^2 * T * cosh(Ek / T))
    end
end

@doc raw"""
    dkF1Allintqs(p0, ps, qsmax, k, m, T)

 $F_1$ is an odd function about $p_0$

when `p0>0`,`dkF1Allintqs=flowpp_intcostheqs`

when `p0<0`,`dkF1Allintqs=-flowpp_intcostheqs(-p0)`

`dkF1Allintqs` doesn't contains the type-1 delta function

`dkF1Allintqs` doesn't contains the type-2 delta function
"""
function dkF1Allintqs(p0, ps, qsmax, k, m, T)
    sign(p0)*flowpp_intcostheqs(abs(p0), ps, qsmax, k, m, T)
end

function dkF1Allintqs_gpu(p0, ps, qsmax, k, m, T)
    sign(p0)*flowpp_intcostheqs_gpu(abs(p0), ps, qsmax, k, m, T)
end

@doc raw"""
    flowpm(p0, ps, k, m, T)

 $F_2$ is an odd function about $p_0$

when `p0>0`,`dkF2Allintqs=flowpm_intcostheqs`

when `p0<0`,`dkF2Allintqs=-flowpm_intcostheqs(-p0)`
"""
function dkF2Allintqs(p0, ps, qsmax, k, m, T)
    sign(p0)*flowpm_intcostheqs(abs(p0), ps, qsmax, k, m, T)
end

function dkF2Allintqs_gpu(p0, ps, qsmax, k, m, T)
    sign(p0)*flowpm_intcostheqs_gpu(abs(p0), ps, qsmax, k, m, T)
end

# function dkF2Allintqs(p0, ps, qsmax, k, m, T)
#     if p0>=0.0
#         0.0
#     elseif p0<0.0
#         0.0
#     end
# end

function dkF4Allintqs(p0, ps, qsmax, k, m, T)
    if p0>=0.0
        F4_intcosthqs(p0, ps, qsmax, k, m, T)
    elseif p0<0.0
        -F4_intcosthqs(-p0, ps, qsmax, k, m, T)
    end
end


text:
include("simulate.jl")

using Ipopt
using LazyArrays
using GenericLinearAlgebra
using SparseArrays

## trim function
function trim_kronLazy(j::Vector{Joint},GravityInInertial::Vector{Float64};
  ix::Vector{Integer}=zeros(Integer,20*(nB-1)+1),
  iy::Vector{Integer}=zeros(Integer,7*(nB-1)))

  nB = length(j) + 1; # number of bodies in the joint tree
  # ix: 0 indicates that the corresponding variable is free during optimization

  # iy: 0 indicates that the corresponding second derivative term needs to be zero
  # (specify indices for iy starting from second body)
  # iy = [1;4;5] means that the ẍ. β̈1 and β̈2 are free for j[1].RB2

  # Find which variables are free for optimization
  freeVarInd = findall(x->x==0,ix); # indices of free variables

  # find which acceleration-level terms are desired to be 0/constrained
  freeAccInd = findall(x->x==1,iy) # indices of free accelerations

  # Initial guesses
  # Initial guess is provided initial location
  x0 = []; # Initial guess
  for k = 1:length(j)
    append!(x0,j[k].RB2.x)
  end
  extFListCopy = externalFTotal(0.0,j);
  u0 = genU(extFListCopy); # Initial guess for control input

  n_xu = length(freeVarInd)-1; # Number of optimization variables
  lb = [-Inf*ones(n_xu);0.0]; ub = [Inf*ones(n_xu);Inf] # Bounds on optimization variables
  # lb[4] = 0.0;
  # lb[14+4] = 0.0;

  m = 1; # Number of nonlinear constraints (xdot = f(x,u)) for second order variables
  lb_g = [-Inf]; ub_g = [0.0];

  # extract states and control from optimization variables
  function getXU(z::AbstractArray{T}) where T<:Real
    z0 = [x0;u0[:,2:end][:];0.0];
    z0[freeVarInd] .= 0;

    Mz = zeros(length(z0), length(freeVarInd))
    for (i,ind) in enumerate(freeVarInd)
      Mz[ind,i] = 1;
    end

    zFull = z0 + Mz*z;

    X = zFull[1:14*(nB-1)];
    U = zFull[14*(nB-1)+1:end-1]
    return X,U
  end

  ## Constraint functions
  function qddFn(xuVar::AbstractArray{T}) where T<:Real
    x = xuVar[1:14*nB]; u = reshape(xuVar[14*nB+1:end],(6,nB));
    xdot = fxdot(x,u,j,g);
    y = Vector{T}(undef,7*nB); # qdd
    for i=1:nB
      y[7*(i-1)+1:7*(i)] = xdot[7*(2*i-1)+1:14*i]
    end
    deleteat!(y,1:7); # inertial frame has no acceleration
    deleteat!(y,freeAccInd); # free acceleration variables
    return sparse(y)
  end

  function quatNormConstr(xuVar::AbstractArray{T}) where T<:Real
    x = xuVar[1:14*nB]; u = reshape(xuVar[14*nB+1:end],(6,nB));
    q_constr = Vector{T}(undef, nB)
    for i=1:nB
        q_constr[i] = norm(x[14*(i-1)+4:14*(i-1)+7])^2 - 1;
    end
    return q_constr
  end

  function jointLocConstr(xuVar::AbstractArray{T}) where T<:Real
    x = xuVar[1:14*nB]; u = reshape(xuVar[14*nB+1:end],(6,nB));
    jLoc_constr = Matrix{T}(undef,3,length(j))
    for i=1:length(j)
        b1_id = j[i].RB1.bodyID; b2_id = j[i].RB2.bodyID;
        x1 = x[(b1_id-1)*14+1:b1_id*14]
        x2 = x[(b2_id-1)*14+1:b2_id*14]
        j_b1 = x1[1:3] + transpose(quat2dcm(x1[4:7]))*j[i].pos1;
        j_b2 = x2[1:3] + transpose(quat2dcm(x2[4:7]))*j[i].pos2;
        jLoc_constr[:,i] = j_b1 - j_b2;
    end
    return jLoc_constr
  end

  function _nonlinear_constraints(z::AbstractArray{T}) where T<:Real
    xVar, uVar = getXU(z)
    x = [Vector{T}(j[1].RB1.x); xVar]
    u = [zeros(T,6) reshape(uVar,(6, nB-1))]
    xuVar = [x;u[:]]; # Vector of x,u

    # Dynamics constraint
    y = qddFn(xuVar);
    # Quaternion norm constraint
    q_constr = quatNormConstr(xuVar);
    # JointLocation constraint  (translation constraint)
    jLoc_constr = jointLocConstr(xuVar);

    constr = ([y;q_constr;jLoc_constr[:]]);

    J = sum(constr.*constr) - z[end];
    return J
  end

  ## Helper functions for derivatives
  function kronSum(A,B)
    # kronecker sum of 2 arrays
    out = kronProd(A, Matrix(I,(size(B)))) + kronProd(Matrix(I,(size(A))),B)
    return (out)
  end

  function kronProd(A,B)
    K = kron((A),(B));
    return sparse(K)
  end

  function createCommMat(M)
    # commutation matrix
    r,m = size(M);
    return createCommMat(r,m);
  end

  function createCommMat(r,m)
    # commutation matrix for a matrix of size r,m
    K = spzeros(m*r,m*r);
    Ir = sparse(I,r,r); Im = sparse(I,m,m);
    for i=1:r
      for j=1:m
          ei = @view Ir[:,i]; # ith-canonical unit vector of dimension r
          ej = @view Im[:,j];# jth-canonical unit vector of dimension m
          K += kronProd(ei*permutedims(ej), ej*permutedims(ei));
      end
    end
    return (K)
  end

  function fdJ_kronAB(A,B, dA, dB)
    n,q = size(A[:,:]); p,r = size(B[:,:]);
    Iq = sparse(1.0I,q,q); Ip = sparse(1.0I,p,p); In = sparse(1.0I,n,n); Ir = sparse(1.0I,r,r);
    Inq = sparse(1.0I,n,q); Ipr = sparse(1.0I,p,r)
    Krn = sparse(createCommMat(r,n));
    Ay1 = sparse(kronProd(Krn,Ip)); Ay2 = sparse(kronProd(In,vec(B)));
    Ay = sparse(kronProd(Iq, Ay1*Ay2));
    Bx1 = sparse(kronProd(Iq,Krn)); Bx2 = sparse(kronProd(vec(A),Ir));
    Bx = sparse(kronProd(Bx1*Bx2, Ip));
    out = sparse(Ay*dA + Bx*dB)
    return out
  end

  function grad_hess_nonlinear_constraints(z, flag)
    ## First derivatives
    xVar, uVar = getXU(z)
    xv = [j[1].RB1.x; xVar]
    uv = [zeros(6) reshape(uVar,(6, nB-1))];
    xu_var = [xv;uv[:]];

    function Mdiff1(x)
      # First derivatives of mass matrix and powers (1/2, -1/2, -1)
      M = Array(assembleM(x,j));
      Ms = (real(sqrt(M)));
      M_inv = (inv(M))
      Ms_inv = (real(sqrt(M_inv)));

      dM = sparse(ForwardDiff.jacobian(z -> assembleM(z,j),x))
      dM_inv = sparse(-(kronProd(permutedims(M_inv),M_inv))*dM)
      dMs = sparse(inv(Array(kronSum(permutedims(Ms), Ms)))*dM)
      dMs_inv = sparse(-kronProd(permutedims(Ms_inv),Ms_inv)*(dMs));
      return sparse(M), sparse(Ms), sparse(Ms_inv), dM, dMs, dMs_inv, sparse(M_inv), dM_inv
    end
    M, Ms, Ms_inv, dM, dMs, dMs_inv, M_inv, dM_inv = Mdiff1(xv);

    function Abdiff1(x)
      A, b = Ab_VecOfMat(x,j);
      dA = ForwardDiff.jacobian(z->Ab_VecOfMat(z,j)[1],x);
      db = ForwardDiff.jacobian(z->Ab_VecOfMat(z,j)[2],x);
      return sparse(A), sparse(dA), sparse(b), sparse(db)
    end
    A, dA, b, db = Abdiff1(xv);

    function Gpdiff1(x)
      function t1diff1(x)
        # t1 = A'*(AM)^{-1}*A';
        sz_A1 = size(A,1)
        t1 = sparse(permutedims(A)*inv(Array(A*M_inv*permutedims(A))))

        dA_tr = sparse(createCommMat(A)*dA);

        Y = A*M_inv*permutedims(A); invY = inv(Array(Y));
        dY1 = sparse(kronProd(A*M_inv,1.0I(sz_A1))*dA);
        dY2 = sparse(kronProd(A,A)*dM_inv);
        dY3 = sparse(kronProd(1.0I(sz_A1), A*M_inv)*dA_tr);
        dY = dY1 + dY2 + dY3;
        dY_inv = sparse(-kronProd(permutedims(invY),invY)*dY);

        dt1_1 = sparse(kronProd(permutedims(invY),1.0I(size(A,2)))*dA_tr);
        dt1_2 = sparse(kronProd(1.0I(sz_A1),permutedims(A))*dY_inv;)
        dt1 = sparse(dt1_1 + dt1_2);
        return t1, dt1
      end
      G = Array(A*Ms_inv);
      Gp = sparse(permutedims(G)*((G*permutedims(G))\I(size(G,1))))

      t1, dt1 = t1diff1(x)
      dGp1 = sparse(kronProd(permutedims(t1),1.0I(size(M,1)))*dMs_inv)

      sz_t2 =size(t1,2)
      t2 = sparse(kronProd(1.0I(sz_t2), Ms_inv))
      dGp2 = sparse(t2*dt1);

      dGp = sparse(dGp1 + dGp2)
      return Gp, dGp
    end
    Gp, dGp = Gpdiff1(xv);

    function Fudiff1(x,u)
      Fu = assembleF(x,u,j,g);
      dFu = ForwardDiff.jacobian(y->assembleF(y,u,j,g),x);
      dFu_u = ForwardDiff.jacobian(y->assembleF(x,y,j,g),u);
      return Fu, dFu, dFu_u
    end
    Fu, dFu, dFu_u = Fudiff1(xv,uv);

    Fu = convert(SparseVector{Float64}, sparse(Fu));
    dFu = convert(SparseMatrixCSC{Float64}, sparse(dFu));
    dFu_u = convert(SparseMatrixCSC{Float64}, sparse(dFu_u));

    function hFn(x,u)
      h = b - A*M_inv*Fu;
      return sparse(h)
    end

    function hdiff1(x,u)
      # has derivatives wrt both x,u
      h = hFn(x,u);

      # dh
      term1_1 = sparse(kronProd(permutedims(M_inv*Fu), 1.0I(size(A,1))));
      # println("typeof(term1_1) = ", typeof(term1_1))
      # println("typeof(dA) = ", typeof(dA))
      term1 = sparse(term1_1*dA);
      term2 = sparse(kronProd(permutedims(Fu), A)*dM_inv);
      term3 = sparse(A*M_inv*dFu);
      dh_x = sparse(db - (term1 + term2 + term3));

      # dh_u
      dh_u = sparse(-A*M_inv*dFu_u);

      return sparse(h), sparse(dh_x), sparse(dh_u)
    end
    h, dh_x, dh_u = hdiff1(xv,uv);

    function Fcdiff1(x,u)
      Fc = Ms*Gp*h

      dFc1 = kronProd(permutedims(Gp*h),I(size(M,1)))*dMs
      dFc2 = kronProd(permutedims(h),Ms)*dGp
      dFc3 = Ms*Gp*dh_x;

      dFc = sparse(dFc1 + dFc2 + dFc3);

      dFc_u = Ms*Gp*dh_u;
      return sparse(Fc), sparse(dFc), sparse(dFc_u)
    end
    Fc, dFc, dFc_u = Fcdiff1(xv,uv);

    function accdiff1(x,u)
      # derivative of qdd w.r.t x
      dacc1 = sparse(kronProd(transpose(Fc+Fu),1.0I(size(M_inv,1)))*dM_inv)
      dacc2 = sparse(M_inv*(dFc + dFu))
      dqdd_x = sparse(dacc1 + dacc2)

      return (dqdd_x)
    end
    dqdd_x = accdiff1(xv,uv);
    ## Constraint derivative functions for gradient

    function dqddFn(xuVar)
      dqdd_u = sparse(M_inv*(dFc_u + dFu_u));
      out = [dqdd_x dqdd_u];
      return sparse(out)
    end
    qdd = qddFn(xu_var);
    qdd = convert(SparseVector{Float64}, sparse(qdd));
    dqdd = dqddFn(xu_var);

    function dynConstr_jac(xuVar)
      out = (2*permutedims(qdd)*dqdd)[:];
      return out
    end

    function quatNormConstr_diff(xuVar)
      f(y) = sum(quatNormConstr(y).*quatNormConstr(y));
      df(x) = ForwardDiff.gradient(y->f(y), x);
      grad_f = df(xuVar);
      d2f = ForwardDiff.jacobian(y->df(y), xuVar);
      return grad_f, d2f
    end

    function jointLocConstr_diff(xuVar)
      f(y) = sum((jointLocConstr(y)[:]).*(jointLocConstr(y)[:]));
      df(x) = ForwardDiff.gradient(y->f(y), x);
      grad_f = df(xuVar);
      d2f = ForwardDiff.jacobian(y->df(y), xuVar);
      return grad_f, d2f
    end

    function grad_nonlinear_constraints()
      dqdd_constr = dynConstr_jac(xu_var); # Dyn
      dquatNorm_constr,_ = quatNormConstr_diff(xu_var); # Quat
      djointLoc_constr, _ = jointLocConstr_diff(xu_var); # JointLoc

      dconstr_z = dqdd_constr + dquatNorm_constr + djointLoc_constr;

      # take care of free optimization variables
      freeVarInd_grad = 14 .+ freeVarInd[1:end-1];
      grad_constr_freeInd = dconstr_z[freeVarInd_grad];
      out = [grad_constr_freeInd;-1.0];
      return out
    end

    if flag == 1
      ## First derivatives required anyway
      out = grad_nonlinear_constraints();
    else
      ##  Derivatives for hessian
      function Mdiff2(x)
        # dM_xx
        dM_fn(y) = ForwardDiff.jacobian(z->assembleM(z,j),y);
        dM_xx = sparse(ForwardDiff.jacobian(z->dM_fn(z),x));

        # dMs_xx
        function z1Fn(x)
          z1= kronSum(permutedims(Ms), Ms);
          z1_inv = sparse(inv(Array(z1)));
          dMs_t = sparse(createCommMat(Ms)*dMs);
          dz1_1 = fdJ_kronAB(permutedims(Ms), sparse(1.0I, size(Ms)...), dMs_t, spzeros(size(dMs_t)...));
          dz1_2 = fdJ_kronAB(sparse(1.0I, size(Ms)...), Ms, spzeros(size(dMs)...), dMs);
          dz1 = sparse(dz1_1 + dz1_2);
          dz1_inv = sparse(-(kronProd(permutedims(z1_inv),z1_inv))*dz1);
          return z1_inv, dz1_inv
        end
        z1_inv, dz1_inv = z1Fn(x);
        out1 = sparse(kronProd(permutedims(dM), 1.0I(size(z1_inv,1)))*dz1_inv);
        out2 = sparse(kronProd(1.0I(size(dM,2)), z1_inv)*dM_xx);
        dMs_xx = sparse(out1 + out2);

        # dMsInv_xx
        R = kronProd(permutedims(Ms_inv), Ms_inv);
        dMs_inv_t = sparse(createCommMat(Ms_inv)*dMs_inv);
        dR = fdJ_kronAB(permutedims(Ms_inv), Ms_inv, dMs_inv_t, dMs_inv);

        out1 = sparse(kronProd(permutedims(dMs),sparse(1.0I(size(R,1))))*dR);
        out2 = kronProd(sparse(1.0I(size(dMs,2))),R)*dMs_xx;
        dMsInv_xx = sparse(-(out1 + out2));

        # dMinv_xx
        Q = kronProd(permutedims(M_inv), M_inv);
        dM_inv_t = sparse(createCommMat(M_inv)*dM_inv);
        dQ = fdJ_kronAB(permutedims(M_inv), M_inv, dM_inv_t, dM_inv);
        out1 = sparse(kronProd(permutedims(dM), sparse(1.0I(size(Q,1))))*dQ);
        out2 = sparse(kronProd(sparse(1.0I(size(dM,2))), Q)*dM_xx);
        dMinv_xx = sparse(-(out1 + out2));

        return dM_xx, dMs_xx, dMsInv_xx, dMinv_xx
      end
      dM_xx, dMs_xx, dMsInv_xx, dMinv_xx = Mdiff2(xv);

      function Abdiff2(x)

        dAFn(y) = ForwardDiff.jacobian(z->Ab_VecOfMat(z,j)[1],y);
        cfg1 = ForwardDiff.JacobianConfig(dAFn, x, ForwardDiff.Chunk{1}());
        dA_xx = ForwardDiff.jacobian(dAFn,x, cfg1);
        dbFn(y) = ForwardDiff.jacobian(z->Ab_VecOfMat(z,j)[2],y);
        cfg1 = ForwardDiff.JacobianConfig(dbFn, x, ForwardDiff.Chunk{1}());
        db_xx = ForwardDiff.jacobian(dbFn,x, cfg1);
        return sparse(dA_xx), sparse(db_xx);
      end
      dA_xx, db_xx = Abdiff2(xv);

      function Gdiff2(x,u)
        G = sparse(A*Ms_inv);
        #dG
        dG1 = sparse(kronProd(permutedims(Ms_inv),sparse(1.0I(size(A,1))))*dA);
        dG2 = sparse(kronProd(sparse(1.0I(size(Ms_inv,2))),A)*dMs_inv);
        dG = sparse(dG1 + dG2);

        # dG_xx
        Q1 = kronProd(permutedims(Ms_inv), sparse(1.0I(size(A,1))));
        dMs_inv_t = sparse(createCommMat(Ms_inv)*dMs_inv);
        dQ1 = fdJ_kronAB(permutedims(Ms_inv), sparse(1.0I(size(A,1))),dMs_inv_t,spzeros(size(A,1)^2,length(x)));

        Q2 = kronProd(sparse(1.0I(size(Ms_inv,2))), A);
        dQ2 = fdJ_kronAB(sparse(1.0I(size(Ms_inv,2))), A, spzeros(size(dMs_inv)...), dA);

        out1 = sparse(kronProd(permutedims(dA),sparse(1.0I(size(Q1,1))))*dQ1);
        out2 = sparse(kronProd(sparse(1.0I(size(dA,2))), Q1)*dA_xx);
        out3 = sparse(kronProd(permutedims(dMs_inv), sparse(1.0I(size(Q2,1))))*dQ2);
        out4 = sparse(kronProd(sparse(1.0I(size(dMs_inv,2))), Q2)*dMsInv_xx);

        dG_xx = sparse(out1 + out2 + out3 + out4);
        return G, dG, dG_xx
      end
      G, dG, dG_xx = Gdiff2(xv,uv);
      # println("typeof(dG_xx) = ", typeof(dG_xx))

      function Hdiff2(x,u)
        # H = G*G^T
        dG_t = sparse(createCommMat(G)*dG);

        H = sparse(G*permutedims(G));

        dH = sparse(kronProd(G,sparse(1.0I(size(G,1))))*dG + kronProd(sparse(1.0I(size(G,1))),G)*dG_t);

        P1 = kronProd(G,sparse(1.0I(size(G,1))));
        dP1 = fdJ_kronAB(G, sparse(1.0I(size(G,1))), dG, spzeros(size(G,1)^2,length(x)));

        P2 = kronProd(sparse(1.0I(size(G,1))), G);
        dP2 = fdJ_kronAB(sparse(1.0I(size(G,1))), G, spzeros(size(G,1)^2,length(x)), dG);

        dG2_t = sparse(kronProd(sparse(1.0I(size(dG,2))), createCommMat(G))*dG_xx);

        out1 = sparse(kronProd(permutedims(dG), sparse(1.0I(size(P1,1))))*dP1);
        out2 = sparse(kronProd(sparse(1.0I(size(dG,2))), P1)*dG_xx);
        out3 = sparse(kronProd(permutedims(dG_t), sparse(1.0I(size(P2,1))))*dP2);
        out4 = sparse(kronProd(sparse(1.0I(size(dG_t,2))), P2)*dG2_t);

        dH_xx = sparse(out1 + out2 + out3 + out4);
        return H, dH, dH_xx
      end
      H, dH, dH_xx = Hdiff2(xv,uv);
      # println("typeof(dH_xx) = ", typeof(dH_xx))

      function Gpdiff2(x,u)
        dG_t = sparse(createCommMat(G)*dG);
        dG2_t = sparse(kronProd(sparse(1.0I(size(dG,2))), createCommMat(G))*dG_xx);

        H_inv = sparse(inv(Array(H)));
        dH_inv = sparse(-kronProd(permutedims(H_inv), H_inv)*dH);
        dH_inv_t = sparse(createCommMat(H_inv)*dH_inv);

        W = kronProd(permutedims(H_inv), H_inv);
        dW = fdJ_kronAB(permutedims(H_inv), H_inv, dH_inv_t, dH_inv);
        h1 = sparse(kronProd(permutedims(dH),sparse(1.0I(size(W,1))))*dW);
        h2 = sparse(kronProd(sparse(1.0I(size(dH,2))),W)*dH_xx);
        dH_inv2 = sparse(-(h1 + h2));

        V1 = kronProd(permutedims(H_inv), sparse(1.0I(size(G,2))));
        dV1 = fdJ_kronAB(permutedims(H_inv), sparse(1.0I(size(G,2))), dH_inv_t, spzeros(size(G,2)^2, length(x)));

        V2 = kronProd(sparse(1.0I(size(H_inv,2))), permutedims(G));
        dV2 = fdJ_kronAB(sparse(1.0I(size(H_inv,2))),permutedims(G), zeros(size(dH_inv_t)), dG_t);

        out1 = sparse(kronProd(permutedims(dG_t), sparse(1.0I(size(V1,1))))*dV1);
        out2 = sparse(kronProd(sparse(1.0I(size(dG_t,2))), V1)*dG2_t);
        out3 = sparse(kronProd(permutedims(dH_inv), sparse(1.0I(size(V2,1))))*dV2);
        out4 = sparse(kronProd(sparse(1.0I(size(dH_inv,2))), V2)*dH_inv2);

        dGp_xx = sparse(out1 + out2 + out3 + out4);

        return dGp_xx;
      end
      dGp_xx = Gpdiff2(xv,uv);
      # println("typeof(dGp_xx) = ", typeof(dGp_xx))

      function Fudiff2(x,u)
        dFu_xx = ForwardDiff.jacobian(y->Fudiff1(y,u)[2],x);
        dFu_ux = ForwardDiff.jacobian(y->Fudiff1(x,y)[2],u);
        dFu_xu = ForwardDiff.jacobian(y->Fudiff1(y,u)[3],x);
        dFu_uu = ForwardDiff.jacobian(y->Fudiff1(x,u)[3],u);
        return dFu_xx, dFu_ux, dFu_xu, dFu_uu
      end
      dFu_xx, dFu_ux, dFu_xu, dFu_uu = Fudiff2(xv,uv);

      dFu_xx = convert(SparseMatrixCSC{Float64}, sparse(dFu_xx));
      dFu_ux = convert(SparseMatrixCSC{Float64}, sparse(dFu_ux));
      dFu_xu = convert(SparseMatrixCSC{Float64}, sparse(dFu_xu));
      dFu_uu = convert(SparseMatrixCSC{Float64}, sparse(dFu_uu));
      Fu = convert(SparseVector{Float64}, sparse(Fu));
      dFu = convert(SparseMatrixCSC{Float64}, sparse(dFu));
      dFu_u = convert(SparseMatrixCSC{Float64}, sparse(dFu_u));

      function hdiff2(x,u)
        # dh_xx
        # terms 1,2
        T1 = kronProd(permutedims(M_inv*Fu), sparse(1.0I(size(A,1))))
        dMinvFu = sparse(kronProd(permutedims(Fu), sparse(1.0I(size(M_inv,1))))*dM_inv + M_inv*dFu);
        dMinvFu_t = dMinvFu;

        dT1 = fdJ_kronAB(permutedims(M_inv*Fu), sparse(1.0I(size(A,1))), dMinvFu_t, spzeros(size(A,1)^2, length(x)));
        term1 = sparse(kronProd(permutedims(dA), sparse(1.0I(size(T1,1))))*dT1)
        term2 = sparse(kronProd(sparse(1.0I(size(dA,2))), T1)*dA_xx);

        # terms 3,4
        T2 = kronProd(permutedims(Fu), A);
        dT2 = fdJ_kronAB(permutedims(Fu), A, dFu, dA);
        term3 = sparse(kronProd(permutedims(dM_inv), sparse(1.0I(size(T2,1))))*dT2);
        term4 = sparse(kronProd(sparse(1.0I(size(dM_inv,2))), T2)*dMinv_xx);

        # terms 5,6
        T3 = sparse(A*M_inv);
        dT3 = sparse(kronProd(permutedims(M_inv), sparse(1.0I(size(A,1))))*dA + kronProd(sparse(1.0I(size(M_inv,2))), A)*dM_inv);
        term5 = sparse(kronProd(permutedims(dFu), sparse(1.0I(size(A,1))))*dT3);
        term6 = sparse(kronProd(sparse(1.0I(length(x))), T3)*dFu_xx);

        dh_xx = sparse(db_xx - (term1 + term2 + term3 + term4 + term5 + term6));

        # dh_ux
        E1 = kronProd(permutedims(M_inv*Fu), sparse(1.0I(size(A,1))));
        dE1_u = fdJ_kronAB(permutedims(M_inv*Fu), sparse(1.0I(size(A,1))), M_inv*dFu_u, spzeros(size(A,1)^2, length(u)));

        E2 = kronProd(permutedims(Fu), A);
        dE2_u = fdJ_kronAB(permutedims(Fu), A, dFu_u, spzeros(length(A), length(u)));

        term1 = sparse(kronProd(permutedims(dA),sparse(1.0I(size(E1,1))))*dE1_u);
        term2 = sparse(kronProd(permutedims(dM_inv), sparse(1.0I(size(E2,1))))*dE2_u);
        term3 = sparse(kronProd(sparse(1.0I(size(dFu,2))), A*M_inv)*dFu_ux);

        dh_ux = sparse(-(term1 + term2 + term3));

        # dh_xu
        out1 = sparse(kronProd(permutedims(M_inv*dFu_u), sparse(1.0I(size(A,1))))*dA);
        out2 = sparse(kronProd(permutedims(dFu_u), A)*dM_inv);
        out3 = sparse(kronProd(sparse(1.0I(length(u))), A*M_inv)*dFu_xu);
        dh_xu = sparse(-(out1 + out2 + out3));

        # dh_uu
        dh_uu = sparse(-kronProd(sparse(1.0I(length(u))), A*M_inv)*dFu_uu);

        return dh_xx, dh_ux, dh_xu, dh_uu
      end
      dh_xx, dh_ux, dh_xu, dh_uu = hdiff2(xv,uv);

      h, dh_x, dh_u = hdiff1(xv,uv);

      function Fcdiff2(x,u)
        # dFc_xx
        function jacT1(x)
          D1 = kronProd(permutedims(Gp*h), sparse(1.0I(size(Ms,1))));
          dGph = sparse(kronProd(permutedims(h), sparse(1.0I(size(Gp,1))))*dGp + Gp*dh_x);
          dGph_t = dGph;
          dD1 = fdJ_kronAB(permutedims(Gp*h), sparse(1.0I(size(Ms,1))), dGph_t, spzeros(size(dMs)...));

          dMs_t = sparse(createCommMat(Ms)*dMs);

          dT1_1 = sparse(kronProd(permutedims(dMs), sparse(1.0I(size(D1,1))))*dD1);
          dT1_2 = sparse(kronProd(sparse(1.0I(size(dMs,2))), D1)*dMs_xx);

          dT1 = sparse(dT1_1 + dT1_2);
          return dT1
        end

        function jacT2(x)
          D2 = kronProd(permutedims(h), Ms);
          dD2 = fdJ_kronAB(permutedims(h), Ms, dh_x, dMs);
          # dGp_xx = Gpdiff2(x,u);

          dT2_1 = sparse(kronProd(permutedims(dGp),sparse(1.0I(size(D2,1))))*dD2);
          dT2_2 = sparse(kronProd(sparse(1.0I(size(dGp,2))), D2)*dGp_xx);
          dT2 = sparse(dT2_1 + dT2_2);
          return dT2
        end

        function jacT3(x)
          dT3_1 = sparse(kronProd(permutedims(Gp*dh_x), sparse(1.0I(size(Ms,1))))*dMs);
          dT3_2 = sparse(kronProd(permutedims(dh_x), Ms)*dGp);
          dT3_3 = sparse(kronProd(sparse(1.0I(size(dh_x,2))), Ms*Gp)*dh_xx);
          dT3 = sparse(dT3_1 + dT3_2 + dT3_3);

          return dT3
        end
        dFc_xx = sparse(jacT1(x) + jacT2(x) + jacT3(x));

        # dFc_ux
        # # first term
        D1 = kronProd(permutedims(Gp*h), sparse(1.0I(size(Ms,1))));
        dGph_u = sparse(Gp*dh_u);
        dGph_u_t = dGph_u;
        dD1_u = fdJ_kronAB(permutedims(Gp*h), sparse(1.0I(size(Ms,1))), dGph_u_t, spzeros(size(dMs,1),length(u)));
        out1 = sparse(kronProd(permutedims(dMs), sparse(1.0I(size(D1,1))))*dD1_u);
        # # second term
        D2 = sparse(kronProd(permutedims(h), Ms)*dGp);
        dD2_u = fdJ_kronAB(permutedims(h), Ms, dh_u, spzeros(size(dMs,1), length(u)));
        out2 = sparse(kronProd(permutedims(dGp), sparse(1.0I(size(D2,1))))*dD2_u);
        # # third term
        out3 = sparse(kronProd(sparse(1.0I(size(dh_x,2))), Ms*Gp)*dh_ux);
        dFc_ux = sparse(out1 + out2 + out3);

        # dFc_xu
        out1 = sparse(kronProd(permutedims(Gp*dh_u), sparse(1.0I(size(Ms,1))))*dMs);
        out2 = sparse(kronProd(permutedims(dh_u), Ms)*dGp);
        out3 = sparse(kronProd(sparse(1.0I(length(u))), Ms*Gp)*dh_xu);
        dFc_xu = sparse(out1 + out2 + out3);

        # dFc_uu
        dFc_uu = sparse(kronProd(sparse(1.0I(size(dh_u,2))), Ms*Gp)*dh_uu);

        return dFc_xx, dFc_ux, dFc_xu, dFc_uu
      end
      dFc_xx, dFc_ux, dFc_xu, dFc_uu = Fcdiff2(xv,uv);

      function hessqdd(x,u)
        T1 = kronProd(permutedims(Fc + Fu), sparse(1.0I(size(M_inv,1))));
        dT1 = fdJ_kronAB(permutedims(Fc + Fu), sparse(1.0I(size(M_inv,1))), (dFc + dFu), spzeros(size(dM_inv)...));
        T2 = sparse(dFc + dFu);
        dT2 = sparse(dFc_xx + dFu_xx);

        out1 = sparse(kronProd(permutedims(dM_inv), sparse(1.0I(size(T1,1))))*dT1);
        out2 = sparse(kronProd(sparse(1.0I(size(dM_inv,2))), T1)*dMinv_xx);
        out3 = sparse(kronProd(permutedims(T2), sparse(1.0I(size(M_inv,1))))*dM_inv);
        out4 = sparse(kronProd(sparse(1.0I(size(T2,2))), M_inv)*dT2);

        dqdd_xx = sparse(out1 + out2 + out3 + out4);
        return dqdd_xx
      end
      dqdd_xx = hessqdd(xv,uv);
      # println("typeof(dqdd_xx) = ", typeof(dqdd_xx))

      function cross_qdd_ux(x,u)
        T1 = kronProd(permutedims(Fc + Fu), sparse(1.0I(size(M_inv,1))));
        dT1_u = fdJ_kronAB(permutedims(Fc + Fu), sparse(1.0I(size(M_inv,1))), dFc_u + dFu_u, spzeros(size(M_inv,1)^2, length(u)));

        T2 = sparse(dFc + dFu);
        dT2_u = sparse(dFc_ux + dFu_ux);

        out1 = sparse(kronProd(permutedims(dM_inv), sparse(1.0I(size(T1,1))))*dT1_u);
        out2 = sparse(kronProd(sparse(1.0I(length(x))), M_inv)*dT2_u);
        dqdd_ux = sparse(out1+out2);
        return dqdd_ux
      end
      dqdd_ux = cross_qdd_ux(xv,uv);
      # println("typeof(dqdd_ux) = ", typeof(dqdd_ux))

      function cross_qdd_xu(x,u)
        out1 = sparse(kronProd(permutedims(dFc_u + dFu_u), sparse(1.0I(size(M_inv,1))))*dM_inv);
        out2 = sparse(kronProd(sparse(1.0I(length(u))), M_inv)*(dFc_xu + dFu_xu));
        dqdd_xu = sparse(out1 + out2);
        return dqdd_xu
      end
      dqdd_xu = cross_qdd_xu(xv,uv);
      # println("typeof(dqdd_xu) = ", typeof(dqdd_xu))

      function hessqdd_u(x,u)
        dqdd_uu = sparse(kronProd(sparse(1.0I(length(u))), M_inv)*(dFc_uu + dFu_uu));
        return dqdd_uu
      end
      dqdd_uu = hessqdd_u(xv,uv);
      # println("typeof(dqdd_uu) = ", typeof(dqdd_uu))

      function hessDyn(xuVar)
        x = xuVar[1:14*nB]; u = reshape(xuVar[14*nB+1:end], (6,nB));

        y = qdd;
        dy = dqdd;

        d2y_xx = dqdd_xx;#  hessqdd(x,u);
        d2y_xu = dqdd_xu;# cross_qdd_xu(x,u);
        d2y_ux = dqdd_ux;# cross_qdd_ux(x,u);
        d2y_uu = dqdd_uu;# hessqdd_u(x,u);

        d2y = [d2y_xx d2y_ux; d2y_xu d2y_uu]
        # d2y = convert(SparseMatrixCSC{Float64},sparse([d2y_xx d2y_ux; d2y_xu d2y_uu]));

        out1 = sparse(kronProd(sparse(1.0I(length(xuVar))), permutedims(y))*d2y);
        # out1 = convert(SparseMatrixCSC{Float64}, out1);
        out2 = sparse(permutedims(dy)*dy);
        out = sparse(2*(out1 + out2)); # hessian of dynamics cost = sum(y.*y)
        return out;
      end
      dqdd_xuVar = hessDyn(xu_var);
      # println("typeof(dqdd_xuVar) = ", typeof(dqdd_xuVar))

      function hess_nonlinear_constraints()
        zLen = length(z);

        hess_dyn = hessDyn(xu_var); # hessian of dynamics cost
        _, hess_qNorm = quatNormConstr_diff(xu_var); # hessian of quatNormConstr cost
        _, hess_jLoc = jointLocConstr_diff(xu_var); # hessian of jointLocConstr cost

        hess_constr = hess_dyn +  hess_qNorm + hess_jLoc;

        # taking only free variables into consideration
        freeVarInd_hess = 14 .+ freeVarInd[1:end-1];
        hess_constr_free = hess_constr[freeVarInd_hess, freeVarInd_hess];

        out = zeros(typeof(hess_constr_free[1]),(zLen, zLen));
        out[1:zLen-1, 1:zLen - 1] = hess_constr_free;
        return out
      end
      out = hess_nonlinear_constraints();
    end
    return out
  end

  ## IpOpt functions
  function eval_f(z)
    J = z[end]
    return J
  end

  function eval_g(z, g)
    g[:] .= _nonlinear_constraints(z)
  end

  function eval_grad_f(z, grad_f)
    v = zeros(length(z)); v[end] = 1.0;
    grad_f[:] = v;
  end

  function eval_jac_g(z, mode, rows, cols, values)
    nvar = length(z);
    ng = 1;

    if mode == :Structure
      index = 1;
      for i=1:ng
          for j=1:nvar
              rows[index] = i;
              cols[index] = j;
              index += 1;
          end
      end
    else
      flag = 1;
      values[:] = grad_hess_nonlinear_constraints(z,flag);
    end
  end

  function eval_h(z, mode, rows, cols, obj_factor, lambda, values) # Hessian of cost.
    nvar = length(z);
    if mode == :Structure
      # Symmetric matrix, fill the lower left triangle only
      idx = 1
      for row = 1:nvar
        for col = 1:row
          rows[idx] = row
          cols[idx] = col
          idx += 1
        end
      end
    else
      # H = obj_factor*zeros(nvar, nvar)
      # for i=1:length(lambda)
      flag = 2;
      H = lambda[1]*grad_hess_nonlinear_constraints(z,flag);
      # end

      # Copy them into return variable -- only lower triangle.
      idx = 1;
      for row = 1:nvar
        for col = 1:row
          values[idx] = H[row,col];
          idx += 1;
        end
      end
    end
  end

  # Initial guess from freeVarInd
  n = length(freeVarInd);
  guessFull = [x0;u0[:,2:end][:];0.0];
  inGuess = guessFull[freeVarInd];

  nele_jac = Integer(m*n);
  nele_hess = Integer(n*(n+1)/2);
  prob = createProblem(n, lb, ub, m, lb_g, ub_g, nele_jac, nele_hess, eval_f, eval_g, eval_grad_f, eval_jac_g, eval_h);
  # Ipopt.addOption(prob, "linear_solver", "pardiso");
  prob.x = inGuess; # Initial guess.
  status = solveProblem(prob);

  trim_x = prob.x[1:length(x0)];
  gam = prob.x[end];
  trim_u = prob.x[length(x0)+1:end-1];
  return trim_x, trim_u, gam
end

## setup for pendulum
# m = 1.0; l = 1.0; # Mass and length of bar
# # Assuming bar revolves about Y axis
# I1 = [1 0 0; 0 m * l^2 / 12 0; 0 0 1];
# # Testing Dynamics with Revolute Joint
# R1 = RigidBody(m, I1, 2);
# RbI = InertialFrameAsRB()
#
# # Suspended at an angle theta from vertical
# theta = deg2rad(30)
# x_temp = [l/2*sin(theta); 0.0; -l/2*cos(theta)];
# x0R1 = ([x_temp;[1;zeros(3)];zeros(3);zeros(4)]);
# initialiseRigidBody!(R1,x0R1)
#
# # Axis about which bar is revolving
# axisY = [0.0 1.0 0.0][:];
#
# rj1 = [0.0 0.0 0.0][:]; # Joint Location in body frame of first body
# rj2 = -R1.x[1:3];
#
# j1 = Joint(RbI, R1, rj1, rj2, type="Revolute", axis=axisY);
# j = [j1]; # Joint tree for pendulum
# g = [0.0,0.0,-9.806]; # Gravity Vector.
# x0Orig, u0Orig = getXU_0(j); # Initial values
# nB = length(j) + 1; # number of bodies
# ix= zeros(Integer,20*(nB-1)+1); # indicates which variables are free for optimization
# ix[14*(nB-1)+1:20*(nB-1)] .= 1; ## keeping u constant, i.e., no force applied on pendulum
# trim_U = u0Orig;
# iy = zeros(Integer,7*(nB-1)); ## indicates which acceleration level terms need not be constrained. iy == 0 means acceleration is zero for corresponding state.
# out = trim_kronLazy(j,g,ix=ix, iy=iy);
# trim_x, trim_u, gam = out;
# println("trim_x = ", trim_x)
# trim_X = [j[1].RB1.x;trim_x]
# println("qconstr = ", norm(trim_x[4:7])-1)
# println("ẍ =", norm(fxdot(trim_X,trim_U, j, g)))

## setup for quadrotor
include("nRotor.jl");
j = gen_nRotor(4);
g =([0.0,0.0,-9.806]);
x0Orig, u0Orig = getXU_0(j); # Initial values
nB = length(j) + 1; # number of bodies
ix= zeros(Integer,20*(nB-1)+1); # indicates which variables are free for optimization
ix[14*(nB-1)+1:20*(nB-1)] .= 1; ## keeping u constant, i.e., hover
trim_U = u0Orig;
iy = zeros(Integer,7*(nB-1)); ## indicates which acceleration level terms need not be constrained. iy == 0 means acceleration is zero for corresponding state.
out = trim_kronLazy(j,g,ix=ix, iy=iy);
trim_x, trim_u, gam = out;
println("trim_x = ", trim_x)
trim_X = [j[1].RB1.x;trim_x]

println("qconstr = ", norm(trim_x[4:7])-1)
println("ẍ =", norm(fxdot(trim_X,trim_U, j, g)))

max_stars_repo_name: isrlab/Multibody-Dynamics

max_issues_repo_name: isrlab/Multibody-Dynamics

max_forks_repo_name: isrlab/Multibody-Dynamics


text:
@testset "Wagner" begin

    submodel = Submodel(Wagner())

    # test provided jacobians
    submodel_jacobian_tests(submodel)

    # test consistency of input/output functions
    submodel_io_tests(submodel)
end


text:
export hanncoeffs, hannsum, hannsum!, hannfilter

"""
    hanncoeffs(tin,tout,L)
# Arguments
- `tin`: input dataset time (vector valued)
- `tout`: time where filtered output will be created
- `L`: filter length
# Output
- `h`: normalized coefficients of Hann(ing) filter
"""
function hanncoeffs(tin,tout,L)
    ctin = collect(tin)
    δ = abs.(ctin .- tout)./(2.0.*L)
    h = 0 .*ctin
    #ih = findall(δ.< 0.5)
    for hh = 1:length(h)
        if δ[hh] < 0.5
            h[hh] = (cos.(π.*δ[hh])).^2
        end
    end
    h ./= sum(h)  # normalize
    return h
end

"""
    hannsum(θin,tin,tout,L)
    use this when a timeseries is available
# Arguments
- `θ`: input value at all times t (e.g., θ(t))
- `tout`: time where filtered output will be created
- `tin`: input dataset time (vector valued)
- `L`: filter length
# Output
- `θout`: output value at time th (e.g., θh(th))
"""
function hannsum(θin,tin,tout,L)
    # function hannsum(θin,tin,tout,L)

    h = hanncoeffs(tin,tout,L)

    # iterate over all filter coeffs
    i = 1
    nh = length(tin)
    θout = 0.0
    for tt = 1:nh
        θout += h[tt]*θin[tt]
    end
    return θout
end

"""
    hannsum!(θout,θin,tin,tout,L)
    use this when a timeseries is available
# Arguments
- `θout`: output value at time th (e.g., θh(th))
- `θin`: input value at all times t (e.g., θ(t))
- `tin`: input dataset time (vector valued)
- `tout`: time where filtered output will be created
- `L`: filter length
"""
function hannsum!(θout,θin,tin,tout,L)
    # function hannsum(θin,tin,tout,L)
    # use this when a timeseries is available
    #
    # θ  = input value at all times t (e.g., θ(t))
    # th = time where filtered output will be created
    # t  = input dataset time (vector valued)
    # L = filter length
    #
    # θh = output value at time th (e.g., θh(th))

    h = hanncoeffs(tin,tout,L)

    # iterate over all filter coeffs
    i = 1
    nh = length(tin)
    #θout = 0.0
    for tt = 1:nh
        θout += h[tt]*θin[tt]
    end
end

"""
    function hannsum!(θout,θin,tin,tout,toutindex,L::Float64)

     Call this version for mesharrays
# Arguments
- `θout::MeshArrays.gcmarray{Float32,2,Array{Float32,2}}`: output value at time tout (e.g., θout(tout))
- `θin::MeshArrays.gcmarray{Float32,2,Array{Float32,2}}`: input value at all times t (e.g., θ(t))
- `tin::StepRangeLen{Float64,Base.TwicePrecision{Float64},Base.TwicePrecision{Float64}}`: input dataset time (vector valued)
- `tout::StepRangeLen{Float64,Base.TwicePrecision{Float64},Base.TwicePrecision{Float64}}`: time where filtered output will be created
- `toutindex::Integer`: output times?
- `L::Float64`: filter length
"""
function hannsum!(θout::MeshArrays.gcmarray{Float32,2,Array{Float32,2}},θin::MeshArrays.gcmarray{Float32,2,Array{Float32,2}},tin::StepRangeLen{Float64,Base.TwicePrecision{Float64},Base.TwicePrecision{Float64}},tout::StepRangeLen{Float64,Base.TwicePrecision{Float64},Base.TwicePrecision{Float64}},toutindex::Integer,L::Float64)

    h = hanncoeffs(tin,tout[toutindex],L)

    # iterate over all filter coeffs
    i = 1
    nin = length(tin)
    for tt = 1:nin
        if h[tt] > 0
            θout[:,toutindex] += h[tt]*θin[:,tt]
        end
    end
end

"""
    function hannfilter(θin,tin,tout,L)
# Arguments
- `θin::Array{Float32,1}`: input
- `tin`
- `tout`
- `L`
# Output
- `θout`: output
"""
function hannfilter(θin::Array{Float32,1},tin,tout,L)
    # assumes all values available in timeseries
    # initialize θh

    nt = length(tin)
    nh = length(tout)
    θout = zeros(Float32,nh)
    println("calling simple hanningfilter")
    for tt= 1:nh
        hannsum!(θout[tt],θin,tin,tout[tt],L)
    end
    return θout
end

"""
    function hannfilter(θin,tin,tout,L,γ)
# Arguments
- `θin::MeshArrays.gcmarray{Float32,2,Array{Float32,2}}`
- `tin::StepRangeLen{Float64,Base.TwicePrecision{Float64},Base.TwicePrecision{Float64}}`
- `tout::StepRangeLen{Float64,Base.TwicePrecision{Float64},Base.TwicePrecision{Float64}}`
- `L::Float64`
- `γ::gcmgrid`
# Output
- `θout`
"""
function hannfilter(θin::MeshArrays.gcmarray{Float32,2,Array{Float32,2}},tin::StepRangeLen{Float64,Base.TwicePrecision{Float64},Base.TwicePrecision{Float64}},tout::StepRangeLen{Float64,Base.TwicePrecision{Float64},Base.TwicePrecision{Float64}},L::Float64,γ::gcmgrid)
    # use this for gcmarray
    # initialize θout

    nin = length(tin)
    nout = length(tout)
    #println(nin,nout)
    #θout = zeros(Float32,nh)
    θout = MeshArray(γ,Float32,nout) # some nans here
    tmp1=zeros(Float32,Tuple(γ.ioSize))

    for tt= 1:nout
        println(tt)
        # initialize on the fly.
        θout[:,tt]=γ.read(tmp1,MeshArray(γ,Float32))
        #println("call hanningsum!")
        hannsum!(θout,θin,tin,tout,tt,L)
    end
    return θout
end


text:
using Pkg
Pkg.activate(".")

using LinearAlgebra

include("FormuleNOp.jl")  # rename push! to push2! for this unit test
push2! = push!  # ça ne marche pas de renommer...
include("FormuleN2Op.jl") # define push! (more efficient)

using LinearOperators
n=5
y = [1; 2.0; 3; 1; 1]
s = [-1; 2.0; 1; 1; 1]

B1 = InverseBFGSOperator(Float64, n; scaling = false)
B2 = InverseBFGSOperator(Float64, n; scaling = false)

B1 = push!(B1,y,s)
B2 = push2!(B2,y,s)

LB = InverseLBFGSOperator(5, scaling = false, mem = 7);
LB = push!(LB,y,s);

using Test
@info "Premiers tests"
@test Matrix(B1) ≈ Matrix(B2)
@test Matrix(B1) ≈ Matrix(LB)

y = [-3;2.0;3;1;1]
s = [-1;2.0;-1;1;1]

B1 = push!(B1,y,s)
B2 = push2!(B2,y,s)
LB = push!(LB,y,s);

@info "Seconds tests"
@test Matrix(B1) ≈ Matrix(B2)
@test Matrix(B1) ≈ Matrix(LB)

let s=s, y=y, B1=B1, B2=B2, LB = LB
    for i=1:5
        @info i
        s = [s[5];s[1:4]]
        y = [y[5];y[1:4]]

        B1 = push!(B1,y,s)
        B2 = push2!(B2,y,s)
        LB = push!(LB,y,s);

        @info "B1=B2"
        @test Matrix(B1) ≈ Matrix(B2)
        @info "B1=LB"
        @test Matrix(B1) ≈ Matrix(LB)
    end
end

max_stars_repo_name: vepiteski/LSDescent

max_issues_repo_name: vepiteski/LSDescent

max_forks_repo_name: vepiteski/LSDescent


text:
module BitonicSort

"Bitonic sort"
function bitonic_sort!(a::AbstractVector, ascending::Bool=true)
    n = length(a)
    if n == 1
        return
    end
    @assert nextpow(2, n) == n "The length of `a` is not power of 2."
    half_n = n >>> 1

    bitonic_sort!(@view(a[1:half_n]), ascending)
    bitonic_sort!(@view(a[(half_n + 1):n]), !ascending)
    bitonic_merge!(a, ascending)
end

"""
Bitonic merge

**Precondition**: `a` is bitonic.
**Postcondition**: `a` is sorted in given order.
"""
function bitonic_merge!(a::AbstractVector, ascending::Bool=true)
    n = length(a)
    if n == 1
        return
    end
    half_n = n >>> 1

    for i in 1:half_n
        if xor(a[i] < a[i + half_n], ascending)
            a[i], a[i + half_n] = a[i + half_n], a[i]
        end
    end
    bitonic_merge!(@view(a[1:half_n]), ascending)
    bitonic_merge!(@view(a[(half_n + 1):n]), ascending)
end

################################################################

#====
Another bitonic sort implementation using OpenCL-like vectorized operations.
It can serve as a prototype of OpenCL code (or not, since Julia is 1-based).

# Reference

- OpenCL in Action, Matthew Scarpino, Chapter 11
====#

MASK1 = [2, 1, 4, 3]
SWAP = [0, 0, 1, 1]
ADD1 = [1, 1, 3, 3]
MASK2 = [3, 4, 1, 2]
ADD2 = [1, 2, 1, 2]
ADD3 = [1, 2, 3, 4]

# dir: 0 - ascending, 1 - descending
function sort_vector!(a::AbstractVector, dir::Int=0)
    @assert length(a) == 4
    comp = @. xor(Int(a > @view a[MASK1]), dir)
    @. a = @view a[xor(comp, SWAP) + ADD1]
    comp = @. xor(Int(a > @view a[MASK2]), dir)
    a .= view(a, comp * 2 + ADD2)
    comp = @. xor(Int(a > @view a[MASK1]), dir)
    a .= view(a, comp + ADD1)
end

function shuffle2(a::AbstractVector, b::AbstractVector, indices)
    n_a = length(a)
    [
        if i > n_a
            b[i - n_a]
        else
            a[i]
        end
        for i in indices
    ]
end

# dir: 0 - ascending, 1 - descending
function swap_vector!(a::AbstractVector, b::AbstractVector, dir::Int=0)
    @assert length(a) == length(b) == 4
    comp = @. xor(Int(a > b), dir) * 4 + ADD3
    a_copy = copy(a)
    a .= shuffle2(a, b, comp)
    b .= shuffle2(b, a_copy, comp)
end

function bsort8!(arr::AbstractVector, dir::Int=0)
    @assert length(arr) == 8
    a = view(arr, 1:4)
    b = view(arr, 5:8)

    sort_vector!(a, 0)
    sort_vector!(b, 1)
    swap_vector!(a, b, dir)
    sort_vector!(a, dir)
    sort_vector!(b, dir)
end

#===== A full bitonic sort =====#
#
# - bsort_init
# - bsort_stage_n
# - bsort_stage_0
# - bsort_merge
# - bsort_merge_last
#
# (Too lazy to implement it now.)

#===== Unit Tests =====#
using Test
using Random: seed!, shuffle, shuffle!

@testset "Bitonic sort" begin
    n = 2048
    seed!()
    a = collect(1:n)
    shuffle!(a)

    bitonic_sort!(a)
    @test issorted(a)

    a = repeat([1, 2], 64)
    bitonic_sort!(a)
    @test issorted(a)
end

@testset "bsort8" begin
    seed!()
    a = shuffle(1:8)
    bsort8!(a)
    @test issorted(a)
end

end # module

max_stars_repo_name: gyk/TrivialSolutions

max_issues_repo_name: gyk/TrivialSolutions

max_forks_repo_name: gyk/TrivialSolutions


text:
using NonlinearEigenproblemsTest
using Test
using NonlinearEigenproblems.RKHelper

@bench @testset "inpolygon" begin
    points = [x + y*im for x=-1:11 for y=-1:11]
    polyx = [0, 0, 5, 10, 10]
    polyy = [0, 10, 5, 10, 0]

    point_inside(p) = inpolygon(real(p), imag(p), polyx, polyy)

    function test_inside_count()
        inside = map(point_inside, points)
        @test length(inside) == 13*13
        @test sum(inside) == 96
    end

    # clock-wise
    test_inside_count()

    # counter-clock-wise
    reverse!(polyx)
    reverse!(polyy)
    test_inside_count()

    # edge cases
    @test !point_inside(NaN)
    @test !point_inside(NaN*im)
    @test !point_inside(Inf)
    @test !point_inside(Inf*im)
end


text:
using Flux
using Flux: onehotbatch, onecold, flatten
using Flux.Losses: logitcrossentropy
using Flux.Data: DataLoader
using Parameters: @with_kw
using Statistics: mean
using CUDA
using ProgressMeter: @showprogress
using Formatting

using DeepUncertainty
include("utils.jl")
include("models/mc_resnet.jl")

if CUDA.has_cuda()
    @info "CUDA is on"
    CUDA.allowscalar(false)
end

@with_kw mutable struct Args
    batchsize::Int = 128
    lr::Float64 = 0.1
    epochs::Int = 200
    valsplit::Float64 = 0.1
    sample_size = 10
    complexity_constant = 1e-8
    weight_decay = 5e-4
end

function accuracy(preds, labels)
    acc = sum(onecold(preds |> cpu) .== onecold(labels |> cpu))
    return acc
end

function test(args, loader, model)
    loss = 0
    acc = 0
    ece = 0
    entropy = 0
    ntot = 0
    for (x, y) in loader
        predictions = []
        x, y = x |> gpu, y |> gpu
        logits = model(x)

        # Loop through each model's predictions
        for ensemble = 1:args.sample_size
            logits = model(x)
            push!(predictions, logits)
        end

        # Get the mean predictions
        mean_preds = Flux.batch(predictions)
        mean_predictions = mean(mean_preds, dims = ndims(mean_preds))
        logits = dropdims(mean_predictions, dims = ndims(mean_predictions))

        n = size(logits)[end]
        loss += logitcrossentropy(logits, y) * n
        acc += accuracy(logits, y)
        ece += expected_calibration_error(logits |> cpu, onecold(y |> cpu)) * n
        entropy += mean(calculate_entropy(logits |> cpu)) * n
        ntot += n
    end

    mean_loss = loss / ntot |> round4
    mean_acc = acc / ntot * 100 |> round4
    mean_ece = ece / ntot |> round4
    mean_entropy = entropy / ntot |> round4

    @info (format(
        "Mean Loss: {} Mean Accuracy: {} Mean ECE: {} Mean Entropy: {}",
        mean_loss,
        mean_acc,
        mean_ece,
        mean_entropy,
    ))
    @info "==========================================================="
    return nothing
end

function train(; kws...)
    # Initialize the hyperparameters
    args = Args(; kws...)

    # Load the train, validation data
    train_loader, test_loader, ood_test_loader = get_data(args)

    @info("Constructing Model")
    m = MCResNet18(nclasses = 10) |> gpu

    ## Training
    # Defining the optimizer
    # opt = Nesterov(args.lr)
    steps_per_epoch = length(train_loader)
    steps = 5 .* steps_per_epoch
    opt =
        Scheduler(Cos(λ0 = 0.1, λ1 = 0., period = steps), Nesterov(args.lr))
    ps = Flux.params(m)
    sqnorm(x) = sum(abs2, x)

    test(args, test_loader, m)

    @info("Training....")

    # Starting to train models
    for epoch = 1:args.epochs
        @info (format("Epoch: {} Learning rate: {}", epoch, opt.optim.eta))

        loss_fn(x, y) = logitcrossentropy(m(x), y)

        @showprogress for (x, y) in train_loader
            x, y = x |> gpu, y |> gpu
            gs = Flux.gradient(ps) do
                loss_fn(x, y) + args.weight_decay * sum(sqnorm, Flux.params(m))
            end
            Flux.update!(opt, ps, gs)
        end
        test(args, test_loader, m)
        test(args, ood_test_loader, m)
    end
    test(args, test_loader, m)
    test(args, ood_test_loader, m)

    return m
end

m = train()


text:
module ResultTypes

export Result, ErrorResult, unwrap, unwrap_error

struct Result{T, E <: Exception}
    result::Union{Some{T}, Nothing}
    error::Union{E, Nothing}
end

"""
    Result(val::T, exception_type::Type{E}=Exception) -> Result{T, E}

Create a `Result` that could hold a value of type `T` or an exception of type `E`, and
store `val` in it.
If the exception type is not provided, the supertype `Exception` is used as `E`.
"""
Result(x::T) where {T} = Result{T, Exception}(Some(x), nothing)

function Result(x::T, ::Type{E}) where {T, E <: Exception}
    return Result{T, E}(Some(x), nothing)
end

# As of Julia 0.7, constructors no longer fall back to convert methods, so we can
# set that up manually to happen. Constructor methods that don't make sense will
# helpfully produce convert MethodErrors.
Result{T, E}(x) where {T, E <: Exception} = convert(Result{T, E}, x)

"""
    ErrorResult(::Type{T}, exception::E) -> Result{T, E}
    ErrorResult(::Type{T}, exception::AbstractString="") -> Result{T, ErrorException}

Create a `Result` that could hold a value of type `T` or an exception of type `E`, and
store `exception` in it.
If the `exception` is provided as text, it is wrapped in the generic `ErrorException`.
If no exception is provided, an `ErrorException` with an empty string is used.
If the type argument is not provided, `Any` is used.

`ErrorResult` is a convenience function for creating a `Result` and is not its own type.
"""
function ErrorResult(::Type{T}, e::E) where {T, E <: Exception}
    return Result{T, E}(nothing, e)
end

function ErrorResult(::Type{T}, e::AbstractString="") where T
    return Result{T, ErrorException}(nothing, ErrorException(e))
end

ErrorResult(e::Union{Exception, AbstractString}="") = ErrorResult(Any, e)

"""
    unwrap(result::Result{T, E}) -> T
    unwrap(val::T) -> T
    unwrap(::Type{T}, result_or_val) -> T

Assumes `result` holds a value of type `T` and returns it.
If `result` holds an exception instead, that exception is thrown.

If `unwrap`'s argument is not a `Result`, it is returned.

The two-argument form of `unwrap` calls `unwrap` on its second argument, then converts it to
type `T`.
"""
function unwrap(r::Result{T, E})::T where {T, E <: Exception}
    if r.result !== nothing
        return something(r.result)
    elseif r.error !== nothing
        throw(r.error)
    else
        error("Empty Result{$T, $E} type")
    end
end

unwrap(x) = x

# auto-converts to T
function unwrap(::Type{T}, x)::T where {T}
    return unwrap(x)
end

"""
    unwrap_error(result::Result{T, E}) -> E
    unwrap_error(exception::E) -> E

Assumes `result` holds an exception of type `E` and returns it.
If `result` holds a value instead, throw an exception.

If `unwrap_error`'s argument is an `Exception`, that exception is returned.
"""
function unwrap_error(r::Result{T, E})::E where {T, E <: Exception}
    if r.error !== nothing
        return r.error
    else
        error("$r is not an ErrorResult")
    end
end

unwrap_error(e::Exception) = e

function Base.promote_rule(
    ::Type{Result{S1, E1}},
    ::Type{Result{S2, E2}},
) where {S1, E1 <: Exception, S2, E2 <: Exception}
    return Result{promote_type(S1, S2), promote_type(E1, E2)}
end

# To avoid ambiguity errors. For example, when returning `Result` types from a map function
# we end up doing a `convert(::Type{ResultTypes.Result{S,E}}, ::ResultTypes.Result{S,E})`.
function Base.convert(::Type{Result{S, E}}, r::Result{S, E}) where {S, E <: Exception}
    return r
end

function Base.convert(::Type{Result{S, E}}, r::Result) where {S, E <: Exception}
    return promote_type(Result{S, E}, typeof(r))(r.result, r.error)
end

function Base.convert(::Type{Result{S, E}}, x::T) where {T, S, E <: Exception}
    return Result{S, E}(Some(convert(S, x)), nothing)
end

function Base.convert(::Type{Result{T, E}}, e::E) where {T, E <: Exception}
    return Result{T, E}(nothing, e)
end

function Base.show(io::IO, r::Result{T, E}) where {T, E <: Exception}
    if iserror(r)
        print(io, "ErrorResult(", T, ", ", unwrap_error(r), ")")
    else
        print(io, "Result(", unwrap(r), ")")
    end
end

"""
    ResultTypes.iserror(x) -> Bool

If `x` is an `Exception`, return `true`.
If `x` is an `ErrorResult` (a `Result` containing an `Exception`), return `true`.
Return `false` in all other cases.
"""
iserror(e::Exception) = true
iserror(r::Result) = r.error !== nothing
iserror(x) = false

end # module


text:
#  notebook(dir=pwd(),detached=true)

import Pkg
Pkg.activate(joinpath(@__DIR__, ".."))
using Revise
# include("jac-main.jl")
using JAC


text:
#
# Jerzy Wroczyński (nr. indeksu 250075)
# Obliczenia Naukowe, Laboratorium
# Lista 5.
#
module utilities

include("types.jl")
import .types.FL, .types.SquareSparseMatrix, .types.CustomSquareSparseMatrixRawPairs

using SparseArrays
using LinearAlgebra

"""
### Reads the `A` matrix from a given file.

### Arguments:
- `filename` — a string containing a path to the file

### Returns
— a sparse matrix containing contents of the provided file

"""
function Amatrix(filename::String, useCustomSparseMatrixSolution::Bool=true)

    lines = (x for x in eachline(filename))
    header = iterate(lines)[1]

    # read the header containing the size of the matrix and the size of submatrices
    n, l = [parse(Int, x) for x in split(header)]

    if n % l != 0
        throw(ErrorException("`n` is not divisible by `l`"))
    end

    let matrix
    end

    # initiate the matrix
    if useCustomSparseMatrixSolution
        v = Int(n/l)
        _size = (v-1) * l * 2 + l^2 * v
        matrix = CustomSquareSparseMatrixRawPairs(undef, _size)
    else
        matrix = spzeros(n, n)
    end

    # load the data into a sparse matrix
    i = 1
    for line in lines

        # parse
        x, y, v = [parse(FL, i) for i in split(line)]
        x = Int(x)
        y = Int(y)

        # save read value
        if useCustomSparseMatrixSolution
            matrix[i] = string(x, " ", y) => v
        else
            matrix[x,y] = v
        end

        i += 1
    end

    if useCustomSparseMatrixSolution
        return SquareSparseMatrix(matrix)
    else
        return matrix
    end

end

function bmatrix(filename::String)::Vector{FL}

    lines = (x for x in eachline(filename))
    header = iterate(lines)[1]

    n = parse(Int, header)

    b = []

    for line in lines

        append!(b, [parse(FL, line)])

    end

    return b

end

function relativeError(x̃::Vector)
    n = length(x̃)
    return (norm(ones(n) - x̃)/norm(ones(n)))
end

function writexvector(x̃::Vector, filename::String, attachRelativeError::Bool = false)::Nothing

    n = length(x̃)

    open(filename, "w") do f

        if attachRelativeError
            println(f, relativeError(x̃))
        end

        # write the contents of the `x` vector in `n` following lines
        for i in 1:n
            println(f, x̃[i])
        end

    end

end

end

max_stars_repo_name: jerry-sky/academic-notebook

max_issues_repo_name: jerry-sky/academic-notebook

max_forks_repo_name: jerry-sky/academic-notebook


text:
module Utils

using CUDA

function cuda(f)
    function g()
        f()
        nothing
    end
    CUDA.@cuda g()
end

end  # module