text:
@inline function eval_f_kernel(n::Int, scale::Float64, x,
                       param,
                       YffR::Float64, YffI::Float64,
                       YftR::Float64, YftI::Float64,
                       YttR::Float64, YttI::Float64,
                       YtfR::Float64, YtfI::Float64,
                       I, J)
                       #=
                       YffR::CuDeviceArray{Float64}, YffI::CuDeviceArray{Float64},
                       YftR::CuDeviceArray{Float64}, YftI::CuDeviceArray{Float64},
                       YttR::CuDeviceArray{Float64}, YttI::CuDeviceArray{Float64},
                       YtfR::CuDeviceArray{Float64}, YtfI::CuDeviceArray{Float64})
                       =#

    # All threads execute the same code.

    f = 0.0

    @inbounds for i=1:6
        f += param[i,I]*x[i]
    end
    @inbounds f += param[25,I]*x[9] + param[26,I]*x[10]
    @inbounds for i=1:6
        f += 0.5*(param[6+i,I]*(x[i] - param[12+i,I])^2)
    end
    @inbounds f += 0.5*(param[27,I]*(x[9] - param[29,I])^2 + param[28,I]*(x[10] - param[30,I])^2)

    @inbounds begin
        c1 = (x[1] - (YffR*x[5] + YftR*x[7] + YftI*x[8]))
        c2 = (x[2] - (-YffI*x[5] - YftI*x[7] + YftR*x[8]))
        c3 = (x[3] - (YttR*x[6] + YtfR*x[7] - YtfI*x[8]))
        c4 = (x[4] - (-YttI*x[6] - YtfI*x[7] - YtfR*x[8]))
        c5 = (x[7]^2 + x[8]^2 - x[5]*x[6])
        c6 = (x[9] - x[10] - atan(x[8], x[7]))

        f += param[19,I]*c1
        f += param[20,I]*c2
        f += param[21,I]*c3
        f += param[22,I]*c4
        f += param[23,I]*c5
        f += param[31,I]*c6

        raug = param[24,I]
        f += 0.5*raug*c1^2
        f += 0.5*raug*c2^2
        f += 0.5*raug*c3^2
        f += 0.5*raug*c4^2
        f += 0.5*raug*c5^2
        f += 0.5*raug*c6^2
    end

    f *= scale
    @synchronize
    return f
end

@inline function eval_grad_f_kernel(n::Int, scale::Float64,
                            x, g,
                            param,
                            YffR::Float64, YffI::Float64,
                            YftR::Float64, YftI::Float64,
                            YttR::Float64, YttI::Float64,
                            YtfR::Float64, YtfI::Float64,
                            I, J)

    # All threads execute the same code.
    tx = J
    ty = I

    @inbounds begin
        c1 = (x[1] - (YffR*x[5] + YftR*x[7] + YftI*x[8]))
        c2 = (x[2] - (-YffI*x[5] - YftI*x[7] + YftR*x[8]))
        c3 = (x[3] - (YttR*x[6] + YtfR*x[7] - YtfI*x[8]))
        c4 = (x[4] - (-YttI*x[6] - YtfI*x[7] - YtfR*x[8]))
        c5 = (x[7]^2 + x[8]^2 - x[5]*x[6])
        c6 = (x[9] - x[10] - atan(x[8], x[7]))

        g1 = param[1,I] + param[7,I]*(x[1] - param[13,I])
        g2 = param[2,I] + param[8,I]*(x[2] - param[14,I])
        g3 = param[3,I] + param[9,I]*(x[3] - param[15,I])
        g4 = param[4,I] + param[10,I]*(x[4] - param[16,I])
        g5 = param[5,I] + param[11,I]*(x[5] - param[17,I])
        g6 = param[6,I] + param[12,I]*(x[6] - param[18,I])

        g9 = param[25,I] + param[27,I]*(x[9] - param[29,I])
        g10 = param[26,I] + param[28,I]*(x[10] - param[30,I])

        raug = param[24,I]
        g1 += param[19,I] + raug*c1
        g2 += param[20,I] + raug*c2
        g3 += param[21,I] + raug*c3
        g4 += param[22,I] + raug*c4

        g5 += param[19,I]*(-YffR) + param[20,I]*(YffI) + param[23,I]*(-x[6]) +
                    raug*(-YffR)*c1 + raug*(YffI)*c2 + raug*(-x[6])*c5
        g6 += param[21,I]*(-YttR) + param[22,I]*(YttI) + param[23,I]*(-x[5]) +
                    raug*(-YttR)*c3 + raug*(YttI)*c4 + raug*(-x[5])*c5
        g7 = param[19,I]*(-YftR) + param[20,I]*(YftI) + param[21,I]*(-YtfR) +
                    param[22,I]*(YtfI) + param[23,I]*(2*x[7]) +
                    raug*(-YftR)*c1 + raug*(YftI)*c2 + raug*(-YtfR)*c3 +
                    raug*(YtfI)*c4 + raug*(2*x[7])*c5
        g8 = param[19,I]*(-YftI) + param[20,I]*(-YftR) + param[21,I]*(YtfI) +
                    param[22,I]*(YtfR) + param[23,I]*(2*x[8]) +
                    raug*(-YftI)*c1 + raug*(-YftR)*c2 + raug*(YtfI)*c3 +
                    raug*(YtfR)*c4 + raug*(2*x[8])*c5

        g7 += (param[31,I] + raug*c6)*(x[8] / (x[7]^2 + x[8]^2))
        g8 += (-((param[31,I] + raug*c6)*(x[7] / (x[7]^2 + x[8]^2))))
        g9 += param[31,I] + raug*c6
        g10 += (-(param[31,I] + raug*c6))

        if tx == 1 && ty == 1
            g[1] = scale*g1
            g[2] = scale*g2
            g[3] = scale*g3
            g[4] = scale*g4
            g[5] = scale*g5
            g[6] = scale*g6
            g[7] = scale*g7
            g[8] = scale*g8
            g[9] = scale*g9
            g[10] = scale*g10
        end
    end

    @synchronize
    return
end

@inline function eval_h_kernel(n::Int, scale::Float64,
                       x, A,
                       param,
                       YffR::Float64, YffI::Float64,
                       YftR::Float64, YftI::Float64,
                       YttR::Float64, YttI::Float64,
                       YtfR::Float64, YtfI::Float64,
                       I, J)

    # All threads execute the same code.
    tx = J
    ty = I

    @inbounds begin
        alrect = param[23,I]
        altheta = param[31,I]
        raug = param[24,I]
        c5 = (x[7]^2 + x[8]^2 - x[5]*x[6])
        c6 = (x[9] - x[10] - atan(x[8], x[7]))

        if tx == 1 && ty == 1
            # 1st column
            A[1,1] = scale*(param[7,I] + raug)
            A[5,1] = scale*(raug*(-YffR))
            A[7,1] = scale*(raug*(-YftR))
            A[8,1] = scale*(raug*(-YftI))

            # 2nd columns
            A[2,2] = scale*(param[8,I] + raug)
            A[5,2] = scale*(raug*(YffI))
            A[7,2] = scale*(raug*(YftI))
            A[8,2] = scale*(raug*(-YftR))

            # 3rd column
            A[3,3] = scale*(param[9,I] + raug)
            A[6,3] = scale*(raug*(-YttR))
            A[7,3] = scale*(raug*(-YtfR))
            A[8,3] = scale*(raug*(YtfI))

            # 4th column
            A[4,4] = scale*(param[10,I] + raug)
            A[6,4] = scale*(raug*(YttI))
            A[7,4] = scale*(raug*(YtfI))
            A[8,4] = scale*(raug*(YtfR))

            # 5th column
            A[5,5] = scale*(param[11,I] + raug*(YffR^2) + raug*(YffI^2) + raug*(x[6]^2))
            A[6,5] = scale*(-(alrect + raug*c5) + raug*(x[5]*x[6]))
            A[7,5] = scale*(raug*(YffR*YftR) + raug*(YffI*YftI) + raug*((-x[6])*(2*x[7])))
            A[8,5] = scale*(raug*(YffR*YftI) + raug*(YffI*(-YftR)) + raug*((-x[6])*(2*x[8])))

            # 6th column
            A[6,6] = scale*(param[12,I] + raug*(YttR^2) + raug*(YttI^2) + raug*(x[5]^2))
            A[7,6] = scale*(raug*(YttR*YtfR) + raug*(YttI*YtfI) + raug*((-x[5])*(2*x[7])))
            A[8,6] = scale*(raug*((-YttR)*YtfI) + raug*(YttI*YtfR) + raug*((-x[5])*(2*x[8])))

            # 7th column
            A[7,7] = scale*((alrect + raug*c5)*2 + raug*(YftR^2) + raug*(YftI^2) +
                raug*(YtfR^2) + raug*(YtfI^2) + raug*((2*x[7])*(2*x[7])) +
                (altheta + raug*c6)*((-2*x[7]*x[8]) / (x[7]^2 + x[8]^2)^2) +
                raug*(x[8] / (x[7]^2 + x[8]^2))^2)
            A[8,7] = scale*(raug*(YftR*YftI) + raug*(YftI*(-YftR)) + raug*((-YtfR)*YtfI) +
                raug*(YtfI*YtfR) + raug*((2*x[7])*(2*x[8])) +
                (altheta + raug*c6)*((x[7]^2 - x[8]^2)/(x[7]^2 + x[8]^2)^2) +
                raug*((x[8]/(x[7]^2 + x[8]^2))*(-x[7]/(x[7]^2 + x[8]^2))))
            A[9,7] = scale*(raug*(x[8]/(x[7]^2 + x[8]^2)))
            A[10,7] = scale*(-raug*(x[8]/(x[7]^2 + x[8]^2)))

            # 8th column
            A[8,8] = scale*((alrect + raug*c5)*2 + raug*(YftI^2) + raug*(YftR^2) +
                raug*(YtfI^2) + raug*(YtfR^2) + raug*((2*x[8])*(2*x[8])) +
                (altheta + raug*c6)*((2*x[7]*x[8]) / (x[7]^2 + x[8]^2)^2) +
                raug*(-x[7]/(x[7]^2 + x[8]^2))^2)
            A[9,8] = scale*(raug*(-x[7]/(x[7]^2 + x[8]^2)))
            A[10,8] = scale*(-raug*(-x[7]/(x[7]^2 + x[8]^2)))

            # 9th column
            A[9,9] = scale*(param[27,I] + raug)
            A[10,9] = scale*(-raug)

            # 10th column
            A[10,10] = scale*(param[28,I] + raug)
        end
    end

    @synchronize
    tx = J
    ty = I

    if tx <= n && ty == 1
        @inbounds for j=1:n
            if tx > j
                A[j,tx] = A[tx,j]
            end
        end
    end

    @synchronize
    return
end

function eval_f_kernel_cpu(I, scale, x, param, YffR, YffI, YftR, YftI, YttR, YttI, YtfR, YtfI)
    f = 0.0

    @inbounds begin
        f += sum(param[i,I]*x[i] for i=1:6)
        f += param[25,I]*x[9] + param[26,I]*x[10]
        f += 0.5*sum(param[6+i,I]*(x[i] - param[12+i,I])^2 for i=1:6)
        f += 0.5*(param[27,I]*(x[9] - param[29,I])^2 + param[28,I]*(x[10] - param[30,I])^2)

        c1 = (x[1] - (YffR[I]*x[5] + YftR[I]*x[7] + YftI[I]*x[8]))
        c2 = (x[2] - (-YffI[I]*x[5] - YftI[I]*x[7] + YftR[I]*x[8]))
        c3 = (x[3] - (YttR[I]*x[6] + YtfR[I]*x[7] - YtfI[I]*x[8]))
        c4 = (x[4] - (-YttI[I]*x[6] - YtfI[I]*x[7] - YtfR[I]*x[8]))
        c5 = (x[7]^2 + x[8]^2 - x[5]*x[6])
        c6 = (x[9] - x[10] - atan(x[8], x[7]))

        f += param[19,I]*c1
        f += param[20,I]*c2
        f += param[21,I]*c3
        f += param[22,I]*c4
        f += param[23,I]*c5
        f += param[31,I]*c6

        raug = param[24,I]
        f += 0.5*raug*c1^2
        f += 0.5*raug*c2^2
        f += 0.5*raug*c3^2
        f += 0.5*raug*c4^2
        f += 0.5*raug*c5^2
        f += 0.5*raug*c6^2
    end

    f *= scale
    return f
end

function eval_grad_f_kernel_cpu(I, scale, x, g, param, YffR, YffI, YftR, YftI, YttR, YttI, YtfR, YtfI)
    g .= 0.0

    @inbounds begin
        c1 = (x[1] - (YffR[I]*x[5] + YftR[I]*x[7] + YftI[I]*x[8]))
        c2 = (x[2] - (-YffI[I]*x[5] - YftI[I]*x[7] + YftR[I]*x[8]))
        c3 = (x[3] - (YttR[I]*x[6] + YtfR[I]*x[7] - YtfI[I]*x[8]))
        c4 = (x[4] - (-YttI[I]*x[6] - YtfI[I]*x[7] - YtfR[I]*x[8]))
        c5 = (x[7]^2 + x[8]^2 - x[5]*x[6])
        c6 = (x[9] - x[10] - atan(x[8], x[7]))

        for i=1:6
            g[i] += param[i,I] + param[6+i,I]*(x[i] - param[12+i,I])
        end

        g[9] += param[25,I] + param[27,I]*(x[9] - param[29,I])
        g[10] += param[26,I] + param[28,I]*(x[10] - param[30,I])

        raug = param[24,I]
        g[1] += param[19,I] + raug*c1
        g[2] += param[20,I] + raug*c2
        g[3] += param[21,I] + raug*c3
        g[4] += param[22,I] + raug*c4
        g[5] += param[19,I]*(-YffR[I]) + param[20,I]*(YffI[I]) + param[23,I]*(-x[6]) +
                raug*(-YffR[I])*c1 + raug*(YffI[I])*c2 + raug*(-x[6])*c5
        g[6] += param[21,I]*(-YttR[I]) + param[22,I]*(YttI[I]) + param[23,I]*(-x[5]) +
                raug*(-YttR[I])*c3 + raug*(YttI[I])*c4 + raug*(-x[5])*c5
        g[7] += param[19,I]*(-YftR[I]) + param[20,I]*(YftI[I]) + param[21,I]*(-YtfR[I]) +
                param[22,I]*(YtfI[I]) + param[23,I]*(2*x[7]) +
                raug*(-YftR[I])*c1 + raug*(YftI[I])*c2 + raug*(-YtfR[I])*c3 +
                raug*(YtfI[I])*c4 + raug*(2*x[7])*c5
        g[8] += param[19,I]*(-YftI[I]) + param[20,I]*(-YftR[I]) + param[21,I]*(YtfI[I]) +
                param[22,I]*(YtfR[I]) + param[23,I]*(2*x[8]) +
                raug*(-YftI[I])*c1 + raug*(-YftR[I])*c2 + raug*(YtfI[I])*c3 +
                raug*(YtfR[I])*c4 + raug*(2*x[8])*c5

        g[7] += (param[31,I] + raug*c6)*(x[8] / (x[7]^2 + x[8]^2))
        g[8] += (-((param[31,I] + raug*c6)*(x[7] / (x[7]^2 + x[8]^2))))
        g[9] += param[31,I] + raug*c6
        g[10] += (-(param[31,I] + raug*c6))

        for i=1:10
            g[i] *= scale
        end
    end

    return
end

function eval_h_kernel_cpu(I, x, mode, scale, rows, cols, lambda, values,
    param, YffR, YffI, YftR, YftI, YttR, YttI, YtfR, YtfI)

    @inbounds begin
        # Sparsity pattern of lower-triangular of Hessian.
        #     1   2   3   4   5   6   7   8   9   10
        #    ---------------------------------------
        # 1 | x
        # 2 |     x
        # 3 |         x
        # 4 |             x
        # 5 | x   x           x
        # 6 |         x   x   x   x
        # 7 | x   x   x   x   x   x   x
        # 8 | x   x   x   x   x   x   x   x
        # 9 |                         x   x   x
        # 10|                         x   x   x   x
        #    ---------------------------------------
        if mode == :Structure
            # This doesn't need parallel computation.
            # Move this routine somewhere else.

            nz = 1
            # 1st column
            rows[nz] = 1; cols[nz] = 1; nz += 1
            rows[nz] = 5; cols[nz] = 1; nz += 1
            rows[nz] = 7; cols[nz] = 1; nz += 1
            rows[nz] = 8; cols[nz] = 1; nz += 1

            # 2nd column
            rows[nz] = 2; cols[nz] = 2; nz += 1
            rows[nz] = 5; cols[nz] = 2; nz += 1
            rows[nz] = 7; cols[nz] = 2; nz += 1
            rows[nz] = 8; cols[nz] = 2; nz += 1

            # 3rd column
            rows[nz] = 3; cols[nz] = 3; nz += 1
            rows[nz] = 6; cols[nz] = 3; nz += 1
            rows[nz] = 7; cols[nz] = 3; nz += 1
            rows[nz] = 8; cols[nz] = 3; nz += 1

            # 4th column
            rows[nz] = 4; cols[nz] = 4; nz += 1
            rows[nz] = 6; cols[nz] = 4; nz += 1
            rows[nz] = 7; cols[nz] = 4; nz += 1
            rows[nz] = 8; cols[nz] = 4; nz += 1

            # 5th column
            rows[nz] = 5; cols[nz] = 5; nz += 1
            rows[nz] = 6; cols[nz] = 5; nz += 1
            rows[nz] = 7; cols[nz] = 5; nz += 1
            rows[nz] = 8; cols[nz] = 5; nz += 1

            # 6th column
            rows[nz] = 6; cols[nz] = 6; nz += 1
            rows[nz] = 7; cols[nz] = 6; nz += 1
            rows[nz] = 8; cols[nz] = 6; nz += 1

            # 7th column
            rows[nz] = 7; cols[nz] = 7; nz += 1
            rows[nz] = 8; cols[nz] = 7; nz += 1
            rows[nz] = 9; cols[nz] = 7; nz += 1
            rows[nz] = 10; cols[nz] = 7; nz += 1

            # 8th column
            rows[nz] = 8; cols[nz] = 8; nz += 1
            rows[nz] = 9; cols[nz] = 8; nz += 1
            rows[nz] = 10; cols[nz] = 8; nz += 1

            # 9th column
            rows[nz] = 9; cols[nz] = 9; nz += 1
            rows[nz] = 10; cols[nz] = 9; nz += 1

            # 10th column
            rows[nz] = 10; cols[nz] = 10; nz += 1
        else
            nz = 1
            alrect = param[23,I]
            altheta = param[31,I]
            raug = param[24,I]
            c5 = (x[7]^2 + x[8]^2 - x[5]*x[6])
            c6 = (x[9] - x[10] - atan(x[8], x[7]))

            # 1st column
            # (1,1)
            values[nz] = param[7,I] + raug
            nz += 1
            # (5,1)
            values[nz] = raug*(-YffR[I])
            nz += 1
            # (7,1)
            values[nz] = raug*(-YftR[I])
            nz += 1
            # (8,1)
            values[nz] = raug*(-YftI[I])
            nz += 1

            # 2nd columns
            # (2,2)
            values[nz] = param[8,I] + raug
            nz += 1
            # (5,2)
            values[nz] = raug*(YffI[I])
            nz += 1
            # (7,2)
            values[nz] = raug*(YftI[I])
            nz += 1
            # (8,2)
            values[nz] = raug*(-YftR[I])
            nz += 1

            # 3rd column
            # (3,3)
            values[nz] = param[9,I] + raug
            nz += 1
            # (6,3)
            values[nz] = raug*(-YttR[I])
            nz += 1
            # (7,3)
            values[nz] = raug*(-YtfR[I])
            nz += 1
            # (8,3)
            values[nz] = raug*(YtfI[I])
            nz += 1

            # 4th column
            # (4,4)
            values[nz] = param[10,I] + raug
            nz += 1
            # (6,4)
            values[nz] = raug*(YttI[I])
            nz += 1
            # (7,4)
            values[nz] = raug*(YtfI[I])
            nz += 1
            # (8,4)
            values[nz] = raug*(YtfR[I])
            nz += 1

            # 5th column
            # (5,5)
            values[nz] = param[11,I] + raug*(YffR[I]^2) + raug*(YffI[I]^2) + raug*(x[6]^2)
            nz += 1
            # (6,5)
            values[nz] = -(alrect + raug*c5) + raug*(x[5]*x[6])
            nz += 1
            # (7,5)
            values[nz] = raug*(YffR[I]*YftR[I]) + raug*(YffI[I]*YftI[I]) + raug*((-x[6])*(2*x[7]))
            nz += 1
            # (8,5)
            values[nz] = raug*(YffR[I]*YftI[I]) + raug*(YffI[I]*(-YftR[I])) + raug*((-x[6])*(2*x[8]))
            nz += 1

            # 6th column
            # (6,6)
            values[nz] = param[12,I] + raug*(YttR[I]^2) + raug*(YttI[I]^2) + raug*(x[5]^2)
            nz += 1
            # (7,6)
            values[nz] = raug*(YttR[I]*YtfR[I]) + raug*(YttI[I]*YtfI[I]) + raug*((-x[5])*(2*x[7]))
            nz += 1
            # (8,6)
            values[nz] = raug*((-YttR[I])*YtfI[I]) + raug*(YttI[I]*YtfR[I]) + raug*((-x[5])*(2*x[8]))
            nz += 1

            # 7th column
            # (7,7)
            values[nz] = (alrect + raug*c5)*2 + raug*(YftR[I]^2) + raug*(YftI[I]^2) +
                raug*(YtfR[I]^2) + raug*(YtfI[I]^2) + raug*((2*x[7])*(2*x[7]))
            values[nz] += (altheta + raug*c6)*((-2*x[7]*x[8]) / (x[7]^2 + x[8]^2)^2) # (l6 + rho*c6)*nabla^2_x7x7 c6
            values[nz] += raug*(x[8] / (x[7]^2 + x[8]^2))^2 # rho*(nabla_x7 c6)^2
            nz += 1
            # (8,7)
            values[nz] = raug*(YftR[I]*YftI[I]) + raug*(YftI[I]*(-YftR[I])) + raug*((-YtfR[I])*YtfI[I]) +
                raug*(YtfI[I]*YtfR[I]) + raug*((2*x[7])*(2*x[8]))
            values[nz] += (altheta + raug*c6)*((x[7]^2 - x[8]^2)/(x[7]^2 + x[8]^2)^2) # (l6 + rho*c6)*nabla^2_x8x7 c6
            values[nz] += raug*((x[8]/(x[7]^2 + x[8]^2))*(-x[7]/(x[7]^2 + x[8]^2))) # (rho*(nabla_x7 c6)*(nabla_x8 c6))
            nz += 1
            # (9,7)
            values[nz] = raug*(x[8]/(x[7]^2 + x[8]^2))
            nz += 1
            # (10,7)
            values[nz] = -raug*(x[8]/(x[7]^2 + x[8]^2))
            nz += 1

            # 8th column
            # (8,8)
            values[nz] = (alrect + raug*c5)*2 + raug*(YftI[I]^2) + raug*(YftR[I]^2) +
                raug*(YtfI[I]^2) + raug*(YtfR[I]^2) + raug*((2*x[8])*(2*x[8]))
            values[nz] += (altheta + raug*c6)*((2*x[7]*x[8]) / (x[7]^2 + x[8]^2)^2) # (l6 + rho*c6)*nabla^2_x8x8 c6
            values[nz] += raug*(-x[7]/(x[7]^2 + x[8]^2))^2
            nz += 1
            # (9,8)
            values[nz] = raug*(-x[7]/(x[7]^2 + x[8]^2))
            nz += 1
            # (10,8)
            values[nz] = -raug*(-x[7]/(x[7]^2 + x[8]^2))
            nz += 1

            # 9th column
            # (9,9)
            values[nz] = param[27,I] + raug
            nz += 1
            # (10,9)
            values[nz] = -raug
            nz += 1

            # 10th column
            # (10,10)
            values[nz] = param[28,I] + raug
            nz += 1

            for i=1:nz-1
                values[nz] *= scale
            end
        end
    end

    return
end

@inline function eval_f_polar_kernel(n::Int, shift::Int, scale::Float64, x,
                             param,
                             YffR::Float64, YffI::Float64,
                             YftR::Float64, YftI::Float64,
                             YttR::Float64, YttI::Float64,
                             YtfR::Float64, YtfI::Float64,
                             I_, J)
    I = I_ + shift
    f = 0.0

    @inbounds begin
        vi_vj_cos = x[1]*x[2]*cos(x[3] - x[4])
        vi_vj_sin = x[1]*x[2]*sin(x[3] - x[4])
        pij = YffR*x[1]^2 + YftR*vi_vj_cos + YftI*vi_vj_sin
        qij = -YffI*x[1]^2 - YftI*vi_vj_cos + YftR*vi_vj_sin
        pji = YttR*x[2]^2 + YtfR*vi_vj_cos - YtfI*vi_vj_sin
        qji = -YttI*x[2]^2 - YtfI*vi_vj_cos - YtfR*vi_vj_sin

        f += param[1,I]*pij
        f += param[2,I]*qij
        f += param[3,I]*pji
        f += param[4,I]*qji
        f += param[5,I]*x[1]^2
        f += param[6,I]*x[2]^2
        f += param[7,I]*x[3]
        f += param[8,I]*x[4]

        f += 0.5*(param[9,I]*(pij - param[17,I])^2)
        f += 0.5*(param[10,I]*(qij - param[18,I])^2)
        f += 0.5*(param[11,I]*(pji - param[19,I])^2)
        f += 0.5*(param[12,I]*(qji - param[20,I])^2)
        f += 0.5*(param[13,I]*(x[1]^2 - param[21,I])^2)
        f += 0.5*(param[14,I]*(x[2]^2 - param[22,I])^2)
        f += 0.5*(param[15,I]*(x[3] - param[23,I])^2)
        f += 0.5*(param[16,I]*(x[4] - param[24,I])^2)
    end

    f *= scale
    @synchronize

    return f
end

@inline function eval_grad_f_polar_kernel(n::Int, shift::Int, scale::Float64,
                                  x, g,
                                  param,
                                  YffR::Float64, YffI::Float64,
                                  YftR::Float64, YftI::Float64,
                                  YttR::Float64, YttI::Float64,
                                  YtfR::Float64, YtfI::Float64,
                                  I_, J)

    tx = J
    ty = I_
    I = I_ + shift

    @inbounds begin
        cos_ij = cos(x[3] - x[4])
        sin_ij = sin(x[3] - x[4])
        vi_vj_cos = x[1]*x[2]*cos_ij
        vi_vj_sin = x[1]*x[2]*sin_ij
        pij = YffR*x[1]^2 + YftR*vi_vj_cos + YftI*vi_vj_sin
        qij = -YffI*x[1]^2 - YftI*vi_vj_cos + YftR*vi_vj_sin
        pji = YttR*x[2]^2 + YtfR*vi_vj_cos - YtfI*vi_vj_sin
        qji = -YttI*x[2]^2 - YtfI*vi_vj_cos - YtfR*vi_vj_sin

        # Derivative with respect to vi.
        dpij_dx = 2*YffR*x[1] + YftR*x[2]*cos_ij + YftI*x[2]*sin_ij
        dqij_dx = -2*YffI*x[1] - YftI*x[2]*cos_ij + YftR*x[2]*sin_ij
        dpji_dx = YtfR*x[2]*cos_ij - YtfI*x[2]*sin_ij
        dqji_dx = -YtfI*x[2]*cos_ij - YtfR*x[2]*sin_ij

        g1 = param[1,I]*(dpij_dx)
        g1 += param[2,I]*(dqij_dx)
        g1 += param[3,I]*(dpji_dx)
        g1 += param[4,I]*(dqji_dx)
        g1 += param[5,I]*(2*x[1])
        g1 += param[9,I]*(pij - param[17,I])*dpij_dx
        g1 += param[10,I]*(qij - param[18,I])*dqij_dx
        g1 += param[11,I]*(pji - param[19,I])*dpji_dx
        g1 += param[12,I]*(qji - param[20,I])*dqji_dx
        g1 += param[13,I]*(x[1]^2 - param[21,I])*(2*x[1])

        # Derivative with respect to vj.
        dpij_dx = YftR*x[1]*cos_ij + YftI*x[1]*sin_ij
        dqij_dx = -YftI*x[1]*cos_ij + YftR*x[1]*sin_ij
        dpji_dx = 2*YttR*x[2] + YtfR*x[1]*cos_ij - YtfI*x[1]*sin_ij
        dqji_dx = -2*YttI*x[2] - YtfI*x[1]*cos_ij - YtfR*x[1]*sin_ij

        g2 = param[1,I]*(dpij_dx)
        g2 += param[2,I]*(dqij_dx)
        g2 += param[3,I]*(dpji_dx)
        g2 += param[4,I]*(dqji_dx)
        g2 += param[6,I]*(2*x[2])
        g2 += param[9,I]*(pij - param[17,I])*dpij_dx
        g2 += param[10,I]*(qij - param[18,I])*dqij_dx
        g2 += param[11,I]*(pji - param[19,I])*dpji_dx
        g2 += param[12,I]*(qji - param[20,I])*dqji_dx
        g2 += param[14,I]*(x[2]^2 - param[22,I])*(2*x[2])

        # Derivative with respect to ti.
        dpij_dx = -YftR*vi_vj_sin + YftI*vi_vj_cos
        dqij_dx = YftI*vi_vj_sin + YftR*vi_vj_cos
        dpji_dx = -YtfR*vi_vj_sin - YtfI*vi_vj_cos
        dqji_dx = YtfI*vi_vj_sin - YtfR*vi_vj_cos

        g3 = param[1,I]*(dpij_dx)
        g3 += param[2,I]*(dqij_dx)
        g3 += param[3,I]*(dpji_dx)
        g3 += param[4,I]*(dqji_dx)
        g3 += param[7,I]
        g3 += param[9,I]*(pij - param[17,I])*dpij_dx
        g3 += param[10,I]*(qij - param[18,I])*dqij_dx
        g3 += param[11,I]*(pji - param[19,I])*dpji_dx
        g3 += param[12,I]*(qji - param[20,I])*dqji_dx
        g3 += param[15,I]*(x[3] - param[23,I])

        # Derivative with respect to tj.

        g4 = param[1,I]*(-dpij_dx)
        g4 += param[2,I]*(-dqij_dx)
        g4 += param[3,I]*(-dpji_dx)
        g4 += param[4,I]*(-dqji_dx)
        g4 += param[8,I]
        g4 += param[9,I]*(pij - param[17,I])*(-dpij_dx)
        g4 += param[10,I]*(qij - param[18,I])*(-dqij_dx)
        g4 += param[11,I]*(pji - param[19,I])*(-dpji_dx)
        g4 += param[12,I]*(qji - param[20,I])*(-dqji_dx)
        g4 += param[16,I]*(x[4] - param[24,I])

        if tx == 1
            g[1] = scale*g1
            g[2] = scale*g2
            g[3] = scale*g3
            g[4] = scale*g4
        end
    end

    @synchronize

    return
end

@inline function eval_h_polar_kernel(n::Int, shift::Int, scale::Float64,
                             x, A,
                             param,
                             YffR::Float64, YffI::Float64,
                             YftR::Float64, YftI::Float64,
                             YttR::Float64, YttI::Float64,
                             YtfR::Float64, YtfI::Float64,
                             I_, J)

    tx = J
    ty = I_
    I = I_ + shift

    @inbounds begin
        cos_ij = cos(x[3] - x[4])
        sin_ij = sin(x[3] - x[4])
        vi_vj_cos = x[1]*x[2]*cos_ij
        vi_vj_sin = x[1]*x[2]*sin_ij
        pij = YffR*x[1]^2 + YftR*vi_vj_cos + YftI*vi_vj_sin
        qij = -YffI*x[1]^2 - YftI*vi_vj_cos + YftR*vi_vj_sin
        pji = YttR*x[2]^2 + YtfR*vi_vj_cos - YtfI*vi_vj_sin
        qji = -YttI*x[2]^2 - YtfI*vi_vj_cos - YtfR*vi_vj_sin

        if tx == 1
            # d2f_dvidvi

            dpij_dvi = 2*YffR*x[1] + YftR*x[2]*cos_ij + YftI*x[2]*sin_ij
            dqij_dvi = -2*YffI*x[1] - YftI*x[2]*cos_ij + YftR*x[2]*sin_ij
            dpji_dvi = YtfR*x[2]*cos_ij - YtfI*x[2]*sin_ij
            dqji_dvi = -YtfI*x[2]*cos_ij - YtfR*x[2]*sin_ij

            # l_pij * d2pij_dvidvi
            v = param[1,I]*(2*YffR)
            # l_qij * d2qij_dvidvi
            v += param[2,I]*(-2*YffI)
            # l_pji * d2pji_dvidvi = 0
            # l_qji * d2qji_dvidvi = 0
            # l_vi * 2
            v += 2*param[5,I]
            # rho_pij*(dpij_dvi)^2 + rho_pij*(pij - tilde_pij)*d2pij_dvidvi
            v += param[9,I]*(dpij_dvi)^2 + param[9,I]*(pij - param[17,I])*(2*YffR)
            # rho_qij*(dqij_dvi)^2 + rho_qij*(qij - tilde_qij)*d2qij_dvidvi
            v += param[10,I]*(dqij_dvi)^2 + param[10,I]*(qij - param[18,I])*(-2*YffI)
            # rho_pji*(dpji_dvi)^2 + rho_pji*(pji - tilde_pji)*d2pji_dvidvi
            v += param[11,I]*(dpji_dvi)^2
            # rho_qji*(dqji_dvi)^2
            v += param[12,I]*(dqji_dvi)^2
            # (2*rho_vi*vi)*(2*vi) + rho_vi*(vi^2 - tilde_vi)*2
            v += 4*param[13,I]*x[1]^2 + param[13,I]*(x[1]^2 - param[21,I])*2
            A[1,1] = scale*v

            # d2f_dvidvj

            dpij_dvj = YftR*x[1]*cos_ij + YftI*x[1]*sin_ij
            dqij_dvj = -YftI*x[1]*cos_ij + YftR*x[1]*sin_ij
            dpji_dvj = 2*YttR*x[2] + YtfR*x[1]*cos_ij - YtfI*x[1]*sin_ij
            dqji_dvj = -2*YttI*x[2] - YtfI*x[1]*cos_ij - YtfR*x[1]*sin_ij

            d2pij_dvidvj = YftR*cos_ij + YftI*sin_ij
            d2qij_dvidvj = -YftI*cos_ij + YftR*sin_ij
            d2pji_dvidvj = YtfR*cos_ij - YtfI*sin_ij
            d2qji_dvidvj = -YtfI*cos_ij - YtfR*sin_ij

            # l_pij * d2pij_dvidvj
            v = param[1,I]*(d2pij_dvidvj)
            # l_qij * d2qij_dvidvj
            v += param[2,I]*(d2qij_dvidvj)
            # l_pji * d2pji_dvidvj
            v += param[3,I]*(d2pji_dvidvj)
            # l_qji * d2qji_dvidvj
            v += param[4,I]*(d2qji_dvidvj)
            # rho_pij*(dpij_dvj)*dpij_dvi + rho_pij*(pij - tilde_pij)*(d2pij_dvidvj)
            v += param[9,I]*(dpij_dvj)*dpij_dvi + param[9,I]*(pij - param[17,I])*(d2pij_dvidvj)
            # rho_qij*(dqij_dvj)*dqij_dvi + rho_qij*(qij - tilde_qij)*(d2qij_dvidvj)
            v += param[10,I]*(dqij_dvj)*dqij_dvi + param[10,I]*(qij - param[18,I])*(d2qij_dvidvj)
            # rho_pji*(dpji_dvj)*dpji_dvi + rho_pji*(pji - tilde_pji)*(d2pji_dvidvj)
            v += param[11,I]*(dpji_dvj)*dpji_dvi + param[11,I]*(pji - param[19,I])*(d2pji_dvidvj)
            # rho_qji*(dqji_dvj)*dqji_dvi + rho_qji*(qji - tilde_qji)*(d2qji_dvidvj)
            v += param[12,I]*(dqji_dvj)*dqji_dvi + param[12,I]*(qji - param[20,I])*(d2qji_dvidvj)
            A[2,1] = scale*v
            A[1,2] = scale*v

            # d2f_dvidti

            dpij_dti = -YftR*vi_vj_sin + YftI*vi_vj_cos
            dqij_dti = YftI*vi_vj_sin + YftR*vi_vj_cos
            dpji_dti = -YtfR*vi_vj_sin - YtfI*vi_vj_cos
            dqji_dti = YtfI*vi_vj_sin - YtfR*vi_vj_cos

            d2pij_dvidti = -YftR*x[2]*sin_ij + YftI*x[2]*cos_ij
            d2qij_dvidti = YftI*x[2]*sin_ij + YftR*x[2]*cos_ij
            d2pji_dvidti = -YtfR*x[2]*sin_ij - YtfI*x[2]*cos_ij
            d2qji_dvidti = YtfI*x[2]*sin_ij - YtfR*x[2]*cos_ij

            # l_pij * d2pij_dvidti
            v = param[1,I]*(d2pij_dvidti)
            # l_qij * d2qij_dvidti
            v += param[2,I]*(d2qij_dvidti)
            # l_pji * d2pji_dvidti
            v += param[3,I]*(d2pji_dvidti)
            # l_qji * d2qji_dvidti
            v += param[4,I]*(d2qji_dvidti)
            # rho_pij*(dpij_dti)*dpij_dvi + rho_pij*(pij - tilde_pij)*(d2pij_dvidti)
            v += param[9,I]*(dpij_dti)*dpij_dvi + param[9,I]*(pij - param[17,I])*(d2pij_dvidti)
            # rho_qij*(dqij_dti)*dqij_dvi + rho_qij*(qij - tilde_qij)*(d2qij_dvidti)
            v += param[10,I]*(dqij_dti)*dqij_dvi + param[10,I]*(qij - param[18,I])*(d2qij_dvidti)
            # rho_pji*(dpji_dti)*dpji_dvi + rho_pji*(pji - tilde_pji)*(d2pji_dvidti)
            v += param[11,I]*(dpji_dti)*dpji_dvi + param[11,I]*(pji - param[19,I])*(d2pji_dvidti)
            # rho_qji*(dqji_dti)*dqji_dvi + rho_qji*(qji - tilde_qji)*(d2qji_dvidti)
            v += param[12,I]*(dqji_dti)*dqji_dvi + param[12,I]*(qji - param[20,I])*(d2qji_dvidti)
            A[3,1] = scale*v
            A[1,3] = scale*v

            # d2f_dvidtj

            # l_pij * d2pij_dvidtj
            v = param[1,I]*(-d2pij_dvidti)
            # l_qij * d2qij_dvidtj
            v += param[2,I]*(-d2qij_dvidti)
            # l_pji * d2pji_dvidtj
            v += param[3,I]*(-d2pji_dvidti)
            # l_qji * d2qji_dvidtj
            v += param[4,I]*(-d2qji_dvidti)
            # rho_pij*(dpij_dtj)*dpij_dvi + rho_pij*(pij - tilde_pij)*(d2pij_dvidtj)
            v += param[9,I]*(-dpij_dti)*dpij_dvi + param[9,I]*(pij - param[17,I])*(-d2pij_dvidti)
            # rho_qij*(dqij_dtj)*dqij_dvi + rho_qij*(qij - tilde_qij)*(d2qij_dvidtj)
            v += param[10,I]*(-dqij_dti)*dqij_dvi + param[10,I]*(qij - param[18,I])*(-d2qij_dvidti)
            # rho_pji*(dpji_dtj)*dpji_dvi + rho_pji*(pji - tilde_pji)*(d2pji_dvidtj)
            v += param[11,I]*(-dpji_dti)*dpji_dvi + param[11,I]*(pji - param[19,I])*(-d2pji_dvidti)
            # rho_qji*(dqji_dtj)*dqji_dvi + rho_qji*(qji - tilde_qji)*(d2qji_dvidtj)
            v += param[12,I]*(-dqji_dti)*dqji_dvi + param[12,I]*(qji - param[20,I])*(-d2qji_dvidti)
            A[4,1] = scale*v
            A[1,4] = scale*v

            # d2f_dvjdvj

            # l_pij * d2pij_dvjdvj = l_qij * d2qij_dvjdvj = 0 since d2pij_dvjdvj = d2qij_dvjdvj = 0
            # l_pji * d2pji_dvjdvj
            v = param[3,I]*(2*YttR)
            # l_qji * d2qji_dvjdvj
            v += param[4,I]*(-2*YttI)
            # l_vj * 2
            v += param[6,I]*2
            # rho_pij*(dpij_dvj)^2 + rho_pij*(pij - tilde_pij)*(d2pij_dvjdvj)
            v += param[9,I]*(dpij_dvj)^2
            # rho_qij*(dqij_dvj)^2 + rho_qij*(qij - tilde_qij)*(d2qij_dvjdvj)
            v += param[10,I]*(dqij_dvj)^2
            # rho_pji*(dpji_dvj)^2 + rho_pji*(pji - tilde_pji)*(d2pji_dvjdvj)
            v += param[11,I]*(dpji_dvj)^2 + param[11,I]*(pji - param[19,I])*(2*YttR)
            # rho_qji*(dqji_dvj)^2 + rho_qji*(qji - tilde_qji)*(d2qji_dvjdvj)
            v += param[12,I]*(dqji_dvj)^2 + param[12,I]*(qji - param[20,I])*(-2*YttI)
            # (2*rho_vj*vj)*(2*vj) + rho_vj*(vj^2 - tilde_vj)*2
            v += 4*param[14,I]*x[2]^2 + param[14,I]*(x[2]^2 - param[22,I])*2
            A[2,2] = scale*v

            # d2f_dvjdti

            d2pij_dvjdti = (-YftR*x[1]*sin_ij + YftI*x[1]*cos_ij)
            d2qij_dvjdti = (YftI*x[1]*sin_ij + YftR*x[1]*cos_ij)
            d2pji_dvjdti = (-YtfR*x[1]*sin_ij - YtfI*x[1]*cos_ij)
            d2qji_dvjdti = (YtfI*x[1]*sin_ij - YtfR*x[1]*cos_ij)

            # l_pij * d2pij_dvjdti
            v = param[1,I]*(d2pij_dvjdti)
            # l_qij * d2qij_dvjdti
            v += param[2,I]*(d2qij_dvjdti)
            # l_pji * d2pji_dvjdti
            v += param[3,I]*(d2pji_dvjdti)
            # l_qji * d2qji_dvjdti
            v += param[4,I]*(d2qji_dvjdti)
            # rho_pij*(dpij_dti)*dpij_dvj + rho_pij*(pij - tilde_pij)*(d2pij_dvjdti)
            v += param[9,I]*(dpij_dti)*dpij_dvj + param[9,I]*(pij - param[17,I])*d2pij_dvjdti
            # rho_qij*(dqij_dti)*dqij_dvj + rho_qij*(qij - tilde_qij)*(d2qij_dvjdti)
            v += param[10,I]*(dqij_dti)*dqij_dvj + param[10,I]*(qij - param[18,I])*d2qij_dvjdti
            # rho_pji*(dpji_dti)*dpji_dvj + rho_pji*(pji - tilde_pji)*(d2pji_dvjdti)
            v += param[11,I]*(dpji_dti)*dpji_dvj + param[11,I]*(pji - param[19,I])*d2pji_dvjdti
            # rho_qji*(dqji_dti)*dqji_dvj + rho_qji*(qji - tilde_qji)*(d2qji_dvjdti)
            v += param[12,I]*(dqji_dti)*dqji_dvj + param[12,I]*(qji - param[20,I])*d2qji_dvjdti
            A[3,2] = scale*v
            A[2,3] = scale*v

            # d2f_dvjdtj

            # l_pij * d2pij_dvjdtj
            v = param[1,I]*(-d2pij_dvjdti)
            # l_qij * d2qij_dvjdtj
            v += param[2,I]*(-d2qij_dvjdti)
            # l_pji * d2pji_dvjdtj
            v += param[3,I]*(-d2pji_dvjdti)
            # l_qji * d2qji_dvjdtj
            v += param[4,I]*(-d2qji_dvjdti)
            # rho_pij*(dpij_dtj)*dpij_dvj + rho_pij*(pij - tilde_pij)*(d2pij_dvjdtj)
            v += param[9,I]*(-dpij_dti)*dpij_dvj + param[9,I]*(pij - param[17,I])*(-d2pij_dvjdti)
            # rho_qij*(dqij_dtj)*dqij_dvj + rho_qij*(qij - tilde_qij)*(d2qij_dvjdtj)
            v += param[10,I]*(-dqij_dti)*dqij_dvj + param[10,I]*(qij - param[18,I])*(-d2qij_dvjdti)
            # rho_pji*(dpji_dtj)*dpji_dvj + rho_pji*(pji - tilde_pji)*(d2pji_dvjdtj)
            v += param[11,I]*(-dpji_dti)*dpji_dvj + param[11,I]*(pji - param[19,I])*(-d2pji_dvjdti)
            # rho_qji*(dqji_dtj)*dqji_dvj + rho_qji*(qji - tilde_qji)*(d2qji_dvjdtj)
            v += param[12,I]*(-dqji_dti)*dqji_dvj + param[12,I]*(qji - param[20,I])*(-d2qji_dvjdti)
            A[4,2] = scale*v
            A[2,4] = scale*v

            # d2f_dtidti

            d2pij_dtidti = (-YftR*vi_vj_cos - YftI*vi_vj_sin)
            d2qij_dtidti = (YftI*vi_vj_cos - YftR*vi_vj_sin)
            d2pji_dtidti = (-YtfR*vi_vj_cos + YtfI*vi_vj_sin)
            d2qji_dtidti = (YtfI*vi_vj_cos + YtfR*vi_vj_sin)

            # l_pij * d2pij_dtidti
            v = param[1,I]*(d2pij_dtidti)
            # l_qij * d2qij_dtidti
            v += param[2,I]*(d2qij_dtidti)
            # l_pji * d2pji_dtidti
            v += param[3,I]*(d2pji_dtidti)
            # l_qji * d2qji_dtidti
            v += param[4,I]*(d2qji_dtidti)
            # rho_pij*(dpij_dti)^2 + rho_pij*(pij - tilde_pij)*(d2pij_dtidti)
            v += param[9,I]*(dpij_dti)^2 + param[9,I]*(pij - param[17,I])*(d2pij_dtidti)
            # rho_qij*(dqij_dti)^2 + rho_qij*(qij - tilde_qij)*(d2qij_dtidti)
            v += param[10,I]*(dqij_dti)^2 + param[10,I]*(qij - param[18,I])*(d2qij_dtidti)
            # rho_pji*(dpji_dti)^2 + rho_pji*(pji - tilde_pji)*(d2pji_dtidti)
            v += param[11,I]*(dpji_dti)^2 + param[11,I]*(pji - param[19,I])*(d2pji_dtidti)
            # rho_qji*(dqji_dti)^2 + rho_qji*(qji - tilde_qji)*(d2qji_dtidti)
            v += param[12,I]*(dqji_dti)^2 + param[12,I]*(qji - param[20,I])*(d2qji_dtidti)
            # rho_ti
            v += param[15,I]
            A[3,3] = scale*v

            # d2f_dtidtj

            # l_pij * d2pij_dtidtj
            v = param[1,I]*(-d2pij_dtidti)
            # l_qij * d2qij_dtidtj
            v += param[2,I]*(-d2qij_dtidti)
            # l_pji * d2pji_dtidtj
            v += param[3,I]*(-d2pji_dtidti)
            # l_qji * d2qji_dtidtj
            v += param[4,I]*(-d2qji_dtidti)
            # rho_pij*(dpij_dtj)*dpij_dti + rho_pij*(pij - tilde_pij)*(d2pij_dtidtj)
            v += param[9,I]*(-dpij_dti^2) + param[9,I]*(pij - param[17,I])*(-d2pij_dtidti)
            # rho_qij*(dqij_dtj)*dqij_dti + rho_qij*(qij - tilde_qij)*(d2qij_dtidtj)
            v += param[10,I]*(-dqij_dti^2) + param[10,I]*(qij - param[18,I])*(-d2qij_dtidti)
            # rho_pji*(dpji_dtj)*dpji_dti + rho_pji*(pji - tilde_pji)*(d2pji_dtidtj)
            v += param[11,I]*(-dpji_dti^2) + param[11,I]*(pji - param[19,I])*(-d2pji_dtidti)
            # rho_qji*(dqji_dtj)*dqji_dti + rho_qji*(qji - tilde_qji)*(d2qji_dtidtj)
            v += param[12,I]*(-dqji_dti^2) + param[12,I]*(qji - param[20,I])*(-d2qji_dtidti)
            A[4,3] = scale*v
            A[3,4] = scale*v

            # d2f_dtjdtj
            # l_pij * d2pij_dtjdtj
            v = param[1,I]*(d2pij_dtidti)
            # l_qij * d2qij_dtjdtj
            v += param[2,I]*(d2qij_dtidti)
            # l_pji * d2pji_dtjdtj
            v += param[3,I]*(d2pji_dtidti)
            # l_qji * d2qji_dtjdtj
            v += param[4,I]*(d2qji_dtidti)
            # rho_pij*(dpij_dtj)^2 + rho_pij*(pij - tilde_pij)*(d2pij_dtjdtj)
            v += param[9,I]*(dpij_dti^2) + param[9,I]*(pij - param[17,I])*(d2pij_dtidti)
            # rho_qij*(dqij_dtj)^2 + rho_qij*(qij - tilde_qij)*(d2qij_dtjdtj)
            v += param[10,I]*(dqij_dti^2) + param[10,I]*(qij - param[18,I])*(d2qij_dtidti)
            # rho_pji*(dpji_dtj)^2 + rho_pji*(pji - tilde_pji)*(d2pji_dtjdtj)
            v += param[11,I]*(dpji_dti^2) + param[11,I]*(pji - param[19,I])*(d2pji_dtidti)
            # rho_qji*(dqji_dtj)^2 + rho_qji*(qji - tilde_qji)*(d2qji_dtjdtj)
            v += param[12,I]*(dqji_dti^2) + param[12,I]*(qji - param[20,I])*(d2qji_dtidti)
            # rho_tj
            v += param[16,I]
            A[4,4] = scale*v
        end
    end

    @synchronize

    return
end

function eval_f_polar_kernel_cpu(I, scale, x, param, _YffR, _YffI, _YftR, _YftI, _YttR, _YttI, _YtfR, _YtfI)
    f = 0.0

    @inbounds begin
        YffR = _YffR[I]; YffI = _YffI[I]
        YftR = _YftR[I]; YftI = _YftI[I]
        YttR = _YttR[I]; YttI = _YttI[I]
        YtfR = _YtfR[I]; YtfI = _YtfI[I]

        vi_vj_cos = x[1]*x[2]*cos(x[3] - x[4])
        vi_vj_sin = x[1]*x[2]*sin(x[3] - x[4])
        pij = YffR*x[1]^2 + YftR*vi_vj_cos + YftI*vi_vj_sin
        qij = -YffI*x[1]^2 - YftI*vi_vj_cos + YftR*vi_vj_sin
        pji = YttR*x[2]^2 + YtfR*vi_vj_cos - YtfI*vi_vj_sin
        qji = -YttI*x[2]^2 - YtfI*vi_vj_cos - YtfR*vi_vj_sin

        f += param[1,I]*pij
        f += param[2,I]*qij
        f += param[3,I]*pji
        f += param[4,I]*qji
        f += param[5,I]*x[1]^2
        f += param[6,I]*x[2]^2
        f += param[7,I]*x[3]
        f += param[8,I]*x[4]

        f += 0.5*(param[9,I]*(pij - param[17,I])^2)
        f += 0.5*(param[10,I]*(qij - param[18,I])^2)
        f += 0.5*(param[11,I]*(pji - param[19,I])^2)
        f += 0.5*(param[12,I]*(qji - param[20,I])^2)
        f += 0.5*(param[13,I]*(x[1]^2 - param[21,I])^2)
        f += 0.5*(param[14,I]*(x[2]^2 - param[22,I])^2)
        f += 0.5*(param[15,I]*(x[3] - param[23,I])^2)
        f += 0.5*(param[16,I]*(x[4] - param[24,I])^2)
    end

    f *= scale
    return f
end

function eval_grad_f_polar_kernel_cpu(I, scale, x, g, param, _YffR, _YffI, _YftR, _YftI, _YttR, _YttI, _YtfR, _YtfI)
    g .= 0.0

    @inbounds begin
        YffR = _YffR[I]; YffI = _YffI[I]
        YftR = _YftR[I]; YftI = _YftI[I]
        YttR = _YttR[I]; YttI = _YttI[I]
        YtfR = _YtfR[I]; YtfI = _YtfI[I]

        cos_ij = cos(x[3] - x[4])
        sin_ij = sin(x[3] - x[4])
        vi_vj_cos = x[1]*x[2]*cos_ij
        vi_vj_sin = x[1]*x[2]*sin_ij
        pij = YffR*x[1]^2 + YftR*vi_vj_cos + YftI*vi_vj_sin
        qij = -YffI*x[1]^2 - YftI*vi_vj_cos + YftR*vi_vj_sin
        pji = YttR*x[2]^2 + YtfR*vi_vj_cos - YtfI*vi_vj_sin
        qji = -YttI*x[2]^2 - YtfI*vi_vj_cos - YtfR*vi_vj_sin

        # Derivative with respect to vi.
        dpij_dx = 2*YffR*x[1] + YftR*x[2]*cos_ij + YftI*x[2]*sin_ij
        dqij_dx = -2*YffI*x[1] - YftI*x[2]*cos_ij + YftR*x[2]*sin_ij
        dpji_dx = YtfR*x[2]*cos_ij - YtfI*x[2]*sin_ij
        dqji_dx = -YtfI*x[2]*cos_ij - YtfR*x[2]*sin_ij

        g[1] = param[1,I]*(dpij_dx)
        g[1] += param[2,I]*(dqij_dx)
        g[1] += param[3,I]*(dpji_dx)
        g[1] += param[4,I]*(dqji_dx)
        g[1] += param[5,I]*(2*x[1])
        g[1] += param[9,I]*(pij - param[17,I])*dpij_dx
        g[1] += param[10,I]*(qij - param[18,I])*dqij_dx
        g[1] += param[11,I]*(pji - param[19,I])*dpji_dx
        g[1] += param[12,I]*(qji - param[20,I])*dqji_dx
        g[1] += param[13,I]*(x[1]^2 - param[21,I])*(2*x[1])

        # Derivative with respect to vj.
        dpij_dx = YftR*x[1]*cos_ij + YftI*x[1]*sin_ij
        dqij_dx = -YftI*x[1]*cos_ij + YftR*x[1]*sin_ij
        dpji_dx = 2*YttR*x[2] + YtfR*x[1]*cos_ij - YtfI*x[1]*sin_ij
        dqji_dx = -2*YttI*x[2] - YtfI*x[1]*cos_ij - YtfR*x[1]*sin_ij

        g[2] = param[1,I]*(dpij_dx)
        g[2] += param[2,I]*(dqij_dx)
        g[2] += param[3,I]*(dpji_dx)
        g[2] += param[4,I]*(dqji_dx)
        g[2] += param[6,I]*(2*x[2])
        g[2] += param[9,I]*(pij - param[17,I])*dpij_dx
        g[2] += param[10,I]*(qij - param[18,I])*dqij_dx
        g[2] += param[11,I]*(pji - param[19,I])*dpji_dx
        g[2] += param[12,I]*(qji - param[20,I])*dqji_dx
        g[2] += param[14,I]*(x[2]^2 - param[22,I])*(2*x[2])

        # Derivative with respect to ti.
        dpij_dx = -YftR*vi_vj_sin + YftI*vi_vj_cos
        dqij_dx = YftI*vi_vj_sin + YftR*vi_vj_cos
        dpji_dx = -YtfR*vi_vj_sin - YtfI*vi_vj_cos
        dqji_dx = YtfI*vi_vj_sin - YtfR*vi_vj_cos

        g[3] = param[1,I]*(dpij_dx)
        g[3] += param[2,I]*(dqij_dx)
        g[3] += param[3,I]*(dpji_dx)
        g[3] += param[4,I]*(dqji_dx)
        g[3] += param[7,I]
        g[3] += param[9,I]*(pij - param[17,I])*dpij_dx
        g[3] += param[10,I]*(qij - param[18,I])*dqij_dx
        g[3] += param[11,I]*(pji - param[19,I])*dpji_dx
        g[3] += param[12,I]*(qji - param[20,I])*dqji_dx
        g[3] += param[15,I]*(x[3] - param[23,I])

        # Derivative with respect to tj.

        g[4] = param[1,I]*(-dpij_dx)
        g[4] += param[2,I]*(-dqij_dx)
        g[4] += param[3,I]*(-dpji_dx)
        g[4] += param[4,I]*(-dqji_dx)
        g[4] += param[8,I]
        g[4] += param[9,I]*(pij - param[17,I])*(-dpij_dx)
        g[4] += param[10,I]*(qij - param[18,I])*(-dqij_dx)
        g[4] += param[11,I]*(pji - param[19,I])*(-dpji_dx)
        g[4] += param[12,I]*(qji - param[20,I])*(-dqji_dx)
        g[4] += param[16,I]*(x[4] - param[24,I])

        for i=1:4
            g[i] *= scale
        end
    end
end

function eval_h_polar_kernel_cpu(I, x, mode, scale, rows, cols, lambda, values,
    param, _YffR, _YffI, _YftR, _YftI, _YttR, _YttI, _YtfR, _YtfI)

    @inbounds begin
        # Sparsity pattern of lower-triangular of Hessian.
        #     1   2   3   4
        #    --------------
        # 1 | x
        # 2 | x   x
        # 3 | x   x   x
        # 4 | x   x   x   x
        #    --------------
        if mode == :Structure
            nz = 1
            for j=1:4
                for i=j:4
                    rows[nz] = i
                    cols[nz] = j
                    nz += 1
                end
            end
        else
            YffR = _YffR[I]; YffI = _YffI[I]
            YftR = _YftR[I]; YftI = _YftI[I]
            YttR = _YttR[I]; YttI = _YttI[I]
            YtfR = _YtfR[I]; YtfI = _YtfI[I]

            cos_ij = cos(x[3] - x[4])
            sin_ij = sin(x[3] - x[4])
            vi_vj_cos = x[1]*x[2]*cos_ij
            vi_vj_sin = x[1]*x[2]*sin_ij
            pij = YffR*x[1]^2 + YftR*vi_vj_cos + YftI*vi_vj_sin
            qij = -YffI*x[1]^2 - YftI*vi_vj_cos + YftR*vi_vj_sin
            pji = YttR*x[2]^2 + YtfR*vi_vj_cos - YtfI*vi_vj_sin
            qji = -YttI*x[2]^2 - YtfI*vi_vj_cos - YtfR*vi_vj_sin

            nz = 1

            # d2f_dvidvi

            dpij_dvi = 2*YffR*x[1] + YftR*x[2]*cos_ij + YftI*x[2]*sin_ij
            dqij_dvi = -2*YffI*x[1] - YftI*x[2]*cos_ij + YftR*x[2]*sin_ij
            dpji_dvi = YtfR*x[2]*cos_ij - YtfI*x[2]*sin_ij
            dqji_dvi = -YtfI*x[2]*cos_ij - YtfR*x[2]*sin_ij

            # l_pij * d2pij_dvidvi
            values[nz] = param[1,I]*(2*YffR)
            # l_qij * d2qij_dvidvi
            values[nz] += param[2,I]*(-2*YffI)
            # l_pji * d2pji_dvidvi = 0
            # l_qji * d2qji_dvidvi = 0
            # l_vi * 2
            values[nz] += 2*param[5,I]
            # rho_pij*(dpij_dvi)^2 + rho_pij*(pij - tilde_pij)*d2pij_dvidvi
            values[nz] += param[9,I]*(dpij_dvi)^2 + param[9,I]*(pij - param[17,I])*(2*YffR)
            # rho_qij*(dqij_dvi)^2 + rho_qij*(qij - tilde_qij)*d2qij_dvidvi
            values[nz] += param[10,I]*(dqij_dvi)^2 + param[10,I]*(qij - param[18,I])*(-2*YffI)
            # rho_pji*(dpji_dvi)^2 + rho_pji*(pji - tilde_pji)*d2pji_dvidvi
            values[nz] += param[11,I]*(dpji_dvi)^2
            # rho_qji*(dqji_dvi)^2
            values[nz] += param[12,I]*(dqji_dvi)^2
            # (2*rho_vi*vi)*(2*vi) + rho_vi*(vi^2 - tilde_vi)*2
            values[nz] += 4*param[13,I]*x[1]^2 + param[13,I]*(x[1]^2 - param[21,I])*2
            nz += 1

            # d2f_dvidvj

            dpij_dvj = YftR*x[1]*cos_ij + YftI*x[1]*sin_ij
            dqij_dvj = -YftI*x[1]*cos_ij + YftR*x[1]*sin_ij
            dpji_dvj = 2*YttR*x[2] + YtfR*x[1]*cos_ij - YtfI*x[1]*sin_ij
            dqji_dvj = -2*YttI*x[2] - YtfI*x[1]*cos_ij - YtfR*x[1]*sin_ij

            d2pij_dvidvj = YftR*cos_ij + YftI*sin_ij
            d2qij_dvidvj = -YftI*cos_ij + YftR*sin_ij
            d2pji_dvidvj = YtfR*cos_ij - YtfI*sin_ij
            d2qji_dvidvj = -YtfI*cos_ij - YtfR*sin_ij

            # l_pij * d2pij_dvidvj
            values[nz] = param[1,I]*(d2pij_dvidvj)
            # l_qij * d2qij_dvidvj
            values[nz] += param[2,I]*(d2qij_dvidvj)
            # l_pji * d2pji_dvidvj
            values[nz] += param[3,I]*(d2pji_dvidvj)
            # l_qji * d2qji_dvidvj
            values[nz] += param[4,I]*(d2qji_dvidvj)
            # rho_pij*(dpij_dvj)*dpij_dvi + rho_pij*(pij - tilde_pij)*(d2pij_dvidvj)
            values[nz] += param[9,I]*(dpij_dvj)*dpij_dvi + param[9,I]*(pij - param[17,I])*(d2pij_dvidvj)
            # rho_qij*(dqij_dvj)*dqij_dvi + rho_qij*(qij - tilde_qij)*(d2qij_dvidvj)
            values[nz] += param[10,I]*(dqij_dvj)*dqij_dvi + param[10,I]*(qij - param[18,I])*(d2qij_dvidvj)
            # rho_pji*(dpji_dvj)*dpji_dvi + rho_pji*(pji - tilde_pji)*(d2pji_dvidvj)
            values[nz] += param[11,I]*(dpji_dvj)*dpji_dvi + param[11,I]*(pji - param[19,I])*(d2pji_dvidvj)
            # rho_qji*(dqji_dvj)*dqji_dvi + rho_qji*(qji - tilde_qji)*(d2qji_dvidvj)
            values[nz] += param[12,I]*(dqji_dvj)*dqji_dvi + param[12,I]*(qji - param[20,I])*(d2qji_dvidvj)
            nz += 1

            # d2f_dvidti

            dpij_dti = -YftR*vi_vj_sin + YftI*vi_vj_cos
            dqij_dti = YftI*vi_vj_sin + YftR*vi_vj_cos
            dpji_dti = -YtfR*vi_vj_sin - YtfI*vi_vj_cos
            dqji_dti = YtfI*vi_vj_sin - YtfR*vi_vj_cos

            d2pij_dvidti = -YftR*x[2]*sin_ij + YftI*x[2]*cos_ij
            d2qij_dvidti = YftI*x[2]*sin_ij + YftR*x[2]*cos_ij
            d2pji_dvidti = -YtfR*x[2]*sin_ij - YtfI*x[2]*cos_ij
            d2qji_dvidti = YtfI*x[2]*sin_ij - YtfR*x[2]*cos_ij

            # l_pij * d2pij_dvidti
            values[nz] = param[1,I]*(d2pij_dvidti)
            # l_qij * d2qij_dvidti
            values[nz] += param[2,I]*(d2qij_dvidti)
            # l_pji * d2pji_dvidti
            values[nz] += param[3,I]*(d2pji_dvidti)
            # l_qji * d2qji_dvidti
            values[nz] += param[4,I]*(d2qji_dvidti)
            # rho_pij*(dpij_dti)*dpij_dvi + rho_pij*(pij - tilde_pij)*(d2pij_dvidti)
            values[nz] += param[9,I]*(dpij_dti)*dpij_dvi + param[9,I]*(pij - param[17,I])*(d2pij_dvidti)
            # rho_qij*(dqij_dti)*dqij_dvi + rho_qij*(qij - tilde_qij)*(d2qij_dvidti)
            values[nz] += param[10,I]*(dqij_dti)*dqij_dvi + param[10,I]*(qij - param[18,I])*(d2qij_dvidti)
            # rho_pji*(dpji_dti)*dpji_dvi + rho_pji*(pji - tilde_pji)*(d2pji_dvidti)
            values[nz] += param[11,I]*(dpji_dti)*dpji_dvi + param[11,I]*(pji - param[19,I])*(d2pji_dvidti)
            # rho_qji*(dqji_dti)*dqji_dvi + rho_qji*(qji - tilde_qji)*(d2qji_dvidti)
            values[nz] += param[12,I]*(dqji_dti)*dqji_dvi + param[12,I]*(qji - param[20,I])*(d2qji_dvidti)
            nz += 1

            # d2f_dvidtj

            # l_pij * d2pij_dvidtj
            values[nz] = param[1,I]*(-d2pij_dvidti)
            # l_qij * d2qij_dvidtj
            values[nz] += param[2,I]*(-d2qij_dvidti)
            # l_pji * d2pji_dvidtj
            values[nz] += param[3,I]*(-d2pji_dvidti)
            # l_qji * d2qji_dvidtj
            values[nz] += param[4,I]*(-d2qji_dvidti)
            # rho_pij*(dpij_dtj)*dpij_dvi + rho_pij*(pij - tilde_pij)*(d2pij_dvidtj)
            values[nz] += param[9,I]*(-dpij_dti)*dpij_dvi + param[9,I]*(pij - param[17,I])*(-d2pij_dvidti)
            # rho_qij*(dqij_dtj)*dqij_dvi + rho_qij*(qij - tilde_qij)*(d2qij_dvidtj)
            values[nz] += param[10,I]*(-dqij_dti)*dqij_dvi + param[10,I]*(qij - param[18,I])*(-d2qij_dvidti)
            # rho_pji*(dpji_dtj)*dpji_dvi + rho_pji*(pji - tilde_pji)*(d2pji_dvidtj)
            values[nz] += param[11,I]*(-dpji_dti)*dpji_dvi + param[11,I]*(pji - param[19,I])*(-d2pji_dvidti)
            # rho_qji*(dqji_dtj)*dqji_dvi + rho_qji*(qji - tilde_qji)*(d2qji_dvidtj)
            values[nz] += param[12,I]*(-dqji_dti)*dqji_dvi + param[12,I]*(qji - param[20,I])*(-d2qji_dvidti)
            nz += 1

            # d2f_dvjdvj

            # l_pij * d2pij_dvjdvj = l_qij * d2qij_dvjdvj = 0 since d2pij_dvjdvj = d2qij_dvjdvj = 0
            # l_pji * d2pji_dvjdvj
            values[nz] = param[3,I]*(2*YttR)
            # l_qji * d2qji_dvjdvj
            values[nz] += param[4,I]*(-2*YttI)
            # l_vj * 2
            values[nz] += param[6,I]*2
            # rho_pij*(dpij_dvj)^2 + rho_pij*(pij - tilde_pij)*(d2pij_dvjdvj)
            values[nz] += param[9,I]*(dpij_dvj)^2
            # rho_qij*(dqij_dvj)^2 + rho_qij*(qij - tilde_qij)*(d2qij_dvjdvj)
            values[nz] += param[10,I]*(dqij_dvj)^2
            # rho_pji*(dpji_dvj)^2 + rho_pji*(pji - tilde_pji)*(d2pji_dvjdvj)
            values[nz] += param[11,I]*(dpji_dvj)^2 + param[11,I]*(pji - param[19,I])*(2*YttR)
            # rho_qji*(dqji_dvj)^2 + rho_qji*(qji - tilde_qji)*(d2qji_dvjdvj)
            values[nz] += param[12,I]*(dqji_dvj)^2 + param[12,I]*(qji - param[20,I])*(-2*YttI)
            # (2*rho_vj*vj)*(2*vj) + rho_vj*(vj^2 - tilde_vj)*2
            values[nz] += 4*param[14,I]*x[2]^2 + param[14,I]*(x[2]^2 - param[22,I])*2
            nz += 1

            # d2f_dvjdti

            d2pij_dvjdti = (-YftR*x[1]*sin_ij + YftI*x[1]*cos_ij)
            d2qij_dvjdti = (YftI*x[1]*sin_ij + YftR*x[1]*cos_ij)
            d2pji_dvjdti = (-YtfR*x[1]*sin_ij - YtfI*x[1]*cos_ij)
            d2qji_dvjdti = (YtfI*x[1]*sin_ij - YtfR*x[1]*cos_ij)

            # l_pij * d2pij_dvjdti
            values[nz] = param[1,I]*(d2pij_dvjdti)
            # l_qij * d2qij_dvjdti
            values[nz] += param[2,I]*(d2qij_dvjdti)
            # l_pji * d2pji_dvjdti
            values[nz] += param[3,I]*(d2pji_dvjdti)
            # l_qji * d2qji_dvjdti
            values[nz] += param[4,I]*(d2qji_dvjdti)
            # rho_pij*(dpij_dti)*dpij_dvj + rho_pij*(pij - tilde_pij)*(d2pij_dvjdti)
            values[nz] += param[9,I]*(dpij_dti)*dpij_dvj + param[9,I]*(pij - param[17,I])*d2pij_dvjdti
            # rho_qij*(dqij_dti)*dqij_dvj + rho_qij*(qij - tilde_qij)*(d2qij_dvjdti)
            values[nz] += param[10,I]*(dqij_dti)*dqij_dvj + param[10,I]*(qij - param[18,I])*d2qij_dvjdti
            # rho_pji*(dpji_dti)*dpji_dvj + rho_pji*(pji - tilde_pji)*(d2pji_dvjdti)
            values[nz] += param[11,I]*(dpji_dti)*dpji_dvj + param[11,I]*(pji - param[19,I])*d2pji_dvjdti
            # rho_qji*(dqji_dti)*dqji_dvj + rho_qji*(qji - tilde_qji)*(d2qji_dvjdti)
            values[nz] += param[12,I]*(dqji_dti)*dqji_dvj + param[12,I]*(qji - param[20,I])*d2qji_dvjdti
            nz += 1

            # d2f_dvjdtj

            # l_pij * d2pij_dvjdtj
            values[nz] = param[1,I]*(-d2pij_dvjdti)
            # l_qij * d2qij_dvjdtj
            values[nz] += param[2,I]*(-d2qij_dvjdti)
            # l_pji * d2pji_dvjdtj
            values[nz] += param[3,I]*(-d2pji_dvjdti)
            # l_qji * d2qji_dvjdtj
            values[nz] += param[4,I]*(-d2qji_dvjdti)
            # rho_pij*(dpij_dtj)*dpij_dvj + rho_pij*(pij - tilde_pij)*(d2pij_dvjdtj)
            values[nz] += param[9,I]*(-dpij_dti)*dpij_dvj + param[9,I]*(pij - param[17,I])*(-d2pij_dvjdti)
            # rho_qij*(dqij_dtj)*dqij_dvj + rho_qij*(qij - tilde_qij)*(d2qij_dvjdtj)
            values[nz] += param[10,I]*(-dqij_dti)*dqij_dvj + param[10,I]*(qij - param[18,I])*(-d2qij_dvjdti)
            # rho_pji*(dpji_dtj)*dpji_dvj + rho_pji*(pji - tilde_pji)*(d2pji_dvjdtj)
            values[nz] += param[11,I]*(-dpji_dti)*dpji_dvj + param[11,I]*(pji - param[19,I])*(-d2pji_dvjdti)
            # rho_qji*(dqji_dtj)*dqji_dvj + rho_qji*(qji - tilde_qji)*(d2qji_dvjdtj)
            values[nz] += param[12,I]*(-dqji_dti)*dqji_dvj + param[12,I]*(qji - param[20,I])*(-d2qji_dvjdti)
            nz += 1

            # d2f_dtidti

            d2pij_dtidti = (-YftR*vi_vj_cos - YftI*vi_vj_sin)
            d2qij_dtidti = (YftI*vi_vj_cos - YftR*vi_vj_sin)
            d2pji_dtidti = (-YtfR*vi_vj_cos + YtfI*vi_vj_sin)
            d2qji_dtidti = (YtfI*vi_vj_cos + YtfR*vi_vj_sin)

            # l_pij * d2pij_dtidti
            values[nz] = param[1,I]*(d2pij_dtidti)
            # l_qij * d2qij_dtidti
            values[nz] += param[2,I]*(d2qij_dtidti)
            # l_pji * d2pji_dtidti
            values[nz] += param[3,I]*(d2pji_dtidti)
            # l_qji * d2qji_dtidti
            values[nz] += param[4,I]*(d2qji_dtidti)
            # rho_pij*(dpij_dti)^2 + rho_pij*(pij - tilde_pij)*(d2pij_dtidti)
            values[nz] += param[9,I]*(dpij_dti)^2 + param[9,I]*(pij - param[17,I])*(d2pij_dtidti)
            # rho_qij*(dqij_dti)^2 + rho_qij*(qij - tilde_qij)*(d2qij_dtidti)
            values[nz] += param[10,I]*(dqij_dti)^2 + param[10,I]*(qij - param[18,I])*(d2qij_dtidti)
            # rho_pji*(dpji_dti)^2 + rho_pji*(pji - tilde_pji)*(d2pji_dtidti)
            values[nz] += param[11,I]*(dpji_dti)^2 + param[11,I]*(pji - param[19,I])*(d2pji_dtidti)
            # rho_qji*(dqji_dti)^2 + rho_qji*(qji - tilde_qji)*(d2qji_dtidti)
            values[nz] += param[12,I]*(dqji_dti)^2 + param[12,I]*(qji - param[20,I])*(d2qji_dtidti)
            # rho_ti
            values[nz] += param[15,I]
            nz += 1

            # d2f_dtidtj

            # l_pij * d2pij_dtidtj
            values[nz] = param[1,I]*(-d2pij_dtidti)
            # l_qij * d2qij_dtidtj
            values[nz] += param[2,I]*(-d2qij_dtidti)
            # l_pji * d2pji_dtidtj
            values[nz] += param[3,I]*(-d2pji_dtidti)
            # l_qji * d2qji_dtidtj
            values[nz] += param[4,I]*(-d2qji_dtidti)
            # rho_pij*(dpij_dtj)*dpij_dti + rho_pij*(pij - tilde_pij)*(d2pij_dtidtj)
            values[nz] += param[9,I]*(-dpij_dti^2) + param[9,I]*(pij - param[17,I])*(-d2pij_dtidti)
            # rho_qij*(dqij_dtj)*dqij_dti + rho_qij*(qij - tilde_qij)*(d2qij_dtidtj)
            values[nz] += param[10,I]*(-dqij_dti^2) + param[10,I]*(qij - param[18,I])*(-d2qij_dtidti)
            # rho_pji*(dpji_dtj)*dpji_dti + rho_pji*(pji - tilde_pji)*(d2pji_dtidtj)
            values[nz] += param[11,I]*(-dpji_dti^2) + param[11,I]*(pji - param[19,I])*(-d2pji_dtidti)
            # rho_qji*(dqji_dtj)*dqji_dti + rho_qji*(qji - tilde_qji)*(d2qji_dtidtj)
            values[nz] += param[12,I]*(-dqji_dti^2) + param[12,I]*(qji - param[20,I])*(-d2qji_dtidti)
            nz += 1

            # d2f_dtjdtj
            # l_pij * d2pij_dtjdtj
            values[nz] = param[1,I]*(d2pij_dtidti)
            # l_qij * d2qij_dtjdtj
            values[nz] += param[2,I]*(d2qij_dtidti)
            # l_pji * d2pji_dtjdtj
            values[nz] += param[3,I]*(d2pji_dtidti)
            # l_qji * d2qji_dtjdtj
            values[nz] += param[4,I]*(d2qji_dtidti)
            # rho_pij*(dpij_dtj)^2 + rho_pij*(pij - tilde_pij)*(d2pij_dtjdtj)
            values[nz] += param[9,I]*(dpij_dti^2) + param[9,I]*(pij - param[17,I])*(d2pij_dtidti)
            # rho_qij*(dqij_dtj)^2 + rho_qij*(qij - tilde_qij)*(d2qij_dtjdtj)
            values[nz] += param[10,I]*(dqij_dti^2) + param[10,I]*(qij - param[18,I])*(d2qij_dtidti)
            # rho_pji*(dpji_dtj)^2 + rho_pji*(pji - tilde_pji)*(d2pji_dtjdtj)
            values[nz] += param[11,I]*(dpji_dti^2) + param[11,I]*(pji - param[19,I])*(d2pji_dtidti)
            # rho_qji*(dqji_dtj)^2 + rho_qji*(qji - tilde_qji)*(d2qji_dtjdtj)
            values[nz] += param[12,I]*(dqji_dti^2) + param[12,I]*(qji - param[20,I])*(d2qji_dtidti)
            # rho_tj
            values[nz] += param[16,I]
            nz += 1

            for i=1:nz-1
                values[i] *= scale
            end
        end
    end
end

# Assume that scaling factor has already been applied to Q and c.
@inline function eval_qp_f_kernel(n::Int, x, Q, c, I, J)
    # f = xQx/2 + cx
    f = 0.0
    @inbounds begin
        for j=1:n
            for i=1:n
                f += x[i]*Q[i,j]*x[j]
            end
        end
        f *= 0.5
        for j=1:n
            f += c[j]*x[j]
        end
    end
    return f
end

@inline function eval_qp_grad_f_kernel(n::Int, x, g, Q, c, I, J)
    # g = Qx + c
    tx = J

    @inbounds begin
        if tx <= n
            g[tx] = c[tx]
        end
        @synchronize
        tx = J
        if tx <= n
            for j=1:n
                g[tx] += Q[tx,j]*x[j]
            end
        end
        @synchronize
    end
    return
end


text:
abstract type ClassificationLikelihood{T<:Real} <: AbstractLikelihood{T} end

include("logistic.jl")
include("bayesiansvm.jl")

""" Return the labels in a vector of vectors for multiple outputs"""
function treat_labels!(y::AbstractVector{<:Real}, likelihood::ClassificationLikelihood)
    labels = unique(y)
    @assert length(labels) <= 2 "Labels of y should be binary {-1,1} or {0,1}"
    if sort(Int64.(labels)) == [0; 1]
        return (y .- 0.5) * 2, 1, likelihood
    elseif sort(Int64.(labels)) == [-1; 1]
        return y, 1, likelihood
    else
        throw(AssertionError("Labels of y should be binary {-1,1} or {0,1}"))
    end
end

function treat_labels!(y::AbstractVector, likelihood::ClassificationLikelihood)
    return error(
        "For classification target(s) should be real valued (Bool, Integer or Float)"
    )
end

predict_y(::ClassificationLikelihood, μ::AbstractVector{<:Real}) = μ .> 0
predict_y(::ClassificationLikelihood, μ::AbstractVector{<:AbstractVector}) = first(μ) .> 0

ext: jl


text:
# Autogenerated wrapper script for HELICS_jll for i686-w64-mingw32-cxx03
export libhelicsSharedLib

using ZeroMQ_jll
JLLWrappers.@generate_wrapper_header("HELICS")
JLLWrappers.@declare_library_product(libhelicsSharedLib, "libhelicsSharedLib.dll")
function __init__()
    JLLWrappers.@generate_init_header(ZeroMQ_jll)
    JLLWrappers.@init_library_product(
        libhelicsSharedLib,
        "bin\\libhelicsSharedLib.dll",
        RTLD_LAZY | RTLD_DEEPBIND,
    )

    JLLWrappers.@generate_init_footer()
end  # __init__()


text:
"""
A `ParamDict` is a dictionary combined with a map, which is the parameterization
of some domain that is the image of the support of the dictionary.

If `Ω` is a domain, parameterized by the function `κ : P → Ω`, then the `ParamDict`
is a dictionary defined on `P`, with each point `t ∈ P` identified with
`x = κ(t) ∈ Ω`. The `ParamDict` acts like any other dictionary on `P`.

Note the difference with a `MappedDict`, which is defined on `Ω`. Operations
on mapped dictionaries require an easily invertible map.
"""
struct ParamDict{D,M,S,T} <: DerivedDict{S,T}
    superdict   ::  D
    map         ::  M
    image       ::  Domain
end

# In the constructor we check the domain and codomain types.
# - The domain of the ParamDict is the same as the domain of the map.
# - The map maps to the same type as elements of the domain
ParamDict(dict::Dictionary{S,T}, map::Map{S}, image::Domain) where {S,T} =
    ParamDict{typeof(dict),typeof(map),S,T}(dict, map, image)

param_dict(dict::Dictionary, map::AbstractMap) = ParamDict(dict, map)

forward_map(dict::ParamDict) = dict.map
image(dict::ParamDict) = dict.image

similardictionary(dict::ParamDict, dict2::Dictionary) =
    ParamDict(dict2, forward_map(dict), image(dict))

iscompatible(dict1::ParamDict, dict2::ParamDict) =
    iscompatible(forward_map(dict1),forward_map(dict2)) && iscompatible(superdict(dict1),superdict(dict2))

## Printing

Display.displaystencil(d::ParamDict) = ["ParamDict(", superdict(d), ", ", forward_map(d), ", ", image(d), ")"]


text:
stages = Dict(1 => Stage(template_uc, 24, Hour(24), 1, c_sys5_uc, GLPK_optimizer,  Dict(0 => Sequential())),
              2 => Stage(template_ed, 12, Minute(5), 24, c_sys5_ed, GLPK_optimizer, Dict(1 => Synchronize(24,1), 0 => Sequential()), TimeStatusChange(:ON_ThermalStandard)))

if !isdir(joinpath(pwd(), "testing_reading_results"))
    file_path = mkdir(joinpath(pwd(), "testing_reading_results"))
else
    file_path = (joinpath(pwd(), "testing_reading_results"))
end
sim = Simulation("test", 2, stages, file_path; verbose = true)
sim_results = execute!(sim)
step = ["step-1", "step-2"]
stages = ["stage-1", "stage-2"]

@testset "testing reading and writing to the results folder" begin
    for (ix, s) in enumerate(stages)
        files = collect(readdir(sim_results.results_folder))
        for f in files
            rm("$(sim_results.results_folder)/$f")
        end
    res = load_simulation_results(s, sim_results; write = true)
    loaded_res = load_operation_results(sim_results.results_folder)
    @test loaded_res.variables == res.variables
    end
end

@testset "testing file names" begin
    for (ix, s) in enumerate(stages)
        files = collect(readdir(sim_results.results_folder))
        for f in files
            rm("$(sim_results.results_folder)/$f")
        end
        res = load_simulation_results(s, sim_results; write = true)
        variable_list = String.(collect(keys(sim.stages[ix].canonical.variables)))
        variable_list = [variable_list; "optimizer_log"; "time_stamp"; "check_sum"]
        file_list = collect(readdir(sim_results.results_folder))
        for name in file_list
            variable = splitext(name)[1]
            @test any(x -> x == variable, variable_list)
        end
    end
end

@testset "testing argument errors" begin
    for (ix, s) in enumerate(stages)
        files = collect(readdir(sim_results.results_folder))
        for f in files
            rm("$(sim_results.results_folder)/$f")
        end
        res = load_simulation_results(s, sim_results)
        @test_throws ArgumentError write_results(res, "nothing", "results")
    end
end

@testset "checking total sums" begin
    for (ix, s) in enumerate(stages)
        files = collect(readdir(sim_results.results_folder))
        for f in files
            rm("$(sim_results.results_folder)/$f")
        end
        res = load_simulation_results(s, sim_results; write = true)
        loaded_res = load_operation_results(sim_results.results_folder)
        @test res.check_sum == loaded_res.check_sum
    end
end

@testset "testing load simulation results functionality stage 2" begin
    for (ix,s) in enumerate(stages)
        variable = (collect(keys(sim.stages[ix].canonical.variables)))
        results = load_simulation_results(s, sim_results)
        res = load_simulation_results(s, step, variable, sim_results)
        @test results.variables == res.variables
    end
end

rm(file_path, recursive=true)


text:
struct Simulator{T, W<:AbstractAccumulator{T}, F}
    prob::TaskStats{T}
    acc::W
    decision::F
    Simulator(p::TaskStats{T}, a::AbstractAccumulator{T}, d::F) where {T, F} = new{T, typeof(a), F}(p, a, d)
end

getaccumulator(s::Simulator) = s.acc
TaskStats(s::Simulator) = s.prob

Base.IteratorSize(::Type{<:Simulator}) = Base.IsInfinite()
Base.IteratorEltype(::Type{<:Simulator}) = Base.HasEltype()

Base.eltype(::Type{<:Simulator{T}}) where {T} =
    NamedTuple{(:value, :reward, :side, :sidehigh), Tuple{T, Bool, Bool, Bool}}

function Base.iterate(s::Simulator, (side, sidehigh) = (rand(Bool), rand(Bool)))
    prob = TaskStats(s)
    rwd = (sidehigh == side) ? (rand() < prob.rwd) : false
    val = update!(getaccumulator(s), rwd, side)
    outcome = (value = val, reward = rwd, side = side, sidehigh = sidehigh)
    (side == sidehigh) && (rand() < prob.dpl) && (sidehigh = !sidehigh)
    newside = s.decision(val, side)
    return outcome, (newside, sidehigh)
end

function inferencesimulator(prob::TaskStats, T, β)
    acc = InferenceAccumulator(prob)
    decision = DiffRule(T, β)
    Simulator(prob, acc, decision)
end

function valuesimulator(prob::TaskStats, γ, T, β)
    acc = ValueAccumulator(γ)
    decision = DiffRule(T, β)
    Simulator(prob, acc, decision)
end

struct DiffRule{T}
    T::T
    β::T
end

DiffRule(a, b) = DiffRule(promote(a, b)...)

function (dr::DiffRule)(diff, side)
    Δind = diff + dr.T*to_sign(side)
    return rand() < logistic(dr.β*Δind)
end


text:
function testmodel(m::Comrade.AbstractModel, npix=1024, atol=1e-4)
    plot(m)
    img = intensitymap(m, 2*Comrade.radialextent(m), 2*Comrade.radialextent(m), npix, npix)
    plot(img)
    img2 = similar(img)
    intensitymap!(img2, m)
    @test eltype(img) === Float64
    @test isapprox(flux(m), flux(img), atol=atol)
    @test isapprox(mean(img .- img2), 0, atol=1e-8)
    cache = Comrade.create_cache(Comrade.FFTAlg(padfac=3), img./flux(img)*flux(m))
    u = fftshift(fftfreq(size(img,1), 1/img.psizex))./30
    Plots.closeall()
    @test isapprox(maximum(abs, (visibility.(Ref(m), u', u) .- cache.sitp.(u', u))), 0.0, atol=atol*10)
    img = nothing
    img2 =nothing
    cache = nothing
    u = nothing
    GC.gc()
end

function testft(m, npix=256, atol=1e-4)
    mn = Comrade.NonAnalyticTest(m)
    uu = 0.25*randn(1000)
    vv = 0.25*randn(1000)
    img = intensitymap(m, 2*Comrade.radialextent(m), 2*Comrade.radialextent(m), npix, npix; pulse=DeltaPulse())
    mimg_ff = modelimage(mn, img, FFTAlg(padfac=3))
    mimg_nf = modelimage(mn, img, NFFTAlg())
    mimg_df = modelimage(mn, img, DFTAlg())

    va = visibilities(m, uu, vv)
    vff = visibilities(mimg_ff, uu, vv)
    vnf = visibilities(mimg_nf, uu, vv)
    vdf = visibilities(mimg_df, uu, vv)

    @test isapprox(maximum(abs, va-vff), 0, atol=atol*5)
    @test isapprox(maximum(abs, va-vnf), 0, atol=atol)
    @test isapprox(maximum(abs, va-vdf), 0, atol=atol)
    img = nothing
    mimg_ff = nothing
    mimg_nf = nothing
    mimg_df = nothing
    GC.gc()
end

@testset "FFTTest" begin
    @testset "Base" begin
        m = Gaussian()
        testft(m)
    end

    @testset "Mod" begin
        m = rotated(stretched(Gaussian(), 0.5, 1.0), π/3)
        testft(m)
        ms = shifted(m, 1.0,1.0)
        testft(ms)
    end

    @testset "Add" begin
        m1 = rotated(stretched(Gaussian(), 0.5, 1.0), π/3) + shifted(Gaussian(), 1.0, 1.0)
        testft(m1)
    end
end

@testset "Primitive models" begin

    @testset "Gaussian" begin
        m = Gaussian()
        testmodel(m, 1024, 1e-5)
    end

    @testset "Disk" begin
        m = smoothed(Disk(), 0.25)
        ComradeBase.intensity_point(Disk(), 0.0, 0.0)
        testmodel(m)
    end

    @testset "Ring" begin
        m = smoothed(Ring(), 0.25)
        ComradeBase.intensity_point(Ring(), 0.0, 0.0)
        testmodel(m, 2048)
    end

    @testset "ParabolicSegment" begin
        m = ParabolicSegment()
        m2 = ParabolicSegment(2.0, 2.0)
        @test stretched(m, 2.0, 2.0) == m2
        @test ComradeBase.intensity_point(m, 0.0, 1.0) != 0.0
        testmodel(m, 2424, 1e-3)
    end

    @testset "MRing1" begin
        α = (0.25,)
        β = (0.1,)

        # We convolve it to remove some pixel effects
        m = convolved(MRing(α, β), stretched(Gaussian(), 0.1, 0.1))
        m2 = convolved(MRing{1}(collect(α), collect(β)), stretched(Gaussian(), 0.1, 0.1))
        @test m == m2
        testmodel(m, 2048, 1e-3)
    end

    @testset "MRing2" begin
        α = (0.25, -0.1)
        β = (0.1, 0.2)

        # We convolve it to remove some pixel effects
        m = convolved(MRing(α, β), stretched(Gaussian(), 0.1, 0.1))
        testmodel(m, 2048, 1e-3)
    end

    @testset "ConcordanceCrescent" begin
        m = ConcordanceCrescent(20.0, 10.0, 5.0, 0.5)
        testmodel(m)
    end

    @testset "Crescent" begin
        m = smoothed(Crescent(5.0, 2.0, 1.0, 0.5), 1.0)
        testmodel(m,1024,1e-3)
    end

    @testset "ExtendedRing" begin
        mr = ExtendedRing(8.0)
        rad = 2.5*Comrade.radialextent(mr)
        m = modelimage(mr, IntensityMap(zeros(1024,1024), rad, rad), Comrade.FFTAlg(padfac=4))
        testmodel(m)
    end
end

@testset "ModelImage" begin
    m1 = Gaussian()
    m2 = ExtendedRing(10.0)
    mimg1 = modelimage(m1)
    mimg2 = modelimage(m2)

    show(mimg1)

    img = similar(mimg2.image)
    intensitymap!(img, m2)
    @test m1 == mimg1
    @test isapprox(mean(img .- mimg2.image), 0.0, atol=1e-8)
end

@testset "Modifiers" begin
    ma = Gaussian()
    mb = ExtendedRing(8.0)
    @testset "Shifted" begin
        mas = shifted(ma, 0.5, 0.5)
        mbs = shifted(mb, 0.5, 0.5)
        testmodel(mas)
        testmodel(modelimage(mbs, IntensityMap(zeros(1024, 1024),
                                               2*Comrade.radialextent(mbs),
                                               2*Comrade.radialextent(mbs))))
    end

    @testset "Renormed" begin
        m1 = 3.0*ma
        m2 = ma*3.0
        m2inv = ma/(1/3)
        @test visibility(m1, 4.0, 0.0) == visibility(m2, 4.0, 0.0)
        @test visibility(m2, 4.0, 0.0) == visibility(m2inv, 4.0, 0.0)
        mbs = 3.0*mb
        testmodel(m1)
        testmodel(modelimage(mbs, IntensityMap(zeros(1024, 1024),
                                               2.5*Comrade.radialextent(mbs),
                                               2.5*Comrade.radialextent(mbs))))
    end

    @testset "Stretched" begin
        mas = stretched(ma, 5.0, 4.0)
        mbs = stretched(mb, 5.0, 4.0)
        testmodel(mas)
        testmodel(modelimage(mbs, IntensityMap(zeros(2024, 2024),
                                               2*Comrade.radialextent(mbs),
                                               2*Comrade.radialextent(mbs))), 1024, 1e-3)
    end

    @testset "Rotated" begin
        mas = rotated(ma, π/3)
        mbs = rotated(mb, π/3)
        testmodel(mas)
        testmodel(modelimage(mbs, IntensityMap(zeros(1024, 1024),
                                               2*Comrade.radialextent(mbs),
                                               2*Comrade.radialextent(mbs))))
    end

    @testset "AllMods" begin
        mas = rotated(stretched(shifted(ma, 0.5, 0.5), 5.0, 4.0), π/3)
        mbs = rotated(stretched(shifted(mb, 0.5, 0.5), 5.0, 4.0), π/3)
        testmodel(mas)
        testmodel(modelimage(mbs, IntensityMap(zeros(2024, 2024),
                                               2*Comrade.radialextent(mbs),
                                               2*Comrade.radialextent(mbs))), 1024, 1e-3)
    end
end

@testset "CompositeModels" begin
    m1 = Gaussian()
    m2 = ExtendedRing(8.0)

    @testset "Add models" begin
        img = IntensityMap(zeros(1024, 1024),
                                        20.0,
                                        20.0)
        mt1 = m1 + m2
        mt2 = shifted(m1, 1.0, 1.0) + m2
        mt3 = shifted(m1, 1.0, 1.0) + 0.5*stretched(m2, 0.9, 0.8)
        mc = Comrade.components(mt1)
        @test mc[1] === m1
        @test mc[2] === m2
        @test flux(mt1) ≈ flux(m1) + flux(m2)

        testmodel(modelimage(mt1, img))
        testmodel(modelimage(mt2, img))
        testmodel(modelimage(mt3, img))
    end

    @testset "Convolved models" begin
        img = IntensityMap(zeros(1024, 1024),
                                        20.0,
                                        20.0)
        mt1 = convolved(m1, m2)
        mt2 = convolved(shifted(m1, 1.0, 1.0), m2)
        mt3 = convolved(shifted(m1, 1.0, 1.0), 0.5*stretched(m2, 0.9, 0.8))
        mc = Comrade.components(mt1)
        @test mc[1] === m1
        @test mc[2] === m2

        testmodel(modelimage(mt1, img))
        testmodel(modelimage(mt2, img))
        testmodel(modelimage(mt3, img))
    end

    @testset "All composite" begin
        img = IntensityMap(zeros(1024, 1024),
                                            20.0,
                                            20.0)

        mt = m1 + convolved(m1, m2)
        mc = Comrade.components(mt)
        @test mc[1] === m1
        @test mc[2] === m1
        @test mc[3] === m2

        testmodel(modelimage(mt, img))

    end
end

@testset "PolarizedModel" begin
    mI = stretched(MRing((0.2,), (0.1,)), 20.0, 20.0)
    mQ = 0.2*stretched(MRing((0.0,), (0.6,)), 20.0, 20.0)
    mU = 0.2*stretched(MRing((0.1,), (-0.6,)), 20.0, 20.0)
    mV = 0.0*stretched(MRing((0.0,), (-0.6,)), 20.0, 20.0)
    m = PolarizedModel(mI, mQ, mU, mV)

    v = coherencymatrix(m, 0.005, 0.01)
    @test evpa(v) == evpa(m, 0.005, 0.01)
    @test m̆(v) == m̆(m, 0.005, 0.01)

    I = IntensityMap(zeros(1024,1024), 100.0, 100.0)
    Q = similar(I)
    U = similar(I)
    V = similar(I)
    pimg1 = IntensityMap(I,Q,U,V)
    intensitymap!(pimg1, m)
    pimg2 = intensitymap(m, 100.0, 100.0, 1024, 1024)
    @test isapprox(sum(abs, (stokes(pimg1, :I) .- stokes(pimg2, :I))), 0.0, atol=1e-12)
    @test isapprox(sum(abs, (stokes(pimg1, :Q) .- stokes(pimg2, :Q))), 0.0, atol=1e-12)
    @test isapprox(sum(abs, (stokes(pimg1, :U) .- stokes(pimg2, :U))), 0.0, atol=1e-12)
    @test isapprox(sum(abs, (stokes(pimg1, :V) .- stokes(pimg2, :V))), 0.0, atol=1e-12)

end

@testset "Image SqExp" begin
   c = intensitymap(rotated(stretched(Gaussian(), 2.0, 1.0), π/8), 12.0, 12.0, 12, 12; pulse=SqExpPulse(3.0))
   #mI = DImage(c.im, SqExpPulse(5.0))
   testmodel(modelimage(c, FFTAlg(padfac=3)), 1024, 1e-3)
end
#@testset "DImage Bspline0" begin
#   mI = DImage(rand(8,8), BSplinePulse{0}())
#   testmodel(mI, 1e-2)
#end
@testset "DImage BSpline1" begin
    c = intensitymap(rotated(stretched(Gaussian(), 2.0, 1.0), π/8), 12.0, 12.0, 12, 12; pulse=BSplinePulse{1}())
    testmodel(modelimage(c, FFTAlg(padfac=3)), 1024, 1e-3)
end

@testset "DImage BSpline3" begin
    c = intensitymap(rotated(stretched(Gaussian(), 2.0, 1.0), π/8), 12.0, 12.0, 12, 12; pulse=BSplinePulse{3}())
    testmodel(modelimage(c, FFTAlg(padfac=3)), 1024, 1e-3)
end


text:
#///////////////////////////////////////
#// File Name: evaluation.jl
#// Author: Haruki Nishimura (hnishimura@stanford.edu)
#// Date Created: 2021/02/25
#// Description: Evaluation helper functions for RiskSensitiveSAC
#///////////////////////////////////////

using Printf
using RobotOS
using PyCall

struct EvaluationResult
    sim_param::SimulationParameter
    cnt_param::ControlParameter
    # scene_param::Union{TrajectronSceneParameter, SyntheticSceneParameter} # error for JLD2?
    # predictor_param::Union{TrajectronPredictorParameter, GaussianPredictorParameter} # error for JLD2?
    nominal_control::Bool
    target_trajectory_history::Vector{Trajectory2D}
    measurement_time_history::Vector{Time}
    sim_horizon::Float64
    w_history::Vector{WorldState}
    u_history::Vector{Vector{Float64}}
    u_schedule_history::Vector{OrderedDict{Time, Vector{Float64}}}
    nominal_trajectory_history::Vector{Vector{Vector{Float64}}}
    prediction_dict_history::Vector{Union{Nothing, Dict{String, Array{Float64, 3}}}}
    u_nominal_idx_history::Vector{Int64}
    total_cnt_cost::Float64
    total_pos_cost::Float64
    total_col_cost::Float64
    log::Vector{Tuple{Time, String}}
end

struct BICEvaluationResult
    sim_param::SimulationParameter
    cnt_param::BICControlParameter
    target_trajectory_history::Vector{Trajectory2D}
    measurement_time_history::Vector{Time}
    sim_horizon::Float64
    w_history::Vector{WorldState}
    u_history::Vector{Vector{Float64}}
    total_cnt_cost::Float64
    total_pos_cost::Float64
    total_col_cost::Float64
    log::Vector{Tuple{Time, String}}
end

struct CrowdNavEvaluationResult
    sim_param::SimulationParameter
    cnt_param::CrowdNavControlParameter
    target_trajectory_history::Vector{Trajectory2D}
    measurement_time_history::Vector{Time}
    state_measurement_history::Vector{Dict{String, Vector{Float64}}}
    sim_horizon::Float64
    w_history::Vector{WorldState}
    u_history::Vector{Vector{Float64}}
    total_cnt_cost::Float64
    total_pos_cost::Float64
    total_col_cost::Float64
    log::Vector{Tuple{Time, String}}
end

function get_clipped_prediction_dict(prediction_dict::Dict{String, Array{Float64, 3}},
                                     num_samples::Int64,
                                     nominal_control_idx::Int64)
    clipped_preds_dict = Dict{String, Array{Float64, 3}}();
    for key in keys(prediction_dict)
        clipped_array =
            prediction_dict[key][num_samples*(nominal_control_idx - 1) + 1:num_samples*nominal_control_idx, :, :];
        clipped_preds_dict[key] = copy(clipped_array);
    end
    return clipped_preds_dict
end

function evaluate(scene_loader::SceneLoader,
                  controller::Union{RSSACController, BICController, CrowdNavController},
                  w_init::WorldState,
                  ego_pos_goal_vec::Vector{Float64},
                  target_speed::Float64,
                  measurement_schedule::Vector{Time},
                  target_trajectory::Trajectory2D,
                  pos_error_replan::Float64;
                  ado_inputs_init::Union{Nothing, Dict{T, Vector{Float64}} where T <: Union{PyObject, String}}=nothing, # only needed for CrowdNavController
                  nominal_control::Union{Nothing, Bool}=nothing, # determines if nominal control is used in RSSAC controller
                  ado_id_removed::Union{Nothing, String}=nothing, # determines if ado_id_removed is removed from scenes with TrajectronSceneLoader
                  predictor::Union{Nothing, GaussianPredictor}=nothing) # needs to feed in GaussianPredictor if BICController is used with SyntheticSceneLoader
    # Assertions
    if typeof(controller) == BICController
        @assert isnothing(nominal_control)
    end
    if typeof(controller) == CrowdNavController
        @assert isnothing(nominal_control)
        @assert !isnothing(ado_inputs_init)
    end
    if typeof(scene_loader) != TrajectronSceneLoader
        @assert isnothing(ado_id_removed)
    end
    if typeof(controller) == BICController && typeof(scene_loader) == SyntheticSceneLoader
        @assert !isnothing(predictor)
    else
        @assert isnothing(predictor)
    end
    # Initial Cost Values
    total_position_cost = 0.0;
    total_control_cost = 0.0;
    total_collision_cost = 0.0;
    # Logs
    w_history = Vector{WorldState}();
    u_history = Vector{Vector{Float64}}();
    target_trajectory_history = Vector{Trajectory2D}();
    u_nominal_idx_history = Vector{Int64}();
    nominal_trajectory_history = Vector{Vector{Vector{Float64}}}();
    log = Vector{Tuple{Time, String}}();
    if typeof(controller) == RSSACController
        u_schedule_history = Vector{OrderedDict{Time, Vector{Float64}}}();
        prediction_dict_history = Vector{Union{Nothing, Dict{String, Array{Float64, 3}}}}();
        @assert istaskdone(controller.prediction_task)
    elseif typeof(controller) == CrowdNavController
        state_measurement_history = Vector{Dict{String, Vector{Float64}}}();
        push!(state_measurement_history, convert_nodes_to_str(deepcopy(ado_inputs_init)));
    end
    # Compute First Control
    if typeof(controller) == CrowdNavController
        schedule_control_update!(controller, w_init.e_state, ado_inputs_init,
                                 log=log);
    else
        schedule_control_update!(controller, w_init, target_trajectory,
                                 log=log);
    end
    last_control_update_time = w_init.t;
    wait(controller.control_update_task);
    push!(w_history, w_init);
    push!(target_trajectory_history, target_trajectory);
    if typeof(controller) == RSSACController
        push!(prediction_dict_history, get_clipped_prediction_dict(controller.prediction_dict,
                                                                   controller.sim_param.num_samples,
                                                                   controller.sim_result_tmp.u_nominal_idx));
        prediction_dict_last = prediction_dict_history[end];
        push!(nominal_trajectory_history, deepcopy(get_position.(controller.sim_result_tmp.e_state_array)));
        #push!(u_nominal_idx_history, controller.sim_result_tmp.u_nominal_idx);
    end
    sleep(1.0)
    global ado_positions = nothing;
    global ado_inputs = ado_inputs_init;

    # Starting Simulation
    m_time_idx = 1;
    sim_end_time = measurement_schedule[end];
    prediction_horizon = controller.sim_param.dto*
                         controller.sim_param.prediction_steps;
    while w_history[end].t <= sim_end_time
        current_time = w_history[end].t;
        if current_time == measurement_schedule[m_time_idx]
            # Get new measurement
            msg_1 = "New measurement is obtained."
            push!(log, (current_time, msg_1))
            if typeof(scene_loader) == TrajectronSceneLoader
                ado_inputs = fetch_ado_positions!(scene_loader, return_full_state=true);
                ado_positions = reduce_to_positions(ado_inputs);
                if !isnothing(ado_id_removed)
                    key_to_remove = nothing
                    for key in keys(ado_positions)
                        if pybuiltin("str")(key) == ado_id_removed
                            key_to_remove = key
                        end
                    end
                    delete!(ado_positions, key_to_remove)
                    delete!(ado_inputs, key_to_remove)
                end
            elseif typeof(scene_loader) == SyntheticSceneLoader
                if typeof(controller) == RSSACController
                    ado_positions = fetch_ado_positions!(scene_loader, controller.prediction_dict);
                else
                    prediction_dict = sample_future_ado_positions!(predictor,
                                                                   w_history[end].ap_dict);
                    ado_positions = fetch_ado_positions!(scene_loader, prediction_dict);
                end
            end
            # Starting timer to keep track of computation time
            process_start_time = time();
            if current_time < sim_end_time
                if typeof(controller) == RSSACController
                    # Schedule prediction
                    previous_ado_pos_dict = deepcopy(w_history[end].ap_dict);
                    msg_2 = "New prediction is scheduled."
                    push!(log, (current_time, msg_2))
                    if typeof(controller.predictor) == TrajectronPredictor &&
                        controller.predictor.param.use_robot_future
                        schedule_prediction!(controller, ado_inputs, previous_ado_pos_dict,
                                             w_history[end].e_state);
                    elseif typeof(controller.predictor) == TrajectronPredictor
                        schedule_prediction!(controller, ado_inputs, previous_ado_pos_dict);
                    else
                        schedule_prediction!(controller, ado_positions, previous_ado_pos_dict);
                    end
                    prediction_dict_history[end] = get_clipped_prediction_dict(controller.prediction_dict,
                                                                               controller.sim_param.num_samples,
                                                                               controller.sim_result.u_nominal_idx);
                end
            end
            w_history[end].ap_dict = convert_nodes_to_str(ado_positions);
            w_history[end].t_last_m = current_time;
            m_time_idx += 1;
        else
            # No new measurement
            # Starting timer to keep track of computation time
            process_start_time = time();
        end

        # Update target trajectory if necessary
        pos_error = get_position(w_history[end].e_state) -
                    get_position(target_trajectory, w_history[end].t);
        if norm(pos_error) > pos_error_replan
            msg = "Position deviation: $(round(norm(pos_error), digits=4)). Target Trajectory is replanned."
            push!(log, (current_time, msg));
            target_trajectory = get_nominal_trajectory(w_history[end].e_state,
                                                       ego_pos_goal_vec,
                                                       target_speed,
                                                       to_sec(sim_end_time - w_init.t),
                                                       prediction_horizon);
        end

        # Proceed further
        if current_time < sim_end_time
            if to_sec(current_time) ≈ to_sec(last_control_update_time) + controller.cnt_param.dtr;
                # Schedule control update
                if typeof(controller) == CrowdNavController
                    schedule_control_update!(controller, w_history[end].e_state, ado_inputs,
                                             log=log);
                else
                    schedule_control_update!(controller, w_history[end], target_trajectory, log=log);
                end
                last_control_update_time = w_history[end].t
            end
            # Get control for current_time
            if typeof(controller) == RSSACController
                u = control!(controller, current_time, nominal_control=nominal_control, log=log)
                prediction_dict_history[end] = get_clipped_prediction_dict(controller.prediction_dict,
                                                                           controller.sim_param.num_samples,
                                                                           controller.sim_result.u_nominal_idx);
            else
                u = control!(controller, current_time, log=log)
            end

            # Stop timer and measure computation time so far in this iteration.
            elapsed = time() - process_start_time;

            # Accumulate cost
            total_position_cost +=
                instant_position_cost(w_history[end].e_state, target_trajectory,
                                      controller.sim_param.cost_param)*
                controller.sim_param.dtc;
            total_control_cost +=
                instant_control_cost(u, controller.sim_param.cost_param)*
                controller.sim_param.dtc;
            for ap in values(w_history[end].ap_dict)
                total_collision_cost +=
                    instant_collision_cost(w_history[end].e_state, ap,
                                           controller.sim_param.cost_param)*
                    controller.sim_param.dtc;
            end

            # Ego transition for next timestep
            e_state_new = transition(w_history[end].e_state, u, controller.sim_param.dtc);
            w_new = WorldState(e_state_new, deepcopy(w_history[end].ap_dict),
                               w_history[end].t_last_m);

            push!(u_history, u);
            push!(w_history, w_new);
            push!(target_trajectory_history, target_trajectory);
            if typeof(controller) == RSSACController
                push!(u_schedule_history, deepcopy(controller.u_schedule))
                u_schedule_history[end][current_time] = u;
                sort!(u_schedule_history[end])
                pop!(u_schedule_history[end])
                push!(prediction_dict_history, get_clipped_prediction_dict(controller.prediction_dict,
                                                                           controller.sim_param.num_samples,
                                                                           controller.sim_result.u_nominal_idx));
                push!(nominal_trajectory_history, deepcopy(get_position.(controller.sim_result.e_state_array)))
                push!(u_nominal_idx_history, controller.sim_result.u_nominal_idx);
            elseif typeof(controller) == CrowdNavController && !isnothing(ado_inputs)
                push!(state_measurement_history, convert_nodes_to_str(deepcopy(ado_inputs)));
            end

            if controller.sim_param.dtc > elapsed
                # sleep for the remaining time.
                sleep(controller.sim_param.dtc - elapsed);
            else
                t = @sprintf "Time %.2f" round(to_sec(current_time), digits=5)
                @warn "$(t) [sec]: This evaluation iteration took $(round(elapsed, digits=3)) [sec], which exceeds dtc."
            end
        else
            # End simulation
            break;
        end
    end

    # Add Terminal Cost
    total_position_cost +=
        terminal_position_cost(w_history[end].e_state, target_trajectory,
                               controller.sim_param.cost_param);
    for ap in values(w_history[end].ap_dict)
        total_collision_cost +=
            terminal_collision_cost(w_history[end].e_state, ap,
                                    controller.sim_param.cost_param);
    end

    # Finish All the Remaining Tasks
    if typeof(controller) == RSSACController && !isnothing(controller.prediction_task)
        wait(controller.prediction_task)
    end
    if !isnothing(controller.control_update_task)
        wait(controller.control_update_task)
    end

    # Return Simulation Results
    if typeof(controller) == RSSACController
        # Only Save Unique Prediction_Dicts for Saving Disk Space
        jj = 1;
        for ii = 2:length(prediction_dict_history)
            if prediction_dict_history[ii] == prediction_dict_history[jj]
                prediction_dict_history[ii] = nothing;
            else
                jj = ii
            end
        end

        eval_result =
        EvaluationResult(controller.sim_param, controller.cnt_param,
                         # scene_loader.param, controller.predictor.param,
                         nominal_control, target_trajectory_history,
                         measurement_schedule, to_sec(sim_end_time - w_init.t),
                         w_history, u_history, u_schedule_history, nominal_trajectory_history,
                         prediction_dict_history, u_nominal_idx_history,
                         total_control_cost, total_position_cost, total_collision_cost, log);
    elseif typeof(controller) == BICController
        eval_result =
        BICEvaluationResult(controller.sim_param, controller.cnt_param,
                            target_trajectory_history, measurement_schedule,
                            to_sec(sim_end_time - w_init.t),
                            w_history, u_history,
                            total_control_cost, total_position_cost, total_collision_cost, log);
    else
        eval_result =
        CrowdNavEvaluationResult(controller.sim_param, controller.cnt_param,
                                 target_trajectory_history, measurement_schedule,
                                 state_measurement_history, to_sec(sim_end_time - w_init.t),
                                 w_history, u_history,
                                 total_control_cost, total_position_cost, total_collision_cost, log);
    end
    if (typeof(controller) == RSSACController &&
       typeof(controller.predictor) == TrajectronPredictor) ||
       (typeof(controller) == CrowdNavController)
        return eval_result, controller, ado_inputs
    else
        return eval_result, controller, ado_positions
    end
end


text:
# module Bukdu

export JSON, JavaScript

# struct JSON
# end

struct JavaScript
end

struct Julia
end

"""
    ApplicationController
"""
abstract type ApplicationController end
abstract type AbstractPlug end
abstract type AbstractRender end

"""
    Render <: AbstractRender
"""
struct Render <: AbstractRender
    content_type::String
    writer::Function
    data::Any
end

struct Route
    C::Type{<:ApplicationController}
    action
    param_types::Dict{Symbol,DataType}
    path_params::Vector{Pair{String,Any}}
    pipelines::Vector{Function}
end

# module Bukdu


text:
using PersistenceDiagrams
using Test

using PersistenceDiagrams:
    BottleneckGraph,
    _adjacency_matrix,
    _depth_layers,
    _augmenting_paths,
    _augment!,
    _hopcroft_karp!

@testset "_adjacency_matrix" begin
    diag1 = PersistenceDiagram([(1, 2), (1, Inf)])
    diag2 = PersistenceDiagram([(3, 4), (5, 10), (7, Inf)])

    @test _adjacency_matrix(diag1, diag2) == [
        # 1,2 1,∞ 3,3 5,5 7,7
        2.0 Inf 1.0 Inf Inf  # 3,4
        8.0 Inf Inf 5.0 Inf  # 5,7
        Inf 6.0 Inf Inf Inf  # 7,∞
        1.0 Inf 0.0 0.0 0.0  # 1,1
        Inf Inf 0.0 0.0 0.0  # 1,1
    ]

    @test _adjacency_matrix(diag1, diag2) == _adjacency_matrix(diag2, diag1)'
end

@testset "Hopcroft-Karp" begin
    adj = [
        1 1 9
        1 9 1
        9 9 2
    ]
    graph = BottleneckGraph(adj, [0, 0, 0], [0, 0, 0], Int[], 3)

    @test _depth_layers(graph, 2) == ([1, 1, 1], 1)
    @test _augmenting_paths(graph, 2) == [[1, 1], [3, 2]]

    graph = BottleneckGraph(adj, [1, 0, 2], [1, 3, 0], Int[], 3)
    @test _depth_layers(graph, 2) == ([1, 2, 3], 3)
    @test _augmenting_paths(graph, 2) == [[2, 1, 1, 2, 3, 3]]
    _augment!(graph, [2, 1, 1, 2, 3, 3])
    @test graph.match_left == [2, 1, 3]
    @test graph.match_right == [2, 1, 3]

    graph = BottleneckGraph(adj, [0, 0, 0], [0, 0, 0], Int[], 3)
    @test _hopcroft_karp!(graph, 2) == ([1 => 2, 2 => 1, 3 => 3], true)
    @test _hopcroft_karp!(graph, 1) == ([1 => 1, 3 => 2], false)
end

@testset "Bottleneck basics" begin
    diag1 = PersistenceDiagram([(1, 2), (5, 8)])
    diag2 = PersistenceDiagram([(1, 2), (3, 4), (5, 10)])

    m = Bottleneck()(diag1, diag2; matching=true)
    @test matching(m) == [(5, 8) => (5, 10)]
    @test matching(m; bottleneck=false) ==
        [(1, 2) => (1, 2), (3, 3) => (3, 4), (5, 8) => (5, 10)]
    @test weight(m) ≡ 2.0
    @test Bottleneck()(diag1, diag2) ≡ 2.0
    @test Bottleneck()(diag1, diag2) == weight(matching(Bottleneck(), diag2, diag1))

    @test weight(Bottleneck(), diag1, diag1) ≡ 0.0
end

@testset "Wasserstein basics" begin
    diag1 = PersistenceDiagram([(1, 2), (5, 8)])
    diag2 = PersistenceDiagram([(1, 2), (3, 4), (5, 10)])

    m = Wasserstein()(diag1, diag2; matching=true)
    @test matching(m) == [(1, 2) => (1, 2), (3, 3) => (3, 4), (5, 8) => (5, 10)]
    @test weight(m) ≡ 3.0
    @test Wasserstein()(diag1, diag2) ≡ 3.0
    @test weight(matching(Wasserstein(2), diag1, diag2)) ≡ √(1 + 4)
    for i in 1:3
        @test Wasserstein(i)(diag1, diag2) ≡ Wasserstein(i)(diag2, diag1)
    end

    @test weight(Wasserstein(), diag1, diag1) ≡ 0.0
end

@testset "Infinite intervals" begin
    diag1 = PersistenceDiagram([(1, 2), (5, 8), (1, Inf)])
    diag2 = PersistenceDiagram([(1, 2), (3, 4), (5, 10)])
    diag3 = PersistenceDiagram([(1, 2), (3, 4), (5, 10), (1, Inf)])
    diag4 = PersistenceDiagram([(1, Inf)])
    diag5 = PersistenceDiagram([(2, Inf)])

    for Distance in (Bottleneck(), Wasserstein(), Wasserstein(2))
        @test Distance(diag1, diag2) ≡ Inf
        @test Distance(diag2, diag1) ≡ Inf
        @test weight(Distance(diag1, diag2; matching=true)) ≡ Inf
        @test isempty(matching(Distance, diag1, diag2))
        @test Distance(diag1, diag1) == 0
        @test 0 < Distance(diag1, diag3) < Inf
        @test Distance(diag4, diag5) == 1
    end
end

@testset "Different sizes" begin
    diag1 = PersistenceDiagram(vcat((90, 100), [(i, i + 1) for i in 1:100]))
    diag2 = PersistenceDiagram([(100, 110)])

    @test Bottleneck()(diag1, diag2) == 10
    @test Bottleneck()(diag2, diag1) == 10
    @test Wasserstein()(diag1, diag2) == 110
    @test Wasserstein()(diag2, diag1) == 110
    @test Wasserstein(2)(diag1, diag2) == √200
    @test Wasserstein(2)(diag2, diag1) == √200
end

@testset "Collections" begin
    diags1 = [
        PersistenceDiagram([(1, 2), (5, 8), (1, Inf)]),
        PersistenceDiagram(vcat((90, 100), [(i, i + 1) for i in 1:100])),
    ]
    diags2 = [
        PersistenceDiagram([(1, 2), (3, 4), (5, 10), (1, Inf)]),
        PersistenceDiagram([(100, 110)]),
    ]

    @test Bottleneck()(diags1, diags2) == 10
    @test Bottleneck()(diags1, diags2; matching=true)[2] isa PersistenceDiagrams.Matching
    @test Wasserstein()(diags1, diags2) == 113
    @test weight(Wasserstein(3)(diags1, diags2; matching=true)[1]) ==
        Wasserstein(3)(diags1[1], diags2[1])

    @test_throws ArgumentError Bottleneck()(diags1[1:1], diags2)
    @test_throws ArgumentError Wasserstein()(diags1, diags2[2:2])
end


text:
using Gadfly, DataArrays, RDatasets

set_default_plot_size(6inch, 3inch)

plot(dataset("datasets", "iris"), x="SepalLength", y="SepalWidth", Geom.point)