text:
using CPUTime
using Plots
using DifferentialEquations
using LinearAlgebra
using Random

function origins(q)
    """世界座標系から見た局所座標系の原点座標をまとめて計算"""

    L = 278e-3
    h = 64e-3
    H = 1104e-3
    L0 = 270.35e-3
    L1 = 69e-3
    L2 = 364.35e-3
    L3 = 69e-3
    L4 = 374.29e-3
    L5 = 10e-3
    L6 = 368.3e-3

    o_Wo = [
        0
        0
        0
    ]

    o_BL = [
        L
        -h
        H
    ]

    o_0 = [
        L
        -h
        H + L0
    ]

    o_1 = [
        L
        -h
        H + L0
    ]

    o_2 = [
        L + cos(pi/4)*L1*sin(q[1]) + cos(pi/4)*L1*cos(q[1])
        cos(pi/4)*L1*sin(q[1]) - cos(pi/4)*L1*cos(q[1]) - h
        H + L0
    ]

    o_3 = [
        L + cos(pi/4)*L1*sin(q[1]) + cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) + cos(pi/4)*L2*cos(q[1])*cos(q[2])
        cos(pi/4)*L1*sin(q[1]) - cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) - cos(pi/4)*L2*cos(q[1])*cos(q[2]) - h
        H + L0 - L2*sin(q[2])
    ]

    o_4 = [
        L + cos(pi/4)*L1*sin(q[1]) + cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) + cos(pi/4)*L2*cos(q[1])*cos(q[2]) + cos(pi/4)*L3*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3])) + cos(pi/4)*L3*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))
        cos(pi/4)*L1*sin(q[1]) - cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) - cos(pi/4)*L2*cos(q[1])*cos(q[2]) - cos(pi/4)*L3*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3])) + cos(pi/4)*L3*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1])) - h
        H + L0 - L2*sin(q[2]) - L3*cos(q[2])*cos(q[3])
    ]

    o_5 = [
        L + cos(pi/4)*L1*sin(q[1]) + cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) + cos(pi/4)*L2*cos(q[1])*cos(q[2]) + cos(pi/4)*L3*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3])) + cos(pi/4)*L3*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1])) - cos(pi/4)*L4*(-(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4])) - cos(pi/4)*L4*(-(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[4]))
        cos(pi/4)*L1*sin(q[1]) - cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) - cos(pi/4)*L2*cos(q[1])*cos(q[2]) - cos(pi/4)*L3*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3])) + cos(pi/4)*L3*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1])) + cos(pi/4)*L4*(-(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4])) - cos(pi/4)*L4*(-(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[4])) - h
        H + L0 - L2*sin(q[2]) - L3*cos(q[2])*cos(q[3]) - L4*(sin(q[2])*cos(q[4]) + sin(q[4])*cos(q[2])*cos(q[3]))
    ]

    o_6 = [
        L + cos(pi/4)*L1*sin(q[1]) + cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) + cos(pi/4)*L2*cos(q[1])*cos(q[2]) + cos(pi/4)*L3*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3])) + cos(pi/4)*L3*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1])) - cos(pi/4)*L4*(-(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4])) - cos(pi/4)*L4*(-(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[4])) + cos(pi/4)*L5*(((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5])) + cos(pi/4)*L5*(((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + (sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*sin(q[5]))
        cos(pi/4)*L1*sin(q[1]) - cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) - cos(pi/4)*L2*cos(q[1])*cos(q[2]) - cos(pi/4)*L3*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3])) + cos(pi/4)*L3*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1])) + cos(pi/4)*L4*(-(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4])) - cos(pi/4)*L4*(-(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[4])) - cos(pi/4)*L5*(((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5])) + cos(pi/4)*L5*(((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + (sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*sin(q[5])) - h
        H + L0 - L2*sin(q[2]) - L3*cos(q[2])*cos(q[3]) - L4*(sin(q[2])*cos(q[4]) + sin(q[4])*cos(q[2])*cos(q[3])) + L5*((sin(q[2])*sin(q[4]) - cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) + sin(q[3])*sin(q[5])*cos(q[2]))
    ]

    o_7 = [
        L + cos(pi/4)*L1*sin(q[1]) + cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) + cos(pi/4)*L2*cos(q[1])*cos(q[2]) + cos(pi/4)*L3*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3])) + cos(pi/4)*L3*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1])) - cos(pi/4)*L4*(-(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4])) - cos(pi/4)*L4*(-(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[4])) + cos(pi/4)*L5*(((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5])) + cos(pi/4)*L5*(((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + (sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*sin(q[5]))
        cos(pi/4)*L1*sin(q[1]) - cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) - cos(pi/4)*L2*cos(q[1])*cos(q[2]) - cos(pi/4)*L3*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3])) + cos(pi/4)*L3*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1])) + cos(pi/4)*L4*(-(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4])) - cos(pi/4)*L4*(-(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[4])) - cos(pi/4)*L5*(((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5])) + cos(pi/4)*L5*(((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + (sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*sin(q[5])) - h
        H + L0 - L2*sin(q[2]) - L3*cos(q[2])*cos(q[3]) - L4*(sin(q[2])*cos(q[4]) + sin(q[4])*cos(q[2])*cos(q[3])) + L5*((sin(q[2])*sin(q[4]) - cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) + sin(q[3])*sin(q[5])*cos(q[2]))
    ]

    o_GL = [
        L + cos(pi/4)*L1*sin(q[1]) + cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) + cos(pi/4)*L2*cos(q[1])*cos(q[2]) + cos(pi/4)*L3*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3])) + cos(pi/4)*L3*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1])) - cos(pi/4)*L4*(-(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4])) - cos(pi/4)*L4*(-(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[4])) + cos(pi/4)*L5*(((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5])) + cos(pi/4)*L5*(((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + (sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*sin(q[5])) + L6*(cos(pi/4)*(((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5]))*sin(q[6]) + cos(pi/4)*(((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + (sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*sin(q[5]))*sin(q[6]) + cos(pi/4)*((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) + cos(q[1])*cos(q[2])*cos(q[4]))*cos(q[6]) + cos(pi/4)*((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) + sin(q[1])*cos(q[2])*cos(q[4]))*cos(q[6]))
        cos(pi/4)*L1*sin(q[1]) - cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) - cos(pi/4)*L2*cos(q[1])*cos(q[2]) - cos(pi/4)*L3*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3])) + cos(pi/4)*L3*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1])) + cos(pi/4)*L4*(-(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4])) - cos(pi/4)*L4*(-(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[4])) - cos(pi/4)*L5*(((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5])) + cos(pi/4)*L5*(((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + (sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*sin(q[5])) + L6*(-cos(pi/4)*(((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5]))*sin(q[6]) + cos(pi/4)*(((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + (sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*sin(q[5]))*sin(q[6]) - cos(pi/4)*((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) + cos(q[1])*cos(q[2])*cos(q[4]))*cos(q[6]) + cos(pi/4)*((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) + sin(q[1])*cos(q[2])*cos(q[4]))*cos(q[6])) - h
        H + L0 - L2*sin(q[2]) - L3*cos(q[2])*cos(q[3]) - L4*(sin(q[2])*cos(q[4]) + sin(q[4])*cos(q[2])*cos(q[3])) + L5*((sin(q[2])*sin(q[4]) - cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) + sin(q[3])*sin(q[5])*cos(q[2])) + L6*(((sin(q[2])*sin(q[4]) - cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) + sin(q[3])*sin(q[5])*cos(q[2]))*sin(q[6]) + (-sin(q[2])*cos(q[4]) - sin(q[4])*cos(q[2])*cos(q[3]))*cos(q[6]))
    ]

    return [o_Wo o_BL o_0 o_1 o_2 o_3 o_4 o_5 o_6 o_7 o_GL]
end

# ヤコビ行列
function jacobi_GL(q)
    L = 278e-3
    h = 64e-3
    H = 1104e-3
    L0 = 270.35e-3
    L1 = 69e-3
    L2 = 364.35e-3
    L3 = 69e-3
    L4 = 374.29e-3
    L5 = 10e-3
    L6 = 368.3e-3
    z = [
        -cos(pi/4)*L1*sin(q[1]) + cos(pi/4)*L1*cos(q[1]) - cos(pi/4)*L2*sin(q[1])*cos(q[2]) + cos(pi/4)*L2*cos(q[1])*cos(q[2]) + cos(pi/4)*L3*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3])) + cos(pi/4)*L3*(sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1])) - cos(pi/4)*L4*((sin(q[1])*sin(q[3]) + sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4])) - cos(pi/4)*L4*((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) + sin(q[1])*cos(q[2])*cos(q[4])) + cos(pi/4)*L5*(((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5])) + cos(pi/4)*L5*(((sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1]))*cos(q[4]) + sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + (-sin(q[1])*sin(q[2])*sin(q[3]) - cos(q[1])*cos(q[3]))*sin(q[5])) + L6*((cos(pi/4)*((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + cos(pi/4)*(-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5]))*sin(q[6]) + (cos(pi/4)*((sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1]))*cos(q[4]) + sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + cos(pi/4)*(-sin(q[1])*sin(q[2])*sin(q[3]) - cos(q[1])*cos(q[3]))*sin(q[5]))*sin(q[6]) + (cos(pi/4)*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) + cos(pi/4)*cos(q[1])*cos(q[2])*cos(q[4]))*cos(q[6]) + (cos(pi/4)*(sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1]))*sin(q[4]) - cos(pi/4)*sin(q[1])*cos(q[2])*cos(q[4]))*cos(q[6])) -cos(pi/4)*L2*sin(q[1])*sin(q[2]) - cos(pi/4)*L2*sin(q[2])*cos(q[1]) - cos(pi/4)*L3*sin(q[1])*cos(q[2])*cos(q[3]) - cos(pi/4)*L3*cos(q[1])*cos(q[2])*cos(q[3]) - cos(pi/4)*L4*(sin(q[1])*sin(q[2])*cos(q[4]) + sin(q[1])*sin(q[4])*cos(q[2])*cos(q[3])) - cos(pi/4)*L4*(sin(q[2])*cos(q[1])*cos(q[4]) + sin(q[4])*cos(q[1])*cos(q[2])*cos(q[3])) + cos(pi/4)*L5*((sin(q[1])*sin(q[2])*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) + sin(q[1])*sin(q[3])*sin(q[5])*cos(q[2])) + cos(pi/4)*L5*((sin(q[2])*sin(q[4])*cos(q[1]) - cos(q[1])*cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) + sin(q[3])*sin(q[5])*cos(q[1])*cos(q[2])) + L6*((cos(pi/4)*(sin(q[1])*sin(q[2])*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) + cos(pi/4)*sin(q[1])*sin(q[3])*sin(q[5])*cos(q[2]))*sin(q[6]) + (cos(pi/4)*(sin(q[2])*sin(q[4])*cos(q[1]) - cos(q[1])*cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) + cos(pi/4)*sin(q[3])*sin(q[5])*cos(q[1])*cos(q[2]))*sin(q[6]) + (-cos(pi/4)*sin(q[1])*sin(q[2])*cos(q[4]) - cos(pi/4)*sin(q[1])*sin(q[4])*cos(q[2])*cos(q[3]))*cos(q[6]) + (-cos(pi/4)*sin(q[2])*cos(q[1])*cos(q[4]) - cos(pi/4)*sin(q[4])*cos(q[1])*cos(q[2])*cos(q[3]))*cos(q[6])) cos(pi/4)*L3*(-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1])) + cos(pi/4)*L3*(sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3])) - cos(pi/4)*L4*(sin(q[1])*cos(q[3]) - sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[4]) - cos(pi/4)*L4*(-sin(q[1])*sin(q[2])*sin(q[3]) - cos(q[1])*cos(q[3]))*sin(q[4]) + cos(pi/4)*L5*((sin(q[1])*sin(q[3]) + sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*cos(q[4])*cos(q[5])) + cos(pi/4)*L5*((sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*cos(q[4])*cos(q[5]) + (sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1]))*sin(q[5])) + L6*((cos(pi/4)*(sin(q[1])*sin(q[3]) + sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[5]) + cos(pi/4)*(-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*cos(q[4])*cos(q[5]))*sin(q[6]) + cos(pi/4)*(-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[4])*cos(q[6]) + (cos(pi/4)*(sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*cos(q[4])*cos(q[5]) + cos(pi/4)*(sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1]))*sin(q[5]))*sin(q[6]) + cos(pi/4)*(sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*sin(q[4])*cos(q[6])) -cos(pi/4)*L4*((sin(q[1])*sin(q[3]) + sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) + sin(q[4])*cos(q[1])*cos(q[2])) - cos(pi/4)*L4*((sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1]))*cos(q[4]) + sin(q[1])*sin(q[4])*cos(q[2])) + cos(pi/4)*L5*(-(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4]))*cos(q[5]) + cos(pi/4)*L5*(-(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[4]))*cos(q[5]) + L6*(cos(pi/4)*(-(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4]))*sin(q[6])*cos(q[5]) + (cos(pi/4)*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - cos(pi/4)*sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[6]) + cos(pi/4)*(-(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[4]))*sin(q[6])*cos(q[5]) + (cos(pi/4)*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - cos(pi/4)*sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[6])) cos(pi/4)*L5*(-((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*sin(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*cos(q[5])) + cos(pi/4)*L5*(-((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*sin(q[5]) + (sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*cos(q[5])) + L6*((-cos(pi/4)*((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*sin(q[5]) + cos(pi/4)*(-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*cos(q[5]))*sin(q[6]) + (-cos(pi/4)*((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*sin(q[5]) + cos(pi/4)*(sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*cos(q[5]))*sin(q[6])) L6*((cos(pi/4)*((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + cos(pi/4)*(-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5]))*cos(q[6]) + (cos(pi/4)*((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + cos(pi/4)*(sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*sin(q[5]))*cos(q[6]) - (cos(pi/4)*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) + cos(pi/4)*cos(q[1])*cos(q[2])*cos(q[4]))*sin(q[6]) - (cos(pi/4)*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) + cos(pi/4)*sin(q[1])*cos(q[2])*cos(q[4]))*sin(q[6])) 0
        cos(pi/4)*L1*sin(q[1]) + cos(pi/4)*L1*cos(q[1]) + cos(pi/4)*L2*sin(q[1])*cos(q[2]) + cos(pi/4)*L2*cos(q[1])*cos(q[2]) + cos(pi/4)*L3*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3])) - cos(pi/4)*L3*(sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1])) - cos(pi/4)*L4*((sin(q[1])*sin(q[3]) + sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4])) + cos(pi/4)*L4*((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) + sin(q[1])*cos(q[2])*cos(q[4])) + cos(pi/4)*L5*(((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5])) - cos(pi/4)*L5*(((sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1]))*cos(q[4]) + sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + (-sin(q[1])*sin(q[2])*sin(q[3]) - cos(q[1])*cos(q[3]))*sin(q[5])) + L6*((cos(pi/4)*((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) + cos(pi/4)*(-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5]))*sin(q[6]) + (-cos(pi/4)*((sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1]))*cos(q[4]) + sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) - cos(pi/4)*(-sin(q[1])*sin(q[2])*sin(q[3]) - cos(q[1])*cos(q[3]))*sin(q[5]))*sin(q[6]) + (cos(pi/4)*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) + cos(pi/4)*cos(q[1])*cos(q[2])*cos(q[4]))*cos(q[6]) + (-cos(pi/4)*(sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1]))*sin(q[4]) + cos(pi/4)*sin(q[1])*cos(q[2])*cos(q[4]))*cos(q[6])) -cos(pi/4)*L2*sin(q[1])*sin(q[2]) + cos(pi/4)*L2*sin(q[2])*cos(q[1]) - cos(pi/4)*L3*sin(q[1])*cos(q[2])*cos(q[3]) + cos(pi/4)*L3*cos(q[1])*cos(q[2])*cos(q[3]) - cos(pi/4)*L4*(sin(q[1])*sin(q[2])*cos(q[4]) + sin(q[1])*sin(q[4])*cos(q[2])*cos(q[3])) + cos(pi/4)*L4*(sin(q[2])*cos(q[1])*cos(q[4]) + sin(q[4])*cos(q[1])*cos(q[2])*cos(q[3])) + cos(pi/4)*L5*((sin(q[1])*sin(q[2])*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) + sin(q[1])*sin(q[3])*sin(q[5])*cos(q[2])) - cos(pi/4)*L5*((sin(q[2])*sin(q[4])*cos(q[1]) - cos(q[1])*cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) + sin(q[3])*sin(q[5])*cos(q[1])*cos(q[2])) + L6*((cos(pi/4)*(sin(q[1])*sin(q[2])*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) + cos(pi/4)*sin(q[1])*sin(q[3])*sin(q[5])*cos(q[2]))*sin(q[6]) + (-cos(pi/4)*(sin(q[2])*sin(q[4])*cos(q[1]) - cos(q[1])*cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) - cos(pi/4)*sin(q[3])*sin(q[5])*cos(q[1])*cos(q[2]))*sin(q[6]) + (-cos(pi/4)*sin(q[1])*sin(q[2])*cos(q[4]) - cos(pi/4)*sin(q[1])*sin(q[4])*cos(q[2])*cos(q[3]))*cos(q[6]) + (cos(pi/4)*sin(q[2])*cos(q[1])*cos(q[4]) + cos(pi/4)*sin(q[4])*cos(q[1])*cos(q[2])*cos(q[3]))*cos(q[6])) -cos(pi/4)*L3*(-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1])) + cos(pi/4)*L3*(sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3])) + cos(pi/4)*L4*(sin(q[1])*cos(q[3]) - sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[4]) - cos(pi/4)*L4*(-sin(q[1])*sin(q[2])*sin(q[3]) - cos(q[1])*cos(q[3]))*sin(q[4]) - cos(pi/4)*L5*((sin(q[1])*sin(q[3]) + sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*cos(q[4])*cos(q[5])) + cos(pi/4)*L5*((sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*cos(q[4])*cos(q[5]) + (sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1]))*sin(q[5])) + L6*((-cos(pi/4)*(sin(q[1])*sin(q[3]) + sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[5]) - cos(pi/4)*(-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*cos(q[4])*cos(q[5]))*sin(q[6]) - cos(pi/4)*(-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[4])*cos(q[6]) + (cos(pi/4)*(sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*cos(q[4])*cos(q[5]) + cos(pi/4)*(sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1]))*sin(q[5]))*sin(q[6]) + cos(pi/4)*(sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*sin(q[4])*cos(q[6])) cos(pi/4)*L4*((sin(q[1])*sin(q[3]) + sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) + sin(q[4])*cos(q[1])*cos(q[2])) - cos(pi/4)*L4*((sin(q[1])*sin(q[2])*cos(q[3]) - sin(q[3])*cos(q[1]))*cos(q[4]) + sin(q[1])*sin(q[4])*cos(q[2])) - cos(pi/4)*L5*(-(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4]))*cos(q[5]) + cos(pi/4)*L5*(-(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[4]))*cos(q[5]) + L6*(-cos(pi/4)*(-(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(q[1])*cos(q[2])*cos(q[4]))*sin(q[6])*cos(q[5]) + (-cos(pi/4)*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) + cos(pi/4)*sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[6]) + cos(pi/4)*(-(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) - sin(q[1])*cos(q[2])*cos(q[4]))*sin(q[6])*cos(q[5]) + (cos(pi/4)*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - cos(pi/4)*sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[6])) -cos(pi/4)*L5*(-((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*sin(q[5]) + (-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*cos(q[5])) + cos(pi/4)*L5*(-((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*sin(q[5]) + (sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*cos(q[5])) + L6*((cos(pi/4)*((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*sin(q[5]) - cos(pi/4)*(-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*cos(q[5]))*sin(q[6]) + (-cos(pi/4)*((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*sin(q[5]) + cos(pi/4)*(sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*cos(q[5]))*sin(q[6])) L6*((-cos(pi/4)*((-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*cos(q[4]) - sin(q[4])*cos(q[1])*cos(q[2]))*cos(q[5]) - cos(pi/4)*(-sin(q[1])*cos(q[3]) + sin(q[2])*sin(q[3])*cos(q[1]))*sin(q[5]))*cos(q[6]) + (cos(pi/4)*((-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*cos(q[4]) - sin(q[1])*sin(q[4])*cos(q[2]))*cos(q[5]) + cos(pi/4)*(sin(q[1])*sin(q[2])*sin(q[3]) + cos(q[1])*cos(q[3]))*sin(q[5]))*cos(q[6]) - (-cos(pi/4)*(-sin(q[1])*sin(q[3]) - sin(q[2])*cos(q[1])*cos(q[3]))*sin(q[4]) - cos(pi/4)*cos(q[1])*cos(q[2])*cos(q[4]))*sin(q[6]) - (cos(pi/4)*(-sin(q[1])*sin(q[2])*cos(q[3]) + sin(q[3])*cos(q[1]))*sin(q[4]) + cos(pi/4)*sin(q[1])*cos(q[2])*cos(q[4]))*sin(q[6])) 0
        0 -L2*cos(q[2]) + L3*sin(q[2])*cos(q[3]) - L4*(-sin(q[2])*sin(q[4])*cos(q[3]) + cos(q[2])*cos(q[4])) + L5*((sin(q[2])*cos(q[3])*cos(q[4]) + sin(q[4])*cos(q[2]))*cos(q[5]) - sin(q[2])*sin(q[3])*sin(q[5])) + L6*(((sin(q[2])*cos(q[3])*cos(q[4]) + sin(q[4])*cos(q[2]))*cos(q[5]) - sin(q[2])*sin(q[3])*sin(q[5]))*sin(q[6]) + (sin(q[2])*sin(q[4])*cos(q[3]) - cos(q[2])*cos(q[4]))*cos(q[6])) L3*sin(q[3])*cos(q[2]) + L4*sin(q[3])*sin(q[4])*cos(q[2]) + L5*(sin(q[3])*cos(q[2])*cos(q[4])*cos(q[5]) + sin(q[5])*cos(q[2])*cos(q[3])) + L6*((sin(q[3])*cos(q[2])*cos(q[4])*cos(q[5]) + sin(q[5])*cos(q[2])*cos(q[3]))*sin(q[6]) + sin(q[3])*sin(q[4])*cos(q[2])*cos(q[6])) -L4*(-sin(q[2])*sin(q[4]) + cos(q[2])*cos(q[3])*cos(q[4])) + L5*(sin(q[2])*cos(q[4]) + sin(q[4])*cos(q[2])*cos(q[3]))*cos(q[5]) + L6*((sin(q[2])*sin(q[4]) - cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[6]) + (sin(q[2])*cos(q[4]) + sin(q[4])*cos(q[2])*cos(q[3]))*sin(q[6])*cos(q[5])) L5*(-(sin(q[2])*sin(q[4]) - cos(q[2])*cos(q[3])*cos(q[4]))*sin(q[5]) + sin(q[3])*cos(q[2])*cos(q[5])) + L6*(-(sin(q[2])*sin(q[4]) - cos(q[2])*cos(q[3])*cos(q[4]))*sin(q[5]) + sin(q[3])*cos(q[2])*cos(q[5]))*sin(q[6]) L6*(((sin(q[2])*sin(q[4]) - cos(q[2])*cos(q[3])*cos(q[4]))*cos(q[5]) + sin(q[3])*sin(q[5])*cos(q[2]))*cos(q[6]) - (-sin(q[2])*cos(q[4]) - sin(q[4])*cos(q[2])*cos(q[3]))*sin(q[6])) 0
    ]
    return z
end

function inv_kinematics(xd, trial=10, α=1, allowance_error=0.001)
    """逆運動学を解く"""

    q1_min, q1_max = -141, 51
    q2_min, q2_max = -123, 60
    q3_min, q3_max = -173, 173
    q4_min, q4_max = -3, 150
    q5_min, q5_max = -175, 175
    q6_min, q6_max = -90, 120
    q7_min, q7_max = -175, 175
    q_min = [q1_min, q2_min, q3_min, q4_min, q5_min, q6_min, q7_min] * π / 180
    q_max = [q1_max, q2_max, q3_max, q4_max, q5_max, q6_max, q7_max] * π / 180

    Δt = 0.01
    for i in 1:trial
        println(i, "回目の試行")
        global q = (rand((7)) .-0.5) .* π  # ランダムな初期値
        for t in 0:Δt:10
            Δx = xd - origins(q)[:, 11]
            error = norm(Δx)
            if error < allowance_error
                break
            else
                dq = α * pinv(jacobi_GL(q)) * Δx
                global q += dq * Δt
            end
        end

        if q_min <= q <= q_max
            return q
        else
            continue
        end
    end
end

xd = [0.3, -0.6, 1]  # 所望のエンドエフェクタ位置
qd = inv_kinematics(xd)  # 逆運動学の解を計算

### plot ###
plot(aspect_ratio=1, xlabel="x", ylabel="y", zlabel="z")
plot!(
    [xd[1]], [xd[2]], [xd[3]],
    marker=:circle, markersize = 10, label="desired ef position"
)

data = origins(qd)  # アームの位置データを用意
plot!(data[1, :], data[2, :], data[3, :], marker=:circle, label="arm")


text:
abstract type BoundaryCondition end
struct ZeroGradient <: BoundaryCondition end
struct ZeroLaplacian <: BoundaryCondition end
struct Free <: BoundaryCondition end

abstract type PoissonSolver end

struct Jacobi <: PoissonSolver
    max_deviation::Float
    max_iterations::UInt64
    Jacobi(; max_deviation = 1e-6, max_iterations = 100000) =
        @check(max_deviation > 0.0) && new(max_deviation, max_iterations)
end

struct GaussSeidel <: PoissonSolver
    relaxation_factor::Float
    max_deviation::Float
    max_iterations::UInt64
    GaussSeidel(;
        relaxation_factor = 1.0,
        max_deviation = 1e-6,
        max_iterations = 100000,
    ) =
        @check(relaxation_factor >= 1.0 && relaxation_factor < 2.0) &&
        @check(max_deviation > 0.0) &&
        new(relaxation_factor, max_deviation, max_iterations)
end

struct Analytical <: PoissonSolver end

function solve_poisson!(
    field::Array{<:Any,N},
    source::Array{<:Any,N},
    grid::Grid{N},
    boundary_condition::Type{<:BoundaryCondition},
    solver::Jacobi,
) where {N}
    old_field = field
    new_field = similar(field)

    total_source = sum(abs.(source))
    if total_source == 0
        field .= 0
        return
    end

    set_poisson_boundaries!(old_field, source, grid, boundary_condition)

    n_iter = 0
    deviation_scale = 1 / compute_solution_scale(total_source, grid, Dim{N}())
    while true
        max_difference = update_poisson!(
            new_field,
            old_field,
            source,
            grid,
            boundary_condition,
            solver,
        )
        n_iter += 1

        if max_difference * deviation_scale < solver.max_deviation
            field[:] = new_field
            break
        elseif n_iter >= solver.max_iterations
            println(stderr, "Warning: Jacobi Poisson solver did not converge")
            field[:] = new_field
            break
        end

        tmp = old_field
        old_field = new_field
        new_field = tmp
    end
end

function solve_poisson!(
    field::Array{<:Any,N},
    source::Array{<:Any,N},
    grid::Grid{N},
    boundary_condition::Type{<:BoundaryCondition},
    solver::GaussSeidel,
) where {N}
    total_source = sum(abs.(source))
    if total_source == 0
        field .= 0
        return
    end

    set_poisson_boundaries!(field, source, grid, boundary_condition)

    n_iter = 0
    deviation_scale = 1 / compute_solution_scale(total_source, grid, Dim{N}())
    while true
        max_difference =
            update_poisson!(field, source, grid, boundary_condition, solver)
        n_iter += 1
        if max_difference * deviation_scale < solver.max_deviation
            break
        elseif n_iter >= solver.max_iterations
            println(
                stderr,
                "Warning: Gauss Seidel Poisson solver did not converge",
            )
            break
        end
    end
end

function solve_poisson!(
    field::Array{<:Any,3},
    source::Array{<:Any,3},
    grid::Grid{3},
    boundary_condition::Type{Free},
    solver::Type{Analytical},
)
    source_indices = findall(!iszero, source)
    if length(source_indices) == 0
        field .= 0
        return
    end
    shape = size(grid)
    coords = lower_edges(grid)
    grid_cell_volume = cell_volume(grid)
    for k = 1:shape[3]
        for j = 1:shape[2]
            for i = 1:shape[1]
                field[i, j, k] = compute_free_potential(
                    SVector(coords[1][i], coords[2][j], coords[3][k]),
                    source,
                    grid_cell_volume,
                    coords,
                    source_indices,
                )
            end
        end
    end
end

function solve_poisson!(
    field::Array{<:Any,2},
    source::Array{<:Any,2},
    grid::Grid{2},
    boundary_condition::Type{Free},
    solver::Type{Analytical},
)
    source_indices = findall(!iszero, source)
    shape = size(grid)
    coords = lower_edges(grid)
    grid_cell_volume = cell_volume(grid)
    for j = 1:shape[2]
        for i = 1:shape[1]
            field[i, j] = compute_free_potential(
                SVector(coords[1][i], coords[2][j]),
                source,
                grid_cell_volume,
                coords,
                source_indices,
            )
        end
    end
end

function compute_solution_scale(total_source, grid::Grid{3}, ::Dim{3})
    total_source * cell_volume(grid) * length(extents(grid)) /
    sum(extents(grid))
end

function compute_solution_scale(total_source, grid::Grid{2}, ::Dim{2})
    total_source *
    cell_volume(grid) *
    log(ℯ - 1 + sum(extents(grid)) / length(extents(grid)))
end

function update_poisson!(
    dest::Array{<:Any,N},
    field::Array{<:Any,N},
    source::Array{<:Any,N},
    grid::Grid{N},
    boundary_condition::Type{<:BoundaryCondition},
    solver::Jacobi,
) where {N}
    @dbgasserteq(size(dest), size(grid))
    @dbgasserteq(size(source), size(grid))
    @dbgassert (dest !== field) && (dest !== source)

    scale = 0.5 / sum(cell_extents⁻²(grid))
    coefs = cell_extents⁻²(grid) .* scale

    dest[interior(grid)...] .=
        poisson_jacobi_update_field(field, grid, coefs) .-
        source[interior(grid)...] .* scale

    update_poisson_boundaries!(dest, field, boundary_condition)

    max_difference = maximum(abs.(dest - field))
    max_difference
end

function poisson_jacobi_update_field(
    field::Array{<:Any,3},
    grid::Grid{3},
    coefs,
)
    shiftedsum(field, grid, UpDown, DIM1) .* coefs[1] .+
    shiftedsum(field, grid, UpDown, DIM2) .* coefs[2] .+
    shiftedsum(field, grid, UpDown, DIM3) .* coefs[3]
end

function poisson_jacobi_update_field(
    field::Array{<:Any,2},
    grid::Grid{2},
    coefs,
)
    shiftedsum(field, grid, UpDown, DIM1) .* coefs[1] .+
    shiftedsum(field, grid, UpDown, DIM2) .* coefs[2]
end

function update_poisson!(
    field::Array{<:Any,N},
    source::Array{<:Any,N},
    grid::Grid{N},
    boundary_condition::Type{<:BoundaryCondition},
    solver::GaussSeidel,
) where {N}
    @dbgasserteq(size(field), size(grid))
    @dbgasserteq(size(source), size(grid))
    @dbgassert field !== source

    source_scale = 0.5 * solver.relaxation_factor / sum(cell_extents⁻²(grid))
    coefs = cell_extents⁻²(grid) .* source_scale
    SOR_scale = 1.0 - solver.relaxation_factor

    max_difference = poisson_gauss_seidel_update_field!(
        field,
        source,
        coefs,
        source_scale,
        SOR_scale,
    )
    update_poisson_boundaries!(field, boundary_condition)
    max_difference
end

function poisson_gauss_seidel_update_field!(
    field::Array{<:Any,3},
    source::Array{<:Any,3},
    coefs,
    source_scale,
    SOR_scale,
)
    shape = size(field)
    max_difference = 0.0
    for k = 2:shape[3]-1
        for j = 2:shape[2]-1
            for i = 2:shape[1]-1
                new_value =
                    (field[i-1, j, k] + field[i+1, j, k]) * coefs[1] +
                    (field[i, j-1, k] + field[i, j+1, k]) * coefs[2] +
                    (field[i, j, k-1] + field[i, j, k+1]) * coefs[3] -
                    source[i, j, k] * source_scale + field[i, j, k] * SOR_scale

                max_difference =
                    max(max_difference, abs(new_value - field[i, j, k]))

                field[i, j, k] = new_value
            end
        end
    end
    max_difference
end

function poisson_gauss_seidel_update_field!(
    field::Array{<:Any,2},
    source::Array{<:Any,2},
    coefs,
    source_scale,
    SOR_scale,
)
    shape = size(field)
    max_difference = 0.0
    for j = 2:shape[2]-1
        for i = 2:shape[1]-1
            new_value =
                (field[i-1, j] + field[i+1, j]) * coefs[1] +
                (field[i, j-1] + field[i, j+1]) * coefs[2] -
                source[i, j] * source_scale + field[i, j] * SOR_scale

            max_difference = max(max_difference, abs(new_value - field[i, j]))

            field[i, j] = new_value
        end
    end
    max_difference
end

function set_poisson_boundaries!(
    field::Array{<:Any,3},
    source::Array{<:Any,3},
    grid::Grid{3},
    ::Type{Free},
)
    source_indices = findall(!iszero, source)
    shape = size(grid)
    coords = lower_edges(grid)
    grid_cell_volume = cell_volume(grid)
    for k in [1, shape[3]]
        for j = 1:shape[2]
            for i = 1:shape[1]
                field[i, j, k] = compute_free_potential(
                    SVector(coords[1][i], coords[2][j], coords[3][k]),
                    source,
                    grid_cell_volume,
                    coords,
                    source_indices,
                )
            end
        end
    end
    for k = 2:shape[3]-1
        for j in [1, shape[2]]
            for i = 1:shape[1]
                field[i, j, k] = compute_free_potential(
                    SVector(coords[1][i], coords[2][j], coords[3][k]),
                    source,
                    grid_cell_volume,
                    coords,
                    source_indices,
                )
            end
        end
    end
    for k = 2:shape[3]-1
        for j = 2:shape[2]-1
            for i in [1, shape[1]]
                field[i, j, k] = compute_free_potential(
                    SVector(coords[1][i], coords[2][j], coords[3][k]),
                    source,
                    grid_cell_volume,
                    coords,
                    source_indices,
                )
            end
        end
    end
end

function set_poisson_boundaries!(
    field::Array{<:Any,2},
    source::Array{<:Any,2},
    grid::Grid{2},
    ::Type{Free},
)
    source_indices = findall(!iszero, source)
    shape = size(grid)
    coords = lower_edges(grid)
    grid_cell_area = cell_volume(grid)
    for j in [1, shape[2]]
        for i = 1:shape[1]
            field[i, j] = compute_free_potential(
                SVector(coords[1][i], coords[2][j]),
                source,
                grid_cell_area,
                coords,
                source_indices,
            )
        end
    end
    for j = 2:shape[2]-1
        for i in [1, shape[1]]
            field[i, j] = compute_free_potential(
                SVector(coords[1][i], coords[2][j]),
                source,
                grid_cell_area,
                coords,
                source_indices,
            )
        end
    end
end

function compute_free_potential(
    potential_position::SVector{N,<:Any},
    source::Array{<:Any,N},
    grid_cell_volume,
    coordinates::SVector{N,Vector{T}},
    source_indices::Vector{CartesianIndex{N}},
) where {N,T<:Any}
    potential = 0.0
    for source_index in source_indices
        distance² = 0.0
        for n = 1:N
            distance² +=
                (potential_position[n] - coordinates[n][source_index[n]])^2
        end
        potential += compute_free_potential_contribution(
            grid_cell_volume * source[source_index],
            sqrt(distance²),
            Dim{N}(),
        )
    end
    potential
end

compute_free_potential_contribution(charge, distance, ::Dim{3}) =
    -charge / (4π * distance)

compute_free_potential_contribution(charge, distance, ::Dim{2}) =
    charge * log(distance) / 2π

function update_poisson_boundaries!(::Array{<:Any,N}, ::Type{Free}) where {N} end

function update_poisson_boundaries!(
    dest::Array{<:Any,3},
    field::Array{<:Any,3},
    ::Type{Free},
)
    dest[1:end, 1:end, 1] .= field[1:end, 1:end, 1]
    dest[1:end, 1:end, end] .= field[1:end, 1:end, end]
    dest[1:end, 1, 2:end] .= field[1:end, 1, 2:end]
    dest[1:end, end, 2:end] .= field[1:end, end, 2:end]
    dest[1, 2:end, 2:end] .= field[1, 2:end, 2:end]
    dest[end, 2:end, 2:end] .= field[end, 2:end, 2:end]
end

function update_poisson_boundaries!(
    dest::Array{<:Any,2},
    field::Array{<:Any,2},
    ::Type{Free},
)
    dest[1:end, 1] .= field[1:end, 1]
    dest[1:end, end] .= field[1:end, end]
    dest[1, 2:end] .= field[1, 2:end]
    dest[end, 2:end] .= field[end, 2:end]
end

set_poisson_boundaries!(
    field::Array{<:Any,N},
    ::Array{<:Any,N},
    ::Grid{N},
    boundary_condition::Type{ZeroGradient},
) where {N} = update_poisson_boundaries!(field, boundary_condition)

set_poisson_boundaries!(
    field::Array{<:Any,N},
    ::Array{<:Any,N},
    ::Grid{N},
    boundary_condition::Type{ZeroLaplacian},
) where {N} = update_poisson_boundaries!(field, boundary_condition)

function update_poisson_boundaries!(
    field::Array{<:Any,N},
    boundary_condition::Type{<:BoundaryCondition},
) where {N}
    update_poisson_boundaries!(field, field, boundary_condition)
end

function update_poisson_boundaries!(
    dest::Array{<:Any,3},
    field::Array{<:Any,3},
    ::Type{ZeroGradient},
)
    dest[2:end-1, 2:end-1, 1] .=
        ghost_cell.(field[2:end-1, 2:end-1, 2], ZeroGradient)
    dest[2:end-1, 2:end-1, end] .=
        ghost_cell.(field[2:end-1, 2:end-1, end-1], ZeroGradient)

    dest[2:end-1, 1, 2:end-1] .=
        ghost_cell.(field[2:end-1, 2, 2:end-1], ZeroGradient)
    dest[2:end-1, end, 2:end-1] .=
        ghost_cell.(field[2:end-1, end-1, 2:end-1], ZeroGradient)

    dest[1, 2:end-1, 2:end-1] .=
        ghost_cell.(field[2, 2:end-1, 2:end-1], ZeroGradient)
    dest[end, 2:end-1, 2:end-1] .=
        ghost_cell.(field[end-1, 2:end-1, 2:end-1], ZeroGradient)
end

function update_poisson_boundaries!(
    dest::Array{<:Any,2},
    field::Array{<:Any,2},
    ::Type{ZeroGradient},
)
    dest[2:end-1, 1] .= ghost_cell.(field[2:end-1, 2], ZeroGradient)
    dest[2:end-1, end] .= ghost_cell.(field[2:end-1, end-1], ZeroGradient)

    dest[1, 2:end-1] .= ghost_cell.(field[2, 2:end-1], ZeroGradient)
    dest[end, 2:end-1] .= ghost_cell.(field[end-1, 2:end-1], ZeroGradient)
end

function update_poisson_boundaries!(
    dest::Array{<:Any,3},
    field::Array{<:Any,3},
    ::Type{ZeroLaplacian},
)
    dest[2:end-1, 2:end-1, 1] .=
        ghost_cell.(
            field[2:end-1, 2:end-1, 2],
            field[2:end-1, 2:end-1, 3],
            ZeroLaplacian,
        )
    dest[2:end-1, 2:end-1, end] .=
        ghost_cell.(
            field[2:end-1, 2:end-1, end-1],
            field[2:end-1, 2:end-1, end-2],
            ZeroLaplacian,
        )

    dest[2:end-1, 1, 2:end-1] .=
        ghost_cell.(
            field[2:end-1, 2, 2:end-1],
            field[2:end-1, 3, 2:end-1],
            ZeroLaplacian,
        )
    dest[2:end-1, end, 2:end-1] .=
        ghost_cell.(
            field[2:end-1, end-1, 2:end-1],
            field[2:end-1, end-2, 2:end-1],
            ZeroLaplacian,
        )

    dest[1, 2:end-1, 2:end-1] .=
        ghost_cell.(
            field[2, 2:end-1, 2:end-1],
            field[3, 2:end-1, 2:end-1],
            ZeroLaplacian,
        )
    dest[end, 2:end-1, 2:end-1] .=
        ghost_cell.(
            field[end-1, 2:end-1, 2:end-1],
            field[end-2, 2:end-1, 2:end-1],
            ZeroLaplacian,
        )
end

function update_poisson_boundaries!(
    dest::Array{<:Any,2},
    field::Array{<:Any,2},
    ::Type{ZeroLaplacian},
)
    dest[2:end-1, 1] .=
        ghost_cell.(field[2:end-1, 2], field[2:end-1, 3], ZeroLaplacian)
    dest[2:end-1, end] .=
        ghost_cell.(field[2:end-1, end-1], field[2:end-1, end-2], ZeroLaplacian)

    dest[1, 2:end-1] .=
        ghost_cell.(field[2, 2:end-1], field[3, 2:end-1], ZeroLaplacian)
    dest[end, 2:end-1] .=
        ghost_cell.(field[end-1, 2:end-1], field[end-2, 2:end-1], ZeroLaplacian)
end

function ghost_cell(u₂, ::Type{ZeroGradient})
    u₂
end

function ghost_cell(u₂, u₃, ::Type{ZeroGradient})
    (4 * u₂ - u₃) / 3
end

function ghost_cell(u₂, u₃, ::Type{ZeroLaplacian})
    2 * u₂ - u₃
end

function ghost_cell(u₂, u₃, u₄, ::Type{ZeroLaplacian})
    0.5 * (5 * u₂ - 4 * u₃ + u₄)
end

ext: jl

max_stars_repo_name: lars-frogner/PIC

max_issues_repo_name: lars-frogner/PIC

max_forks_repo_name: lars-frogner/PIC


text:
x = randn(3)   # Random vector

A = randn(3,3) # Random matrix

using LinearAlgebra

y = randn(3)
x - 3y

x_dot_y_1 = dot(x,y)   # Dot product
x_dot_y_2 = x ⋅ y      # Same thing

x_cross_y = cross(x,y)

a = norm(x)                 # 2-norm of x
b = sqrt(sum(abs.(x).^2))   # Should be the same
a - b                       # Confirm

B = randn(4,3)
C = B*A     # OK, since B is 4-by-3 and A is 3-by-3

AA = A*A     # Square of matrix
A2 = A.*A    # Square of each entry in matrix
A2 - AA      # These are not the same

A_to_the_power_of_2 = A^2       # Same as A*A
A_to_the_power_of_3_5 = A^3.5   # A^3.5, much harder to compute!

BT = transpose(B)     # B is 4-by-3, so BT is 3-by-4
BT2 = B'              # Same thing (since B is real so conjugate does not matter)

A * B'                # Well-defined, since A is 3-by-3 and B' is 3-by-4

x_dot_y_3 = x' * y

a = randn(3,1)   # Column vector
b = randn(1,3)   # Row vector

a_dot_b = dot(a,b)

A = randn(3,3)
println("det(A) = ", det(A))
println("tr(A) = ", tr(A))
println("inv(A) = ", inv(A))

A = randn(3,3)
lambda = eigvals(A)     # Eigenvalues

X = eigvecs(A)          # Eigenvectors

norm(A*X - X*Diagonal(lambda))     # Confirm A*X = X*LAMBDA

using PyPlot
n = 1000
A = randn(n,n)
l = eigvals(A)
plot(real(l), imag(l), ".")
axis("equal");
# Draw circle
radius = sqrt(n)
phi = 2π*(0:100)/100
plot(radius*cos.(phi), radius*sin.(phi));

num = rand(-10:10, 3, 3)
den = rand(1:10, 3, 3)
rat = num .// den
display(rat)
inv(rat)


text:
# # Generate a Mandelbrot set with Julia

# Notebook from [HelloJulia.jl](https://github.com/ablaom/HelloJulia.jl)

# Instantiate package environment:

using Pkg
Pkg.activate(@__DIR__)
Pkg.instantiate()

#-

using Plots
plotly() # choose plotting backend

#-

function mandel(z)
    c = z     # starting value and constant shift
    max_iterations = 20
    for n = 1:max_iterations
        if abs(z) > 2
            return n-1
        end
        z = z^2 + c
    end
    return max_iterations
end

#-

xs = -2.5:0.005:0.75
ys = -1.5:0.005:1.5
z = [mandel(x + im*y) for y in ys, x in xs]
heatmap(xs, ys, z)

#-

savefig("mandelbrot.html")


text:
julia_version = v"1.5.3"
gcc_version = v"8"
include("../common.jl")

max_stars_repo_name: sharanry/Yggdrasil

max_issues_repo_name: sharanry/Yggdrasil

max_forks_repo_name: sharanry/Yggdrasil


text:
function RK4(du, u, p, update_du!, Δt::T) where T<:AbstractFloat

    # initialize derivative ponderated sum
    du_sum = zeros(eltype(u),size(u))

    # increment derivative ponderated sum
    update_du!(du,u,p,0.0)
    du_sum .+= du

    update_du!(du,u.+((Δt/2.0).*du),p,0.0)
    du_sum .+= 2.0 .* du

    update_du!(du,u.+(Δt/2.0).*du,p,0.0)
    du_sum .+= 2.0 .* du

    update_du!(du,u.+Δt.*du,p,0.0)
    du_sum .+= du

    # update u with the mean of the ponderated sum over Δt
    return (Δt/6).*du_sum .+ u
end

function timestep_RK4_adaptative(du, u, p, update_du!, Δt::T, e₀::T) where T<: AbstractFloat
    u1 = RK4(du, u, p, update_du!, Δt)
    #println("RK4 u1",u1)
    uint = RK4(du, u, p, update_du!, Δt/2)
    #println("RK4 uint",uint)
    u2 = RK4(du, uint, p, update_du!, Δt/2)
    #println("RK4 u2",u2)

    e = abs(u2[1]-u1[1]) # error calculated on damage only
    if e < e₀
        return u1, Δt, min(Δt*abs(e₀/e),Δt*2)
    else
        timestep_RK4_adaptative(du, u, p, update_du!, float(Δt*abs(e₀/e)^2), e₀)
    end
end

function compute_RK4_adaptative(u0,p,update_du!,tspan,Δt_ini,e₀)
    u_vec = Vector{Vector{Float64}}()
    t_vec = Float64[tspan[1]]
    du = zeros(eltype(u0),size(u0))
    u_next = similar(u0)
    cohesion_loss_flag = false
    push!(u_vec,u0)
    next_Δt = Δt_ini
    while t_vec[end] < tspan[2]
        (t_vec[end]+next_Δt > tspan[2]) && (next_Δt = tspan[2] - t_vec[end]) # prevents time overshoot
        u_next, used_Δt, next_Δt = timestep_RK4_adaptative(du, u_vec[end], p, update_du!,next_Δt,e₀)
        push!(u_vec,u_next)
        push!(t_vec,t_vec[end]+used_Δt)

        # break if D >= 1
        if u_next[1] >= 1
            cohesion_loss_flag = true
            pop!(u_vec)
            push!(u_vec, 1.0)
            break
        end
    end
    return u_vec, t_vec, cohesion_loss_flag
end

function compute_RK4_adaptative_push_version!(u_vec,t_vec,u0,p,update_du!,tspan,Δt_ini,e₀)

    du = zeros(eltype(u0),size(u0))
    u_next = similar(u0)
    cohesion_loss_flag = false
    next_Δt = Δt_ini
    while t_vec[end] < tspan[2]
        (t_vec[end]+next_Δt > tspan[2]) && (next_Δt = tspan[2] - t_vec[end]) # prevents time overshoot
        u_next, used_Δt, next_Δt = timestep_RK4_adaptative(du, u_vec[end], p, update_du!,next_Δt,e₀)
        push!(u_vec,u_next)
        push!(t_vec,t_vec[end]+used_Δt)

        # break if D >= 1
        if u_next[1] >= 1
            cohesion_loss_flag = true
            pop!(u_vec)
            push!(u_vec, 1.0)
            break
        end
    end
    return nothing
end


text:
@info "Adding Packges..."

# required
using Pkg

# activate this project
pkg"activate ."

# helper
function install_pkgs(pkgs;extern=true)
  installed = false
  pkg_list = Pkg.installed()

  for (pkg_name,use) in pkgs
    pkg_symbol = Symbol(pkg_name)

    if extern && !haskey(pkg_list,pkg_name)
      @debug "Install $pkg_name..."
      @eval Pkg.add($pkg_name)
      installed = true
    end

    if use @eval using $pkg_symbol end

    @debug "$pkg_symbol is ready"
    #exit(0)# exit, cannot create documentations!
  end
  installed
end

# install packages
@info "Install packages..."
pkgs = (x->(x,false)).(readlines(joinpath(root,"REQUIRE")))
if install_pkgs(pkgs) Pkg.update() end

@info "Installed packages:"
show(stdout, "text/plain", sort(collect(Pkg.installed())))
println("\n---------------------------------------------------")

max_stars_repo_name: Gilga/AI-Cywwtaip

max_issues_repo_name: Gilga/AI-Cywwtaip

max_forks_repo_name: Gilga/AI-Cywwtaip


text:
Base.@kwdef struct LinearScaling{T} <: AbstractParametersScaling{T}
    xrange::Vector{T}
    xmin::Vector{T}
end

Base.@kwdef struct SigmoidScaling{T} <: AbstractParametersScaling{T}
    xrange::Vector{T}
    xmin::Vector{T}
    xsigm::Vector{T}
end

function LinearScaling{T}(df::AbstractDataFrame) where {T}
    df_view = getparams(df, Val(:tanks), Val(:var))
    xrange = @with(df_view, :Max_value .- :Min_value)
    xmin = @with(df_view, @. -:Min_value / xrange)
    LinearScaling{T}(; xrange, xmin)
end

function SigmoidScaling{T}(df::AbstractDataFrame) where {T}
    df_view = getparams(df, Val(:tanks), Val(:var))
    xrange = @with(df_view, :Max_value .- :Min_value)
    xmin = @with(df_view, @. -:Min_value / xrange)
    xsigm = Array{T}(undef, length(xrange))
    SigmoidScaling{T}(; xrange, xmin, xsigm)
end

function scalex!(x, y, scale::LinearScaling{T}) where {T}
    @unpack xrange, xmin = scale
    @turbo for i = 1:length(x)
        x[i] = clamp(xmin[i] + y[i] / xrange[i], zero(T), one(T))
    end
end

function scalex!(x, y, scale::SigmoidScaling{T}) where {T}
    @unpack xrange, xmin = scale
    @turbo for i = 1:length(x)
        zmin = T(0.0001) / xrange[i]
        z = clamp(xmin[i] + y[i] / xrange[i], zmin, one(T) - zmin)
        x[i] = log(z / (one(T) - z))
    end
end

function unscalex!(y, x, scale::LinearScaling)
    @unpack xrange, xmin = scale
    @turbo for i = 1:length(y)
        y[i] = (x[i] - xmin[i]) * xrange[i]
    end
end

function unscalex!(y, x, scale::SigmoidScaling{T}) where {T}
    @unpack xrange, xmin, xsigm = scale
    @turbo for i = 1:length(y)
        xsigm[i] = one(T) / (one(T) + exp(-x[i]))
        y[i] = xrange[i] * (xsigm[i] - xmin[i])
    end
end

function unscaleg!(g, scale::LinearScaling)
    @unpack xrange = scale
    @turbo for i = 1:length(g)
        g[i] *= xrange[i]
    end
end

function unscaleg!(g, scale::SigmoidScaling{T}) where {T}
    @unpack xrange, xsigm = scale
    @turbo for i = 1:length(g)
        g[i] *= xrange[i] * xsigm[i] * (one(T) - xsigm[i])
    end
end


text:
const DEFAULT_SUITES = "UnitTests"

for test in split(get(ENV, "TEST", DEFAULT_SUITES), ",")
    include(joinpath(@__DIR__, "$test.jl"))
end


text:
## AWS Lambda Runtime for Julia Lang

# AWS Lambda Runtime Interface - https://docs.aws.amazon.com/lambda/latest/dg/runtimes-api.html
# AWS Lambda Custom Runtimes   - https://docs.aws.amazon.com/lambda/latest/dg/runtimes-custom.html
# AWS Lambda Runtime ENV Vars  - https://docs.aws.amazon.com/lambda/latest/dg/current-supported-versions.html

__precompile__()

module AWSLambdaJuliaRuntime

using HTTP
using JSON
using MicroLogging

export success_invocation_response,
       failure_invocation_response,
       InvocationRequest,
       InvocationResponse

global AWS_LAMBDA_RUNTIME_API_INIT_ENDPOINT = ""
global AWS_LAMBDA_RUNTIME_API_NEXT_ENDPOINT = ""
global AWS_LAMBDA_RUNTIME_API_RESULTS_ENDPOINT = ""

const REQUEST_ID_HEADER = "Lambda-Runtime-Aws-Request-Id"
const TRACE_ID_HEADER = "Lambda-Runtime-Trace-Id"
const CLIENT_CONTEXT_HEADER = "Lambda-Runtime-Client-Context"
const COGNITO_IDENTITY_HEADER = "Lambda-Runtime-Cognito-Identity"
const DEADLINE_MS_HEADER = "Lambda-Runtime-Deadline-Ms"
const FUNCTION_ARN_HEADER = "Lambda-Runtime-Invoked-Function-Arn"

# InvocationRequest represent Lambda invocation request data
mutable struct InvocationRequest
    # The user's payload represented as a UTF-8 string.
    payload::String

    # An identifier unique to the current invocation.
    request_id::String

    # X-Ray tracing ID of the current invocation.
    xray_trace_id::String

    # Information about the client application and device when invoked through the AWS Mobile SDK.
    client_context::String

    # Information about the Amazon Cognito identity provider when invoked through the AWS Mobile SDK.
    cognito_identity::String

    # The ARN requested. This can be different in each invoke that executes the same version.
    function_arn::String

    # Function execution deadline counted in milliseconds since the Unix epoch.
    deadline::UInt64

    function InvocationRequest()
        return new("", "", "", "", "", "", 0)
    end
end

# The number of milliseconds left before lambda terminates the current execution.
function get_time_remaining(req::InvocationRequest)
    return (time() / 1000) - req.deadline
end

# InvocationResponse represents response from the lambda invocation
mutable struct InvocationResponse
     # The output of the function which is sent to the lambda caller.
    payload::String

     # The MIME type of the payload.
    content_type::String

     # Flag to distinguish if the contents are for successful or unsuccessful invocations.
    success::Bool

    function InvocationResponse(payload::String, content_type::String, success::Bool)
        new(payload, content_type, success)
    end
end

function success_invocation_response(payload::String, content_type::String="text/html")
    return InvocationResponse(payload, content_type, true)
end

function failure_invocation_response(err_msg::String, err_type::String)
    payload = Dict(
        "errorMessage"=>err_msg,
        "errorType"=>err_type,
        "stackTrace"=>Vector{String, 1}(0)
    )
    return InvocationResponse(
        JSON.json(payload),
        "application/json",
        false
    )
end

function http_resp_success(http_code::Integer)
    return ((http_code >= 200) && (http_code <= 299))
end

function initialise()
    global AWS_LAMBDA_RUNTIME_API_INIT_ENDPOINT
    global AWS_LAMBDA_RUNTIME_API_NEXT_ENDPOINT
    global AWS_LAMBDA_RUNTIME_API_RESULTS_ENDPOINT

    LOG_LEVEL = get(ENV, "JULIA_RUNTIME_LOG_LEVEL", "error")
    configure_logging(AWSLambdaJuliaRuntime, min_level=parse(LOG_LEVEL))

    @info "[AWSLambdaJuliaRuntime] Initialising..."

    AWS_LAMBDA_RUNTIME_API = get(ENV, "AWS_LAMBDA_RUNTIME_API", nothing)
    if AWS_LAMBDA_RUNTIME_API == nothing
        @error "[AWSLambdaJuliaRuntime] AWS_LAMBDA_RUNTIME_API not defined!"
        error("[AWSLambdaJuliaRuntime] AWS_LAMBDA_RUNTIME_API not defined!")
    end
    AWS_LAMBDA_RUNTIME_API_INIT_ENDPOINT = "http://$(AWS_LAMBDA_RUNTIME_API)/2018-06-01/runtime/init/error"
    AWS_LAMBDA_RUNTIME_API_NEXT_ENDPOINT = "http://$(AWS_LAMBDA_RUNTIME_API)/2018-06-01/runtime/invocation/next"
    AWS_LAMBDA_RUNTIME_API_RESULTS_ENDPOINT = "http://$(AWS_LAMBDA_RUNTIME_API)/2018-06-01/runtime/invocation/"

    @info "[AWSLambdaJuliaRuntime] Initialised"
end

function get_next_event()
    resp = HTTP.request("GET", AWS_LAMBDA_RUNTIME_API_NEXT_ENDPOINT; readtimeout=5, retry=true, retries=2)
    @debug "[AWSLambdaJuliaRuntime] get_next_event : event=" resp
    if !http_resp_success(resp.status)
        return resp.status, nothing
    end

    invoc_req = InvocationRequest()

    # parse request body
    invoc_req.payload = String(resp.body)

    # process request HTTP header
    for (k,v) in resp.headers
        if k == REQUEST_ID_HEADER
            invoc_req.request_id = String(v)
        end
        if k == TRACE_ID_HEADER
            invoc_req.xray_trace_id = String(v)
        end
        if k == CLIENT_CONTEXT_HEADER
            invoc_req.client_context = String(v)
        end
        if k == COGNITO_IDENTITY_HEADER
            invoc_req.cognito_identity = String(v)
        end
        if k == FUNCTION_ARN_HEADER
            invoc_req.function_arn = String(v)
        end
        if k == DEADLINE_MS_HEADER
            ms = parse(UInt64, String(v))
            invoc_req.deadline = ms * 1000 # deadline in mu sec
        end
    end

    @debug "[AWSLambdaJuliaRuntime] Received event: " invoc_req

    if invoc_req.request_id == nothing
        return -1, invoc_req
    end

    return resp.status, invoc_req
end

# Send back the result of lambda invocation (success or failure)
function post_handler_response(handler_resp::InvocationResponse, invoc_req::InvocationRequest)
    @debug "[AWSLambdaJuliaRuntime] post_handler_response handler_resp=" handler_resp

    url = ""
    if handler_resp.success
        url = "$(AWS_LAMBDA_RUNTIME_API_RESULTS_ENDPOINT)$(invoc_req.request_id)/response"
    else
        url = "$(AWS_LAMBDA_RUNTIME_API_RESULTS_ENDPOINT)$(invoc_req.request_id)/error"
    end

    headers = Vector{Pair{String, String}}()
    push!(headers, Pair("content-type", handler_resp.content_type))
    push!(headers, Pair("content-length", string(length(handler_resp.payload))))

    @debug "[AWSLambdaJuliaRuntime] Posting response url=$(url) headers=$(headers) payload=$(handler_resp.payload)"

    resp = HTTP.request("POST", url, headers, handler_resp.payload)

    if !http_resp_success(resp.status)
        @error "[AWSLambdaJuliaRuntime] Post response failed. error=$(resp.status) resp=$(resp.body)"
        return
    end

    return
end

"""
main entry point to the module
This gets called by the `bootstrap` script
"""
function main(lambda_module::Module)
    # Initialise runtime
    initialise()

    retries = 0
    max_retries = 3
    invoc_req = nothing

    # In (infinite) loop, process Lambda invocations
    while retries < max_retries
        ## Get an event
        resp_status, invoc_req = get_next_event()
        if !http_resp_success(resp_status)
            @error "[AWSLambdaJuliaRuntime] get_next_event failed: code=$(resp_status) req=" invoc_req
            retries += 1
            continue
        end

        retries = 0

        ## Call handler function in the client module
        handler_resp = lambda_module.handler(invoc_req)

        ## Send the response
        post_handler_response(handler_resp, invoc_req)
    end

    if retries == max_retries
        @error "[AWSLambdaJuliaRuntime] Exhausted all retries ($(retries)/$(max_retries)). Exiting!"
    end
end

precompile(main, (Module,))
precompile(post_handler_response, (InvocationResponse, InvocationRequest))
precompile(get_next_event, ())
precompile(initialise, ())
precompile(success_invocation_response, (String, String))
precompile(failure_invocation_response, (String, String))
precompile(get_time_remaining, (InvocationRequest,))
precompile(http_resp_success, (Integer,))

end # module AWSLambdaJuliaRuntime

max_stars_repo_name: FugroRoames/aws-lambda-julia-runtime

max_issues_repo_name: FugroRoames/aws-lambda-julia-runtime

max_forks_repo_name: FugroRoames/aws-lambda-julia-runtime


text:
@testset "Approximators" begin

    @testset "TabularApproximator" begin
        A = TabularApproximator(ones(3))

        @test A(1) == 1.0
        @test A(2) == 1.0

        update!(A, 2 => 3.0)
        @test A(2) == 4.0
    end

    @testset "NeuralNetworkApproximator" begin
        NN = NeuralNetworkApproximator(; model = Dense(2, 3), optimizer = Descent())

        q_values = NN(rand(2))
        @test size(q_values) == (3,)

        v = NN(rand(2), 2)
        @test v isa Number

        batch_q_values = batch_estimate(NN, rand(2, 5))
        @test size(batch_q_values) == (3, 5)

        gs = gradient(params(NN)) do
            sum(NN(rand(2, 5)))
        end

        old_params = deepcopy(collect(params(NN).params))
        update!(NN, gs)
        new_params = collect(params(NN).params)

        @assert old_params != new_params
    end
end


text:
## Constants

const OPEN = -1

struct Polyhedron{Dp,Tp,Td}
  vertices::Vector{Point{Dp,Tp}}
  edge_vertex_graph::Vector{Vector{Int32}}
  isopen::Bool
  data::Td
end

struct PolyhedronData
  vertex_to_planes::Vector{Vector{Int32}}
  vertex_to_original_faces::Vector{Vector{Int32}}
  vertex_to_parent_vertex::Vector{Int32}
  vertex_to_parent_edge::Vector{Tuple{Int32,Int32}}
  plane_to_vertex_to_distances::Vector{Vector{Float64}}
  plane_to_ref_plane::Vector{Int32}
  plane_to_ids::Vector{Int32}
end

# Constructors

function Polyhedron(stl::STL)
  𝓖 = compute_graph(stl)
  X = get_vertex_coordinates(stl)
  isopen = is_open_surface(stl)
  p = Polyhedron(X,𝓖;isopen)
  set_original_faces!(p,stl)
  p
end

function Polyhedron(
  vertices::AbstractVector{<:Point},
  graph::Vector{<:Vector},
  isopen,
  data)

  Polyhedron(collect(vertices),graph,isopen,data)
end

function Polyhedron(
  vertices::AbstractVector{<:Point},
  graph::Vector{<:Vector}
  ;isopen=false::Bool)

  Polyhedron(vertices,graph,isopen,polyhedron_data(length(vertices)))
end

function Polyhedron(
  vertices::AbstractVector{<:Point},
  graph::Vector{<:Vector},
  data)

  isopen = false
  Polyhedron(vertices,graph,isopen,data)
end

function Polyhedron(p::Polytope{2},vertices::AbstractVector{<:Point})
  if p == TRI
    e_v_graph = [[2,3],[3,1],[1,2]]
  elseif p == QUAD
    e_v_graph = [[2, 3],[4, 1],[1, 4],[3, 2]]
  else
    @unreachable
  end
  e_v_graph = map(i->Int32.(i),e_v_graph)
  data = polyhedron_data(p)
  Polyhedron(vertices,e_v_graph,data)
end

function Polyhedron(p::Polytope{3},vertices::AbstractVector{<:Point})
  if p == TET
    e_v_graph = [[2,4,3],[3,4,1],[1,4,2],[1,2,3]]
  elseif p == HEX
    e_v_graph = [
      [5, 2, 3],
      [6, 4, 1],
      [7, 1, 4],
      [8, 3, 2],
      [1, 7, 6],
      [2, 5, 8],
      [3, 8, 5],
      [4, 6, 7] ]
  else
    @unreachable
  end
  e_v_graph = map(i->Int32.(i),e_v_graph)
  data = polyhedron_data(p)
  Polyhedron(vertices,e_v_graph,data)
end

function Polyhedron(p::Polytope)
  Polyhedron(p,get_vertex_coordinates(p))
end

# Operations

function clip(poly::Polyhedron,Π;inside=true,inout=trues(length(Π)),boundary=nothing)
  p = poly
  for (i,Πi) in enumerate(Π)
    side = inside == inout[i] ? :left : :right
    p = clip(p,Πi,side;boundary)
    !isnothing(p) || break
  end
  p
end

function split_overlapping(p::Polyhedron,Π)
  p⁻ = clip(p,Π,:left,boundary=true)
  p⁺ = clip(p,Π,:right,boundary=true)
  p⁻,p⁺
end

function split_gapping(p::Polyhedron,Π)
  p⁻ = clip(p,Π,:left,boundary=false)
  p⁺ = clip(p,Π,:right,boundary=false)
  p⁻,p⁺
end

function clip(p::Polyhedron,Π,side;boundary=nothing::Union{Nothing,Bool},invert=false)
  @assert side ∈ (:right,:left)
  if boundary === nothing
    p⁻,p⁺ = split(p,Π,side;invert)
    p  = side == :left ? p⁻ : p⁺
  else
    invert = false
    if side == :right && !boundary
      invert = true
      side = :left
    elseif side == :left && boundary
      invert = true
      side = :right
    end
    p = clip(p,Π,side;invert)
  end
  p
end

function split(p::Polyhedron,Π,side=:both;invert=false)
  @assert side ∈ (:both,:left,:right)
  distances = get_plane_distances(get_data(p),Π)
  _sign = invert ? (-) : (+)
  distances = lazy_map(_sign,distances)
  smin = Inf
  smax = -Inf
  for v in 1:num_vertices(p)
    isactive(p,v) || continue
    smin = min(smin,distances[v])
    smax = max(smax,distances[v])
  end
  if smin ≥ 0
    return nothing,p
  end
  if smax < 0
    if has_coplanars(get_data(p),Π)
      add_plane!(get_data(p),Π)
    end
    return p,nothing
  end
  new_vertices = empty( get_vertex_coordinates(p) )
  in_graph = _copy(get_graph(p))
  if side == :both
    out_graph = _copy(get_graph(p))
  end
  ≶ = side ∈ (:both,:left) ? (≥) : (<)
  data = copy( get_data(p) )
  D = num_dims(p)
  for v in 1:num_vertices(p)
    isactive(p,v) || continue
    distances[v] ≶ 0 && continue
    for (i,vneig) in enumerate( get_graph(p)[v] )
      vneig ∉ (UNSET,OPEN) || continue
      distances[vneig] ≶ 0 || continue
      vertex = compute_intersection(p[v],distances[v],p[vneig],distances[vneig])
      push!( new_vertices, vertex )
      add_vertex!(data,v,vneig,Π)
      push!( in_graph, fill(UNSET,D) )
      in_graph[v][i] = num_vertices(p) + length(new_vertices)
      in_graph[end][1] = v
      if side == :both
        push!( out_graph, fill(UNSET,D) )
        ineig = findfirst( isequal(v), get_graph(p)[vneig] )
        out_graph[vneig][ineig] = num_vertices(p) + length(new_vertices)
        out_graph[end][1] = vneig
      end
    end
  end
  if side == :both
    p_out = split_postprocess!(out_graph,copy(data),new_vertices,p,distances,(<))
  else
    p_out = nothing
  end
  p_in = split_postprocess!(in_graph,data,new_vertices,p,distances,(≶))
  p⁻ = side ≠ :right ? p_in : p_out
  p⁺ = side ≠ :right ? p_out : p_in
  p⁻,p⁺
end

function simplexify(poly::Polyhedron{3})
  !isopen(poly) || return simplexify_surface(poly)
  vstart = fill(UNSET,num_vertices(poly))
  stack = Int32[]
  for v in 1:num_vertices(poly)
    isactive(poly,v) || continue
    vstart[v] == UNSET || continue
    vstart[v] = v
    empty!(stack)
    push!(stack,v)
    while !isempty(stack)
      vcurrent = pop!(stack)
      for vneig in get_graph(poly)[vcurrent]
        if vstart[vneig] == UNSET
          vstart[vneig] = v
          push!(stack,vneig)
        end
      end
    end
  end
  v_to_pv = get_data(poly).vertex_to_parent_vertex
  istouch = map( i -> falses(length(i)), get_graph(poly) )
  vertex_coordinates = get_vertex_coordinates(poly)
  T = Vector{Int32}[]
  X = vertex_coordinates
  for v in 1:num_vertices(poly)
    isactive(poly,v) || continue
    for i in 1:length(get_graph(poly)[v])
      !istouch[v][i] || continue
      istouch[v][i] = true
      vcurrent = v
      vnext = get_graph(poly)[v][i]
      while vnext != v
        inext = findfirst( isequal(vcurrent), get_graph(poly)[vnext] )
        !isnothing(inext) || break
        inext = ( inext % length( get_graph(poly)[vnext] ) ) + 1
        istouch[vnext][inext] = true
        vcurrent = vnext
        vnext = get_graph(poly)[vnext][inext]
        @assert vcurrent ≠ vnext
        vcurrent ≠ vnext || break
        if v ∉ (vstart[v],vcurrent,vnext)
          k = [vstart[v],v,vcurrent,vnext]
          push!(T,k)
        end
      end
    end
  end
  T,X
end

function simplexify_surface(poly::Polyhedron{3})
  stack = Int32[]
  v_to_pv = get_data(poly).vertex_to_parent_vertex
  istouch = map( i -> falses(length(i)), get_graph(poly) )
  T = Vector{Int32}[]
  for v in 1:num_vertices(poly)
    isactive(poly,v) || continue
    for i in 1:length(get_graph(poly)[v])
      !istouch[v][i] || continue
      istouch[v][i] = true
      vcurrent = v
      vnext = get_graph(poly)[v][i]
      vnext ∉ (OPEN,UNSET) || continue
      while vnext != v
        inext = findfirst( isequal(vcurrent), get_graph(poly)[vnext] )
        inext = ( inext % length( get_graph(poly)[vnext] ) ) + 1
        istouch[vnext][inext] = true
        vcurrent = vnext
        vnext = get_graph(poly)[vnext][inext]
        vnext ∉ (OPEN,UNSET) || break
        if v ∉ (vcurrent,vnext)
          k = [v,vcurrent,vnext]
          push!(T,k)
        end
      end
    end
  end
  T,get_vertex_coordinates(poly)
end

function simplexify_boundary(poly::Polyhedron{3},stl::STL)
  D = 3
  stack = Int32[]
  v_to_pv = get_data(poly).vertex_to_parent_vertex
  if isopen(poly)
    v_to_Π = get_data(poly).vertex_to_original_faces
  else
    v_to_Π = get_data(poly).vertex_to_planes
  end
  facedims = get_facedims(stl)
  facet_list = Int32[]
  istouch = map( i -> falses(length(i)), get_graph(poly) )
  vertex_coordinates = get_vertex_coordinates(poly)
  T = Vector{Int32}[]
  X = empty(vertex_coordinates)
  f_to_stlf = Int32[]
  for v in 1:num_vertices(poly)
    isactive(poly,v) || continue
    for i in 1:length(get_graph(poly)[v])
      !istouch[v][i] || continue
      istouch[v][i] = true
      vcurrent = v
      vnext = get_graph(poly)[v][i]
      vnext ∉ (OPEN,UNSET) || continue
      empty!(facet_list)
      append!( facet_list, v_to_Π[v] )
      filter!( i -> i > 0, facet_list)
      filter!( i -> facedims[i] == num_dims(stl), facet_list)
      filter!( i -> i ∈ v_to_Π[vnext], facet_list )
      while vnext != v
        inext = findfirst( isequal(vcurrent), get_graph(poly)[vnext] )
        inext = ( inext % length( get_graph(poly)[vnext] ) ) + 1
        istouch[vnext][inext] = true
        vcurrent = vnext
        vnext = get_graph(poly)[vnext][inext]
        vnext ∉ (OPEN,UNSET) || break
        filter!( i -> i ∈ v_to_Π[vnext], facet_list )
        !isempty(facet_list) || break
        if v ∉ (vcurrent,vnext)
          kx = (v,vcurrent,vnext)
          k = (1:D) .+ length(X)
          for x in kx
            push!(X,vertex_coordinates[x])
          end
          push!(T,k)
          push!(f_to_stlf,first(facet_list))
        end
      end
    end
  end
  T,X,f_to_stlf
end

function simplexify_boundary(::Nothing,args...)
  nothing,nothing,nothing
end

function simplexify(polys::AbstractVector{<:Polyhedron{Dp,Tp}}) where {Dp,Tp}
  T = Vector{Int32}[]
  X = Point{Dp,Tp}[]
  for poly in polys
    Ti,Xi = simplexify(poly)
    append!(T, map(i->i.+length(X),Ti) )
    append!(X,Xi)
  end
  T,X
end

function simplexify_boundary(
  polys::AbstractVector{<:Polyhedron{Dp,Tp}},
  stl::STL) where {Dp,Tp}

  T = Vector{Int32}[]
  X = Point{Dp,Tp}[]
  f_to_stlf = Int32[]
  for poly in polys
    Ti,Xi,f_to_f_i = simplexify_boundary(poly,stl)
    append!(T, map(i->i.+length(X),Ti) )
    append!(X,Xi)
    append!(f_to_stlf,f_to_f_i)
  end
  T,X,f_to_stlf
end

function simplexify_boundary(::NTuple{N,Nothing},args...) where N
  nothing,nothing,nothing,nothing
end

function simplexify_boundary(p::Tuple,labels::Tuple,stl::STL)
  P = eltype(p[1])
  Dp = num_dims(P)
  Tp = point_eltype(P)
  T = Vector{Int32}[]
  X = Point{Dp,Tp}[]
  f_to_stlf = Int32[]
  f_to_ios = Int8[]
  for (p_i,ios) in zip( p, labels )
    Ti, Xi, f_to_f_i = simplexify_boundary(p_i,stl)
    if !isnothing(Ti)
      append!(T, map(i->i.+length(X),Ti) )
      append!(X,Xi)
      append!(f_to_stlf,f_to_f_i)
      append!(f_to_ios, fill(Int8(ios),length(Ti)) )
    end
  end
  T,X,f_to_stlf,f_to_ios
end

function surface(poly::Polyhedron{3})
  T,X = simplexify_surface(poly)
  p = TRI
  c = array_cache(T)
  !isempty(T) || return 0.0
  sum( i -> measure(get_cell!(c,T,X,p,i)), 1:length(T) )
end

function surface(poly::Polyhedron{3},stl::STL)
  T,X, = simplexify_boundary(poly,stl)
  p = TRI
  c = array_cache(T)
  !isempty(T) || return 0.0
  sum( i -> measure(get_cell!(c,T,X,p,i)), 1:length(T) )
end

function surface(polys::AbstractVector{<:Polyhedron},args...)
  s = 0.0
  for p in polys
    s += surface(p,args...)
  end
  s
end

function volume(poly::Polyhedron{3})
  T,X = simplexify(poly)
  p = TET
  c = array_cache(T)
  sum( i -> measure(get_cell!(c,T,X,p,i)), 1:length(T) )
end

function volume(polys::AbstractVector{<:Polyhedron},args...)
  v = 0.0
  for p in polys
    v += volume(p,args...)
  end
  v
end

function restrict(poly::Polyhedron,stl::STL,stl_facets)
  cell_to_nodes = get_cell_vertices(stl)
  c = array_cache(cell_to_nodes)
  nodes = Int32[]
  for f in stl_facets
    for n in getindex!(c,cell_to_nodes,f)
      if n ∉ nodes
        push!(nodes,n)
      end
    end
  end
  sort!(nodes)
  restrict(poly,nodes)
end

function restrict(p::Polyhedron,nodes)
  graph = get_graph(p)[nodes]
  f = i -> Int32( i ∈ nodes ? findfirst(isequal(i),nodes) : OPEN )
  graph = map(i->map(f,i),graph)
  data = restrict(get_data(p),nodes)
  vertices = get_vertex_coordinates(p)[nodes]
  isopen = true
  Polyhedron(vertices,graph,isopen,data)
end

function restrict(data::PolyhedronData,nodes)
  v_to_Π = data.vertex_to_planes[nodes]
  v_to_of = data.vertex_to_original_faces[nodes]
  v_to_v = collect(1:length(nodes))
  v_to_e = fill((UNSET,UNSET),length(nodes))
  Π_to_rΠ = copy(data.plane_to_ref_plane)
  Π_to_id = copy(data.plane_to_ids)
  _Π_to_v_to_d = data.plane_to_vertex_to_distances
  Π_to_v_to_d = [ _Π_to_v_to_d[i][nodes] for i in 1:length(Π_to_id) ]
  args = v_to_Π, v_to_of, v_to_v, v_to_e, Π_to_v_to_d, Π_to_rΠ, Π_to_id
  PolyhedronData(args...)
end

# Printers

function plot(p::Polyhedron,filename=nothing)
  vertices = Int32[]
  for i in 1:num_vertices(p)
    if isactive(p,i)
      push!(vertices,i)
    end
  end
  vertex_to_node = Dict( zip( vertices, 1:length(vertices) ) )
  g = empty( get_graph(p) )
  for (i,vneigs) in enumerate( get_graph(p) )
    isactive(p,i) || continue
    nodes = Int32[]
    for (j,vneig) in enumerate(vneigs)
      if vneig ∈ (UNSET,OPEN)
        push!(vertices,UNSET)
        n = length(vertices)
      else
        n = vertex_to_node[vneig]
      end
      push!(nodes,n)
    end
    push!(g,nodes)
  end
  v_to_pv = p.data.vertex_to_parent_vertex
  names = String[]
  for v in vertices
    if v == UNSET
      name = ""
    else
      name =  v_to_pv[v] == v ? "$v" : "$v($(v_to_pv[v]))"
    end
    push!(names,name)
  end
  kwargs = (fontsize=10,node_size=0,line=(:dot,0.5,1),thickness_scaling=0.5)
  GraphRecipes.graphplot(g,names=names,curves=false;kwargs...)
  isnothing(filename) || Plots.savefig(filename)
  Plots.plot!()
end

function writevtk(p::Polyhedron,filename;kwargs...)
  writevtk(edge_mesh(p),filename;kwargs...)
end

function writevtk(ps::Array{<:Polyhedron},filename)
  for (i,p) in enumerate(ps)
    writevtk(p,filename*"$i")
  end
end

# Getters

num_dims(::T) where T<:Polyhedron = num_dims(T)

num_dims(::Type{<:Polyhedron{D}}) where D = D

point_eltype(::T) where T<:Polyhedron = point_eltype(T)

point_eltype(::Type{<:Polyhedron{D,T}}) where {D,T} = T

num_vertices(a::Polyhedron) = length(a.vertices)

get_vertex_coordinates(a::Polyhedron) = a.vertices

Base.getindex(a::Polyhedron,i::Integer) = a.vertices[i]

@inline get_graph(a::Polyhedron) = a.edge_vertex_graph

get_data(a::Polyhedron) = a.data

Base.isopen(a::Polyhedron) = a.isopen

get_vertex_to_planes(a::PolyhedronData) = a.vertex_to_planes

function isactive(p::Polyhedron,vertex::Integer)
  !isempty( get_graph(p)[vertex] )
end

function get_original_reflex_faces(p::Polyhedron{D},stl::STL;empty=true) where D
  get_original_faces(p,stl,Val{D-2}();empty)
end

function get_original_facets(p::Polyhedron{D},stl::STL;empty=false) where D
  get_original_faces(p,stl,Val{D-1}();empty)
end

function get_original_faces(p::Polyhedron,stl::STL,::Val{d};empty) where d
  faces = collect_original_faces(p,stl,d)
  filter!(f->has_original_face(p,f,Val{d}();empty),faces)
  faces
end

function collect_original_faces(p::Polyhedron,stl::STL,d::Integer)
  v_to_f = get_data(p).vertex_to_original_faces
  facedims = get_facedims(stl)
  faces = Int32[]
  for v in 1:num_vertices(p)
    if isactive(p,v)
      for f ∈ v_to_f[v]
        if facedims[f] == d && f ∉ faces
          push!(faces,f)
        end
      end
    end
  end
  faces
end

function has_original_reflex_face(p::Polyhedron{D},face::Integer;empty=true) where D
  has_original_face(p,face,Val{D-2}();empty)
end

function has_original_facet(p::Polyhedron{D},face::Integer;empty=false) where D
  has_original_face(p,face,Val{D-1}();empty)
end

function has_original_face(p::Polyhedron,face::Integer,::Val{0};empty=false)
  v_to_f = get_data(p).vertex_to_original_faces
  for v in 1:num_vertices(p)
    if isactive(p,v) && face ∈ v_to_f[v]
      return true
    end
  end
  false
end

function has_original_face(p::Polyhedron,face::Integer,::Val{1};empty=true)
  v_to_f = get_data(p).vertex_to_original_faces
  v_to_v = get_data(p).vertex_to_parent_vertex
  for v in 1:num_vertices(p)
    if isactive(p,v) && face ∈ v_to_f[v]
      for vneig in get_graph(p)[v]
        vneig ∉ (OPEN,UNSET) || continue
        if face ∈ v_to_f[vneig] && ( empty || v_to_v[v] ≠ v_to_v[vneig] )
          return true
        end
      end
    end
  end
  false
end

function has_original_face(p::Polyhedron,face::Integer,::Val{2};empty=false)
  d = 2
  v_to_f = get_data(p).vertex_to_original_faces
  v_to_v = get_data(p).vertex_to_parent_vertex
  for v in 1:num_vertices(p)
    if isactive(p,v) && face ∈ v_to_f[v]
      for vneig in get_graph(p)[v]
        vneig ∉ (OPEN,UNSET) || continue
        if face ∈ v_to_f[vneig]
          num_v = 1
          vcurrent = v
          vnext = vneig
          while vnext ≠ v
            if v_to_v[vnext] ∉ (v_to_v[v],v_to_v[vcurrent]) || empty
              num_v += 1
            end
            vcurrent,vnext = vnext,next_vertex(p,vcurrent,vnext)
            vnext ∉ (UNSET,OPEN) || break
            face ∈ v_to_f[vnext] || break
          end
          if vnext == v && num_v ≥ d+1
            return true
          end
        end
      end
    end
  end
  false
end

function has_faces(p::Polyhedron,::Val{0})
  for v in 1:num_vertices(p)
    isactive(p,v) || continue
    return true
  end
  false
end

function has_faces(p::Polyhedron,::Val{1})
  for v in 1:num_vertices(p)
    isactive(p,v) || continue
    for vneig in get_graph(p)[v]
      @assert isactive(p,vneig)
      vneig ∉ (OPEN,UNSET) || continue
      v_to_v[v] ≠ v_to_v[vneig] || continue
      return true
    end
    @unreachable
  end
  false
end

function has_faces(p::Polyhedron,::Val{2})
  d = 2
  v_to_v = get_data(p).vertex_to_parent_vertex
  for v in 1:num_vertices(p)
    isactive(p,v) || continue
    for vneig in get_graph(p)[v]
      vneig ∉ (OPEN,UNSET) || continue
      num_v = 1
      vcurrent = v
      vnext = vneig
      while vnext ≠ v
        if v_to_v[vnext] ∉ (v_to_v[v],v_to_v[vcurrent])
          num_v += 1
        end
        vcurrent,vnext = vnext,next_vertex(p,vcurrent,vnext)
        vnext ∉ (UNSET,OPEN) || break
        if vnext == v && num_v ≥ d+1
          return true
        end
      end
    end
  end
  false
end

has_vertices(p::Polyhedron) = has_faces(p,Val{0}())

has_edges(p::Polyhedron) = has_faces(p,Val{1}())

has_facets(p::Polyhedron{D}) where D = has_faces(p,Val{D-1}())

function get_plane_distances(data::PolyhedronData)
  data.plane_to_vertex_to_distances
end

function get_plane_distances(data::PolyhedronData,Π)
  i = findfirst(isequal(Π),get_plane_ids(data))
  get_plane_distances(data)[i]
end

function get_plane_ids(data::PolyhedronData)
  data.plane_to_ids
end

function has_plane(data::PolyhedronData,Π)
  Π ∈ get_plane_ids(data)
end

## Helpers

function check_graph(p::Polyhedron)
  check_graph(get_graph(p))
end

function check_graph(graph::AbstractVector{<:AbstractVector})
  for v in 1:length(graph)
    !isempty(graph[v]) || continue
    for vneig in graph[v]
      vneig ∉ (OPEN,UNSET) || continue
      v ∈ graph[vneig] || return false
    end
  end
  true
end

function compact!(p::Polyhedron)
  ids = findall(i->!isactive(p,i),1:num_vertices(p))
  old_to_new = fill(UNSET,num_vertices(p))
  new = 0
  for i in 1:num_vertices(p)
    isactive(p,i) || continue
    new += 1
    old = i
    old_to_new[old] = new
  end
  vertices = get_vertex_coordinates(p)
  graph = get_graph(p)
  deleteat!(vertices,ids)
  deleteat!(graph,ids)
  f = i -> i ∈ (OPEN,UNSET) ? i : old_to_new[i]
  map!(i->map!(f,i,i),graph,graph)
  compact!(p.data,ids,old_to_new)
  p
end

function compact!(data::PolyhedronData,ids,old_to_new)
  v_to_Π = data.vertex_to_planes
  v_to_of = data.vertex_to_original_faces
  v_to_pv = data.vertex_to_parent_vertex
  v_to_pe = data.vertex_to_parent_edge
  Π_to_v_to_dist = data.plane_to_vertex_to_distances
  Π_to_ref_Π = data.plane_to_ref_plane
  Π_to_id = data.plane_to_ids

  deleteat!(v_to_Π,ids)
  deleteat!(v_to_of,ids)
  deleteat!(v_to_pv,ids)
  for (i,pv) in enumerate(v_to_pv)
    if old_to_new[pv] == UNSET
      old_to_new[pv] = i
    end
    v_to_pv[i] = old_to_new[pv]
  end
  deleteat!(v_to_pe,ids)
  f = i -> i == UNSET ? i : old_to_new[i]
  for (i,pe) in enumerate(v_to_pe)
    v_to_pe[i] = (f(pe[1]),f(pe[2]))
  end
  for v_to_dist in Π_to_v_to_dist
    deleteat!(v_to_dist,ids)
  end
  data
end

function Base.copy(poly::Polyhedron)
  vertices = get_vertex_coordinates(poly)
  graph = get_graph(poly)
  open = isopen(poly)
  data = copy(get_data(poly))
  Polyhedron(vertices,graph,open,data)
end

function Base.copy(data::PolyhedronData)
  v_to_Π = copy(data.vertex_to_planes)
  v_to_of = copy(data.vertex_to_original_faces)
  v_to_pv = copy(data.vertex_to_parent_vertex)
  v_to_pe = copy(data.vertex_to_parent_edge)
  Π_to_v_to_dist = _copy(data.plane_to_vertex_to_distances)
  Π_to_ref_Π = copy(data.plane_to_ref_plane)
  Π_to_id = copy(data.plane_to_ids)
  args = v_to_Π, v_to_of, v_to_pv, v_to_pe, Π_to_v_to_dist, Π_to_ref_Π, Π_to_id
  PolyhedronData(args...)
end

function compute_distances!(p::Polyhedron,Π,faces;atol=0)
  data = get_data(p)
  v_to_f = get_data(p).vertex_to_original_faces
  Π_to_v_to_d = get_plane_distances(data)
  Π_ids = get_plane_ids(data)
  Π_to_rΠ = data.plane_to_ref_plane
  num_Π = length(Π_ids)
  append!(Π_ids,faces)
  append!(Π_to_rΠ,fill(UNSET,length(faces)))
  append!(Π_to_v_to_d, [ zeros(num_vertices(p)) for _ in faces ] )
  for (i,(Πi,fi)) in enumerate(zip(Π,faces))
    v_to_d = Π_to_v_to_d[i+num_Π]
    for v in 1:num_vertices(p)
      if fi ∈ v_to_f[v]
        dist = 0.0
      else
        dist = signed_distance(p[v],Πi)
        if abs(dist) < atol
          dist = 0.0
        end
      end
      v_to_d[v] = dist
    end
  end
end

function edge_mesh(p::Polyhedron)
  edge_to_vertices = Vector{Int32}[]
  for v in 1:num_vertices(p)
    for vneig in get_graph(p)[v]
      if vneig > v
        push!(edge_to_vertices,[v,vneig])
      end
    end
  end
  X = get_vertex_coordinates(p)
  T = Table(edge_to_vertices)
  UnstructuredGrid(X,T,[SEG2],fill(1,length(T)))
end

function compute_graph(stl::STL{2})
  @notimplementedif get_polytope(stl) ≠ TRI
  D = 2
  v_to_e = get_faces(stl,0,1)
  v_to_f = get_faces(stl,0,D)
  f_to_v = get_faces(stl,D,0)
  ec = array_cache(v_to_e)
  fc = array_cache(v_to_f)
  vc = array_cache(f_to_v)
  graph = [ Int32[] for _ in 1:num_vertices(stl) ]
  for v in 1:num_vertices(stl)
    vfacets = getindex!(fc,v_to_f,v)
    !isempty(vfacets) || continue
    f0 = first(vfacets)
    fnext = f0
    while true
      i = findfirst(isequal(v), getindex!(vc,f_to_v,fnext) )
      inext = i == D+1 ? 1 : i+1
      vnext = getindex!(vc,f_to_v,fnext)[inext]
      push!(graph[v],vnext)
      faces = getindex!(fc,v_to_f,vnext)
      i = findfirst( f -> f ≠ fnext && v ∈ getindex!(vc,f_to_v,f), faces )
      fnext = isnothing(i) ? UNSET : faces[i]
      fnext ≠ UNSET || break
      fnext ≠ f0 || break
    end
    if fnext == UNSET
      n_edges = length( getindex!(ec,v_to_e,v) )
      if length(graph[v]) < n_edges
        v0 = first(graph[v])
        fnext = f0
        while fnext ≠ UNSET
          i = findfirst(isequal(v), getindex!(vc,f_to_v,fnext) )
          inext = i == 1 ? D+1 : i-1
          vnext = getindex!(vc,f_to_v,fnext)[inext]
          pushfirst!(graph[v],vnext)
          faces = getindex!(fc,v_to_f,vnext)
          i = findfirst( f -> f ≠ fnext && v ∈ getindex!(vc,f_to_v,f), faces )
          fnext = isnothing(i) ? UNSET : faces[i]
        end
        @assert length(graph[v]) == n_edges
      end
      push!(graph[v],OPEN)
    end
  end
  check_graph(graph) || error("Unable to build vertex-edge graph")
  graph
end

function set_original_faces!(p::Polyhedron,a...)
  set_original_faces!(get_data(p),a...)
  p
end

function polyhedron_data(num_vertices::Integer)
  v_to_Π = [ Int32[] for _ in 1:num_vertices ]
  v_to_of = [ Int32[] for _ in 1:num_vertices ]
  v_to_v = collect(1:num_vertices)
  v_to_e = fill((UNSET,UNSET),num_vertices)
  Π_to_v_to_d = Vector{Int32}[]
  Π_to_rΠ = Int32[]
  Π_to_id = Int32[]
  args = v_to_Π, v_to_of, v_to_v, v_to_e, Π_to_v_to_d, Π_to_rΠ, Π_to_id
  PolyhedronData(args...)
end

function polyhedron_data(p::Polytope)
  v_to_Π = map( i-> Int32.(i), -get_faces(p,0,num_dims(p)-1) )
  v_to_of = [ Int32[] for _ in 1:num_vertices(p) ]
  v_to_v = Int32.(collect(1:num_vertices(p)))
  v_to_e = fill((Int32(UNSET),Int32(UNSET)),num_vertices(p))
  Π_to_v_to_d = Vector{Int32}[]
  Π_to_rΠ = Int32[]
  Π_to_id = Int32[]
  args = v_to_Π, v_to_of, v_to_v, v_to_e, Π_to_v_to_d, Π_to_rΠ, Π_to_id
  PolyhedronData(args...)
end

@inline function split_postprocess!(graph,data,new_vertices,input_poly,distances,(≶))
  complete_graph!(graph,num_vertices(input_poly))
  disconnect_graph!(graph,num_vertices(input_poly),distances,(≶))
  add_open_vertices!(graph,input_poly)
  vertices = [input_poly.vertices;new_vertices]
  update_data!(data,graph,num_vertices(input_poly))
  poly = Polyhedron(vertices,graph,isopen(input_poly),data)
  compact!(poly)
end

function complete_graph!(edge_graph,num_vertices::Integer)
  for v in num_vertices+1:length(edge_graph)
    vnext = next_vertex(edge_graph,num_vertices,v)
    vnext ∉ (UNSET,OPEN) || continue
    edge_graph[v][end] = vnext
    edge_graph[vnext][2] = v
  end
end

function add_open_vertices!(graph,poly::Polyhedron)
  if isopen(poly)
    for v in num_vertices(poly)+1:length(graph)
      if UNSET ∉ graph[v]
        push!(graph[v],graph[v][end])
        graph[v][end-1] = OPEN
      end
    end
  end
end

function disconnect_graph!(edge_graph,num_vertices,distances,(≶)::Function)
  for i in 1:num_vertices
    if distances[i] ≶ 0
      edge_graph[i] = empty!( edge_graph[i] )
    end
  end
end

function update_data!(data,graph,num_vertices)
  v_to_pv = data.vertex_to_parent_vertex
  v_to_pe = data.vertex_to_parent_edge
  Π_to_v_to_d = get_plane_distances(data)
  for v in num_vertices+1:length(graph)
    v_to_pv[v] == v || continue
    for vnext in num_vertices+1:length(graph)
      v_to_pv[vnext] == vnext || continue
      if v_to_pe[v] == v_to_pe[vnext] && v_to_pe[v] ≠ (UNSET,UNSET)
        v_to_pv[vnext] = v
        for v_to_d in Π_to_v_to_d
          v_to_d[v] = v_to_d[v_to_pv[v]]
        end
      end
    end
  end
end

function next_vertex(edge_graph::Vector,num_vertices::Integer,vstart::Integer)
  vcurrent = vstart
  vnext = first( edge_graph[vcurrent] )
  while vnext ≤ num_vertices && vnext ∉ (UNSET,OPEN)
    i = findfirst( isequal(vcurrent), edge_graph[vnext] )
    vcurrent = vnext
    inext = ( i % length( edge_graph[vcurrent] ) ) + 1
    vnext = edge_graph[vcurrent][ inext ]
  end
  vnext
end

function compute_intersection(p1::Point,d1::Real,p2::Point,d2::Real)
  ( p1*(abs(d2))+p2*(abs(d1)) ) / ( abs(d1) + abs(d2) )
end

add_vertex!(data::Nothing,a...) = data

function _intersect(a::AbstractVector{T},b::AbstractVector{T}) where T
  n = count(in(a),b)
  r = Vector{T}(undef,n)
  n = 0
  for i in a
    if i ∈ b
      n += 1
      r[n] = i
    end
  end
  r
end

function _copy(a::AbstractVector{<:AbstractVector})
  [ copy(i) for i in a ]
end

function add_vertex!(data::PolyhedronData,v1::Integer,v2::Integer,Πid::Integer)
  v_to_Π = data.vertex_to_planes
  v_to_f = data.vertex_to_original_faces
  v_to_pv = data.vertex_to_parent_vertex
  v_to_pe = data.vertex_to_parent_edge
  Πs = _intersect( v_to_Π[v1], v_to_Π[v2] )
  fs = _intersect( v_to_f[v1], v_to_f[v2] )
  pe = (v_to_pv[v1],v_to_pv[v2])
  push!( Πs, Πid )
  push!( v_to_Π, Πs )
  push!( v_to_f, fs )
  push!( v_to_pe, pe)
  Π_to_v_to_d = get_plane_distances(data)
  Π_to_id = get_plane_ids(data)
  i = findfirst(isequal(Πid),Π_to_id)
  d1 = Π_to_v_to_d[i][v1]
  d2 = Π_to_v_to_d[i][v2]
  pv = iszero(d1) ? v_to_pv[v1] : length(v_to_pv)+1
  pv = iszero(d2) ? v_to_pv[v2] : pv
  push!(v_to_pv,pv)
  @assert d1 ≠ d2
  ref_i = data.plane_to_ref_plane[i]
  for i in 1:length(Π_to_v_to_d)
    v_to_d = Π_to_v_to_d[i]
    if pv ≠ length(v_to_pv)
      dist = v_to_d[pv]
    elseif ref_i == UNSET || data.plane_to_ref_plane[i] ∈ (UNSET,i)
      if Π_to_id[i] == Πid
        dist = 0.0
      else
        dist = (d1*v_to_d[v2]-d2*v_to_d[v1])/(d1-d2)
      end
    else
      j = data.plane_to_ref_plane[i]
      @assert abs(j) < i
      dist = sign(j) * Π_to_v_to_d[abs(j)][end]
    end
    push!(v_to_d,dist)
  end
  data
end

function set_original_faces!(data::PolyhedronData,stl::STL)
  Dc = num_dims(stl)
  v_to_f = data.vertex_to_original_faces
  for d in 0:Dc
    offset = get_offset(stl,d)
    _v_to_f = get_faces(stl,0,d)
    c = array_cache(_v_to_f)
    for v in 1:length(v_to_f)
      append!( v_to_f[v], map( i -> i.+offset, getindex!(c,_v_to_f,v) ) )
    end
  end
  data
end

function next_vertex(p::Polyhedron,vprevious::Integer,vcurrent::Integer)
  i = findfirst( isequal(vprevious), get_graph(p)[vcurrent] )
  i = ( i % length( get_graph(p)[vcurrent] ) ) + 1
  get_graph(p)[vcurrent][ i ]
end

## Kd Tree stuff

function refine_by_vertices(Γ::Polyhedron,K::Polyhedron,atol=0)
  refine_by_vertices(Γ,K,get_vertex_coordinates(Γ),atol)
end

function refine_by_vertices(Γ,K,vertices,atol)
  i = findfirst( v -> has_intersection(K,v), vertices )
  if isnothing(i)
    R = [Γ],[K]
  else
    v = vertices[i]
    d = get_direction(v,K)
    Π,Π⁻,Π⁺ = get_planes(v,d,atol)
    Πid,Πid⁻,Πid⁺ = get_new_plane_ids(Γ)
    compute_distances!(Γ,[Π⁻,Π⁺],[Πid⁻,Πid⁺])
    compute_distances!(K,[Π],[Πid])
    _,γ⁻ = split(Γ,Πid⁺)
    _,γ⁺ = split(Γ,Πid⁻)
    k⁻,k⁺ = split(K,Πid)
    round_distances!(k⁻,atol/10)
    round_distances!(k⁺,atol/10)
    Γn = [γ⁻,γ⁺]
    Kn = [k⁻,k⁺]
    if length(vertices) == i
      R = Γn,Kn
    else
      Γr,Kr = empty(Γn),empty(Kn)
      vertices = view(vertices,i+1:length(vertices))
      for (γ,k) in zip(Γn,Kn)
        Γi,Ki = refine_by_vertices(γ,k,vertices,atol)
        append!(Γr,Γi)
        append!(Kr,Ki)
      end
      R = Γr,Kr
    end
  end
  R
end

function get_bounding_box(cell::Polyhedron)
  i = findfirst(i->isactive(cell,i),1:num_vertices(cell))
  pmin,pmax = cell[i],cell[i]
  for i in 1:num_vertices(cell)
    if isactive(cell,i)
      pmin = Point( min.(Tuple(cell[i]),Tuple(pmin)) )
      pmax = Point( max.(Tuple(cell[i]),Tuple(pmax)) )
    end
  end
  pmin,pmax
end

function has_intersection(cell::Polyhedron,v::Point)
  pmin,pmax = get_bounding_box(cell)
  all( Tuple(pmin) .< Tuple(v) ) || return false
  all( Tuple(pmax) .> Tuple(v) ) || return false
  true
end

function get_direction(v,cell)
  pmin,pmax = get_bounding_box(cell)
  d0 = Tuple(v) .- Tuple(pmin)
  d1 = Tuple(pmax) .- Tuple(v)
  d = min.(d0,d1)
  d_max = maximum(d)
  findfirst(isequal(d_max),d)
end

function get_planes(v,d,atol)
  δ = VectorValue(Base.setindex(Tuple(zero(v)),atol,d))
  Π = CartesianPlane(v,d,+1)
  Π⁻ = CartesianPlane(v-δ,d,+1)
  Π⁺ = CartesianPlane(v+δ,d,-1)
  Π,Π⁻,Π⁺
end

function get_new_plane_ids(poly)
  id = minimum( get_plane_ids(poly.data) )
  id-1,id-2,id-3
end

function round_distances!(poly,atol)
  Π_to_v_to_dist = poly.data.plane_to_vertex_to_distances
  for i in 1:length(Π_to_v_to_dist)
    v_to_dist = Π_to_v_to_dist[i]
    for v in 1:length(v_to_dist)
      if abs(v_to_dist[v]) < atol
        v_to_dist[v] = 0.0
      end
    end
  end
end

function delete_inactive_planes!(Γ::Polyhedron,K::Polyhedron,stl::STL)
  @assert isopen(Γ)
  planes = get_active_planes(Γ,K,stl)
  restrict_planes!(Γ,planes)
  restrict_planes!(K,planes)
end

function get_active_planes(Γ::Polyhedron,K::Polyhedron,stl::STL)
  used_planes_Γ = _get_used_planes(K)
  used_planes_K = _get_used_planes(Γ)
  used_planes = lazy_append( used_planes_Γ, used_planes_K )
  face_planes = _get_face_planes(Γ,stl)
  planes = lazy_append( used_planes, face_planes )
  _append_ref_planes( Γ, planes )
end

function restrict_planes!(poly::Polyhedron,planes)
  restrict_planes!(get_data(poly),planes)
end

function restrict_planes!(data::PolyhedronData,planes)
  Π_to_id = data.plane_to_ids
  Π_to_v_to_dist = data.plane_to_vertex_to_distances
  Π_to_ref_Π = data.plane_to_ref_plane
  is_inactive = trues(length(Π_to_id))
  is_touch = falses(length(Π_to_id))
  for Π in planes
    i = findfirst(isequal(Π),Π_to_id)
    !isnothing(i) || continue
    is_inactive[i] = false
  end
  id = 0
  for i in 1:length(Π_to_id)
    if !is_inactive[i]
      id += 1
      if !is_touch[i]
        ref_plane = abs( Π_to_ref_Π[i] )
        if ref_plane != UNSET
          @assert i == ref_plane
          for j in i:length(Π_to_id)
            if !is_inactive[j] &&
               !is_touch[j] &&
               abs( Π_to_ref_Π[j] ) == ref_plane

              _sign = sign( Π_to_ref_Π[j] )
              Π_to_ref_Π[j] = _sign * id
              is_touch[j] = true
            end
          end
        end
      end
    end
  end
  deleteat!( Π_to_v_to_dist, is_inactive)
  deleteat!( Π_to_ref_Π, is_inactive)
  deleteat!( Π_to_id, is_inactive)
end

function _get_used_planes(poly::Polyhedron)
  v_to_Π = get_data(poly).vertex_to_planes
  planes = Int32[]
  for v in 1:length(v_to_Π)
    for Π in v_to_Π[v]
      if Π ∉ planes
        push!(planes,Π)
      end
    end
  end
  planes
end

function _get_face_planes(poly::Polyhedron,stl::STL)
  rfaces = get_original_reflex_faces(poly,stl,empty=true)
  facets = get_original_facets(poly,stl,empty=true)
  lazy_append(rfaces,facets)
end

function _append_ref_planes(poly::Polyhedron,planes)
  ref_planes = Int32[]
  Π_to_ref_Π = get_data(poly).plane_to_ref_plane
  Π_to_id = get_plane_ids( get_data(poly) )
  for Π in planes
    i = findfirst( isequal(Π), Π_to_id )
    i !== nothing || continue
    iref = abs(Π_to_ref_Π[i])
    if iref ≠ UNSET
      Π_ref = Π_to_id[iref]
      if Π_ref ∉ ref_planes && Π_ref ∉ planes
        push!(ref_planes,Π_ref)
      end
    end
  end
  lazy_append(planes,ref_planes)
end


text:
struct BruteTree{V <: AbstractVector,M <: Metric} <: NNTree{V,M}
    data::Vector{V}
    metric::M
    reordered::Bool
end

"""
    BruteTree(data [, metric = Euclidean()) -> brutetree

Creates a `BruteTree` from the data using the given `metric`.
"""
function BruteTree(data::AbstractVector{V}, metric::Metric = Euclidean();
                   reorder::Bool=false, leafsize::Int=0, storedata::Bool=true) where {V <: AbstractVector}
    BruteTree(storedata ? data : Vector{V}(), metric, reorder)
end

function BruteTree(data::AbstractMatrix{T}, metric::Metric = Euclidean();
                   reorder::Bool=false, leafsize::Int=0, storedata::Bool=true) where {T}
    dim = size(data, 1)
    npoints = size(data, 2)
    BruteTree(copy_svec(T, data, Val(dim)),
              metric, reorder = reorder, leafsize = leafsize, storedata = storedata)
end

function _knn(tree::BruteTree{V},
                 point::AbstractVector,
                 best_idxs::Vector{Int},
                 best_dists::Vector,
                 skip::Function) where {V}

    knn_kernel!(tree, point, best_idxs, best_dists, skip)
    return
end

function knn_kernel!(tree::BruteTree{V},
                     point::AbstractVector,
                     best_idxs::Vector{Int},
                     best_dists::Vector,
                     skip::F) where {V, F}
    for i in 1:length(tree.data)
        if skip(i)
            continue
        end

        @POINT 1
        dist_d = evaluate(tree.metric, tree.data[i], point)
        if dist_d <= best_dists[1]
            best_dists[1] = dist_d
            best_idxs[1] = i
            percolate_down!(best_dists, best_idxs, dist_d, i)
        end
    end
end

function _inrange(tree::BruteTree,
                  point::AbstractVector,
                  radius::Number,
                  idx_in_ball::Vector{Int})
    inrange_kernel!(tree, point, radius, idx_in_ball)
    return
end

function inrange_kernel!(tree::BruteTree,
                         point::AbstractVector,
                         r::Number,
                         idx_in_ball::Vector{Int})
    for i in 1:length(tree.data)
        @POINT 1
        d = evaluate(tree.metric, tree.data[i], point)
        if d <= r
            push!(idx_in_ball, i)
        end
    end
end


text:
using ThesisTools
using Test

@testset "ThesisTools.jl" begin
    # Write your own tests here.
end