Thrusters = {} -- Table of thruster tables; see docs.md####Thrusters for the details of the thruster table structure

ThrustDirections = {
    Angular = {
        Negative = {
            x = "pitchdown",
            y = "yawport",
            z = "rollport"
        },
        Positive = {
            x = "pitchup",
            y = "yawstar",
            z = "rollstar"
        }
    },
    Lateral = {
        Negative = {
            x = "star",
            y = "down",
            z = "aft"
        },
        Positive = {
            x = "port",
            y = "up",
            z = "fore"
        }
    }
}

SensorData = {}

ThrusterTypes = {
    "thruster",
    "solid_fuel_thruster",
    "ion_thruster",
    "vector_thruster",
    "liquid_vector_thruster"
}

PropellerTypes = {
    "gyroscopic_propeller_bearing",
    "propeller",
    "propeller_bearing"
}

TransmissionTypes = {
    "analog_transmission",
    "Create_RotationSpeedController"
}

Config = {}
Config.ConfigPath = "/caero-attitude-control/config/config.txt"
Config.Debug = false -- this controls debug prints

-- File serializatino and deserialization
-- taken from https://gist.github.com/yuhanz/6688d474a3c391daa6d6
function tableToString(table)
    return "return"..serializeTable(table)
end

-- loadstring is deprecated, so modified to run without it
function stringToTable(str)
    local f = {}
    local chunk, err = load(str)
    if chunk then
        f = chunk()
    else
        print("Error loading config string: "..err)
    end
    return f
end

function serializeTable(val, name, skipnewlines, depth)
    skipnewlines = skipnewlines or false
    depth = depth or 0

    local tmp = string.rep(" ", depth)
    if name then 
    	if not string.match(name, '^[a-zA-Z_][a-zA-Z0-9_]*$') then
    		name = string.gsub(name, "'", "\\'")
    		name = "['".. name .. "']"
    	end
    	tmp = tmp .. name .. " = "
     end

    if type(val) == "table" then
        tmp = tmp .. "{" .. (not skipnewlines and "\n" or "")

        for k, v in pairs(val) do
            tmp =  tmp .. serializeTable(v, k, skipnewlines, depth + 1) .. "," .. (not skipnewlines and "\n" or "")
        end

        tmp = tmp .. string.rep(" ", depth) .. "}"
    elseif type(val) == "number" then
        tmp = tmp .. tostring(val)
    elseif type(val) == "string" then
        tmp = tmp .. string.format("%q", val)
    elseif type(val) == "boolean" then
        tmp = tmp .. (val and "true" or "false")
    else
        tmp = tmp .. "\"[inserializeable datatype:" .. type(val) .. "]\""
    end

    return tmp
end

-- Monitor Configuration  
Config.Monitors = {}
Config.Monitors.InstrumentPanelMonitor = nil
Config.Monitors.AutopilotControlMonitor = nil

local function identifyMonitor(monitorType)
    -- Find all available monitors
    local monitors = { peripheral.find("monitor") }
    
    if not monitors or #monitors == 0 then
        print("No monitors found on the network.")
        return nil
    end
    
    print("Available monitors:")
    for i, monitor in ipairs(monitors) do
        if monitor then
            local width, height = monitor.getSize()
            print(i .. ". " .. peripheral.getName(monitor) .. " (" .. width .. "x" .. height .. ")")
        end
    end
    
    print("\nPlease select the monitor you want to use for " .. monitorType .. " (enter number):")
    
    -- Simple input handling for demonstration
    local input = read()
    local choice = tonumber(input)
    
    if not choice or choice < 1 or choice > #monitors then
        print("Invalid selection.")
        return nil
    end
    
    local selectedMonitor = monitors[choice]
    local selectedMonitorName = peripheral.getName(selectedMonitor)
    
    print("Selected monitor: " .. selectedMonitorName)
    
    -- Return monitor details as a table
    return {
        name = selectedMonitorName,
        peripheral = selectedMonitor
    }
end

-- Autopilot
Config.Autopilot = {
    AutopilotEngaged = false,
    AutoForeAft = false,
    AutopilotDesiredAltitude = nil,
    AutopilotDesiredSpeed = nil,
    AutopilotDesiredHeading = nil
}

-- Correctional values for when some sensor values are offset
Config.SensorCorrection = {
    Heading = 0 -- this should be +/- 180
}

Config.Throttles = nil

function ThrottleInit() 

    if Config.Throttles ~= nil then
        return
    end

    Config.Throttles = {}

    local sides = {
        "top",
        "bottom",
        "left",
        "right",
        "front",
        "back"
    }

    local function getRelayStates()
        local states = {}
        local sides = {
            "top",
            "bottom",
            "left",
            "right",
            "front",
            "back"
        }

        local names = peripheral.getNames()
        for _, name in ipairs(names) do
            if peripheral.getType(name) == "redstone_relay" then
                local wrapper = peripheral.wrap(name)
                for _, side in ipairs(sides) do
                    states[name] = states[name] or {}
                    states[name][side] = wrapper.getAnalogInput(side)
                end
            end
        end

        return states
    end

    local function identifyThrottle(throttleType)
        print("Identifying "..throttleType.." throttle.")
        print("\nPlease change the input signal for the throttle.")

        local initialStates = getRelayStates()

        os.pullEvent("redstone")
        
        local currentStates = getRelayStates()

        Config.Throttles[throttleType] = {}

        for pname, currentState in pairs(currentStates) do
            local initialState = initialStates[pname]
            for k, v in pairs(currentState) do
                if initialState[k] ~= v then
                    Config.Throttles[throttleType].name = pname
                    Config.Throttles[throttleType].side = k
                    return
                end
            end
        end
    end

    identifyThrottle("fore")
    identifyThrottle("aft")
    identifyThrottle("down")
end

function PropellerInit()
    local transmissions = {}
    local propellers = {}

    for _, v in ipairs(TransmissionTypes) do
        local vlist = { peripheral.find(v) }
        if vlist ~= nil and vlist[1] ~= nil then
            table.insert(transmissions, vlist)
        end
    end

    for _, v in ipairs(PropellerTypes) do
        local vlist = peripheral.find(v)
        if vlist ~= nil and vlist[1] ~= nil then
            table.insert(propellers, vlist)
        end
    end

    for pi, pv in ipairs(propellers) do 
        if Thrusters[peripheral.getName(pv)] == nil then
            for _, tv in pairs(transmissions) do
                if pv.getSourceId() == tv.getSelfId() then
                    if Thrusters[peripheral.getName(pv)] == nil then
                        Thrusters[peripheral.getName(pv)] = {
                            type = "rotator",
                            thruster = pv,
                            transmission = peripheral.wrap(tv),
                            --[[
                                NOTE:
                                affectVectors will have values depending on how the thruster's 
                                thrust vector interacts with the vessel's attitude
                                    angular:
                                possible directions for angular.yaw: "port"|"star"
                                possible directions for angular.roll: "port"|"star"
                                possible directions for angular.pitch: "up"|"down"
                                    lateral:
                                possible directions for lateral.x: "port"|"star"
                                possible directions for lateral.y: "up"|"down"
                                possible directions for lateral.z: "fore"|"aft"
                            ]]
                            affectVectors = {
                                angular = {
                                    yaw = nil,
                                    pitch = nil,
                                    roll = nil
                                },
                                lateral = {
                                    x = nil,
                                    y = nil,
                                    z = nil
                                }
                            },
                            power = (tv.getSignal())/15
                        }
                    end
                end
            end
        elseif Config.Debug then
            print("DEBUG: thruster of name "..peripheral.getName(pv).." already indexed, skipping (called by PropellerInit)")
        end
    end
end

function ThrusterInit()
    local thrusters = {}

    for _, v in ipairs(ThrusterTypes) do
        local vlist = peripheral.find(v)
        if vlist ~= nil and vlist[1] ~= nil then
            table.insert(thrusters, vlist)
        end
    end

    for ti, tv in ipairs(thrusters) do
        if Thrusters[peripheral.getName(tv)] == nil then
            Thrusters[peripheral.getName(tv)] = {
                thruster = tv,
                transmission = nil,
                type = "thruster",
                --[[
                    NOTE:
                    affectVectors will have values depending on how the thruster's 
                    thrust vector interacts with the vessel's attitude
                        angular:
                    possible directions for angular.yaw: "port"|"star"
                    possible directions for angular.roll: "port"|"star"
                    possible directions for angular.pitch: "up"|"down"
                        lateral:
                    possible directions for lateral.x: "port"|"star"
                    possible directions for lateral.y: "up"|"down"
                    possible directions for lateral.z: "fore"|"aft"
                ]]
                affectVectors = {
                    angular = {
                        yaw = nil,
                        pitch = nil,
                        roll = nil
                    },
                    lateral = {
                        x = nil,
                        y = nil,
                        z = nil
                    }
                },
                power = tv.getPower()
            }
        elseif Config.Debug then
            print("DEBUG: thruster of name "..peripheral.getName(tv).." already indexed, skipping (called by ThrusterInit)")
        end
    end
end

-- this is for populating nil values from thruster config files
local function populateThrusterValues()
    for _, thruster in pairs(Thrusters) do
        if thruster.name == nil and thruster.thruster == nil then -- as long as we have one of thruster or name, we can fetch other data
            print("ERROR: Thruster ".._.." has no name or thruster object. Removing entry in thruster list")
            Thrusters[_] = nil
            return
        end
        if thruster.name == nil and thruster.thruster ~= nil then
            thruster.name = peripheral.getName(thruster.thruster)
        end
        
        if thruster.thruster == nil and thruster.name ~= nil then
            thruster.thruster = peripheral.wrap(thruster.name)
        end
        if thruster.type == nil then
            if Config.Debug then print("DEBUG: Thruster "..thruster.name.." does not have a type.") end
            for _, tt in ipairs(ThrusterTypes) do
                if peripheral.getType(thruster.thruster) == tt then
                    if Config.Debug then print("DEBUG: Detected thruster "..thruster.name.." is of type 'thruster'.") end
                    thruster.type = "thruster"
                    break
                end
            end

            for _, pt in ipairs(PropellerTypes) do
                if peripheral.getType(thruster.thruster) == pt then
                    if Config.Debug then print("DEBUG: Detected thruster "..thruster.name.." is of type 'rotator'.") end
                    thruster.type = "rotator"
                    break
                end
            end
        end
        if thruster.type == "rotator" and thruster.transmission == nil then
            if Config.Debug then print("DEBUG: Thruster "..thruster.name.." does not have an associated transmission.") end
            local subnetworkId = peripheral.wrap(thruster.name).getSourceId()
            for _, transmissionType in ipairs(TransmissionTypes) do
                local transmissions = { peripheral.find(transmissionType) }
                if transmissions ~= nil and transmissions[1] ~= nil then
                    if Config.Debug then print("DEBUG: transmission of type "..transmissionType.." found.") end
                    for _, transmission in ipairs(transmissions) do
                        local thisTransmissionId = transmission.getSelfId()
                        if Config.Debug then print("DEBUG: Comparing subnetwork IDs values: "..thisTransmissionId.." == "..subnetworkId) end
                        if transmission.getSelfId() == subnetworkId then
                            if Config.Debug then print("DEBUG: Detected that thruster "..thruster.name.." is connected to transmission "..peripheral.getName(transmission)) end
                            thruster.transmission = peripheral.getName(transmission)
                            break
                        elseif Config.Debug then
                            print("DEBUG: Comparison inequal")
                        end
                    end
                elseif Config.Debug then
                    print("DEBUG: No transmissions of type "..transmissionType.." (print from populateThrusterValues)")
                end
            end
        end
        if thruster.power == nil then
            if Config.Debug then print("DEBUG: Thruster "..thruster.name..": thruster power is not populated") end
            if thruster.type == "thruster" then
                thruster.power = thruster.thruster.getPower()
            elseif thruster.type == "rotator" then
                if thruster.transmission ~= nil then
                    local transmissionType = peripheral.getType(thruster.transmission)
                    if transmissionType == "analog_transmission" then
                        thruster.power = (thruster.transmission.getSignal())/15
                    elseif transmissionType == "Create_RotationSpeedController" then
                        thruster.power = (thruster.transmission.getTargetSpeed())/256
                    end
                else
                    print("ERROR: Thruster "..peripheral.getName(thruster.thruster).." has no transmission.")
                end
            end
        end
    end
end

function Update()
    populateThrusterValues()
    PropellerInit()
    ThrusterInit()
end

local function checkIfThrusterIsIndexed()
    local thrusters = {}
    local unindexedThrusters = {}

    for _, v in ipairs(ThrusterTypes) do
        local thrustersList = peripheral.find(v)
        if thrustersList then
            table.insert(thrusters, thrustersList)
        end
    end

    for _, v in ipairs(PropellerTypes) do
        local thrustersList = peripheral.find(v)
        if thrustersList then
            table.insert(thrusters, thrustersList)
        end
    end

    for _, v in ipairs(thrusters) do
        if Thrusters[peripheral.getName(v)] == nil then
            table.insert(unindexedThrusters, v)
        end
    end

    if #unindexedThrusters == 0 then
        return nil
    end
    return unindexedThrusters
end

local function partiallyUpdateThrusters(thrusterList)
    local thrusterTypes = {
        "thruster",
        "ion_thruster",
        "vector_thruster",
        "liquid_vector_thruster"
    }

    local propellerTypes = {
        "gyroscopic_propeller_bearing",
        "propeller",
        "propeller_bearing"
    }

    -- for every thruster in the input array
    -- check if they are one of thrusterTypes
    for _, tv in ipairs(thrusterList) do
        for _, tt in ipairs(ThrusterTypes) do
            if peripheral.getType(tv) == tt then
                Thrusters[peripheral.getName(tv)] = {
                    thruster = tv,
                    transmission = nil,
                    type = "thruster",
                    affectVectors = {
                        angular = {
                            yaw = nil,
                            roll = nil,
                            pitch = nil
                        },
                        lateral = {
                            x = nil,
                            y = nil,
                            z = nil
                        }
                    },
                    power = tv.getPower()
                }
            end
        end

        for _, pt in ipairs(PropellerTypes) do
            if peripheral.getType(tv) == pt then
                local thisThruster = {
                    thruster = tv,
                    transmission = nil,
                    type = "rotator",
                    affectVectors = {
                        angular = {
                            yaw = nil,
                            roll = nil,
                            pitch = nil
                        },
                        lateral = {
                            x = nil,
                            y = nil,
                            z = nil
                        }
                    },
                    power = tv.getPower()
                }

                for _, tt in ipairs(TransmissionTypes) do 
                    local transmissions = peripheral.find(tt)
                    for _, t in ipairs(transmissions) do
                        if tv.getSourceId() == t.getSelfId() then
                            thisThruster.transmission = peripheral.getName(t)
                        end
                    end
                end

                Thrusters[peripheral.getName(tv)] = thisThruster
            end
        end
    end
end

function PollThrottle()
    for _, v in pairs(Config.Throttles) do
        if v.name and v.side then
            v.input = peripheral.wrap(v.name).getAnalogInput(v.side)
        end
    end
end

function PollSensors()
    PollNavTable()
    PollAltitude()
    PollGimbal()
    PollVelocity()
end

function PollVelocity()
    local velSensors = { peripheral.find("velocity_sensor") }

    -- Velocity Sensors
    VelocityVectors = {}

    if velSensors[1] ~= nil then -- verify array is not empty

        SensorData.Velocity = {}

        for _, velsensor in pairs(velSensors) do
            
            local vsAxis = velsensor.getAxis()
            local vsVelocity = velsensor.getVelocity()
            
            -- correct for heading correction value
            if Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
                if vsAxis == "x" then vsAxis = "z" end
                if vsAxis == "z" then vsAxis = "x" end
            elseif Config.SensorCorrection.Heading == 180 or Config.SensorCorrection.Heading == -180 then
                vsVelocity = -vsVelocity
            elseif Config.SensorCorrection.Heading == -90 or Config.SensorCorrection.Heading == 270 then
                if vsAxis == "x" then vsAxis = "z" end
                if vsAxis == "z" then vsAxis = "x" end
                vsVelocity = -vsVelocity
            end

            VelocityVectors[vsAxis] = vsVelocity
            
            SensorData.Velocity.Raw = VelocityVectors
            
        end

        if Config.Debug then
            print("DEBUG: PollVelocity fetched sensor data: "..tableToString(SensorData.Velocity))
        end
    else
        if Config.Debug then
            print("DEBUG: No velocity sensors found.") 
        end
    end
    
end

function PollAltitude()
    local altSensor = peripheral.find("altitude_sensor") -- enforce one alt sensor

    -- Altitude Sensor
    if altSensor then

        SensorData.Altitude = {}

        Altitude = altSensor.getHeight()
        AirPressure = altSensor.getAirPressure()
        VerticalSpeed = altSensor.getVerticalSpeed()

        SensorData.Altitude.Altitude = Altitude
        SensorData.Altitude.AirPressure = AirPressure
        SensorData.Altitude.VerticalSpeed = VerticalSpeed

        if Config.Debug then
            print("DEBUG: PollAltitude fetched sensor data: "..tableToString(SensorData.Altitude))
        end
    end
end

function PollNavTable()
    local navTable = peripheral.find("navigation_table") -- enforce one nav table
    
    -- Navigation Table
    if navTable then

        SensorData.NavTable = {}

        Heading = navTable.getHeading()

        if Config.SensorCorrection.Heading ~= 0 then
            local CorrectedHeading = Heading + Config.SensorCorrection.Heading
            print("DEBUG: Heading = "..Heading.." | Correction = "..Config.SensorCorrection.Heading )
            print("DEBUG: Corrected Heading = "..CorrectedHeading)
            if CorrectedHeading > 360 then
                CorrectedHeading = CorrectedHeading - 360
            elseif CorrectedHeading < 0 then
                CorrectedHeading = CorrectedHeading + 360
            end

            Heading = CorrectedHeading
        end

        SensorData.NavTable.Heading = Heading

        NavTableHasTarget = navTable.hasTarget()
        SensorData.NavTable.HasTarget = NavTableHasTarget
        if NavTableHasTarget then
            BearingToTarget = navTable.getBearing()
            TargetClosureRate = navTable.getClosureRate()
            TargetVerticalOffset = navTable.getVerticalOffsetToTarget()
            TargetRelativeAngle = navTable.getRelativeAngle()
        end
        if not NavTableHasTarget then
            BearingToTarget = 0
            TargetClosureRate = 0
            TargetVerticalOffset = 0
            TargetRelativeAngle = 0
        end

        SensorData.NavTable.TargetBearing = BearingToTarget
        SensorData.NavTable.TargetClosureRate = TargetClosureRate
        SensorData.NavTable.TargetVerticalOffset = TargetVerticalOffset
        SensorData.NavTable.TargetRelativeAngle = TargetRelativeAngle
    end
end

function PollGimbal()
    local gimbalSensor = peripheral.find("gimbal_sensor") -- enforce one gimbal sensor

    -- Gimbal Sensor
    if gimbalSensor then

        SensorData.Gimbal = {}

        Angles = {
            xAngle = 0, -- pitch angle
            zAngle = 0  -- roll angle
        }
        for i, v in ipairs(gimbalSensor.getAngles()) do
            if i == 1 then
                Angles.xAngle = v
            elseif i == 2 then
                Angles.zAngle = v
            end
        end

        -- correct for heading correction value
        if Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
            local temp = Angles.xAngle
            Angles.xAngle = Angles.zAngle
            Angles.zAngle = temp
        elseif Config.SensorCorrection.Heading == 180 or Config.SensorCorrection.Heading == -180 then
            Angles.xAngle = -Angles.xAngle
            Angles.zAngle = -Angles.zAngle
        elseif Config.SensorCorrection.Heading == -90 or Config.SensorCorrection.Heading == 270 then
            local temp = Angles.xAngle
            Angles.xAngle = -Angles.zAngle
            Angles.zAngle = -temp
        end

        AngularRates = {
            wx = 0, -- pitch rate
            wy = 0, -- yaw rate
            wz = 0  -- roll rate
        }
        for i, v in ipairs(gimbalSensor.getAngularRates()) do
            if i == 0 then
                AngularRates.wx = v
            elseif i == 1 then
                AngularRates.wy = v
            elseif i == 2 then
                AngularRates.wz = v
            end
        end

        -- correct for heading correction value
        if Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
            local temp = AngularRates.wx
            AngularRates.wx = AngularRates.wz
            AngularRates.wz = temp
        elseif Config.SensorCorrection.Heading == 180 or Config.SensorCorrection.Heading == -180 then
            AngularRates.wx = -AngularRates.wx
            AngularRates.wz = -AngularRates.wz
        elseif Config.SensorCorrection.Heading == -90 or Config.SensorCorrection.Heading == 270 then
            local temp = AngularRates.wx
            AngularRates.wx = -AngularRates.wz
            AngularRates.wz = -temp
        end

        LinearAcceleration = {
            ax = 0, -- port-starboard axis
            ay = 0, -- up-down axis
            az = 0  -- fore-aft axis
        }
        for i, v in ipairs(gimbalSensor.getLinearAcceleration()) do
            if i == 0 then
                LinearAcceleration.ax = v
            elseif i == 1 then
                LinearAcceleration.ay = v
            elseif i == 2 then
                LinearAcceleration.az = v
            end
        end

        -- correct for heading correction value
        if Config.SensorCorrection.Heading == 90 or Config.SensorCorrection.Heading == -270 then
            local temp = LinearAcceleration.ax
            LinearAcceleration.ax = LinearAcceleration.az
            LinearAcceleration.az = temp
        elseif Config.SensorCorrection.Heading == 180 or Config.SensorCorrection.Heading == -180 then
            LinearAcceleration.ax = -LinearAcceleration.ax
            LinearAcceleration.az = -LinearAcceleration.az
        elseif Config.SensorCorrection.Heading == -90 or Config.SensorCorrection.Heading == 270 then
            local temp = LinearAcceleration.ax
            LinearAcceleration.ax = -LinearAcceleration.az
            LinearAcceleration.az = -temp
        end

        SensorData.Gimbal.Angles = Angles
        SensorData.Gimbal.AngularRates = AngularRates
        SensorData.Gimbal.LinearAcceleration = LinearAcceleration

        if Config.Debug then
            print("DEBUG: PollGimbal fetched sensor data: "..tableToString(SensorData.Gimbal))
        end
    end
end


-- the sigmoid is probably the best function to use for determining how much thrust to apply based on the current velocity or angular rate
function Sigmoid(x)
    local e = 2.718281828459045
    return 1 / (1 + e^(-x))
end

-- this sigmoid is modified to return a value between -1 and 1, and inverted so that the output is negative when the input is positive, and vice versa
-- the idea is to take the velocity or angular rate, and return a value that can be used to determine how much thrust to apply in the opposite direction to counteract it
-- then, the output can be given to the update thrust function which will propogate the value to the thrusters
function CustomSigmoid(x)
    return ((-2 * Sigmoid(x)) + 1)
end

-- thruster is a thruster type, power is a vector from 0.0 to 1.0
function SetThrusterPower(thruster, power)
    if thruster == nil then
        print("ERROR: SetThrusterPower called with nil thruster.")
        return
    end

    if Config.Debug then print("DEBUG: thruster "..thruster.name..": power "..power) end

    if thruster.type == "rotator" then
        thruster.power = power
        local actualPower = 0
        if thruster.transmission == nil then
            print("ERROR: SetThrusterPower called with nil transmission for rotator thruster.")
            print("Thruster: "..thruster.name)
            return
        end
        if peripheral.getType(peripheral.wrap(thruster.transmission)) == "Create_RotationSpeedController" then
            if (math.ceil(power*256) - power*256 <= 0.5) then
                actualPower = math.ceil(power*256)
            end
            if (math.ceil(power*256) - power*256) > 0.5 then
                actualPower = math.floor(power*256)
            end

            thruster.transmission.setTargetSpeed(actualPower)
        end
        if peripheral.getType(peripheral.wrap(thruster.transmission)) == "analog_transmission" then
            if (math.ceil(power*15) - power*15) <= 0.5 then
                actualPower = math.ceil(power*15)
            end
            if (math.ceil(power*15) - power*15) > 0.5 then
                actualPower = math.floor(power*15)
            end

            if actualPower < 1 then
                actualPower = 15 -- signal 15 will decouple the speed and stop the motion
            elseif actualPower <= 15 then
                actualPower = actualPower - 1
            end

            peripheral.wrap(thruster.transmission).setSignal(actualPower)

            if Config.Debug then print("DEBUG: actualPower: "..actualPower) end
        end
        
    elseif thruster.type == "thruster" then
        thruster.power = power
        thruster.thruster.setPowerNormalized(power)
    end
end

function GetThrusterPower(thruster)
    if thruster.type == "rotator" then
        return thruster.power
    end
    if thruster.type == "thruster" then
        return thruster.thruster.getPower()
    end
end

-- PID systems for stabilization and autopilot
LastTime = os.epoch("utc")

function GetDeltaTime()
    local currentTime = os.epoch("utc")
    local dt = (currentTime - LastTime) / 1000 -- convert milliseconds to seconds
    LastTime = currentTime
    if dt <= 0 then dt = 0.001 end -- prevent division by zero
    return dt
end

function CreatePID(kp, ki, kd)
    return {
        kp = kp, 
        ki = ki, 
        kd = kd,
        integral = 0, lastError = 0,
        update = function(self, setpoint, pv, dt)
            local error = setpoint - pv
            self.integral = self.integral + (error * dt)
            local derivative = (error - self.lastError) / dt
            self.lastError = error
            return (self.kp * error) + (self.ki * self.integral) + (self.kd * derivative)
        end
    }
end

-- tune these for the best values for each vessel
PIDs = {
-- angular
    -- attitude PIDs
    RollAttitudePID = CreatePID(4.5, 0.01, 0.05),
    PitchAttitudePID = CreatePID(4.5, 0.01, 0.05),
    YawAttitudePID = CreatePID(0.1, 0.01, 0.05),

    -- rate PIDs
    RollRatePID = CreatePID(1.2, 0.01, 0.05),
    PitchRatePID = CreatePID(1.2, 0.01, 0.05),
    YawRatePID = CreatePID(0.5, 0.01, 0.05),

-- lateral
    -- value PIDs
    AltitudePID = CreatePID(0.6, 0.01, 0.05),
    PortStarPID = CreatePID(0.1, 0.01, 0.05),
    ForeAftPID  = CreatePID(0.1, 0.01, 0.05),

    -- rate PIDs
    VerticalRatePID = CreatePID(1.2, 0.01, 0.05),
    PortStarRatePID = CreatePID(0.1, 0.01, 0.05),
    ForeAftRatePID  = CreatePID(0.1, 0.01, 0.05)
}

function UpdateGlobalThrust() 
    -- for each thruster, there is a table of affectVectors that determine in what directions the thruster can apply thrust
    -- each thruster has a power value from 0 to 1, which is the amount of thrust the thruster is currently applying
    -- depending on the desired thrust vectors, some counteractive desired thrust vectors will be ignored

    local dt = GetDeltaTime()

    if SensorData.Gimbal == nil or SensorData.Altitude == nil or SensorData.Velocity == nil then
        print("ERROR: One or more of SensorData is nil. Cannot update thrust.")
        return
    end
    
    local AltitudeRateOutput = PIDs.VerticalRatePID:update(Config.Autopilot.AutopilotDesiredAltitude, SensorData.Altitude.Altitude, dt)
    local throttleOutput = PIDs.AltitudePID:update(AltitudeRateOutput, SensorData.Altitude.VerticalSpeed, dt)

    local pitchOutput        = PIDs.PitchAttitudePID:update(0, SensorData.Gimbal.Angles.xAngle, dt)
    local pitchRateOutput    = PIDs.PitchRatePID:update(pitchOutput, SensorData.Gimbal.AngularRates.wx or 0, dt)

    local rollOutput         = PIDs.RollAttitudePID:update(0, SensorData.Gimbal.Angles.zAngle, dt)
    local rollRateOutput     = PIDs.RollRatePID:update(rollOutput, SensorData.Gimbal.AngularRates.wz or 0, dt)

    local yawOutput = 5
    if Config.Autopilot.AutopilotDesiredHeading ~= nil then yawOutput = PIDs.YawAttitudePID:update(Config.Autopilot.AutopilotDesiredHeading, SensorData.NavTable.Heading, dt) end
    local yawRateOutput      = PIDs.YawRatePID:update(Config.Autopilot.AutopilotDesiredYawRate or 0, SensorData.Gimbal.AngularRates.wy or 0, dt)

    for _, thruster in pairs(Thrusters) do
        local desiredThrust = 0

        -- Calculate desired thrust based on affectVectors and PID outputs
        if thruster.affectVectors.angular.pitch ~= nil and thruster.primary_pitch_thruster then
            desiredThrust = desiredThrust + pitchRateOutput
        end
        if thruster.affectVectors.angular.roll ~= nil and thruster.primary_roll_thruster then
            desiredThrust = desiredThrust + rollRateOutput
        end
        if thruster.affectVectors.angular.yaw ~= nil and thruster.primary_yaw_thruster then
            desiredThrust = desiredThrust + yawRateOutput
        end

        if thruster.affectVectors.lateral.y == "up" then
            desiredThrust = desiredThrust + throttleOutput 
        end

        -- Normalize the desired thrust to be between 0 and 1
        local normalizedThrust = math.max(0, math.min(1, (desiredThrust + 1) / 2)) -- Convert from [-1, 1] to [0, 1]

        if Config.Debug then print("DEBUG: "..thruster.name..": desiredThrust "..desiredThrust..", normalizedThrust "..normalizedThrust) end

        SetThrusterPower(thruster, normalizedThrust)
    end
end

function Init()
    
    local configFile = io.open(Config.ConfigPath, "r")
    if configFile then
        local configContent = configFile:read("*all")
        Config = stringToTable(configContent)
        configFile:close()
    else
        print("Could not open the config file")
    end

    -- Initialize monitors - identify which monitor is for which type  
    if Config.Monitors.InstrumentPanelMonitor == nil then
        print("Identifying instrument panel monitor...")
        Config.Monitors.InstrumentPanelMonitor = identifyMonitor("instrument panel")
    else
        Config.Monitors.InstrumentPanelMonitor.peripheral = peripheral.wrap(Config.Monitors.InstrumentPanelMonitor.name)
    end
    
    if Config.Monitors.AutopilotControlMonitor == nil then
        print("Identifying autopilot control monitor...")
        Config.Monitors.AutopilotControlMonitor = identifyMonitor("autopilot control")
    else
        Config.Monitors.AutopilotControlMonitor.peripheral = peripheral.wrap(Config.Monitors.AutopilotControlMonitor.name)
    end


    ThrottleInit()
    
    Thrusters = {}

    if Config.thrusterConfigPath ~= nil then
        local thrusterConfigFile = io.open(Config.thrusterConfigPath, "r")
        if thrusterConfigFile then
            local configContent = thrusterConfigFile:read("*all")
            local configChunk, err = load(configContent)
            if configChunk then
                Thrusters = configChunk()
            else
                print("ERROR: Error loading thruster config string: "..err)
            end
            thrusterConfigFile:close()
        else
            print("Could not open the thruster config file")
        end
    end

    -- If we're here, the config loaded successfully.

    populateThrusterValues()
    Update()
end

-- Display functions for monitors
function displayInstrumentPanel(monitor, sensorData)
    if not monitor or not monitor.peripheral then return end
    
    local m = monitor.peripheral
    m.clear()
    m.setCursorPos(1, 1)
    
    -- Simple instrument panel display
    m.write("Caero Attitude Control")
    m.setCursorPos(1, 2)
    m.write("----------------------")

    local next = next
    if next(sensorData) ~= nil then
        if sensorData then
            m.setCursorPos(1, 3)
            m.write("Pitch:   " .. string.format("%.2f", SensorData.Gimbal.Angles.xAngle or 0))
            m.setCursorPos(1, 4)
            m.write("Roll:    " .. string.format("%.2f", SensorData.Gimbal.Angles.zAngle or 0))
            m.setCursorPos(1, 5)
            m.write("Yaw:     " .. string.format("%.2f", SensorData.NavTable.Heading or 0))
            m.setCursorPos(1, 6)
            m.write("Velocity: " .. string.format("%.2f", sensorData.velocity or 0))
            m.setCursorPos(1, 7)
            m.write("Altitude: " .. string.format("%.2f", SensorData.Altitude.Altitude or 0))
        else
            m.setCursorPos(1, 3)
            m.write("Pitch:   0.00")
            m.setCursorPos(1, 4)
            m.write("Roll:    0.00")
            m.setCursorPos(1, 5)
            m.write("Yaw:     0.00")
            m.setCursorPos(1, 6)
            m.write("Velocity: 0.00")
            m.setCursorPos(1, 7)
            m.write("Altitude: 0.00")
        end
        
        -- Display autopilot status
        local autopilotStatus = Config.Autopilot.AutopilotEngaged and "ON" or "OFF"
        m.setCursorPos(1, 9)
        m.write("Autopilot: " .. autopilotStatus)
    end
    
    
end

function displayAutopilotControls(monitor, sensorData)
    if not monitor or not monitor.peripheral then return end
    
    local m = monitor.peripheral
    m.clear()
    m.setCursorPos(1, 1)
    
    -- Simple autopilot control panel  
    m.write("Autopilot Controls")
    m.setCursorPos(1, 2)
    m.write("------------------")
    m.setCursorPos(1, 3)
    m.write("Engage: [ ]")
    m.setCursorPos(1, 4)
    m.write("Speed:  [0.0]")
    m.setCursorPos(1, 5)
    m.write("Heading: [0.0]")
    m.setCursorPos(1, 6)
    m.write("Manual: [ ]")
    m.setCursorPos(1, 7)
    m.setCursorPos(1, 8)
    m.write("Mode: Auto")
end

-- Sensor data collection - returns a table with all relevant sensor readings
function collectSensorData()
    local data = {}
    
    -- Collect gimbal data
    data.gimbal = {
        pitch = 0,
        roll = 0
    }
    
    -- Collect navigation table data
    data.navTable = {
        heading = 0,
        hasTarget = false
    }
    
    -- Collect altitude data
    data.altitude = 0
    
    -- Collect velocity data
    data.velocity = 0
    
    return data
end

function WriteConfigFiles()
    -- since the thruster object will change every time, set each thruster's thruster object to nil before writing to the config file, so that the config file only contains the thruster names and not the thruster objects
    for _, thruster in pairs(Thrusters) do
        thruster.thruster = nil
        thruster.transmission = nil
    end

    print("Writing to thruster config file.")
    local thrusterConfigFile = fs.open(Config.thrusterConfigPath, "w+")
    thrusterConfigFile.write(tableToString(Thrusters))
    thrusterConfigFile.close()

    print("Writing to config file")
    local configFile = fs.open(Config.ConfigPath, "w+")
    configFile.write(tableToString(Config))
    configFile.close()
end

function Main()
    Init()

    os.sleep(5) -- pause for any initialization errors to display

    local sentinel = true
    print("Mainloop starting. Press 'q' to stop the loop and save configuration changes.")
    while sentinel do

        PollSensors()
        UpdateGlobalThrust()

        -- Update monitor displays
        if Config.Monitors.InstrumentPanelMonitor then
            displayInstrumentPanel(Config.Monitors.InstrumentPanelMonitor, SensorData)
        end
        
        if Config.Monitors.AutopilotControlMonitor then
            displayAutopilotControls(Config.Monitors.AutopilotControlMonitor, SensorData)
        end

        local function getTerminateEvent()
            local event, key, is_held = os.pullEventRaw("key")
            if keys.getName(key) == "q" then 
                print("Quit Input Received") 

                sentinel = false
            else
                return
            end
        end

        local function do_sleep() os.sleep(0.1) end

        parallel.waitForAny(do_sleep, getTerminateEvent)
    end

    for _, t in pairs(Thrusters) do
        if t.type == "rotator" then
            SetThrusterPower(t, 0.0)
            peripheral.wrap(t.transmission).releaseSignal() -- release control of transmissions on quit
        else
            SetThrusterPower(t, 0.0)
        end
    end
    
    WriteConfigFiles()
end

-- Run the main function when the script starts
Main()