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


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

void tank0204_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);
	}else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 80);
	}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);
	}
}

void tank0204_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);
	}else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 70);
	} 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);
	}  
}

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

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

void   tank0204_change(unsigned char *buf) {
    unsigned char mode = buf[1];
    unsigned char val = buf[2];
	static int modecount_tank0203=0;
	static int tank0203_index =0;
	// static int s_val_1=0;
	// static int s_val_2=0;
	
    if(mode == 1 ) {
        tank0204_mode_lift_flont(val);
        tank0204_mode_right_flont(val);
		modecount_tank0203=0;
		//s_val_1 = val;
    }else if(mode == 2 ) {
        tank0204_mode_lift_back(val);
        tank0204_mode_right_back(val);
		modecount_tank0203=1;
		//s_val_2 = val;
    }
	if((mode == 1||mode == 2)&&val == 0) {
		modecount_tank0203 = 0;
		tank0203_index = 0;
	}
	if((mode == 1||mode == 2)&&val != 0) tank0203_index = 1;

	if(mode == 3) {
		if(modecount_tank0203 == 0){
			if(tank0203_index == 1) {
				tank0204_mode_lift_flont(0);
				//tank0204_mode_right_flont(s_val_1-20);
			}
			else {
				tank0204_mode_lift_back(val+30);
				tank0204_mode_right_flont(val+30);
			}
		}
		if(modecount_tank0203 == 1) {
			if(tank0203_index == 1) {
				tank0204_mode_lift_back(0);
				//tank0204_mode_lift_back(s_val_2-20);
			}else {
				tank0204_mode_lift_back(val+30);
				tank0204_mode_right_flont(val+30);
			}
			
		}

    }else if(mode == 4) {
		if(modecount_tank0203 == 1){
			if(tank0203_index == 1) {
				//tank0204_mode_right_back(s_val_1);
				tank0204_mode_right_back(0);
			}
			else {
				tank0204_mode_lift_flont(val+30);
				tank0204_mode_right_back(val+30);
			}

		}
		if(modecount_tank0203 == 0) {
			if(tank0203_index == 1) {
				//tank0204_mode_lift_flont(s_val_1-20);
				tank0204_mode_right_flont(0);
			}
			else {
				tank0204_mode_lift_flont(val+30);
				tank0204_mode_right_back(val+30);
			}
		}

    }
    
}

