#include "flight_app.h"

void Control_Init(CommandState *command) {
    command->target_speed = 200.0f;
    command->target_altitude = 2500.0f;
    command->pitch_limit = 10.0f;
    command->auto_mode = 0U;
}

float Control_ComputeSpeedError(const CommandState *command, const SensorData *data) {
    return command->target_speed - data->air_speed;
}

float Control_ComputeAltitudeError(const CommandState *command, const SensorData *data) {
    return command->target_altitude - data->altitude;
}

float Control_PidStep(float error, float gain, float limit) {
    float output = error * gain;
    if (output > limit) {
        return limit;
    }
    if (output < -limit) {
        return -limit;
    }
    return output;
}

void Control_Update(const CommandState *command, const SensorData *data, ActuatorState *actuator) {
    float speed_error = Control_ComputeSpeedError(command, data);
    float altitude_error = Control_ComputeAltitudeError(command, data);
    actuator->throttle = Control_PidStep(speed_error, 0.01f, 1.0f);
    actuator->elevator = Control_PidStep(altitude_error, 0.002f, command->pitch_limit);
}