#include "tank_common.h"
#include "devcontrol_common.h"
#include "gpio_init.h"
#include "pthrpoll.h"
#include "tank_angle.h"
#include "gpio_control.h"

int tank_shot_back_stop(unsigned char pin,unsigned char val);

const tank_common_back *g_tank_common_config_t = NULL;

int g_tank_shot_index=0;//状态机，用于状态机,坦克接收射击倒退状态机全局变量
int g_tank_shot_index_cool=1;//状态机，用于冷却状态机,坦克接收射击倒退状态机全局变量

/*击打后退速度设置，混空模式下为单路，单独模式下为双路控制，后续需要优化*/
void tank_shot_back(unsigned char gval) {

	int b=0;
   if (gval < 50) {
		pwmWrite(PWM_PIN_CHANGE, 75);
		pwmWrite(PWM_PIN_SPEED, 75);
	}else if (gval <= 60) {
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0202) pwmWrite(PWM_PIN_CHANGE, 71);
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0203)	pwmWrite(PWM_PIN_CHANGE, 79);
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0203) pwmWrite(PWM_PIN_SPEED, 71);
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0204)	pwmWrite(PWM_PIN_CHANGE, 79);
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0204) pwmWrite(PWM_PIN_SPEED, 71);
	}else if (gval <= 70) {
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0202) pwmWrite(PWM_PIN_CHANGE, 70);
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0203)pwmWrite(PWM_PIN_CHANGE, 80);
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0203) pwmWrite(PWM_PIN_SPEED, 70);
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0204)pwmWrite(PWM_PIN_CHANGE, 80);
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0204) pwmWrite(PWM_PIN_SPEED, 70);
	}else if(gval >70){
		int change_1 = 80+(gval-70)/10+b;
		int speed_2= 70 - (gval-70)/10-b;
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0202) pwmWrite(PWM_PIN_CHANGE, speed_2);
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0203) pwmWrite(PWM_PIN_SPEED, speed_2);
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0203) pwmWrite(PWM_PIN_CHANGE, change_1);
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0204) pwmWrite(PWM_PIN_SPEED, speed_2);
		if(g_tank_common_config_t->device_id ==DEVICE_TANK0204) pwmWrite(PWM_PIN_CHANGE, change_1);
	}
}
 
 
const tank_common_back tank_common_config_t[]={
    {
        .device_id = DEVICE_TANK0202,
        .back_time=30,//后退多少ms
        .back_interval_back=1000, //间隔多少次才能再次使用，一次等于5ms
        .shot_back_speed=90,
        .shot_back=tank_shot_back_stop
        
    },
    {
        .device_id = DEVICE_TANK0203,
        .back_time=30,//后退多少ms
        .back_interval_back=1000, //间隔多少次才能再次使用，一次等于5ms
        .shot_back_speed=90,
        .shot_back=tank_shot_back_stop
    },
	{
        .device_id = DEVICE_TANK0204,
        .back_time=30,//后退多少ms
        .back_interval_back=1000, //间隔多少次才能再次使用，一次等于5ms
        .shot_back_speed=90,
        .shot_back=tank_shot_back_stop
    },


    { .device_id = -1 }
};

//多线程处理坦克发射后退线程池
void tank_shot_back_stop_task_function(void *arg) {
	
	if(arg!= NULL){
		free(arg);
	}

	// 设置 GPIO 23 为输入模式，启用内部上拉电阻
    // pinMode(23, INPUT);
    // pullUpDnControl(23, PUD_UP);
    // my_zlog_debug("开始监测 GPIO 23 状态...\n");

	while(1){

		//下面一部分为检测射击后后退
		// if(digitalRead(27)==LOW){
		// 	if(g_device_delay_back_count  <= g_tank_common_config_t->back_time) {
		// 		tank_shot_back(g_tank_common_config_t->shot_back_speed);
		// 		my_zlog_debug("操作耗时: %lld 毫秒", interval);
		// 	}
		// }else tank_shot_back(0);

		if(g_device_delay_back_count  <= g_tank_common_config_t->back_time) {
			tank_shot_back(g_tank_common_config_t->shot_back_speed);
			my_zlog_debug("击打成功");	
		}
		else if(g_device_delay_back_count  > g_tank_common_config_t->back_time&&g_device_delay_back_count  < (g_tank_common_config_t->back_time+30)){
			tank_shot_back(0);
		}
		else if(g_device_delay_back_count>g_tank_common_config_t->back_interval_back){
			g_tank_shot_index = 1;
		}else{
			delay_ms(1);
		}


	}
	
}

ThreadPool_t *pool_tank_t;//坦克后退线程函数

/*坦克后退线程池打开*/
void tank_shot_pthrpoll_task_init(){
	int *arg = malloc(sizeof(int));
	*arg = 1;
	pool_tank_t=thread_pool_init(1,1);
	thread_pool_add_task(pool_tank_t, tank_shot_back_stop_task_function, arg);
	my_zlog_debug("线程池打开");
}

/*坦克后退射击*/
int tank_shot_back_stop(unsigned char pin,unsigned char val){

	if(pin !=27){
		my_zlog_debug("非27引脚");
		return 1;
	}	
	
	if(val == 0) {
		tank_shot_back(0);
	}
	if(val != 0) {
		if(g_tank_shot_index == 0){
			tank_shot_pthrpoll_task_init();
			g_tank_shot_index=1;
		}
		if(g_tank_shot_index == 1 && g_tank_shot_index_cool==0){
			g_device_delay_back_count =0;
			g_tank_shot_index = 2;
			g_tank_shot_index_cool=1;
		}
	}
	
}

/*销毁坦克使用的线程池，让其正常销毁，只有在tank相关设备号下才有用，最后销毁都会到device——common.h中*/
void tank_thread_close(){
	thread_pool_destroy(pool_tank_t);
	thread_pool_destroy(g_pool_device_gpio_control_t);
}

/*坦克射击接口，只有在特定设备号下使用*/
void tank_shot_stop_control(int device_id,unsigned char pin,unsigned char val) {

    if (!g_tank_common_config_t || g_tank_common_config_t->device_id != device_id) {
        // 查找设备配置
        for(int i = 0; tank_common_config_t[i].device_id != -1; i++) {
            if(tank_common_config_t[i].device_id == device_id) {
                g_tank_common_config_t = &tank_common_config_t[i];
                break;
            }
        }
    }
    
    if(!g_tank_common_config_t) {
        my_zlog_error("Error: Device tank stop ID %d not found!", device_id);
        return;
    }
    g_tank_common_config_t->shot_back(pin,val);
             
}

