gN = input.getNumber
m = math
sin, cos = m.sin, m.cos
s = screen
DT = s.drawTriangle

Sensi = property.getNumber("Sensitivity") * 0.001
S_Compensation = property.getBool("Speed Compensation")
A_Compensation = property.getBool("Acceleration Compensation")
ticks = property.getNumber("Ticks compensation")
GPSbuttonToggle = property.getBool("GPS targeting mode")
w, h = 32, 32
o = { 0, 0, 0 }
zoom, Output, offset, pointABS, point = 0, { 0, 0 }, { 0, 0, 0.892 }, o, o
angOld, gpsOld, speedOld, angspeedOld, AimAngle = o, o, o, o, o

function onTick()
    --input
    inX, inY = gN(18), gN(19)
    dist = gN(20)
    ScreenClicked = input.getBool(1)
    targetModule = input.getBool(2)

    -- angular stuff
    local ang = { gN(4), gN(5), gN(6) }
    local angspeed = VectorSub(ang, angOld)
    local angacceleration = VectorSub(angspeed, angspeedOld)

    -- x y z stuff
    local gps = { gN(1), gN(2), gN(3) }

    --buttons

    if targetModule then
        if GPSbuttonToggle then
            if hit(-1, 23, 6, 6) and noClickLastTick then GPStargeting = not GPStargeting end
        else
            GPStargeting = hit(-1, 23, 6, 6)
        end

        pointABS = GPStargeting and { gN(21), gN(22), gN(23) } or pointABS
    end

    if noClickLastTick then
        if hit(-1, 13, 6, 6) then IR = not IR end
        if hit(-1, 18, 6, 6) then LaserOn = not LaserOn end
        if hit(-1, 8, 6, 6) then tracking = not tracking end
    end
    Zup, Zdown = hit(0, -1, 6, 6), hit(0, 4, 6, 6)
    zoom = Counter(zoom, Zup, Zdown, 0.005, 0, 1)

    -- tick compensation
    -- D = 1/2 a * t^2 + v0 * t + D0
    angCompensated = ang
    if S_Compensation then
        angCompensated = VectorAdd(angCompensated, VectorScall(angspeed, ticks))
    end
    if A_Compensation then
        angCompensated = VectorAdd(angCompensated, VectorScall(angacceleration, (ticks ^ 2) / 2))
    end

    --the real deal
    if tracking or GPStargeting then
        point = VectorSub(VectorRot(XYZ(angCompensated), VectorSub(pointABS, gps)), offset)
        --[[    - Substracts pod coordinates to the point coordinates (-> gets the vector in the global frame)
        - Apply reversed rotation to get the vector in the local frame
        - Adjust the offset between the laser and the sensor
]]
        dist2 = (point[1] ^ 2 + point[3] ^ 2 + point[2] ^ 2) ^ 0.5
        AimAngle = { m.atan(point[1], point[3]), m.asin(point[2] / dist2), m.asin((point[2] + 0.25) / dist2) }
        --- Azimuth, Elevation, adjusted Elevation for the camera

        Output = VectorScall(AimAngle, 4 / m.pi)
        --- convertion from radians to gimbal parts rotation
    end
    canAim = ScreenClicked and (inX > 6)
    --- locks aiming if a button is pressed
    if canAim or not tracking then
        if canAim then
            local r = Sensi / (zoom * 5 + 1)
            Output = { clamp(Output[1] + ((inX * 2 - w) / w * r), -1, 1),
                clamp(Output[2] - ((inY * 2 - h) / h * r), -1, 1) }
            --- adds or substracts to the Azimut (1) or Elevation (2) based on the distance to the center of the screen and zooming

            AimAngle = VectorScall(Output, m.pi / 4)
            --- convertion from gimbal parts rotation to radians
        end

        point    = { sin(AimAngle[1]) * cos(AimAngle[2]) * dist, sin(AimAngle[2]) * dist,
            cos(AimAngle[1]) * cos(AimAngle[2]) * dist }
        pointABS = VectorAdd(VectorRot(ZYX(angCompensated), VectorAdd(point, offset)), gps)
        --[[    -  Adjust the offset between the laser and the sensor
        - Apply rotation to get the vector in the global frame
        - Add pod coordinates to the vector (-> gets the point gps)
]]
        Output[3] = LaserOn and m.atan(point[2], point[3]+0.25) * 4 / m.pi or Output[2]
        --- Adjusted Elevation for the camera
    end

    -- outputs
    Out = { Output[1], Output[2], Output[3], pointABS[1], pointABS[2], pointABS[3], zoom }
    for i = 1, #Out do
        output.setNumber(i, Out[i])
    end

    output.setBool(1, LaserOn)
    output.setBool(2, IR)

    --tick variables
    angOld = ang
    angspeedOld = angspeed


    noClickLastTick = not ScreenClicked
end

function onDraw()
    w = s.getWidth()
    h = s.getHeight()

    s.setColor(0, 255, 0, 225)
    s.drawLine(13, 15, 15, 15)
    s.drawLine(18, 15, 20, 15)
    s.drawLine(16, 12, 16, 14)
    s.drawLine(16, 17, 16, 19)

    s.drawLine(28, 10, 28, 30)
    s.drawLine(28, 10, 30, 10)
    s.drawLine(28, 30, 30, 30)

    zoomlevel = 29 - zoom * 18
    s.drawLine(29, zoomlevel, 30, zoomlevel)

    dist_display = math.floor(dist)
    dStr(32 - string.len(dist_display) * 4, 1, dist_display)

    s.setColor(10, 10, 10)
    s.drawRectF(0, 0, 5, 32)
    s.setColor(5, 5, 5)
    s.drawRectF(0, 1, 5, 3)
    s.drawRectF(0, 5, 5, 3)
    for i = 1, targetModule and 4 or 3 do s.drawRectF(0, 4 + 5 * i, 5, 4) end

    s.setColor(50, 50, 50, Zup and 150 or 255) --Z UP
    s.drawLine(2, 1, 2, 4)
    s.drawLine(1, 2, 4, 2)

    s.setColor(50, 50, 50, Zdown and 150 or 255) -- ZDown
    s.drawLine(1, 6, 4, 6)

    s.setColor(50, 50, 50, tracking and 150 or 255) --tracking
    dStr(1, 9, "T")

    s.setColor(50, 50, 50, IR and 150 or 255) --IR
    dStr(2, 14, "R")
    s.drawLine(0, 14, 0, 18)

    s.setColor(50, 50, 50, LaserOn and 150 or 255) --Laser
    dStr(1, 19, "L")

    if targetModule then
        s.setColor(50, 50, 50, GPStargeting and 150 or 255)
        dStr(1, 24, "C")
    end

    s.setColor(50, 50, 50, ScreenClicked and (inX > 6) and 200 or 0)
    s.drawCircle(inX, inY, 2)
end

--- Rotation Matrix
function ZYX(angtable)
    local sx, sy, sz, cx, cy, cz = m.sin(angtable[1]), m.sin(angtable[2]), m.sin(angtable[3]), m.cos(angtable[1]),
        m.cos(angtable[2]), m.cos(angtable[3])
    rotationMatrix = {
        { cz * cy, -sz * cx + cz * sx * sy, sz * sx + cz * sy * cx },
        { sz * cy, cz * cx + sz * sy * sx,  -sx * cz + sz * sy * cx },
        { -sy,     cy * sx,                 cx * cy }
    }
    return rotationMatrix
end

function XYZ(angtable)
    local sx, sy, sz, cx, cy, cz = m.sin(angtable[1]), m.sin(angtable[2]), m.sin(angtable[3]), m.cos(angtable[1]),
        m.cos(angtable[2]), m.cos(angtable[3])
    rotationMatrix = {
        { cz * cy,                 cy * sz,                 -sy },
        { -sz * cx + cz * sy * sx, cz * cx + sz * sy * sx,  sx * cy },
        { sx * sz + cx * sy * cz,  -sx * cz + cx * sy * sz, cx * cy }
    }
    return rotationMatrix
end

--- Vector Stuff
VectorRot = function(rotmatrix, vector) --Assuming matrix multiplication is possible
    local res = {}

    for i = 1, #rotmatrix do
        res[i] = 0
        for k = 1, #vector do
            res[i] = res[i] + rotmatrix[i][k] * vector[k]
        end
    end

    return res
end

function VectorAdd(Vec1, Vec2)
    return { Vec1[1] + Vec2[1], Vec1[2] + Vec2[2], Vec1[3] + Vec2[3] }
end

function VectorSub(Vec1, Vec2)
    return { Vec1[1] - Vec2[1], Vec1[2] - Vec2[2], Vec1[3] - Vec2[3] }
end

function VectorScall(Vec, factor)
    return { Vec[1] * factor, Vec[2] * factor, Vec[3] and Vec[3] * factor or nil }
end

--- User interface stuff
function hit(x, y, w, h)
    return inX > x and inY > y and inX < x + w and inY < y + h and ScreenClicked
end

function Counter(value, Up, Down, Reason, min, max)
    value = value + (Up and Reason or 0) - (Down and Reason or 0)
    return clamp(value, min, max)
end

--- mini Text function
font34 = {
    0x0D0, 0xC0C, 0xFAF, 0x2F4, 0xB2D, 0x6F5, 0x0C0, 0x690, 0x096, 0xAEA, 0x4E4, 0x560, 0x444, 0x010, 0x168, 0x79E,
    0x5F1,
    0x9B5, 0x9DA, 0x6F2, 0xDDA, 0x6DA, 0x9AC, 0x3FC, 0x4A7, 0x050, 0x1A0, 0x44A, 0xAAA, 0xA44, 0xA41, 0x69D, 0x7A7,
    0xFD6,
    0x699, 0xF96, 0xFD9, 0xFA8, 0x69B, 0xF2F, 0x9F9, 0x19E, 0xF4B, 0xF11, 0xF4F, 0xF6F, 0x696, 0xFA4, 0x6B7, 0xFA5,
    0x5BA,
    0x8F8, 0xE1E, 0xC3C, 0xF5F, 0x969, 0xC7C, 0xBD9, 0xF90, 0x861, 0x09F, 0x484, 0x111, 0x084, 0x2F9, 0x0F0, 0x9F4, 0x462 }
function dDot(x, y)
    s.drawLine(x, y, x, y + 1)
end

function dChar(x, y, char)
    c = string.byte(string.upper(char)) - 32
    if c > 64 then c = c - 26 end
    if c > 0 and c < 69 then
        for i = 0, 11 do
            if font34[c] & (1 << (11 - i)) > 0 then dDot(x + i // 4, y + i % 4) end
        end
    end
end

function dStr(x, y, str)
    cs = string.len(str)
    for i = 0, cs - 1 do
        dChar(x + 4 * i, y, string.sub(str, i + 1, i + 1))
    end
end

function clamp(value, min, max)
    return math.min(math.max(value, min), max)
end