#include "common.h"
#include "gpio_init.h"
#include "pthrpoll.h"
#include "tank02002_control.h"

int g_modecount_tank0202=0;

void tank0202_middle() {
    pwmWrite(PWM_PIN_SPEED,75);
    pwmWrite(PWM_PIN_CHANGE,75);
}

void mode_lift_flont(unsigned char gval) {
   if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	} else if (gval <= 55) {
		pwmWrite(PWM_PIN_SPEED, 78);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 79);
	} else if (gval <= 65) {
		pwmWrite(PWM_PIN_SPEED, 80);
	} else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 81);
	} else if (gval <= 75) {
		pwmWrite(PWM_PIN_SPEED, 82);
	} else if (gval <= 90) {
		pwmWrite(PWM_PIN_SPEED, 83);
	} else if (gval <= 100) {
		pwmWrite(PWM_PIN_SPEED, 84);
	} else if (gval <= 110) {
		pwmWrite(PWM_PIN_SPEED, 85);
	} else if (gval <= 120) {
		pwmWrite(PWM_PIN_SPEED, 86);
	} else if (gval <= 130) {
		pwmWrite(PWM_PIN_SPEED, 87);
	} else if (gval <= 140) {
		pwmWrite(PWM_PIN_SPEED, 88);
	} else if (gval <= 150) {
		pwmWrite(PWM_PIN_SPEED, 89);
	} else if (gval <= 160) {
		pwmWrite(PWM_PIN_SPEED, 90);
	} else if (gval <= 170) {
		pwmWrite(PWM_PIN_SPEED, 91);
	} else if (gval <= 180) {
		pwmWrite(PWM_PIN_SPEED, 92);
	} else if (gval <= 190) {
		pwmWrite(PWM_PIN_SPEED, 93);
	} else if (gval <= 200) {
		pwmWrite(PWM_PIN_SPEED, 94);
	} 
}

void mode_lift_back(unsigned char gval) {
	if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	} else if (gval <= 55) {
		pwmWrite(PWM_PIN_SPEED, 72);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 71);
	} else if (gval <= 65) {
		pwmWrite(PWM_PIN_SPEED, 70);
	} else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 69);
	} else if (gval <= 75) {
		pwmWrite(PWM_PIN_SPEED, 68);
	} else if (gval <= 90) {
		pwmWrite(PWM_PIN_SPEED, 67);
	} else if (gval <= 100) {
		pwmWrite(PWM_PIN_SPEED, 66);
	} else if (gval <= 110) {
		pwmWrite(PWM_PIN_SPEED, 65);
	} else if (gval <= 120) {
		pwmWrite(PWM_PIN_SPEED, 64);
	} else if (gval <= 130) {
		pwmWrite(PWM_PIN_SPEED, 63);
	} else if (gval <= 140) {
		pwmWrite(PWM_PIN_SPEED, 62);
	} else if (gval <= 150) {
		pwmWrite(PWM_PIN_SPEED, 61);
	} else if (gval <= 160) {
		pwmWrite(PWM_PIN_SPEED, 60);
	} else if (gval <= 170) {
		pwmWrite(PWM_PIN_SPEED, 59);
	} else if (gval <= 180) {
		pwmWrite(PWM_PIN_SPEED, 58);
	} else if (gval <= 190) {
		pwmWrite(PWM_PIN_SPEED, 57);
	} else if (gval <= 200) {
		pwmWrite(PWM_PIN_SPEED, 56);
	}    
}

void mode_right_flont(unsigned char gval) {
   if (gval < 50) {
		pwmWrite(PWM_PIN_CHANGE, 75);
	} else if (gval <= 55) {
		pwmWrite(PWM_PIN_CHANGE, 78);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_CHANGE, 79);
	} else if (gval <= 65) {
		pwmWrite(PWM_PIN_CHANGE, 80);
	} else if (gval <= 70) {
		pwmWrite(PWM_PIN_CHANGE, 81);
	} else if (gval <= 75) {
		pwmWrite(PWM_PIN_CHANGE, 82);
	} else if (gval <= 90) {
		pwmWrite(PWM_PIN_CHANGE, 83);
	} else if (gval <= 100) {
		pwmWrite(PWM_PIN_CHANGE, 84);
	} else if (gval <= 110) {
		pwmWrite(PWM_PIN_CHANGE, 85);
	} else if (gval <= 120) {
		pwmWrite(PWM_PIN_CHANGE, 86);
	} else if (gval <= 130) {
		pwmWrite(PWM_PIN_CHANGE, 87);
	} else if (gval <= 140) {
		pwmWrite(PWM_PIN_CHANGE, 88);
	} else if (gval <= 150) {
		pwmWrite(PWM_PIN_CHANGE, 89);
	} else if (gval <= 160) {
		pwmWrite(PWM_PIN_CHANGE, 90);
	} else if (gval <= 170) {
		pwmWrite(PWM_PIN_CHANGE, 91);
	} else if (gval <= 180) {
		pwmWrite(PWM_PIN_CHANGE, 92);
	} else if (gval <= 190) {
		pwmWrite(PWM_PIN_CHANGE, 93);
	} else if (gval <= 200) {
		pwmWrite(PWM_PIN_CHANGE, 94);
	} 
}

void mode_right_back(unsigned char gval) {
	if (gval < 50) {
		pwmWrite(PWM_PIN_CHANGE, 75);
	} else if (gval <= 55) {
		pwmWrite(PWM_PIN_CHANGE, 71);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_CHANGE, 70);
	} else if (gval <= 65) {
		pwmWrite(PWM_PIN_CHANGE, 70);
	} else if (gval <= 70) {
		pwmWrite(PWM_PIN_CHANGE, 70);
	} else if (gval <= 75) {
		pwmWrite(PWM_PIN_CHANGE, 69);
	} else if (gval <= 90) {
		pwmWrite(PWM_PIN_CHANGE, 68);
	} else if (gval <= 100) {
		pwmWrite(PWM_PIN_CHANGE, 68);
	} else if (gval <= 110) {
		pwmWrite(PWM_PIN_CHANGE, 66);
	} else if (gval <= 120) {
		pwmWrite(PWM_PIN_CHANGE, 65);
	} else if (gval <= 130) {
		pwmWrite(PWM_PIN_CHANGE, 64);
	} else if (gval <= 140) {
		pwmWrite(PWM_PIN_CHANGE, 62);
	} else if (gval <= 150) {
		pwmWrite(PWM_PIN_CHANGE, 60);
	} else if (gval <= 160) {
		pwmWrite(PWM_PIN_CHANGE, 59);
	} else if (gval <= 170) {
		pwmWrite(PWM_PIN_CHANGE, 58);
	} else if (gval <= 180) {
		pwmWrite(PWM_PIN_CHANGE, 57);
	} else if (gval <= 190) {
		pwmWrite(PWM_PIN_CHANGE, 56);
	} else if (gval <= 200) {
		pwmWrite(PWM_PIN_CHANGE, 55);
	}    
}

void   tank0202_change(unsigned char *buf) {
    unsigned char mode = buf[1];
    unsigned char val = buf[2];
	
    if(mode == 1 ) {
        //mode_lift_flont(val);
        mode_right_flont(val);
		g_modecount_tank0202=0;
    }else if(mode == 2 ) {
        //mode_lift_back(val);
        mode_right_back(val);
		g_modecount_tank0202=1;
    }
	
	if((mode == 1||mode ==2)&&val == 0) g_modecount_tank0202=0;

	if(mode == 3) {
        if(g_modecount_tank0202 == 0) mode_lift_back(val+25);
        if(g_modecount_tank0202 == 1) mode_lift_flont(val+25);
    }else if(mode == 4) {
        if(g_modecount_tank0202 == 0) mode_lift_flont(val+25);
        if(g_modecount_tank0202 == 1) mode_lift_back(val+25);
    }
    
}

long long shot_device_time_start =0 ;
long long shot_device_time_end = 0;

// 函数功能：获取当前精确到毫秒的时间戳
// 返回值：当前时间的毫秒数
long long get_current_time_millis() {
    struct timeval tv;
    // 获取当前时间，存储在tv结构体中，第二个参数一般传NULL
    if (gettimeofday(&tv, NULL)!= 0) {
        perror("gettimeofday error");
        return -1;
    }
    // 秒数部分转换为毫秒（乘以1000），微秒数部分转换为毫秒（除以1000），然后相加
    return (long long)tv.tv_sec * 1000 + tv.tv_usec / 1000;
}

void tank_shot_back_stop_task_function(void *arg) {//多线程处理坦克发射后退线程池
	while(1){
		long long interval=shot_device_time_start-shot_device_time_end;
		if(g_device_delay_count>10&&g_device_delay_count<=25) mode_right_back(0);
		if(interval>100){
			if(g_device_delay_count <= 10) {
				mode_right_back(80);
				my_zlog_debug("操作耗时: %lld 毫秒", interval);
			}
		}

	}
	free(arg);
}

ThreadPool_t *pool_tank;
void tank_shot_back_stop_task_end(){
	thread_pool_destroy(pool_tank);
}

void tank_shot_pthrpoll_task_init(){
	int *arg = malloc(sizeof(int));
	*arg = 1;
	pool_tank=thread_pool_init(1,1);
	thread_pool_add_task(pool_tank, tank_shot_back_stop_task_function, &arg);
	my_zlog_debug("线程池打开");
}

int tank_shot_back_stop(int pin,int val){
	static int shot_count=0;
	shot_device_time_start=get_current_time_millis();
	if(pin !=27){
		my_zlog_debug("非27引脚");
		return 1;
	}	
	
	if(val == 0) {
		mode_right_back(0);
		shot_count=2;
	}
	if(val != 0) {
		if(shot_count == 0){
			tank_shot_pthrpoll_task_init();
			shot_count=1;
		}
		shot_count=1;
		if(shot_count == 1){
			g_device_delay_count=0;
		}
	}

	shot_device_time_end=get_current_time_millis();
}

/*销毁坦克使用的线程池，让其正常销毁，只有在tank0202设备号下才有用*/
void tank0202_thread_close(){
	thread_pool_destroy(pool_tank);
}