#ifndef BALANCE_H
#define BALANCE_H

#include <stdint.h>
#include "mpu6000.h"
#include "slope_estimator.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      = 1,  /* Active balancing */
    BALANCE_TILT_FAULT = 2,  /* Tilt exceeded limit, motors killed */
    BALANCE_PARKED     = 3,  /* PID frozen, motors off — quick re-arm via button (Issue #682) */
} 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 */

    /* Slope compensation (Issue #600) */
    slope_estimator_t slope;
} 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);
void balance_park(balance_t *b);    /* ARMED -> PARKED: freeze PID, zero motors (Issue #682) */
void balance_unpark(balance_t *b);  /* PARKED -> ARMED if pitch < 20 deg (Issue #682) */

#endif
