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

//将角度转化为对应的舵机pwm值
void car0102_calculate_L_R(int angle) {
    if (angle < 0) {
        angle = 0;
    } else if (angle > 180) {
        angle = 180;
    }
	float pulse_width=(2 / 180.0) * angle + 0.5;
	// 周期（ms）
    float period = 1000.0 / 50;
	int val = (int)((pulse_width / period) * 1000);
	pwmWrite(PWM_PIN_CHANGE,val);
}

void car0102_speed_stop() {
    pwmWrite(PWM_PIN_SPEED,0);
	car0102_calculate_L_R(90);
}

void car0102_mode_1_flont(unsigned char gval) {
    device_gpio_control(g_device_type,26,1);
	if(gval == 0) {
		device_gpio_control(g_device_type,26,0);
		pwmWrite(PWM_PIN_SPEED, 0);
	}else if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 1000);
	} else if (gval <= 55) {
		pwmWrite(PWM_PIN_SPEED, 1000-550);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 1000-550);
	} else if (gval <= 65) {
		pwmWrite(PWM_PIN_SPEED, 1000-600);
	} else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 1000-600);
	} else if (gval <= 75) {
		pwmWrite(PWM_PIN_SPEED, 1000-650);
	} else if (gval <= 90) {
		pwmWrite(PWM_PIN_SPEED, 1000-650);
	} else if (gval <= 100) {
		pwmWrite(PWM_PIN_SPEED, 1000-700);
	} else if (gval <= 110) {
		pwmWrite(PWM_PIN_SPEED, 1000-700);
	} else if (gval <= 120) {
		pwmWrite(PWM_PIN_SPEED, 1000-750);
	} else if (gval <= 130) {
		pwmWrite(PWM_PIN_SPEED, 1000-750);
	} else if (gval <= 140) {
		pwmWrite(PWM_PIN_SPEED, 1000-800);
	} else if (gval <= 150) {
		pwmWrite(PWM_PIN_SPEED, 1000-800);
	} else if (gval <= 160) {
		pwmWrite(PWM_PIN_SPEED, 1000-850);
	} else if (gval <= 170) {
		pwmWrite(PWM_PIN_SPEED, 1000-900);
	} else if (gval <= 180) {
		pwmWrite(PWM_PIN_SPEED, 1000-950);
	} else if (gval <= 190) {
		pwmWrite(PWM_PIN_SPEED, 1000-950);
	} else if (gval <= 200) {
		pwmWrite(PWM_PIN_SPEED, 1000-1000);
	}
}

void car0102_mode_2_back(unsigned char gval) {
	int k = 5;
	int b = 100;
    device_gpio_control(g_device_type,26,0);
    if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 0);
	} else if (gval <= 55) {
		pwmWrite(PWM_PIN_SPEED,b + 40 * k);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED,b + 40 * k);
	} else if (gval <= 65) {
		pwmWrite(PWM_PIN_SPEED, b + 40* k);
	} else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, b + 40 * k);
	} else if (gval <= 75) {
		pwmWrite(PWM_PIN_SPEED, b + 40 * k);
	} else if (gval <= 90) {
		pwmWrite(PWM_PIN_SPEED, b + 40 * k);
	} else if (gval <= 100) {
		pwmWrite(PWM_PIN_SPEED, b + 40 * k);
	} else if (gval <= 110) {
		pwmWrite(PWM_PIN_SPEED, b + 40 * k);
	} else if (gval <= 120) {
		pwmWrite(PWM_PIN_SPEED, b + 45 * k);
	} else if (gval <= 130) {
		pwmWrite(PWM_PIN_SPEED, b + 50 * k);
	} else if (gval <= 140) {
		pwmWrite(PWM_PIN_SPEED, b + 55 * k);
	} else if (gval <= 150) {
		pwmWrite(PWM_PIN_SPEED, b + 60 * k);
	} else if (gval <= 160) {
		pwmWrite(PWM_PIN_SPEED, b + 60 * k);
	} else if (gval <= 170) {
		pwmWrite(PWM_PIN_SPEED, b + 65 * k);
	} else if (gval <= 180) {
		pwmWrite(PWM_PIN_SPEED, b + 70 * k);
	} else if (gval <= 190) {
		pwmWrite(PWM_PIN_SPEED, b + 75 * k);
	} else if (gval <= 200) {
		pwmWrite(PWM_PIN_SPEED, b + 100 * k);
	}
}

void car0102_mode_3_left(unsigned char gval) {
	int b=7;
	if(gval<45){
		car0102_calculate_L_R(90);
	}else if(gval<70){
		car0102_calculate_L_R(50+gval+b);
	}else if(gval>=70){
		car0102_calculate_L_R(135);
	}
	// if (gval < 45) {
	// 	car0102_calculate_L_R(90);
	// } else if (gval <= 51) {
	// 	car0102_calculate_L_R(110);
	// } else if (gval <= 57) {
	// 	car0102_calculate_L_R(120);
	// } else if (gval <= 63) {
	// 	car0102_calculate_L_R(130);
	// } else if (gval <= 69) {
	// 	car0102_calculate_L_R(135);
	// } else if (gval <= 75) {
	// 	car0102_calculate_L_R(140);
	// } else if (gval <= 81) {
	// 	car0102_calculate_L_R(145);
	// } else if (gval <= 87) {
	// 	car0102_calculate_L_R(150);
	// } else if (gval <= 93) {
	// 	car0102_calculate_L_R(150);
	// } else if (gval <= 100) {
	// 	car0102_calculate_L_R(160);
	// }else if (gval <= 107) {
	// 	car0102_calculate_L_R(170);
	// }else if (gval <= 120) {
	// 	car0102_calculate_L_R(180);
	// }	
}

void car0102_mode_4_right(unsigned char gval) {
	int b=5;
	if(gval<45){
		car0102_calculate_L_R(90);
	}else if(gval<70){
		car0102_calculate_L_R(130-gval-b);
	}else if(gval>=70){
		car0102_calculate_L_R(135);
	}
	
	// if (gval < 45) {
	// 	car0102_calculate_L_R(90);
	// } else if (gval <= 51) {
	// 	car0102_calculate_L_R(70);
	// } else if (gval <= 57) {
	// 	car0102_calculate_L_R(66);
	// } else if (gval <= 63) {
	// 	car0102_calculate_L_R(62);
	// } else if (gval <= 69) {
	// 	car0102_calculate_L_R(55);
	// } else if (gval <= 75) {
	// 	car0102_calculate_L_R(45 );
	// } else if (gval <= 81) {
	// 	car0102_calculate_L_R(40);
	// } else if (gval <= 87) {
	// 	car0102_calculate_L_R(30);
	// } else if (gval <= 93) {
	// 	car0102_calculate_L_R(30);
	// } else if (gval <= 100) {
	// 	car0102_calculate_L_R(20);
	// } else if (gval <= 107) {
	// 	car0102_calculate_L_R(10);
	// } else if (gval <= 120) {
	// 	car0102_calculate_L_R(0);
	// }         
}

//车速度和转向引脚数值处理函数
void car0102_speed_change(unsigned char *buf) {
    unsigned char mode=buf[1];
    unsigned char val=buf[2];
    switch(mode){
        case 1:		
			car0102_mode_1_flont(val);					 
			break;
		case 2:	
			car0102_mode_2_back(val);
			break;
		case 3:
			car0102_mode_3_left(val);
			break;
		case 4:
			car0102_mode_4_right(val);
			break; 
        default:
			break;
    } 
}