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

#define CTRL_NONE     0
#define CTRL_PHONE    1
#define CTRL_STEERING 2

static pthread_mutex_t s_car0107_hw_mutex = PTHREAD_MUTEX_INITIALIZER;
static int s_control_source = CTRL_NONE;

static int s_target_angle = 90;
static int s_current_angle = 90;
static int s_steering_mode = 0;
static int s_steering_timeout = 0;
static int s_steering_cmd_timeout = 0;

static int s_target_speed = 0;
static int s_current_speed = 0;
static int s_speed_mode = 0;
static int s_speed_timeout = 0;

#define STEER_STEP_TURN    60
#define STEER_STEP_RETURN  90
#define REV_TARGET_SCALE_NUM 6
#define REV_TARGET_SCALE_DEN 10
#define ACCEL_STEP 5
#define DECEL_STEP 6
#define STEER_GVAL_MAX 150

static void car0107_calculate_L_R(int angle) {
    if (angle < 0) {
        angle = 0;
    } else if (angle > 180) {
        angle = 180;
    }
	float pulse_width=( angle / 180.0) *2 + 0.5;
    float period = 1000.0 / 50;
	int val = (int)((pulse_width / period) * 1000);
	pwmWrite(PWM_PIN_CHANGE,val);
}

static void car0107_reset_steering_state(void) {
    s_control_source = CTRL_NONE;
    s_target_speed = 0;
    s_current_speed = 0;
    s_speed_mode = 0;
    s_target_angle = 90;
    s_current_angle = 90;
    s_steering_mode = 0;
    s_steering_cmd_timeout = 0;
    s_speed_timeout = 0;
    s_steering_timeout = 0;
}

static int car0107_speed_to_gval(int speed) {
    if (speed <= 0) {
        return 0;
    }
    int gval = 50 + (speed * 100) / 160;
    if (gval > STEER_GVAL_MAX) {
        gval = STEER_GVAL_MAX;
    }
    return gval;
}

static void car0107_apply_stop(void) {
    pwmWrite(PWM_PIN_SPEED, 75);
    car0107_calculate_L_R(90);
}

/*车中间值，停止舵机中间函数*/
void car0107_middle_pwm() {
    pthread_mutex_lock(&s_car0107_hw_mutex);
    car0107_reset_steering_state();
    car0107_apply_stop();
    pthread_mutex_unlock(&s_car0107_hw_mutex);
}

static void car0107_mode_1_flont(int gval) {
   	int a=0,b=1;
	if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	} else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 71-b+a);
	}else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 70-b+a);
	} else if(gval >70){
		int back_speed = 70 - (gval-70)/10-b+a;
		pwmWrite(PWM_PIN_SPEED, back_speed);
	}
}

static void car0107_mode_2_back(int gval) {
	int b=0,a=1;
   if (gval < 50) {
		pwmWrite(PWM_PIN_SPEED, 75);
	}
	else if (gval <= 60) {
		pwmWrite(PWM_PIN_SPEED, 79+b-a);
	}else if (gval <= 70) {
		pwmWrite(PWM_PIN_SPEED, 80+b-a);
	}else if(gval >70){
		int flont_speed = 80+(gval-70)/10+b-a;
		pwmWrite(PWM_PIN_SPEED, flont_speed);
	}
}

static void car0107_mode_3_left(int gval) {
	int b=15;
	if(gval<45){
		car0107_calculate_L_R(90);
	}else if(gval<70){
		car0107_calculate_L_R(50+gval+b);
	}else if(gval>=70){
		car0107_calculate_L_R(135);
	}
}

static void car0107_mode_4_right(int gval) {
	int b=10;
	if(gval<45){
		car0107_calculate_L_R(90);
	}else if(gval<70){
		car0107_calculate_L_R(130-gval-b);
	}else if(gval>=70){
		car0107_calculate_L_R(35);
	}
}

void car0107_control_change(int *buf) {
    int mode=buf[1];
    int val=buf[2];

    pthread_mutex_lock(&s_car0107_hw_mutex);
    s_control_source = CTRL_PHONE;
    s_current_speed = 0;
    s_speed_timeout = 0;
    s_steering_timeout = 0;

    switch(mode){
        case 1:
			car0107_mode_2_back(val);
			break;
		case 2:
			car0107_mode_1_flont(val);
			break;
		case 3:
			car0107_mode_3_left(val);
			break;
		case 4:
			car0107_mode_4_right(val);
			break;
        default:
			break;
    }
    pthread_mutex_unlock(&s_car0107_hw_mutex);
}

static void car0107_speed_smooth_process(void) {
    if (s_control_source != CTRL_STEERING) {
        return;
    }

    s_speed_timeout--;
    if (s_speed_timeout <= 0) {
        s_current_speed = 0;
        car0107_apply_stop();
        if (s_steering_timeout <= 0) {
            s_control_source = CTRL_NONE;
        }
        return;
    }

    int limited_target = s_target_speed;
    if (s_speed_mode == 2) {
        limited_target = (limited_target * REV_TARGET_SCALE_NUM) / REV_TARGET_SCALE_DEN;
    }

    if (s_speed_mode == 0) {
        if (s_current_speed > 0) {
            s_current_speed -= DECEL_STEP;
            if (s_current_speed < 0) {
                s_current_speed = 0;
            }
        }
    } else if (s_speed_mode == 1 || s_speed_mode == 2) {
        if (s_current_speed < limited_target) {
            s_current_speed += ACCEL_STEP;
        } else if (s_current_speed > limited_target) {
            s_current_speed -= DECEL_STEP;
        }
    }

    if (s_current_speed <= 0 || s_speed_mode == 0) {
        pwmWrite(PWM_PIN_SPEED, 75);
    } else if (s_speed_mode == 1) {
        car0107_mode_2_back(car0107_speed_to_gval(s_current_speed));
    } else if (s_speed_mode == 2) {
        car0107_mode_1_flont(car0107_speed_to_gval(s_current_speed));
    }
}

static void car0107_steering_angle_smooth_process(void) {
    if (s_control_source != CTRL_STEERING) {
        return;
    }

    s_steering_timeout--;
    if (s_steering_timeout <= 0) {
        if (s_speed_timeout <= 0) {
            s_control_source = CTRL_NONE;
        }
        s_steering_mode = 0;
        s_target_angle = 90;
        return;
    }

    if (s_steering_cmd_timeout > 0) {
        s_steering_cmd_timeout--;
    }
    if (s_steering_cmd_timeout <= 0) {
        s_steering_mode = 0;
        s_target_angle = 90;
    }

    int wanted_angle = 90;
    int input_angle = s_target_angle;
    int smooth_step = STEER_STEP_TURN;

    if (input_angle < 0) input_angle = 0;
    if (input_angle > 180) input_angle = 180;

    if (s_steering_mode == 3) {
        wanted_angle = 90 + (input_angle * 37) / 180;
    } else if (s_steering_mode == 4) {
        wanted_angle = 90 - (input_angle * 37) / 180;
    } else {
        wanted_angle = 90;
        smooth_step = STEER_STEP_RETURN;
    }

    if (wanted_angle > 130) wanted_angle = 130;
    if (wanted_angle < 50) wanted_angle = 50;

    if (s_current_angle < wanted_angle) {
        s_current_angle += smooth_step;
        if (s_current_angle > wanted_angle) s_current_angle = wanted_angle;
    } else if (s_current_angle > wanted_angle) {
        s_current_angle -= smooth_step;
        if (s_current_angle < wanted_angle) s_current_angle = wanted_angle;
    }

    car0107_calculate_L_R(s_current_angle);
}

void car0107_steering_smooth_all(void) {
    pthread_mutex_lock(&s_car0107_hw_mutex);
    car0107_speed_smooth_process();
    car0107_steering_angle_smooth_process();
    pthread_mutex_unlock(&s_car0107_hw_mutex);
}

void car0107_control_steering_change_process(int *buf) {
    int mode = buf[1];
    int val = buf[2];

    pthread_mutex_lock(&s_car0107_hw_mutex);
    s_control_source = CTRL_STEERING;
    s_steering_timeout = 200;
    s_speed_timeout = 200;

    if (mode == 1) {
        s_speed_mode = 1;
        if (val < 50) {
            s_target_speed = 0;
            s_current_speed = 0;
            s_speed_mode = 0;
            car0107_apply_stop();
        } else if (val <= 200) {
            s_target_speed = val - 40;
        } else {
            s_target_speed = 160;
        }
    } else if (mode == 2) {
        s_speed_mode = 2;
        if (val < 50) {
            s_target_speed = 0;
            s_current_speed = 0;
            s_speed_mode = 0;
            car0107_apply_stop();
        } else if (val <= 200) {
            s_target_speed = (val - 40) * 3 / 4;
        } else {
            s_target_speed = 80;
        }
    } else if (mode == 3) {
        if (val == 0) {
            s_steering_mode = 0;
            s_target_angle = 90;
            s_current_angle = 90;
            s_steering_cmd_timeout = 0;
            car0107_calculate_L_R(90);
        } else {
            s_steering_cmd_timeout = 60;
            s_target_angle = val;
            s_steering_mode = 3;
            if (s_target_angle < 0) s_target_angle = 0;
            if (s_target_angle > 180) s_target_angle = 180;
        }
    } else if (mode == 4) {
        if (val == 0) {
            s_steering_mode = 0;
            s_target_angle = 90;
            s_current_angle = 90;
            s_steering_cmd_timeout = 0;
            car0107_calculate_L_R(90);
        } else {
            s_steering_cmd_timeout = 60;
            s_target_angle = val;
            s_steering_mode = 4;
            if (s_target_angle < 0) s_target_angle = 0;
            if (s_target_angle > 180) s_target_angle = 180;
        }
    }
    pthread_mutex_unlock(&s_car0107_hw_mutex);
}
