#ifndef CHASSIS_POWER_H
#define CHASSIS_POWER_H

#include <stdint.h>

#include "rls.h"

#define CHASSIS_POWER_RLS_ORDER 4u

typedef struct
{
    float k_current_speed;
    float k_speed2;
    float k_current2;
    float k0;
} ChassisPowerMotorModel_s;

typedef struct
{
    ChassisPowerMotorModel_s wheel_model;
    float default_power_limit;
    float min_power_limit;
    float buffer_low;
    float buffer_high;
    float buffer_gain;
    float steering_power_reserve;
    float min_scale;
    float rls_lambda;
    float rls_init_covariance;
    uint8_t rls_enable;
} ChassisPower_Init_Config_s;

typedef struct
{
    float rf_speed_ref;
    float lb_speed_ref;
    float rf_speed;
    float lb_speed;
    float rf_current_cmd;
    float lb_current_cmd;
    float rf_output_cmd;
    float lb_output_cmd;
    float rf_feedback_current;
    float lb_feedback_current;
    float referee_limit;
    float referee_power;
    float power_buffer;
    float measured_power;
    float steering_power_estimate;
    uint8_t power_meter_online;
} ChassisPower_Limit_Input_s;

typedef struct
{
    float rf_ref_raw;
    float lb_ref_raw;
    float rf_ref_limited;
    float lb_ref_limited;
    float rf_power;
    float lb_power;
    float estimated_wheel_power;
    float estimated_total_power;
    float allowed_power;
    float wheel_budget;
    float scale;
    float measured_power;
    float referee_power;
    float power_buffer;
    float rf_speed;
    float lb_speed;
    float rf_current_cmd;
    float lb_current_cmd;
    float rf_output_cmd;
    float lb_output_cmd;
    float rf_feedback_current;
    float lb_feedback_current;
    float features[CHASSIS_POWER_RLS_ORDER];
    float rls_theta[CHASSIS_POWER_RLS_ORDER];
    uint8_t limited;
    uint8_t power_meter_online;
} ChassisPower_State_s;

typedef struct
{
    ChassisPowerMotorModel_s wheel_model;
    RLSInstance rls;
    ChassisPower_State_s state;
    float default_power_limit;
    float min_power_limit;
    float buffer_low;
    float buffer_high;
    float buffer_gain;
    float steering_power_reserve;
    float min_scale;
    uint8_t rls_enable;
} ChassisPowerInstance;

extern ChassisPower_State_s chassis_power_debug_state;

ChassisPowerMotorModel_s ChassisPowerGetDefault3508Model(void);
void ChassisPowerInit(ChassisPowerInstance *instance, const ChassisPower_Init_Config_s *config);
float ChassisPowerEstimateMotor(const ChassisPowerMotorModel_s *model, float speed, float current_cmd);
void ChassisPowerUpdateMeasureState(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input);
float ChassisPowerLimitWheelRefs(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input, float *rf_ref, float *lb_ref);
void ChassisPowerUpdateRLS(ChassisPowerInstance *instance, const ChassisPower_Limit_Input_s *input, float measured_wheel_power);
void ChassisPowerSetWheelModel(ChassisPowerInstance *instance, const ChassisPowerMotorModel_s *model);
const ChassisPower_State_s *ChassisPowerGetState(const ChassisPowerInstance *instance);

#endif