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

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

void mode_car0103_lift_flont(int gval) {
	int b=1;
   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);
	}
}

void mode_car0103_lift_back(int gval) {
	int b=1;
	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);
	}  
}

void mode_car0103_right_flont(int gval) {
  	int b=1;
   if (gval < 50) {
		pwmWrite(PWM_PIN_CHANGE, 75);
	}else if (gval <= 60) {
		pwmWrite(PWM_PIN_CHANGE, 79+b);
	}else if (gval <= 70) {
		pwmWrite(PWM_PIN_CHANGE, 80+b);
	}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 mode_car0103_right_back(int gval) {
	int b=1;
	if (gval < 50) {
		pwmWrite(PWM_PIN_CHANGE, 75);
	}else if (gval <= 60) {
		pwmWrite(PWM_PIN_CHANGE, 71-b);
	} else if (gval <= 70) {
		pwmWrite(PWM_PIN_CHANGE, 70-b);
	} 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  car0103_change(int *buf) {
    int mode = buf[1];
    int val = buf[2];
	static int modecount_car0103=0;
    if(mode == 1 ) {
        //mode_lift_flont(val);
		mode_car0103_right_flont(val);
		modecount_car0103=0;
    }else if(mode == 2 ) {
        //mode_lift_back(val);
        mode_car0103_right_back(val);
		modecount_car0103=1;
    }
	
	if((mode == 1||mode ==2)&&val == 0) modecount_car0103=0;

	if(mode == 3) {
        if(modecount_car0103 == 0) mode_car0103_lift_back(val);
        if(modecount_car0103 == 1) mode_car0103_lift_flont(val);
    }else if(mode == 4) {
        if(modecount_car0103 == 0) mode_car0103_lift_flont(val);
        if(modecount_car0103 == 1) mode_car0103_lift_back(val);
    }
}