#ifndef __BALANCER_H
#define __BALANCER_H

#include "pid.h"
#include "mpu6050.h"
#include "motor.h"
#include "encoder.h"

/* Control loop frequencies */
#define LOOP_ANGLE_HZ       200
#define LOOP_ANGLE_PERIOD_MS 5
#define LOOP_SPEED_HZ       50
#define LOOP_SPEED_PERIOD_MS 20

/* Safety limits */
#define ANGLE_LIMIT_DEG     40.0f
#define MOTOR_RAMP_STEP     30      /* per loop iteration */

/* Complementary filter coefficient */
#define COMP_ALPHA          0.98f

/* Mahony filter gains */
#define MAHONY_KP           0.3f
#define MAHONY_KI           0.05f

typedef struct {
    /* Sensors */
    MPU6050_Data sensor;
    Attitude att;

    /* Gyro bias (calibrated at startup) */
    float gyro_bias_x;
    float gyro_bias_y;
    float gyro_bias_z;

    /* Motors */
    Motor motor_l;
    Motor motor_r;

    /* Encoders */
    Encoder enc_l;
    Encoder enc_r;

    /* PIDs */
    PID pid_angle;
    PID pid_speed;

    /* Control state */
    float angle_ref;            /* angle target from speed PID */
    float speed_ref;            /* speed target (0 = stationary) */
    float motor_output;         /* -1000 ~ +1000 */
    float motor_output_l;
    float motor_output_r;

    /* Timing */
    uint32_t tick_angle;
    uint32_t tick_speed;

    /* Flags */
    uint8_t calibrated;
    uint8_t running;
    uint8_t fault;
} Balancer;

void Balancer_Init(Balancer *bal);
void Balancer_Calibrate(Balancer *bal);
void Balancer_Run(Balancer *bal, float dt_angle, float dt_speed);

/* Emergency stop */
void Balancer_EmergencyStop(Balancer *bal);

#endif /* __BALANCER_H */