#ifndef EBRAKE_CTRL_H__
#define EBRAKE_CTRL_H__
#include "os/os_types.h"
#include "foc/core/ramp_ctrl.h"
#include "foc/foc_config.h"
#include "math/fast_math.h"
#include "math/fix_math.h"

typedef struct {
	float start;
	float target;
	float interpolation;
	float step_val;
	float first_target;
	float first_step;
	float A;
	float acct;
	float dect;
	float time;
}e_Ramp;

typedef struct {
	u16  ebrk_time; //能量回收，时间越短，刹车性能或者回收越好
	u16  accl_time; //加速时间（ms），时间越短，加速性能越好
	u16  dec_time;  //降速时间
	bool hw_brake;
	bool is_ebrake;
	u32  brake_ts;//检测到刹车开始时间
	e_Ramp current;
	e_Ramp torque;
	e_Ramp speed;
	u16  ebrk_time_shadow;
	u16  accl_time_shadow;
	u16  dec_time_shadow;
	float ebrake_current;
	float current_shadow;
	float torque_shadow;
	float speed_shadow;
}e_Ctrl;

static void eRamp_init(e_Ramp *r, u32 acc, u32 dec) {
	r->start = 0;
	r->target = 0;
	r->first_target = 0;
	r->interpolation = 0;
	r->step_val = 0;
	r->first_step = 0;
	r->acct = (float)acc;
	r->dect = (float)dec;
}

static void eRamp_init_target(e_Ramp *r, float target, u32 acc, u32 dec) {
	r->start = target;
	r->target = target;
	r->first_target = target;
	r->interpolation = target;
	r->step_val = 0;
	r->first_step = 0;
	r->acct = (float)acc;
	r->dect = (float)dec;
}

static void eRamp_init_target2(e_Ramp *r, float target, u32 time) {
	eRamp_init_target(r, target, time, time);
} 


static void eRamp_reset_target(e_Ramp *r, float target) {
	r->start = target;
	r->target = target;
	r->first_target = target;
	r->interpolation = target;
	r->step_val = 0;
	r->first_step = 0;
}
static void eRamp_set_time(e_Ramp *r, u32 acc, u32 dec) {
	r->acct = (float)acc;
	r->dect = (float)dec;
}

static void eRamp_set_target(e_Ramp *r, float target) {
	r->target = target;
}
static void eRamp_set_step(e_Ramp *r, float step) {
	r->step_val = step;
}

static void eRamp_running(e_Ramp *r) {
	float target = r->interpolation + r->step_val;
	if (r->step_val < 0) {
		if (target < r->target) {
			target = r->target;
		}
	}else {
		if (target > r->target) {
			target = r->target;
		}
	}
	r->interpolation = target;	
}

static float eRamp_get_intepolation(e_Ramp *r) {
	return r->interpolation;
}

static float eRamp_get_target(e_Ramp *r) {
	return r->target;
}

static void eRamp_set_step_target(e_Ramp *ramp, float c, u32 intval) {
	float c_now = eRamp_get_intepolation(ramp);
	float step_val = 0;
	float delta = c - c_now;
	float step_ms = intval;

	if (delta >= 0) {
		step_val = (delta)/(ramp->acct/step_ms);
	}else {
		step_val = (delta)/(ramp->dect/step_ms);
	}
	eRamp_set_target(ramp, c);
	eRamp_set_step(ramp, step_val);
}

#if 0
extern float PMSM_FOC_GetSpeed(void);
static void eRamp_X2_running(e_Ramp *r) {
#if 1
	float target = r->target;
	float v_now = r->interpolation;
	bool cross_zero = false;
	if (target > 0) {
		if (v_now >= -CONFIG_RAMP_SECOND_TARGET && v_now <= CONFIG_RAMP_SECOND_TARGET * 1.5f) {
			if (PMSM_FOC_GetSpeed() <= 20.0f) {
				step_towards(&r->interpolation, target, 0.02f);
			}else {
				step_towards(&r->interpolation, target, 0.04f);
			}
			cross_zero = true;
		}
	}else if (target == 0) {
		if (v_now >= 0 && v_now <= CONFIG_RAMP_SECOND_TARGET) {
			step_towards(&r->interpolation, target, 0.01f);
			cross_zero = true;
		}
	}else {
		if (v_now >= -CONFIG_RAMP_SECOND_TARGET && v_now <= CONFIG_RAMP_SECOND_TARGET) {
			step_towards(&r->interpolation, target, 0.01f);
			cross_zero = true;
		}
	}
	if (!cross_zero) {
		step_towards(&r->interpolation, target, 1.0f);
	}
#else
	if (r->first_step != 0) {
		float interpolation = r->interpolation + r->first_step;
		if ((r->first_step > 0) && (interpolation >= r->first_target)) {
			interpolation = r->first_target;
			r->first_step = r->first_target = 0;
		}else if ((r->first_step < 0) && (interpolation <= r->first_target)) {
			interpolation = r->first_target;
			r->first_step = r->first_target = 0;
		}
		r->interpolation = interpolation;
		return;
	}
	
	eRamp_running(r);
#endif
}
#else
static void eRamp_X2_running(e_Ramp *r) {
	eRamp_running(r);
}
#endif

#if 0
static void eRamp_set_X2_target(e_Ramp *r, float c) {
#if 1
	eRamp_set_target(r, c);
#else
	float c_now = eRamp_get_intepolation(ramp);
	float step_val = 0;
	float delta = c - c_now;

	float step_ms = CONFIG_eCTRL_STEP_TS;	
	if (delta > 0) {
		step_val = (delta)/(ramp->acct/step_ms);
		if (step_val > CONFIG_RAMP_SECOND_STEP) {
			float first_delta = min(delta, CONFIG_RAMP_SECOND_TARGET);
			ramp->first_target = c_now + first_delta;
			ramp->first_step = CONFIG_RAMP_SECOND_STEP;
			delta -= first_delta;
			step_val = (delta)/(ramp->acct/step_ms);
		}else {
			ramp->first_target = ramp->first_step = 0.0f;
		}
	}else if (delta < 0){
		step_val = (delta)/(ramp->dect/step_ms);
		if (ABS(step_val) > CONFIG_RAMP_SECOND_STEP) {
			float first_delta = MAX(delta, -CONFIG_RAMP_SECOND_TARGET);
			ramp->first_target = c_now + first_delta;
			ramp->first_step = -CONFIG_RAMP_SECOND_STEP;
			delta -= first_delta;
			step_val = (delta)/(ramp->dect/step_ms);
		}else {
			ramp->first_target = ramp->first_step = 0.0f;
		}
	}else {
		step_val = 0;
		ramp->first_step = ramp->first_target = 0;
	}
	eRamp_set_target(ramp, c);
	eRamp_set_step(ramp, step_val);

#endif
}
#else
static void eRamp_set_X2_target(e_Ramp *r, float c) {
	eRamp_set_step_target(r, c, CONFIG_eCTRL_STEP_TS);
}

#endif
extern e_Ctrl g_eCtrl;
static u16 eCtrl_get_torque_acc_time(void) {
	return (u16)g_eCtrl.torque.acct;
}

static void eCtrl_Set_eBrk_RampTime(u16 t) {
	g_eCtrl.ebrk_time_shadow = t;
}

//y=Ax^2;
void eCtrl_init(u16 accl_time, u16 dec_time);
void eCtrl_brake_signal(bool hw_brake);
bool eCtrl_is_eBrk_Running(void);
void eCtrl_set_TgtCurrent(float c);
void eCtrl_set_TgtTorque(float t);
void eCtrl_set_TgtSpeed(float s);
bool eCtrl_enable_eBrake(bool enable);
float eCtrl_get_RefSpeed(void);
float eCtrl_get_RefCurrent(void);
float eCtrl_get_RefTorque(void);
float eCtrl_get_FinalSpeed(void);
float eCtrl_get_FinalCurrent(void);
float eCtrl_get_FinalTorque(void);
void eCtrl_Running(void);
void eCtrl_Reset(void);
void eCtrl_reset_Torque(float init_trq);
void eCtrl_reset_Current(float init_curr);
void eCtrl_set_accl_time(u16 time);

#endif /* EBRAKE_CTRL_H__ */