#include "zf_common_headfile.h"
#include "zf_common_debug.h"

#include "DOG_solve.h"
#include "math.h"

// ŷǽ
void euler_angle(struct DOG_SOLVE* this, struct DOG_IMU imu_data){
	if((imu_data.gyro_calibration_flag == True || imu_data.acc_calibration_flag == True) && this -> solve_flag == True)
	{
		this -> roll += imu_data.gyro_x*this -> solve_IT_time/1000.;
		this -> pitch += imu_data.gyro_y*this -> solve_IT_time/1000.;
		if(this -> encoder_merge_enable_flag == True)	// ںʹ this->diff_yaw ں
			this -> yaw += (imu_data.gyro_z*this -> solve_IT_time/1000+this -> diff_yaw)/2.0;
		else
			this -> yaw += imu_data.gyro_z*this -> solve_IT_time/1000;
		
		this -> roll =  fmod(this -> roll, 360);
		this -> pitch = fmod(this -> pitch, 360);
		this -> yaw = fmod(this -> yaw, 360);
	}
	else if(this -> solve_flag == False)
	{
		this -> roll = 0;
		this -> pitch = 0;
		this -> yaw = 0;
	}
}

/*
	˶
	˵
	kind ˶
	data_1 ٶ/Xٶ
	data_2 /Yٶ
	angular_speed תٶ
*/ 
void move_inv_solve(struct DOG_SOLVE* this, _move_solve_kind_ kind, float data_1, float data_2, float angular_speed){
	float x_speed = 0;
	float y_speed = 0;
	
	if(kind == SPEED_YAW_SOLVE){
		x_speed = data_1*sinf(DEG2RAD(data_2));
		y_speed = (float)data_1*cosf(DEG2RAD(data_2));
	}
	else{
		x_speed = data_1;
		y_speed = data_2;
	}
	this -> wheel_1_speed = -x_speed+angular_speed;
	this -> wheel_2_speed = x_speed*COS_60+y_speed*COS_30+angular_speed;
	this -> wheel_3_speed = x_speed*COS_60-y_speed*COS_30+angular_speed;
}

/*
	˶
	˵
	wheel_1_speed_real 1 ʵ
	wheel_2_speed_real 2 ʵ
	wheel_3_speed_real 2 ʵ
	yaw ʵʺ
	˵
	
*/ 
void move_solve(struct DOG_SOLVE* this, float wheel_1_speed_real, float wheel_2_speed_real, float wheel_3_speed_real, float yaw){
	if(this -> solve_flag == True)
	{
		// ӵλ΢
		this -> diff_wheel_1_displacement = wheel_1_speed_real*this -> solve_IT_time/1000.;
		this -> diff_wheel_2_displacement = wheel_2_speed_real*this -> solve_IT_time/1000.;
		this -> diff_wheel_3_displacement = wheel_3_speed_real*this -> solve_IT_time/1000.;
		
		// Ӻϳɵٶ
		this -> x_speed = (-2.*wheel_1_speed_real+wheel_2_speed_real+wheel_3_speed_real)/3.;
		this -> y_speed = (wheel_2_speed_real-wheel_3_speed_real)/SQRT_3;
		this -> angular_speed = (wheel_1_speed_real+wheel_2_speed_real+wheel_3_speed_real)/3.;
		
		// ֽ⵽XYϵλ΢
		this -> diff_x_displacement = (-2.*this -> diff_wheel_1_displacement+this -> diff_wheel_2_displacement+this -> diff_wheel_3_displacement)/3.;
		this -> diff_y_displacement = (this -> diff_wheel_2_displacement-this -> diff_wheel_3_displacement)/SQRT_3;
		this -> diff_yaw = RAD2DEG((this -> diff_wheel_1_displacement+this -> diff_wheel_2_displacement+this -> diff_wheel_3_displacement)/(3.0*this -> radius));
		
		// 纽
		this -> world_yaw += this -> diff_yaw;
		this -> world_yaw = fmod(this -> world_yaw, 360);
		// 纽
		if(this -> world_yaw > 180)this -> world_yaw = this -> world_yaw-360;
		else if(this -> world_yaw < -180)this -> world_yaw = this -> world_yaw+360;
		
		// λ΢
		this -> diff_displacement = sqrt(this -> diff_x_displacement*this -> diff_x_displacement+this -> diff_y_displacement*this -> diff_y_displacement);
		
		// ƽ΢֣󸺣
		if(this -> diff_y_displacement != 0)
			this -> diff_displacement_yaw = RAD2DEG(atan(this -> diff_x_displacement/this -> diff_y_displacement));
		else if(this -> diff_x_displacement >= 0 && this -> diff_y_displacement == 0)
			this -> diff_displacement_yaw = 0;
		else if(this -> diff_x_displacement < 0 && this -> diff_y_displacement == 0)
			this -> diff_displacement_yaw = -180;
		
		// ƽ
		if(this -> diff_x_displacement > 0 && this -> diff_y_displacement <0)
			this -> diff_displacement_yaw = this -> diff_displacement_yaw+180;
		else if(this -> diff_x_displacement < 0 && this -> diff_y_displacement <0)
			this -> diff_displacement_yaw = this -> diff_displacement_yaw-180;
		
		/*  */
		if(yaw > 180)yaw = yaw-360;
		else if(yaw < -180)yaw = yaw+360;
		this -> world_yaw = (yaw+this -> world_yaw)/2.0;
		
		// X,Yϵλ΢ֺλƣϵ
		this -> diff_world_x_displacement = this -> diff_displacement*sinf(DEG2RAD(this -> diff_displacement_yaw+this -> world_yaw*1.06));
		this -> diff_world_y_displacement = this -> diff_displacement*cosf(DEG2RAD(this -> diff_displacement_yaw+this -> world_yaw*1.04));
		this -> world_x_displacement += this -> diff_world_x_displacement;
		this -> world_y_displacement += this -> diff_world_y_displacement;
	
		// λ
		this -> displacement = sqrt(this -> world_x_displacement*this -> world_x_displacement+this -> world_y_displacement*this -> world_y_displacement);
		// λƷ
		if(this -> world_y_displacement != 0)
			this -> displacement_yaw = RAD2DEG(atan(this -> world_x_displacement/this -> world_y_displacement));
		else if(this -> world_x_displacement >= 0 && this -> world_y_displacement == 0)
			this -> displacement_yaw = 0;
		else if(this -> world_x_displacement < 0 && this -> world_y_displacement == 0)
			this -> displacement_yaw = -180;
		// λƷ
		if(this -> world_x_displacement > 0 && this -> world_y_displacement <0)
			this -> displacement_yaw = this -> displacement_yaw+180;
		else if(this -> world_x_displacement < 0 && this -> world_y_displacement <0)
			this -> displacement_yaw = this -> displacement_yaw-180;
		
		// λ
		this -> wheel_1_displacement += this -> diff_wheel_1_displacement;
		this -> wheel_2_displacement += this -> diff_wheel_2_displacement;
		this -> wheel_3_displacement += this -> diff_wheel_3_displacement;
		
		// ·̣λ΢ֻ֣
		this -> distance += this -> diff_displacement;
	}
	else if(this -> solve_flag == False)
	{
		this -> wheel_1_displacement = 0;	// λ
		this -> wheel_2_displacement = 0;	
		this -> wheel_3_displacement = 0;	
		this -> world_x_displacement = 0;
		this -> world_y_displacement = 0;
		this -> distance = 0;				// ·
		this -> displacement = 0;			// λ
		this -> displacement_yaw = 0;		// λƺ
	}
}

// 캯
void solve(struct DOG_SOLVE* this, float radius, uint16 solve_IT_time){
	/* Ա */
	this -> roll = 0.;
	this -> pitch = 0.;
	this -> yaw = 0.;	
	this -> wheel_1_displacement = 0;	// λ
	this -> wheel_2_displacement = 0;	
	this -> wheel_3_displacement = 0;	
	this -> world_x_displacement = 0;
	this -> world_y_displacement = 0;
	this -> distance = 0;				// ·
	this -> displacement = 0;			// λ
	this -> displacement_yaw = 0;		// λƺ
	this -> radius = radius;	
	this -> solve_IT_time = solve_IT_time;
	this -> solve_flag = False;
	this -> encoder_merge_enable_flag = False;
	this -> diff_world_x_displacement = 0.;	// Xλ΢
	this -> diff_world_y_displacement = 0.;	// Yλ΢
	this -> diff_x_displacement = 0.;		// Xλ΢
	this -> diff_y_displacement = 0.;		// Yλ΢
	this -> diff_displacement = 0.;			// λ΢
	this -> diff_yaw = 0.;					// ΢
	this -> diff_wheel_1_displacement = 0.;	// 1λ
	this -> diff_wheel_2_displacement = 0.; // 2λ
	this -> diff_wheel_3_displacement = 0.; // 3λ
	this -> diff_displacement_yaw = 0.;		// λƺ΢
	
	/* Ա */
	this -> euler_angle = euler_angle;
	this -> move_inv_solve = move_inv_solve;
	this -> move_solve = move_solve;
	
	return;
}

// 
void _solve(struct DOG_SOLVE* this){
	this -> roll = 0.;
	this -> pitch = 0.;
	this -> yaw = 0.;	
	this -> wheel_1_displacement = 0;	// λ
	this -> wheel_2_displacement = 0;	
	this -> wheel_3_displacement = 0;	
	this -> world_x_displacement = 0;
	this -> world_y_displacement = 0;
	this -> distance = 0;				// ·
	this -> displacement = 0;			// λ
	this -> displacement_yaw = 0;		// λƺ
	this -> solve_flag = False;
	this -> encoder_merge_enable_flag = False;
	this -> diff_world_x_displacement = 0.;	// Xλ΢
	this -> diff_world_y_displacement = 0.;	// Yλ΢
	this -> diff_x_displacement = 0.;		// Xλ΢
	this -> diff_y_displacement = 0.;		// Yλ΢
	this -> diff_displacement = 0.;			// λ΢
	this -> diff_yaw = 0.;					// ΢
	this -> diff_wheel_1_displacement = 0.;	// 1λ
	this -> diff_wheel_2_displacement = 0.; // 2λ
	this -> diff_wheel_3_displacement = 0.; // 3λ
	this -> diff_displacement_yaw = 0.;		// λƺ΢
}