-- ============================================================================
-- PROJECT ONYX: V38.0 (PRECISION FIRE CONTROL SYSTEM)
-- ============================================================================

local reader     = peripheral.wrap("block_reader_5")
local speedYaw   = peripheral.wrap("Create_RotationSpeedController_4")
local speedPitch = peripheral.wrap("Create_RotationSpeedController_3")
local relay1     = peripheral.wrap("redstone_relay_1")

local PIN_ASSEMBLE = "front"
local PIN_FIRE     = "back"

-- ----------------------------------------------------------------------------
-- CONFIG
-- ----------------------------------------------------------------------------
local C_X, C_Y, C_Z = -5, -40, -2
local V0 = 160.0
local G  = 20.0
local MU = 1 - 0.99^20
local L  = 11

local YAW_OFFSET = 0
local YAW_INVERT = -1

local FIXED_RPM = 1
local REAL_DEG_PER_TICK = 0.002344

local MAX_PITCH, MIN_PITCH = 60, -30

local SETTINGS = {
    shots = 1,
    reload = 3.2,
    predictive = true,
}

local bookmarks = {
    spawn = {x=0,y=64,z=0},
}

-- ----------------------------------------------------------------------------
-- UTILS
-- ----------------------------------------------------------------------------
local function log(cat, msg, color)
    if term.isColor() then term.setTextColor(color or colors.white) end
    print(string.format("[%s] %-8s | %s", os.date("%H:%M:%S"), cat, msg))
    term.setTextColor(colors.white)
end

local function getPos()
    local d = reader.getBlockData()
    if not d then return nil, nil end
    local yaw = ((d.CannonYaw or 0) * YAW_INVERT) % 360
    if yaw < 0 then yaw = yaw + 360 end
    return yaw, (d.CannonPitch or 0)
end

-- ----------------------------------------------------------------------------
-- SOLVER (NEWTON + BARREL OFFSET)
-- ----------------------------------------------------------------------------
local function solve(tx, ty, tz)
    local dx, dz = tx - C_X, tz - C_Z
    local yaw = math.atan2(dx, dz)

    -- barrel offset
    local ox = math.sin(yaw) * L
    local oz = math.cos(yaw) * L

    local rx = tx - (C_X + ox)
    local rz = tz - (C_Z + oz)

    local dist = math.sqrt(rx*rx + rz*rz)
    local height = ty - C_Y

    local phi = (math.deg(yaw) + YAW_OFFSET) % 360

    -- smart initial guess
    local theta = math.deg(math.atan2(height, dist))

    for i=1,50 do
        local rad = math.rad(theta)
        local v0x = V0 * math.cos(rad)
        local v0y = V0 * math.sin(rad)

        if math.abs(v0x) < 1e-6 then
            theta = theta + 1
            goto continue
        end

        local max_x = v0x / MU
        if dist >= max_x * 0.999 then
            theta = theta + 1
            goto continue
        end

        local inside = 1 - (dist * MU / v0x)
        if inside <= 0 then
            theta = theta + 1
            goto continue
        end

        local t = -math.log(inside) / MU
        local exp = math.exp(-MU * t)

        local y = (1/MU)*(v0y + G/MU)*(1-exp) - (G*t/MU)
        local err = height - y

        if math.abs(err) < 0.01 then break end

        -- slope
        local eps = 0.05
        local rad2 = math.rad(theta + eps)
        local v0x2 = V0 * math.cos(rad2)
        local v0y2 = V0 * math.sin(rad2)

        local inside2 = 1 - (dist * MU / v0x2)

        if inside2 > 0 and math.abs(v0x2) > 1e-6 then
            local t2 = -math.log(inside2) / MU
            local exp2 = math.exp(-MU * t2)
            local y2 = (1/MU)*(v0y2 + G/MU)*(1-exp2) - (G*t2/MU)

            local slope = (y2 - y) / eps

            if math.abs(slope) > 1e-6 then
                theta = theta + err / slope
            else
                theta = theta + err * 0.1
            end
        else
            theta = theta + 1
        end

        -- clamp
        if theta > MAX_PITCH then theta = MAX_PITCH end
        if theta < MIN_PITCH then theta = MIN_PITCH end

        ::continue::
    end

    if theta >= MIN_PITCH and theta <= MAX_PITCH then
        return {p = theta, y = phi}
    end

    return nil
end

-- ----------------------------------------------------------------------------
-- ALIGNMENT
-- ----------------------------------------------------------------------------
local function align(sol)
    while true do
        local cy, cp = getPos()

        local dy = sol.y - cy
        while dy > 180 do dy = dy - 360 end
        while dy < -180 do dy = dy + 360 end

        local dp = sol.p - cp

        if math.abs(dy) < 1 and math.abs(dp) < 1 then break end

        speedYaw.setTargetSpeed((math.abs(dy)>1) and 128*(dy>0 and 1 or -1) or 0)
        speedPitch.setTargetSpeed((math.abs(dp)>1) and 128*(dp>0 and 1 or -1) or 0)

        sleep(0.05)
    end

    speedYaw.setTargetSpeed(0)
    speedPitch.setTargetSpeed(0)
end

-- ----------------------------------------------------------------------------
-- PREDICTIVE FIRE
-- ----------------------------------------------------------------------------
local function fire(sol, shots)
    shots = shots or SETTINGS.shots

    for s=1,shots do
        local lastY, lastP = getPos()

        while true do
            local cy, cp = getPos()

            local dy = sol.y - cy
            while dy > 180 do dy = dy - 360 end
            while dy < -180 do dy = dy + 360 end

            local dp = sol.p - cp

            local vY = cy - lastY
            local vP = cp - lastP

            local eY = math.abs(dy)
            local eP = math.abs(dp)

            -- ------------------------------------------------------------
            -- ✧ CASE 1: Perfect stop → FORCE MICRO MOVEMENT (key fix)
            -- ------------------------------------------------------------
            if math.abs(vY) < 1e-6 and math.abs(vP) < 1e-6 then
                if eY > 0.002 then
                    speedYaw.setTargetSpeed(FIXED_RPM * (dy > 0 and 1 or -1))
                else
                    speedYaw.setTargetSpeed(0)
                end

                if eP > 0.002 then
                    speedPitch.setTargetSpeed(FIXED_RPM * (dp > 0 and 1 or -1))
                else
                    speedPitch.setTargetSpeed(0)
                end
            end

            -- ------------------------------------------------------------
            -- ✧ CASE 2: Predictive firing (when moving)
            -- ------------------------------------------------------------
            local nextY = cy + vY
            local nextP = cp + vP

            local eNow = math.abs(sol.y - cy) + math.abs(sol.p - cp)
            local eNext = math.abs(sol.y - nextY) + math.abs(sol.p - nextP)

            if SETTINGS.predictive and (math.abs(vY) > 1e-6 or math.abs(vP) > 1e-6) then
                if eNext > eNow then
                    break
                end
            end

            -- ------------------------------------------------------------
            -- ✧ CASE 3: Hard fallback (like your old code)
            -- ------------------------------------------------------------
            if eY <= 0.01 and eP <= 0.01 then
                break
            end

            lastY, lastP = cy, cp
            sleep(0.05)
        end

        -- stop motion before firing
        speedYaw.setTargetSpeed(0)
        speedPitch.setTargetSpeed(0)

        log("FIRE", "Shot "..s.."/"..shots, colors.red)

        relay1.setOutput(PIN_FIRE, true)
        sleep(0.6)
        relay1.setOutput(PIN_FIRE, false)

        if s < shots then
            sleep(SETTINGS.reload)
        end
    end
end


-- ----------------------------------------------------------------------------
-- COMMAND SYSTEM
-- ----------------------------------------------------------------------------
local function help()
    print("\nCommands:")
    print(" shoot x y z [n]")
    print(" set shots n")
    print(" set reload t")
    print(" toggle predictive")
    print(" bookmark name x y z")
    print(" goto name [n]")
    print(" exit\n")
end

local function main()
    term.clear()
    term.setCursorPos(1,1)

    log("SYSTEM","ONYX V38 ONLINE",colors.cyan)
    relay1.setOutput(PIN_ASSEMBLE,true)

    help()

    while true do
        term.setTextColor(colors.lime)
        write("\n> ")
        term.setTextColor(colors.white)

        local input = read()
        if input == "exit" then break end

        local args = {}
        for w in input:gmatch("%S+") do table.insert(args,w) end

        local cmd = args[1]

        if cmd == "shoot" and #args >= 4 then
            local x,y,z = tonumber(args[2]), tonumber(args[3]), tonumber(args[4])
            local n = tonumber(args[5]) or SETTINGS.shots

            local sol = solve(x,y,z)
            if sol then
                log("AIM", string.format("Y:%.2f P:%.2f",sol.y,sol.p), colors.blue)
                align(sol)
                fire(sol,n)
            else
                log("ERROR","Unreachable",colors.red)
            end

        elseif cmd == "set" then
            if args[2]=="shots" then SETTINGS.shots = tonumber(args[3]) end
            if args[2]=="reload" then SETTINGS.reload = tonumber(args[3]) end

        elseif cmd == "toggle" and args[2]=="predictive" then
            SETTINGS.predictive = not SETTINGS.predictive
            log("CONFIG","Predictive: "..tostring(SETTINGS.predictive),colors.orange)

        elseif cmd == "bookmark" and #args>=5 then
            bookmarks[args[2]] = {
                x=tonumber(args[3]),
                y=tonumber(args[4]),
                z=tonumber(args[5])
            }
            log("SAVE","Bookmark "..args[2],colors.green)

        elseif cmd == "goto" and bookmarks[args[2]] then
            local t = bookmarks[args[2]]
            local n = tonumber(args[3]) or SETTINGS.shots

            local sol = solve(t.x,t.y,t.z)
            if sol then
                align(sol)
                fire(sol,n)
            end

        else
            help()
        end
    end
end

local ok, err = pcall(main)
speedYaw.setTargetSpeed(0)
speedPitch.setTargetSpeed(0)

if not ok then print("CRASH: "..err) end