/*
  限制器
*/

#include "limiter.h"
#include "user_math.h"

#include <math.h>
#include <stddef.h>

#define POWER_BUFF_THRESHOLD 20
#define CHASSIS_POWER_CHECK_FREQ 10
#define CHASSIS_POWER_FACTOR_PASS 0.9f
#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f

#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f

/**
 * @brief 限制底盘功率不超过power_limit
 *
 * @param power_limit 最大功率
 * @param motor_out 电机输出值
 * @param speed 电机转速
 * @param len 电机数量
 * @return int8_t 0对应没有错误
 */
int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
                                float *speed, uint32_t len) {
  /* power_limit小于0时不进行限制 */
  if (motor_out == NULL || speed == NULL || power_limit < 0) return -1;

  float sum_motor_out = 0.0f;
  for (uint32_t i = 0; i < len; i++) {
    /* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */
    sum_motor_out +=
        fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE;
  }

  /* 保持每个电机输出值缩小时比例不变 */
  if (sum_motor_out > power_limit) {
    for (uint32_t i = 0; i < len; i++) {
      motor_out[i] *= power_limit / sum_motor_out;
    }
  }

  return 0;
}

/**
 * @brief 电容输入功率计算
 *
 * @param power_in 底盘当前功率
 * @param power_limit 裁判系统功率限制值
 * @param power_buffer 缓冲能量
 * @return float 裁判系统输出最大值
 */
float PowerLimit_CapInput(float power_in, float power_limit,
                          float power_buffer) {
  float target_power = 0.0f;

  /* 计算下一个检测周期的剩余缓冲能量 */
  float heat_buff = power_buffer - (float)(power_in - power_limit) /
                                       (float)CHASSIS_POWER_CHECK_FREQ;
  if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */
    target_power = power_limit * CHASSIS_POWER_FACTOR_PASS;
  } else {
    target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS;
  }

  return target_power;
}

/**
 * @brief 使用缓冲能量计算底盘最大功率
 *
 * @param power_limit 裁判系统功率限制值
 * @param power_buffer 缓冲能量
 * @return float 底盘输出最大值
 */
float PowerLimit_TargetPower(float power_limit, float power_buffer) {
  float target_power = 0.0f;

  /* 根据剩余缓冲能量计算输出功率 */
  target_power = power_limit * (power_buffer - 10.0f) / 20.0f;
  if (target_power < 0.0f) target_power = 0.0f;

  return target_power;
}

/**
 * @brief 射击频率控制
 *
 * @param heat 当前热量
 * @param heat_limit 热量上限
 * @param cooling_rate 冷却速率
 * @param heat_increase 冷却增加
 * @param shoot_freq 经过热量限制后的射击频率
 * @return float 射击频率
 */
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
                          float heat_increase, bool is_big) {
  float heat_percent = heat / heat_limit;
  float stable_freq = cooling_rate / heat_increase;
  if (is_big)
    return stable_freq;
  else
    return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq;
}

/**
 * @brief 计算电机与IMU坐标系之间的偏差（处理跨越±π的情况）
 * @param motor_angle 电机角度
 * @param imu_angle IMU角度
 * @return 偏差值（已归一化到 -π ~ π）
 */
static float CalcMotorImuOffset(float motor_angle, float imu_angle) {
  float offset = motor_angle - imu_angle;
  if (offset > M_PI) offset -= M_2PI;
  if (offset < -M_PI) offset += M_2PI;
  return offset;
}

/**
 * @brief 圆周角度限位器 - 考虑电机与IMU坐标系偏差
 */
void CircleAngleLimit(float *setpoint, float motor_angle, float imu_angle,
                      float limit_max, float limit_min, float range) {
  if (setpoint == NULL) return;
  
  /* 计算电机与IMU坐标系偏差 */
  float motor_imu_offset = CalcMotorImuOffset(motor_angle, imu_angle);
  
  /* 将IMU setpoint转换为电机角度后进行限位检查 */
  float motor_target = *setpoint + motor_imu_offset;
  
  /* 检查是否超过最大限位 */
  if (CircleError(motor_target, limit_max, range) > 0) {
    *setpoint = limit_max - motor_imu_offset;
  }
  /* 检查是否低于最小限位 */
  if (CircleError(motor_target, limit_min, range) < 0) {
    *setpoint = limit_min - motor_imu_offset;
  }
}

/**
 * @brief 圆周角度增量限位器 - 考虑电机与IMU坐标系偏差
 */
void CircleAngleDeltaLimit(float *delta, float setpoint, float motor_angle,
                           float imu_angle, float limit_max, float limit_min,
                           float range) {
  if (delta == NULL) return;
  
  /* 计算电机与IMU坐标系偏差 */
  float motor_imu_offset = CalcMotorImuOffset(motor_angle, imu_angle);
  
  /* 计算应用增量后在电机坐标系下的目标角度 */
  float motor_target_after_delta = setpoint + motor_imu_offset + *delta;
  
  /* 计算到限位边界的距离 */
  float delta_max = CircleError(limit_max, motor_target_after_delta, range);
  float delta_min = CircleError(limit_min, motor_target_after_delta, range);
  
  /* 限制增量 */
  if (*delta > delta_max) *delta = delta_max;
  if (*delta < delta_min) *delta = delta_min;
}