#include "e_ctrl.h"
#include "foc/foc_config.h"
#include "math/fix_math.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "app/nv_storage.h"
#include "libs/logger.h"

e_Ctrl g_eCtrl;
static void _eCtrl_set_TgtCurrent(float c);
static void _eCtrl_set_TgtSpeed(float s);
static void _eCtrl_set_TgtTorque(float t);
static bool _eCtrl_isHwBrk_shutPower(void);

void eCtrl_init(u16 accl_time, u16 dec_time){
	g_eCtrl.dec_time = dec_time;
	g_eCtrl.accl_time = accl_time;
	if (g_eCtrl.accl_time == 0) {
		g_eCtrl.accl_time = DEFAULT_D_TIME;
	}
	if (g_eCtrl.dec_time == 0) {
		g_eCtrl.dec_time = DEFAULT_D_TIME;
	}
	g_eCtrl.ebrk_time = 2;
	
	g_eCtrl.ebrk_shadow = g_eCtrl.ebrk_time;
	g_eCtrl.accl_shadow = g_eCtrl.accl_time;
	g_eCtrl.dec_shadow = g_eCtrl.dec_time;
	eRamp_init(&g_eCtrl.current);
	eRamp_init(&g_eCtrl.speed);
	eRamp_init(&g_eCtrl.torque);
}

void eCtrl_set_ebrk_time(u16 ebrk_time) {
	g_eCtrl.ebrk_shadow = ebrk_time;
}


void eCtrl_set_TgtCurrent(float c) {
	g_eCtrl.current_shadow = c;
}

void eCtrl_set_TgtTorque(float t) {
	g_eCtrl.torque_shadow = t;
}

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

bool eCtrl_enable_eBrake(bool enable) {
	if (PMSM_FOC_GeteBrkPhaseCurrent() == 0.0f) {
		g_eCtrl.is_ebrake_shadow = false;
		return g_eCtrl.is_ebrake_shadow;
	}
	if (enable && PMSM_FOC_GetSpeed() > MIN_RPM_FOR_EBRAKE) {
		g_eCtrl.is_ebrake_shadow = true;
	}else if (!enable && !_eCtrl_isHwBrk_shutPower()){
		g_eCtrl.is_ebrake_shadow = false;
	}
	return g_eCtrl.is_ebrake_shadow;
}

void _eCtrl_clear_ramp(void) {
	eRamp_init(&g_eCtrl.current);
	eRamp_init(&g_eCtrl.speed);
	eRamp_init(&g_eCtrl.torque);
	g_eCtrl.current_shadow = 0.0f;
	g_eCtrl.torque_shadow = 0.0f;
	g_eCtrl.speed_shadow = 0.0f;
}

void _eCtrl_process_eBrake(void) {
	_eCtrl_clear_ramp();
	if (g_eCtrl.is_ebrake) {
		PMSM_FOC_SetCtrlMode(CTRL_MODE_CURRENT_BRK);
		eCtrl_set_TgtCurrent(-PMSM_FOC_GeteBrkPhaseCurrent());
	}else {
		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
	}
}

bool eCtrl_is_eBrk_enabled(void) {
	return (g_eCtrl.is_ebrake || g_eCtrl.is_ebrake_shadow);
}

void eCtrl_Running(void) {
	bool etime_changed = false;
	if (g_eCtrl.accl_shadow != g_eCtrl.accl_time || g_eCtrl.dec_shadow != g_eCtrl.dec_time) {
		g_eCtrl.dec_time = g_eCtrl.dec_shadow;
		g_eCtrl.accl_time = g_eCtrl.accl_shadow;
		etime_changed = true;
	}
	if (g_eCtrl.current_shadow != g_eCtrl.current.target || etime_changed) {
		_eCtrl_set_TgtCurrent(g_eCtrl.current_shadow);
	}
	if (g_eCtrl.speed_shadow != g_eCtrl.speed.target || etime_changed) {
		_eCtrl_set_TgtSpeed(g_eCtrl.speed_shadow);
	}
	if (g_eCtrl.torque_shadow != g_eCtrl.torque.target || etime_changed) {
		_eCtrl_set_TgtTorque(g_eCtrl.torque_shadow);
	}
	if (g_eCtrl.is_ebrake_shadow != g_eCtrl.is_ebrake) {
		g_eCtrl.is_ebrake = g_eCtrl.is_ebrake_shadow;
		_eCtrl_process_eBrake();
	}
	eRamp_running(&g_eCtrl.current);
	eRamp_running(&g_eCtrl.speed);
	eRamp_running(&g_eCtrl.torque);
}

static bool _eCtrl_isHwBrk_shutPower(void) {
	return (g_eCtrl.hw_brake && nv_get_foc_params()->n_brkShutPower);
}

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 - c_now)/(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.dec_time/step_ms);
		if (step_val < MIN_S16Q5) {
			step_val = MIN_S16Q5;
		}
		step_val = -step_val;
	}
	eRamp_set_target(ramp, c);
	eRamp_set_step(ramp, step_val);
}

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

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

static void _eCtrl_set_TgtTorque(float t) {
	if (g_eCtrl.hw_brake && nv_get_foc_params()->n_brkShutPower){
		return;
	}
	_eCtrl_set_target(&g_eCtrl.torque, t);
}

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

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

float eCtrl_get_RefTorque(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.current);
}

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

void eCtrl_brake_signal(bool hw_brake) {
	u32 mask = cpu_enter_critical();
	if (hw_brake != g_eCtrl.hw_brake) {
		g_eCtrl.hw_brake = hw_brake;
		if (hw_brake) {
			g_eCtrl.brake_ts = get_tick_ms();
		}
	}
	if (_eCtrl_isHwBrk_shutPower()) {
		if (!eCtrl_is_eBrk_enabled() && !eCtrl_enable_eBrake(true)) {
			_eCtrl_clear_ramp();
		}
	}
	cpu_exit_critical(mask);
}