#include "e_ctrl.h"
#include "foc/foc_config.h"
#include "math/fix_math.h"
#include "libs/logger.h"

static e_Ctrl g_eCtrl;
static void _eCtrl_set_TgtCurrent(float c);
static void _eCtrl_set_TgtSpeed(float s);

void eCtrl_init(u16 ebrk_time, u16 accl_time){
	g_eCtrl.ebrk_time = ebrk_time;
	g_eCtrl.accl_time = accl_time;
	if (g_eCtrl.accl_time == 0) {
		g_eCtrl.accl_time = DEFAULT_D_TIME;
	}
	if (g_eCtrl.ebrk_time == 0) {
		g_eCtrl.ebrk_time = DEFAULT_D_TIME;
	}
	g_eCtrl.ebrk_shadow = g_eCtrl.ebrk_time;
	g_eCtrl.accl_shadow = g_eCtrl.accl_time;

	eRamp_init(&g_eCtrl.torque);
	eRamp_init(&g_eCtrl.speed);
}

void eCtrl_set_accl_brk(u16 accl_time, u16 ebrk_time) {
	g_eCtrl.accl_shadow = accl_time;
	g_eCtrl.ebrk_shadow = ebrk_time;
}


void eCtrl_set_TgtCurrent(float c) {
	g_eCtrl.torque_shadow = c;
}
void eCtrl_set_TgtSpeed(float s) {
	g_eCtrl.speed_shadow = s;
}

void eCtrl_Running(void) {
	bool etime_changed = false;
	if (g_eCtrl.accl_shadow != g_eCtrl.accl_time || g_eCtrl.ebrk_shadow != g_eCtrl.ebrk_time) {
		g_eCtrl.ebrk_time = g_eCtrl.ebrk_shadow;
		g_eCtrl.accl_time = g_eCtrl.accl_shadow;
		etime_changed = true;
	}
	if (g_eCtrl.torque_shadow != g_eCtrl.torque.target || etime_changed) {
		_eCtrl_set_TgtCurrent(g_eCtrl.torque_shadow);
	}
	if (g_eCtrl.speed_shadow != g_eCtrl.speed.target || etime_changed) {
		_eCtrl_set_TgtSpeed(g_eCtrl.speed_shadow);
	}
	eRamp_running(&g_eCtrl.torque);
	eRamp_running(&g_eCtrl.speed);
}

static void _eCtrl_set_target(e_Ramp *ramp, float c) {
	float c_now = eRamp_get_intepolation(ramp);
	float step_val = 0;
	int   sign = 1;

	if (c < c_now) {
		sign = -1;
	}
	u32 step_ms = eCTRL_STEP_TS;	
	if (sign > 0) { //增加扭矩
		step_val = (c_now - c)/(g_eCtrl.accl_time/step_ms);
		if (step_val < MIN_S16Q5) {
			step_val = MIN_S16Q5;
		}
	}else if (sign < 0) {
		step_val = (c_now - c)/(g_eCtrl.ebrk_time/step_ms);
		if (step_val < MIN_S16Q5) {
			step_val = MIN_S16Q5;
		}
	}
	eRamp_set_target(ramp, c);
	eRamp_set_step(ramp, step_val);

}
static void _eCtrl_set_TgtCurrent(float c) {
	_eCtrl_set_target(&g_eCtrl.torque, c);
}

static void _eCtrl_set_TgtSpeed(float s) {
	_eCtrl_set_target(&g_eCtrl.speed, s);
}



float eCtrl_get_RefSpeed(void) {
	return eRamp_get_intepolation(&g_eCtrl.speed);
}

float eCtrl_get_RefCurrent(void) {
	return eRamp_get_intepolation(&g_eCtrl.torque);
}


float eCtrl_get_FinalSpeed(void) {
	return eRamp_get_target(&g_eCtrl.speed);
}

float eCtrl_get_FinalCurrent(void) {
	return eRamp_get_target(&g_eCtrl.torque);
}

void eCtrl_brake_signal(bool hw_brake) {
	if (hw_brake != g_eCtrl.hw_brake) {
		g_eCtrl.hw_brake = hw_brake;
		if (hw_brake) {
			g_eCtrl.brake_ts = shark_get_mseconds();
		}
	}
	
	if (g_eCtrl.hw_brake) {
		float ebrk_torque = 0.0f;
		float ebrk_speed = 0.0f;
		if (shark_get_mseconds() - g_eCtrl.brake_ts >= eCTRL_Brake_TIME) {
			if (g_eCtrl.accl_time != DEFAULT_D_TIME) {
				ebrk_torque = eCTRL_NEG_TORQUE;
			}
		}
		eCtrl_set_TgtCurrent(ebrk_torque);
		eCtrl_set_TgtSpeed(ebrk_speed);
	}
}