#ifndef FLIGHT_APP_H
#define FLIGHT_APP_H

#include <stdint.h>

typedef struct {
    int16_t raw_air_speed;
    int16_t raw_altitude;
    int16_t raw_pitch;
    int16_t raw_temperature;
} SensorRawData;

typedef struct {
    float air_speed;
    float altitude;
    float pitch;
    float temperature;
} SensorData;

typedef struct {
    float target_speed;
    float target_altitude;
    float pitch_limit;
    uint8_t auto_mode;
} CommandState;

typedef struct {
    float throttle;
    float elevator;
    uint8_t armed;
} ActuatorState;

typedef struct {
    uint16_t sensor_fault;
    uint16_t actuator_fault;
    uint16_t command_fault;
    uint16_t cycle_count;
} DiagnosticState;

typedef struct {
    SensorData sensor;
    CommandState command;
    ActuatorState actuator;
    DiagnosticState diagnostic;
} AppState;

void System_Init(AppState *state);
void System_LoadDefaults(CommandState *command);
void System_RunCycle(AppState *state);
void System_Shutdown(AppState *state);

void Sensor_Init(SensorRawData *raw);
void Sensor_ReadAdc(SensorRawData *raw);
uint8_t Sensor_Validate(const SensorRawData *raw);
void Sensor_Scale(const SensorRawData *raw, SensorData *data);
void Sensor_Collect(SensorRawData *raw, SensorData *data);

void Filter_Init(SensorData *data);
float Filter_Median3(float a, float b, float c);
float Filter_LowPass(float previous, float current, float alpha);
float Filter_Limit(float value, float min_value, float max_value);
void Filter_Process(SensorData *data);

void Control_Init(CommandState *command);
float Control_ComputeSpeedError(const CommandState *command, const SensorData *data);
float Control_ComputeAltitudeError(const CommandState *command, const SensorData *data);
float Control_PidStep(float error, float gain, float limit);
void Control_Update(const CommandState *command, const SensorData *data, ActuatorState *actuator);

void Actuator_Init(ActuatorState *actuator);
void Actuator_Arm(ActuatorState *actuator);
void Actuator_WriteThrottle(ActuatorState *actuator, float throttle);
void Actuator_WriteElevator(ActuatorState *actuator, float elevator);
void Actuator_UpdateOutputs(ActuatorState *actuator);
void Actuator_Disarm(ActuatorState *actuator);

void Comm_Init(CommandState *command);
uint16_t Comm_PackStatus(const AppState *state);
uint8_t Comm_ReceiveCommand(CommandState *command, uint16_t word);
void Comm_ApplyCommand(CommandState *command, uint16_t word);
void Comm_SendStatus(uint16_t status_word);

void Diag_Init(DiagnosticState *diagnostic);
uint8_t Diag_CheckSensor(const SensorData *data);
uint8_t Diag_CheckActuator(const ActuatorState *actuator);
void Diag_UpdateFaults(DiagnosticState *diagnostic, const SensorData *data, const ActuatorState *actuator);
uint16_t Diag_ReportStatus(const DiagnosticState *diagnostic);

void Log_Init(void);
void Log_RecordSample(const SensorData *data);
void Log_RecordFault(uint16_t status_word);
void Log_Flush(void);

#endif