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


float low_pass_filter_360(float prev_angle, float new_angle, float alpha) {
    // 规整到 [0, 360)
    prev_angle = fmod(prev_angle, 360.0f);
    if (prev_angle < 0) prev_angle += 360.0f;
    new_angle = fmod(new_angle, 360.0f);
    if (new_angle < 0) new_angle += 360.0f;
    
    // 计算最短路径角度差
    float diff = new_angle - prev_angle;
    if (diff > 180.0f) diff -= 360.0f;
    else if (diff < -180.0f) diff += 360.0f;
    
    // 应用滤波
    float filtered = prev_angle + alpha * diff;
    return fmod(filtered + 360.0f, 360.0f); // 确保输出在 [0, 360)
}

//上报角度计时，限制到这里
static int tank_angle_count=0;
static pthread_mutex_t tank_angle_mutex = PTHREAD_MUTEX_INITIALIZER;

void set_tank_angle_count(){
    pthread_mutex_lock(&tank_angle_mutex);
    tank_angle_count++;
    pthread_mutex_unlock(&tank_angle_mutex);
}

//角度计时
void set_tank_angle_count_clear(){
    pthread_mutex_lock(&tank_angle_mutex);
    tank_angle_count=0;
    pthread_mutex_unlock(&tank_angle_mutex);
}

int get_tank_angle_count(){
    return tank_angle_count;
}