#ifndef BALANCE_H
#define BALANCE_H

#include <stdint.h>
#include "mpu6000.h"

/*
 * SaltyLab Balance Controller
 *
 * Consumes fused IMUData (pitch + pitch_rate from mpu6000 complementary filter)
 * PID controller → motor speed command
 * Safety: tilt cutoff, arming, watchdog
 */

typedef enum {
    BALANCE_DISARMED = 0,   /* Motors off, waiting for arm command */
    BALANCE_ARMED,          /* Active balancing */
    BALANCE_TILT_FAULT,     /* Tilt exceeded limit, motors killed */
} balance_state_t;

typedef struct {
    /* State */
    balance_state_t state;
    float pitch_deg;        /* Current pitch angle (degrees) */
    float pitch_rate;       /* Gyro pitch rate (deg/s) */

    /* PID internals */
    float integral;
    float prev_error;
    int16_t motor_cmd;      /* Output to ESC: -1000..+1000 */

    /* Tuning */
    float kp, ki, kd;
    float setpoint;         /* Target pitch angle (degrees) — tune for COG offset */

    /* Safety */
    float max_tilt;         /* Cutoff angle (degrees) */
    int16_t max_speed;      /* Speed limit */
} balance_t;

void balance_init(balance_t *b);
void balance_update(balance_t *b, const IMUData *imu, float dt);
void balance_arm(balance_t *b);
void balance_disarm(balance_t *b);

#endif