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,
    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
        table.insert(transmissions, peripheral.find(v))
    end

    for _, v in ipairs(PropellerTypes) do
        table.insert(propellers, peripheral.find(v))
    end

    for pi, pv in ipairs(propellers) do 
        for ti, tv in ipairs(transmissions) do
            if pv.getSubnetworkAnchorId() == tv.getSelfId() then
                if Thrusters[peripheral.getName(pv)] == nil then
                    Thrusters[peripheral.getName(pv)] = {
                        type = "rotator",
                        thruster = pv,
                        transmission = 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
                if Thrusters[peripheral.getName(pv)] ~= nil then
                    if Thrusters[peripheral.getName(pv)].type == nil then 
                        Thrusters[peripheral.getName(pv)].type = "rotator" 
                    end
                    Thrusters[peripheral.getName(pv)].thruster = pv
                    Thrusters[peripheral.getName(pv)].transmission = tv
                    if Thrusters[peripheral.getName(pv)].affectVectors == nil then 
                        Thrusters[peripheral.getName(pv)].affectVectors = {} 
                    end
                    Thrusters[peripheral.getName(pv)].power = (tv.getSignal())/15
                end
            end
        end
    end
end

function ThrusterInit()
    local thrusters = {}

    for _, v in ipairs(ThrusterTypes) do
        table.insert(thrusters, peripheral.find(v))
    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()
            }
        end
        if Thrusters[peripheral.getName(tv)] ~= nil then
            if Thrusters[peripheral.getName(tv)].type == nil then 
                Thrusters[peripheral.getName(tv)].type = "thruster" 
            end
            Thrusters[peripheral.getName(tv)].thruster = tv
            Thrusters[peripheral.getName(tv)].transmission = nil
            if Thrusters[peripheral.getName(tv)].affectVectors == nil then 
                Thrusters[peripheral.getName(tv)].affectVectors = {} 
            end
            Thrusters[peripheral.getName(tv)].power = tv.getPower()
        end
    end
end

function Update()
    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.getSubnetworkAnchorId() == t.getSelfId() then
                            thisThruster.transmission = 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[0] == nil then -- verify array is not empty

        SensorData.Velocity = {}

        for _, velsensor in pairs(velSensors) do
            
            local vsAxis = velsensor.getAxis()
            local vsVelocity = velsensor.getVelocity()
            VelocityVectors[vsAxis] = vsVelocity
            
            SensorData.Velocity.Raw = VelocityVectors

            if Config.Debug then
                print("DEBUG: PollVelocity fetched sensor data: "..tableToString(SensorData.Velocity))
            end
            
        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 == 0 then
                Angles.xAngle = v
            elseif i == 1 then
                Angles.zAngle = v
            end
        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
        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

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

        if Config.Debug then
            print("DEBUG: PollGimbaal 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.type == "rotator" then
        thruster.power = power
        local actualPower = 0
        if peripheral.getType(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(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
            end
            if actualPower <= 15 then
                actualPower = actualPower - 1
            end

            thruster.transmission.setSignal(actualPower)
        end
        
    end
    if 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

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

    -- lateral thrust
    UpdateGlobalLateralThrust()

    -- angular thrust
    UpdateGlobalAngularThrust()
end

function UpdateGlobalLateralThrust() 
    -- lateral thrust
    -- verify that SensorData.Velocity.Raw is not nil
    if SensorData.Velocity == nil or SensorData.Velocity.Raw == nil then
        if Config.Debug then
            print("DEBUG: SensorData.Velocity.Raw or SensorData.Velocity is nil, skipping UpdateGlobalLateralThrust")
        end
        return
    end

    -- TODO: Refactor this

    local desiredLateralThrustVectors = {}

    for f, v in pairs(SensorData.Velocity.Raw) do
        desiredLateralThrustVectors[f] = CustomSigmoid(v)
    end

    --[[
        desiredLateralThrustVectors is a table that is similar to SensorData.Velocity.Raw, but with values between -1 and 1.
        essentially, it reverses the velocity vector and then clamps the range between -1 and 1, so that the value can be used for thrust power
        
    ]]

    local thrustDirections = {}
    for f, v in pairs(desiredLateralThrustVectors) do
        if f == "x" then
            if v > 0 then table.insert(thrustDirections, { port = v }) else table.insert(thrustDirections, { star = math.abs(v) }) end
        elseif f == "y" then
            if v > 0 then table.insert(thrustDirections, { down = v }) else table.insert(thrustDirections, { up = math.abs(v) }) end
        elseif f == "z" then
            if v > 0 then table.insert(thrustDirections, { fore = v }) else table.insert(thrustDirections, { aft = math.abs(v) }) end
        end
    end

    --[[
        thrustDirections is an array of tables, each table has a key that is the direction of the desired thrust, and a value that is the magnitude of the desired thrust
        like so:
        thrustDirections = {
            { port = 0.5 },
            { down = 0.1 },
            { aft = 0.6 }
        }
        there will only ever be three tables in thrustDirections, as there are only three axes of movement, and each axis can only have one desired thrust direction at a time
        (angular thrust is the same, but with different keys for the tables, like pitchup/pitchdown, yawport/yawstar, rollport/rollstar)
        
    ]]


end

function UpdateGlobalAngularThrust()
    -- angular thrust
    -- verify that SensorData.Gimbal.AngularRates is not nil
    if SensorData.Gimbal == nil or SensorData.Gimbal.AngularRates == nil then
        if Config.Debug then
            print("DEBUG: SensorData.Gimbal.AngularRates or SensorData.Gimbal is nil, skipping UpdateGlobalAngularThrust")
        end
        return
    end

    -- TODO: Implement this
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 and the initialization is done.
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[0] or 0))
            m.setCursorPos(1, 4)
            m.write("Roll:    " .. string.format("%.2f", SensorData.Gimbal.Angles[1] 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()
    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()

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

        PollSensors()
        
        UpdateStabilization()
        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 unindexedThrusters = checkIfThrusterIsIndexed()
        if unindexedThrusters ~= nil then
            partiallyUpdateThrusters(unindexedThrusters)
        end

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

                sentinel = false
            else
                return
            end
        end

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

        parallel.waitForAny(do_sleep, getTerminateEvent)
    end
end

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