#pragma once

#include <stdbool.h>
#include <fix16.h>

typedef struct {
    fix16_t P; // output units / input units
    fix16_t I; // output units / (s * input units)
    fix16_t D; // output units / (input units / s)
    fix16_t lowpass; // e.g. 0.95 for ~20 sample IIR lowpass
    fix16_t modulus; // 0 for no effect, e.g. 360° to take modulus of error.
    fix16_t min_output;
    fix16_t max_output;
    fix16_t current_integral;
    fix16_t previous_filtered;
} pid_controller_t;

extern pid_controller_t pitch_ctrl;
// extern pid_controller_t roll_ctrl;
extern pid_controller_t heading_ctrl;

volatile extern bool manual_ctrl;
extern int32_t CONTROL_ZERO_PITCH;

void run_control();