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

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;
	
}


// double angle_ema_filter(double filtered, double raw, double alpha, double deadband) {
//     filtered = wrap_deg(filtered);
//     raw = wrap_deg(raw);

//     double diff = shortest_angle_diff(filtered, raw); // 从 filtered 指向 raw 的差
//     if (fabs(diff) <= deadband) {
//         // 抖动范围内，不更新
//         return filtered;
//     }
//     // 角度版 EMA：沿最短方向移动 alpha * diff
//     double new_filtered = filtered + alpha * diff;
//     return wrap_deg(new_filtered);
// }
