#pragma once

#include <fix16.h>

typedef struct {
    fix16_t integral;
    fix16_t prev_measured;
} pid_state_t;

typedef struct {
    fix16_t P;
    fix16_t I;
    fix16_t D;
    fix16_t min_output;
    fix16_t max_output;
    fix16_t lowpass;
} pid_config_t;

fix16_t pid_step(pid_state_t *state, pid_config_t *conf,
                 fix16_t measured, fix16_t goal,
                 fix16_t timestep
                );