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
        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 = 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
                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 = peripheral.wrap(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 = 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[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(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
            end
            if actualPower <= 15 then
                actualPower = actualPower - 1
            end

            peripheral.wrap(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

-- 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(0.1, 0.01, 0.05),
    PitchAttitudePID = CreatePID(0.1, 0.01, 0.05),
    YawAttitudePID = CreatePID(0.1, 0.01, 0.05),

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

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

    -- rate PIDs
    VerticalRatePID = CreatePID(0.1, 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()

    
    local throttleOutput = PIDs.AltitudePID:update(Config.Autopilot.AutopilotDesiredAltitude or 0, SensorData.Altitude.Altitude or 0, dt)
    
    local pitchOutput    = PIDs.PitchRatePID:update(Config.Autopilot.AutopilotDesiredPitchRate or 0, SensorData.Gimbal.AngularRates.wx or 0, dt)
    local rollOutput     = PIDs.RollRatePID:update(Config.Autopilot.AutopilotDesiredRollRate or 0, SensorData.Gimbal.AngularRates.wz or 0, dt)
    local yawOutput      = 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 + pitchOutput
        end
        if thruster.affectVectors.angular.roll ~= nil and thruster.primary_roll_thruster then
            desiredThrust = desiredThrust + rollOutput
        end
        if thruster.affectVectors.angular.yaw ~= nil and thruster.primary_yaw_thruster then
            desiredThrust = desiredThrust + yawOutput
        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]

        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 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()
        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()