#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;

static int s_phone_speed_mode = 0;
static int s_phone_current_gval = 0;
static int s_phone_pending_mode = 0;
static int s_phone_pending_gval = 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 BRAKE_STEP 10
#define CAR0107_MAX_INPUT_SPEED  200
#define CAR0107_ACTIVE_THRESHOLD 50
#define CAR0107_MAX_TARGET_BACK  100
#define CAR0107_MAX_TARGET_FRONT 80
#define STEER_GVAL_MAX           120
#define CAR0107_PWM_FORWARD_MIN  58
#define CAR0107_PWM_BACK_MAX     84

static int car0107_clamp_input(int val) {
    if (val < 0) {
        return 0;
    }
    if (val > CAR0107_MAX_INPUT_SPEED) {
        return CAR0107_MAX_INPUT_SPEED;
    }
    return val;
}

static int car0107_input_to_gval(int val) {
    val = car0107_clamp_input(val);
    if (val < CAR0107_ACTIVE_THRESHOLD) {
        return 0;
    }
    return 51 + (val - CAR0107_ACTIVE_THRESHOLD) *
                     (STEER_GVAL_MAX - 51) /
                     (CAR0107_MAX_INPUT_SPEED - CAR0107_ACTIVE_THRESHOLD);
}

static int car0107_input_to_target_speed(int val, int max_target) {
    val = car0107_clamp_input(val);
    if (val < CAR0107_ACTIVE_THRESHOLD) {
        return 0;
    }
    return (val - CAR0107_ACTIVE_THRESHOLD) * max_target /
           (CAR0107_MAX_INPUT_SPEED - CAR0107_ACTIVE_THRESHOLD);
}

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 void car0107_reset_phone_state(void) {
    s_phone_speed_mode = 0;
    s_phone_current_gval = 0;
    s_phone_pending_mode = 0;
    s_phone_pending_gval = 0;
}

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

static void car0107_stop_speed_now(void) {
    s_target_speed = 0;
    s_current_speed = 0;
    s_speed_mode = 0;
    s_speed_timeout = 0;
    car0107_reset_phone_state();
    car0107_apply_stop();
}

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

/*车中间值，停止舵机中间函数*/
void car0107_middle_pwm() {
    pthread_mutex_lock(&s_car0107_hw_mutex);
    car0107_reset_steering_state();
    car0107_reset_phone_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;
		if (back_speed < CAR0107_PWM_FORWARD_MIN) {
			back_speed = CAR0107_PWM_FORWARD_MIN;
		}
		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;
		if (flont_speed > CAR0107_PWM_BACK_MAX) {
			flont_speed = CAR0107_PWM_BACK_MAX;
		}
		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);
	}  
}

static void car0107_phone_apply_speed(void) {
    if (s_phone_speed_mode == 1) {
        car0107_mode_2_back(s_phone_current_gval);
    } else if (s_phone_speed_mode == 2) {
        car0107_mode_1_flont(s_phone_current_gval);
    } else {
        pwmWrite(PWM_PIN_SPEED, 75);
    }
}

static void car0107_set_phone_speed_command(int new_mode, int new_gval) {
    if (new_gval < CAR0107_ACTIVE_THRESHOLD) {
        car0107_reset_phone_state();
        car0107_apply_stop();
        return;
    }

    if (s_phone_current_gval > CAR0107_ACTIVE_THRESHOLD &&
        s_phone_speed_mode != 0 && new_mode != s_phone_speed_mode) {
        s_phone_pending_mode = new_mode;
        s_phone_pending_gval = new_gval;
        s_phone_speed_mode = 0;
        pwmWrite(PWM_PIN_SPEED, 75);
        return;
    }

    if (s_phone_speed_mode == 0 && s_phone_pending_mode != 0) {
        s_phone_pending_mode = new_mode;
        s_phone_pending_gval = new_gval;
        return;
    }

    s_phone_pending_mode = 0;
    s_phone_pending_gval = 0;
    s_phone_speed_mode = new_mode;
    s_phone_current_gval = new_gval;
    car0107_phone_apply_speed();
}

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

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

    if ((mode == 1 || mode == 2) && raw_val == 0) {
        car0107_stop_speed_now();
        pthread_mutex_unlock(&s_car0107_hw_mutex);
        return;
    }

    int val = car0107_input_to_gval(raw_val);

    switch(mode){
        case 1:
			car0107_set_phone_speed_command(1, val);
			break;
		case 2:
			car0107_set_phone_speed_command(2, 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_phone_smooth_process(void) {
    if (s_control_source != CTRL_PHONE) {
        return;
    }
    if (s_phone_speed_mode != 0 || s_phone_pending_mode == 0) {
        return;
    }
    if (s_phone_current_gval < CAR0107_ACTIVE_THRESHOLD) {
        s_phone_speed_mode = s_phone_pending_mode;
        s_phone_current_gval = s_phone_pending_gval;
        s_phone_pending_mode = 0;
        s_phone_pending_gval = 0;
        car0107_phone_apply_speed();
        return;
    }

    s_phone_current_gval -= BRAKE_STEP;
    if (s_phone_current_gval < CAR0107_ACTIVE_THRESHOLD) {
        s_phone_current_gval = 0;
        pwmWrite(PWM_PIN_SPEED, 75);
        s_phone_speed_mode = s_phone_pending_mode;
        s_phone_current_gval = s_phone_pending_gval;
        s_phone_pending_mode = 0;
        s_phone_pending_gval = 0;
        car0107_phone_apply_speed();
    } else {
        pwmWrite(PWM_PIN_SPEED, 75);
    }
}

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) {
        car0107_apply_stop();
    } 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_phone_smooth_process();
    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;
    car0107_reset_phone_state();
    s_steering_timeout = 200;
    s_speed_timeout = 200;

    if (mode == 1) {
        val = car0107_clamp_input(val);
        if (val == 0 || val < CAR0107_ACTIVE_THRESHOLD) {
            car0107_stop_speed_now();
        } else {
            s_speed_mode = 1;
            s_target_speed = car0107_input_to_target_speed(val, CAR0107_MAX_TARGET_BACK);
        }
    } else if (mode == 2) {
        val = car0107_clamp_input(val);
        if (val == 0 || val < CAR0107_ACTIVE_THRESHOLD) {
            car0107_stop_speed_now();
        } else {
            s_speed_mode = 2;
            s_target_speed = car0107_input_to_target_speed(val, CAR0107_MAX_TARGET_FRONT);
        }
    } 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);
}
