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

int g_modecount_tank0203=0;

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

void tank0203_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 tank0203_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 tank0203_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 tank0203_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   tank0203_change(unsigned char *buf) {
    unsigned char mode = buf[1];
    unsigned char val = buf[2];
	
    if(mode == 1 ) {
        //tank0203_mode_lift_flont(val);
        tank0203_mode_right_flont(val);
		g_modecount_tank0203=0;
    }else if(mode == 2 ) {
        //tank0203_mode_lift_back(val);
        tank0203_mode_right_back(val);
		g_modecount_tank0203=1;
    }
	
	if((mode == 1||mode ==2)&&val == 0) g_modecount_tank0203=0;

	if(mode == 3) {
        if(g_modecount_tank0203 == 0) tank0203_mode_lift_back(val+15);
        if(g_modecount_tank0203 == 1) tank0203_mode_lift_flont(val+15);
    }else if(mode == 4) {
        if(g_modecount_tank0203 == 0) tank0203_mode_lift_flont(val+15);
        if(g_modecount_tank0203 == 1) tank0203_mode_lift_back(val+15);
    }
    
}

