#include "common.h"
#include "gpio_init.h"
#include "tank0202_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) {
		int b=0;
   if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	}
	else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 79+b);
	}else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 80+b);
	}else if(gval >70){
		int flont_speed = 80+(gval-70)/10+b;
		if(flont_speed>95) flont_speed = 95;
		pwmWrite(PWM_PIN_SPEED, flont_speed);
	}
//    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) {
	int b=0;
	if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 71-b);
	}else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 70-b);
	} else if(gval >70){
		int back_speed = 70 - (gval-70)/10-b;
		if(back_speed<55) back_speed =55;
		pwmWrite(PWM_PIN_SPEED, back_speed);
	}  
	// 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, 80);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_CHANGE, 80);
	} 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, 70);
	} 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, 69);
	} else if (gval <= 75) {
		pwmWrite(PWM_PIN_CHANGE, 68);
	} else if (gval <= 90) {
		pwmWrite(PWM_PIN_CHANGE, 67);
	} else if (gval <= 100) {
		pwmWrite(PWM_PIN_CHANGE, 66);
	} else if (gval <= 110) {
		pwmWrite(PWM_PIN_CHANGE, 65);
	} else if (gval <= 120) {
		pwmWrite(PWM_PIN_CHANGE, 64);
	} else if (gval <= 130) {
		pwmWrite(PWM_PIN_CHANGE, 63);
	} else if (gval <= 140) {
		pwmWrite(PWM_PIN_CHANGE, 62);
	} else if (gval <= 150) {
		pwmWrite(PWM_PIN_CHANGE, 61);
	} else if (gval <= 160) {
		pwmWrite(PWM_PIN_CHANGE, 60);
	} else if (gval <= 170) {
		pwmWrite(PWM_PIN_CHANGE, 59);
	} else if (gval <= 180) {
		pwmWrite(PWM_PIN_CHANGE, 58);
	} else if (gval <= 190) {
		pwmWrite(PWM_PIN_CHANGE, 57);
	} else if (gval <= 200) {
		pwmWrite(PWM_PIN_CHANGE, 56);
	}    
}


int tank0202_change_grading(int mode){
	int mode_val=0;
		switch(mode){
			case 2:
				mode_val=10;
				break;
			case 3:
				mode_val=15;
				break;
			case 4:
				mode_val=20;
				break;
			case 5:
				mode_val=25;
				break;
			case 6:
				mode_val=30;
				break;
			case 7:
				mode_val=35;
				break;
			case 8:
				mode_val=40;
				break;
			case 9:
				mode_val=45;
				break;
			case 10:
				mode_val=50;
				break;
			default:
				break;
		}
		return mode_val;
}

void  tank0202_change(unsigned char *buf) {
	unsigned char type = buf[0];
    unsigned char mode = buf[1];
    unsigned char val = buf[2];

	static int tank0202_steering_t=0;
	static int tank0202_front_t =0;

	static int tank0202_front_val=0;
	static int tank0202_steering_val=0;
	
	int tank0202_count=40;

	if(type != 1) tank0202_count=tank0202_change_grading(type);

	if((mode == 1 ||mode == 2)&&val == 0) tank0202_front_t =0;
	if((mode == 3 ||mode == 4)&&val == 0) tank0202_steering_t=0;

	if(mode == 1&&val != 0) {
		tank0202_front_val=val;
		tank0202_front_t =1;
	}else if(mode == 2&&val != 0) {
		tank0202_front_t =2;
		tank0202_front_val=val;
	}

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

	if(tank0202_front_t ==0&&tank0202_steering_t==0){
		mode_lift_flont(0);
        mode_right_flont(0);
	}else if(tank0202_front_t ==1&&tank0202_steering_t==0){
			mode_right_flont(tank0202_front_val);
			mode_lift_flont(0);
	}else if(tank0202_front_t ==2&&tank0202_steering_t==0){
			mode_right_back(tank0202_front_val);
			mode_lift_back(0);
	}else if(tank0202_front_t ==0&&tank0202_steering_t==1){
			mode_lift_back(tank0202_steering_val + tank0202_count);
			mode_right_back(0);
	}else if(tank0202_front_t ==0&&tank0202_steering_t==2){
			mode_lift_flont(tank0202_steering_val + tank0202_count);
			mode_right_back(0);
	}
	else if(tank0202_front_t ==1&&tank0202_steering_t==1){
		mode_lift_back(tank0202_steering_val+ tank0202_count-30);
        mode_right_flont(tank0202_steering_val + tank0202_count-30);
	}else if(tank0202_front_t ==1&&tank0202_steering_t==2){
		mode_lift_flont(tank0202_steering_val +tank0202_count -30);
        mode_right_flont(tank0202_steering_val + tank0202_count-30);
	}else if(tank0202_front_t ==2&&tank0202_steering_t==1){
		mode_lift_back(tank0202_steering_val + tank0202_count -30);
        mode_right_back(tank0202_steering_val + tank0202_count-30);
	}else if(tank0202_front_t ==2&&tank0202_steering_t==2){
		mode_lift_flont(tank0202_steering_val +tank0202_count-30);
        mode_right_back(tank0202_steering_val + tank0202_count-30);
	}
	
    // 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+20);
    //     if(g_modecount_tank0202 == 1) mode_lift_flont(val+20);
    // }else if(mode == 4) {
    //     if(g_modecount_tank0202 == 0) mode_lift_flont(val+20);
    //     if(g_modecount_tank0202 == 1) mode_lift_back(val+20);
    // }
    
}
