#include "common.h"
#include "modules_common.h"
#include "tank0206_control.h"
#include "gpio_common.h"

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

void tank0206_mode_lift_flont(int gval) {
	int b=0;
   if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	}
	else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 80);
	}else if (gval <= 90) {
		pwmWrite(PWM_PIN_SPEED, 79);
	}else if (gval <= 100) {
		pwmWrite(PWM_PIN_SPEED, 79);
	}else if(gval >100){
		int flont_speed = 80+(gval-70)/10+b;
		if(flont_speed>95) flont_speed = 95;
		pwmWrite(PWM_PIN_SPEED, flont_speed);
	}
}

void tank0206_mode_lift_back(int gval) {
	int b=0;
	if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 70);
	}else if (gval <= 90) {
		pwmWrite(PWM_PIN_SPEED, 71);
	}else if (gval <= 100) {
		pwmWrite(PWM_PIN_SPEED, 71);
	} else if(gval >100){
		int back_speed = 71 - (gval-70)/10-b;
		if(back_speed<55) back_speed =55;
		pwmWrite(PWM_PIN_SPEED, back_speed);
	}  
}

void tank0206_mode_right_flont(int gval) {

	int b=0;
   if (gval < 50) {
		pwmWrite(PWM_PIN_CHANGE, 75);
	}
	else if (gval <= 60) {
		pwmWrite(PWM_PIN_CHANGE, 80);
	}else if (gval <= 90) {
		pwmWrite(PWM_PIN_CHANGE, 79);
	}else if (gval <= 100) {
		pwmWrite(PWM_PIN_CHANGE, 79);
	}else if(gval >100){
		int flont_speed = 80+(gval-70)/10+b;
		if(flont_speed>95) flont_speed = 95;
		pwmWrite(PWM_PIN_CHANGE, flont_speed);
	}
}

void tank0206_mode_right_back(int gval) {
	int b=0;
	if (gval < 50) {
		pwmWrite(PWM_PIN_CHANGE, 75);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_CHANGE, 70);
	}else if (gval <= 90) {
		pwmWrite(PWM_PIN_CHANGE, 71);
	}else if (gval <= 100) {
		pwmWrite(PWM_PIN_CHANGE, 71);
	} else if(gval >100){
		int back_speed = 71 - (gval-70)/10-b;
		if(back_speed<55) back_speed =55;
		pwmWrite(PWM_PIN_CHANGE, back_speed);
	}  
}

int tank0206_change_grading(int mode){
	int mode_val=0;
		switch(mode){
			case 0:
				mode_val=0;
				break;
			case 2:
				mode_val=10;
				break;
			case 3:
				mode_val=20;
				break;
			case 4:
				mode_val=40;
				break;
			case 5:
				mode_val=50;
				break;
			case 6:
				mode_val=60;
				break;
			case 7:
				mode_val=70;
				break;
			default:
				mode_val=30;
				break;
		}
		return mode_val;
}

void  tank0206_change(int *buf) {
	int type = buf[0];
    int mode = buf[1];
    int val = buf[2];

	static int tank0206_steering_t=0;
	static int tank0206_front_t =0;

	static int tank0206_front_val=0;
	static int tank0206_steering_val=0;

	int tank0206_count=30;

	if(type != 1) tank0206_count=tank0206_change_grading(type);

	if((mode == 1 ||mode == 2)&&val == 0) tank0206_front_t =0;
	if((mode == 3 ||mode == 4)&&val == 0) tank0206_steering_t=0;

	if(mode == 1&&val != 0) {
		tank0206_front_val=val;
		tank0206_front_t =1;
	}else if(mode == 2&&val != 0) {
		tank0206_front_t =2;
		tank0206_front_val=val;
	}

	if(mode == 3&&val != 0) {
		tank0206_steering_t =1;
		tank0206_steering_val = val;
	}else if(mode == 4&&val != 0) {
		tank0206_steering_t =2;
		tank0206_steering_val = val;
	}

	if(tank0206_front_t ==0&&tank0206_steering_t==0){
	    tank0206_mode_lift_flont(0);
		tank0206_mode_right_flont(0);
	}else if(tank0206_front_t ==1&&tank0206_steering_t==0){
		tank0206_mode_lift_flont(tank0206_front_val);
		tank0206_mode_right_flont(tank0206_front_val);
	}else if(tank0206_front_t ==2&&tank0206_steering_t==0){
		tank0206_mode_lift_back(tank0206_front_val);
        tank0206_mode_right_back(tank0206_front_val);
	}else if(tank0206_front_t ==0&&tank0206_steering_t==1){
		tank0206_mode_right_back(tank0206_steering_val+tank0206_count);
		tank0206_mode_lift_flont(tank0206_steering_val+tank0206_count);
	}else if(tank0206_front_t ==0&&tank0206_steering_t==2){
		tank0206_mode_lift_back(tank0206_steering_val+tank0206_count);
		tank0206_mode_right_flont(tank0206_steering_val+tank0206_count);
	}
	else if(tank0206_front_t ==1&&tank0206_steering_t==1){
		tank0206_mode_lift_flont(tank0206_steering_val+tank0206_count);
        tank0206_mode_right_flont(0);
	}else if(tank0206_front_t ==1&&tank0206_steering_t==2){
		tank0206_mode_lift_flont(0);
        tank0206_mode_right_flont(tank0206_steering_val+tank0206_count);
	}else if(tank0206_front_t ==2&&tank0206_steering_t==1){
		tank0206_mode_lift_back(tank0206_steering_val+tank0206_count);
        tank0206_mode_right_back(0);
	}else if(tank0206_front_t ==2&&tank0206_steering_t==2){
		tank0206_mode_lift_back(0);
        tank0206_mode_right_back(tank0206_steering_val+tank0206_count);
	}
    
}
    


