#include "tank_angle.h"
#include "common.h"
#include "ads1115.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

double tank_angle(){
    double angle=0;
    float angle_shot=ads1115_read_channel(2);
    if(angle_shot>0){  
    angle_shot=angle_shot*360/5.05;
    angle = round(angle_shot * 100) / 100;
    }

    return angle;
}



int angle_limit(){
    if(ANGLE_LIMIT_INDEX == 1 ){
        int tank_limit_angele = tank_angle();
        if(tank_limit_angele >=LIFT_LIMIT && tank_limit_angele<=MIDDLE_LIMIT) return 1;
        else if(tank_limit_angele >=MIDDLE_LIMIT && tank_limit_angele<=RIGHT_LIMIT) return 2;
        return 0;
    }else return 0;
	
}


/*
  角度 EMA 滤波
  - filter : 上次滤波值（度）
  - raw      : 当前原始角度（度）
  - alpha    : 衰减因子（0..1），越大越跟随原始值（推荐 0.05~0.5）
  - deadband : 死区（度），小于该值认为是抖动
*/
// EN滤波结构体
// typedef struct {
//     double alpha;      // 滤波系数 (0~1)
//     double filtered;   // 上一次滤波角度
// } ENFilter;

// static ENFilter filter = {.alpha = 0.5, .filtered = 0.05};

// // 支持360°→0°跳变的滤波函数
// double en_filter_update(double new_angle)
// {
//     double diff = new_angle - filter.filtered;

//     // 处理角度环绕（支持359→0或0→359平滑过渡）
//     if (diff > 180.0)
//         diff -= 360.0;
//     else if (diff < -180.0)
//         diff += 360.0;

//     // 一阶低通滤波
//     filter.filtered += filter.alpha * diff;

//     // 结果保持在[0, 360)
//     if (filter.filtered < 0)
//         filter.filtered += 360.0;
//     else if (filter.filtered >= 360.0)
//         filter.filtered -= 360.0;

//     return filter.filtered;
// }

// //上次的角度滤波后的值
// double return_en_filter_last_angle(){
//     return filter.filtered;
// }

