#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 = acc;
	r->dect = 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 = acc;
	r->dect = dec;
}

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 = acc;
	r->dect = 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);
}

static void eRamp_X2_running(e_Ramp *r) {
#if 0
	if (r->first_target != 0) {
		r->interpolation += r->step_val;
		if (r->step_val > 0) {
			if (r->interpolation >= r->first_target) {
				r->interpolation = r->first_target;
				r->first_target = 0.0f;
			}
		}else {
			if (r->interpolation <= r->first_target) {
				r->interpolation = r->first_target;
				r->first_target = 0.0f;
			}
		}
	}
	if (r->first_target != 0) {
		return;
	}
	if(r->time < 0xFFFFFFFu) {
		r->time ++;
	}
	r->interpolation = r->start + r->A * (float)SQ(r->time);
	if ((r->A > 0) && (r->interpolation > r->target)) {
		r->interpolation = r->target;
	}else if ((r->A < 0) && (r->interpolation < r->target)) {
		r->interpolation = r->target;
	}
#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
}

static void eRamp_set_X2_target(e_Ramp *ramp, float c) {
#if 0
	float c_now = eRamp_get_intepolation(ramp);

	float delta = c - c_now;
	float first_delta = 0;
	if (delta > 0) {
		first_delta = 0;//min(delta, 20.0f);
		ramp->first_target = 0;//c_now + first_delta;
		ramp->step_val = 0.05f;
		ramp->A = (delta - first_delta)/SQ(ramp->acct);
	}else {
		first_delta = 0;//MAX(delta, -10.0f);
		ramp->first_target = 0;//c_now + first_delta;
		ramp->step_val = -0.01f;
		ramp->A = (delta - first_delta)/SQ(ramp->dect);
	}
	ramp->start = c_now + first_delta;
	ramp->time = 0;
	eRamp_set_target(ramp, 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 > 0.1f) {
			float first_delta = min(delta, 5.0f);
			ramp->first_target = c_now + first_delta;
			ramp->first_step = 0.1f;
			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) > 0.1f) {
			float first_delta = MAX(delta, -5.0f);
			ramp->first_target = c_now + first_delta;
			ramp->first_step = -0.1f;
			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
}

//y=Ax^2;
void eCtrl_init(u16 accl_time, u16 dec_time);
void eCtrl_set_ebrk_time(u16 ebrk_time);
void eCtrl_brake_signal(bool hw_brake);
bool eCtrl_is_eBrk_enabled(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);

#endif /* EBRAKE_CTRL_H__ */