#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);
static void _eCtrl_process_eBrake(void);

#define CURRENT_RAMP_TIME 5*1000

void eCtrl_init(u16 accl_time, u16 dec_time){
	g_eCtrl.dec_time = dec_time;
	g_eCtrl.accl_time = accl_time;

	g_eCtrl.ebrk_time = nv_get_foc_params()->n_ebrk_time;
	
	g_eCtrl.ebrk_time_shadow = g_eCtrl.ebrk_time;
	g_eCtrl.accl_time_shadow = g_eCtrl.accl_time;
	g_eCtrl.dec_time_shadow = g_eCtrl.dec_time;
	g_eCtrl.hw_brake = false;
	g_eCtrl.is_ebrake = false;
	eRamp_init(&g_eCtrl.current, CURRENT_RAMP_TIME, CURRENT_RAMP_TIME);
	eRamp_init(&g_eCtrl.speed, g_eCtrl.accl_time, g_eCtrl.dec_time);
	eRamp_init(&g_eCtrl.torque, g_eCtrl.accl_time, g_eCtrl.dec_time);
}

void eCtrl_debug_log(void) {
	sys_debug("final %f, step %f, inter %f, shadow %f\n", g_eCtrl.torque.target, g_eCtrl.torque.step_val, g_eCtrl.torque.interpolation, g_eCtrl.torque_shadow);
	sys_debug("final %f, %f\n", g_eCtrl.torque.acct, g_eCtrl.torque.dect);
}

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

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

void eCtrl_reset_Torque(float init_trq) {
	g_eCtrl.torque_shadow = init_trq;
	eRamp_reset_target(&g_eCtrl.torque, init_trq);
}

void eCtrl_reset_Current(float init_curr) {
	g_eCtrl.current_shadow = init_curr;
	eRamp_reset_target(&g_eCtrl.current, init_curr);
}

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

bool eCtrl_enable_eBrake(bool enable) {
	float ebrak = g_eCtrl.is_ebrake;
	if (PMSM_FOC_GetEbrkTorque() == 0.0f) {
		ebrak = false;
	}else {
		if (enable && ((PMSM_FOC_GetSpeed() > CONFIG_MIN_RPM_FOR_EBRAKE))) {
			ebrak = true;
		}else if (!enable && !_eCtrl_isHwBrk_shutPower()){
			ebrak = false;
		}
	}
	if (ebrak != g_eCtrl.is_ebrake) {
		g_eCtrl.is_ebrake = ebrak;
		_eCtrl_process_eBrake();
	}
	return ebrak;
}

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

static void _eCtrl_process_eBrake(void) {
	if (g_eCtrl.is_ebrake) {
		PMSM_FOC_SetCtrlMode(CTRL_MODE_EBRAKE);
	}else {
		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
	}
}

bool eCtrl_is_eBrk_Running(void) {
	return g_eCtrl.is_ebrake;
}

void eCtrl_Running(void) {
	if (g_eCtrl.accl_time_shadow != g_eCtrl.accl_time || g_eCtrl.dec_time_shadow != g_eCtrl.dec_time) {
		g_eCtrl.dec_time = g_eCtrl.dec_time_shadow;
		g_eCtrl.accl_time = g_eCtrl.accl_time_shadow;
		/* 重新计算ramp */
		eRamp_set_time(&g_eCtrl.speed, g_eCtrl.accl_time, g_eCtrl.dec_time);
		eRamp_set_time(&g_eCtrl.torque, g_eCtrl.accl_time, g_eCtrl.dec_time);
		_eCtrl_set_TgtSpeed(g_eCtrl.speed_shadow);
		_eCtrl_set_TgtTorque(g_eCtrl.torque_shadow);
	}
	if (g_eCtrl.ebrk_time_shadow != g_eCtrl.ebrk_time) {
		g_eCtrl.ebrk_time = g_eCtrl.ebrk_time_shadow;
		eRamp_set_time(&g_eCtrl.torque, g_eCtrl.ebrk_time, g_eCtrl.ebrk_time);
		_eCtrl_set_TgtTorque(g_eCtrl.torque_shadow);
	}
	if (g_eCtrl.current_shadow != g_eCtrl.current.target) {
		_eCtrl_set_TgtCurrent(g_eCtrl.current_shadow);
	}
	if (g_eCtrl.speed_shadow != g_eCtrl.speed.target) {
		_eCtrl_set_TgtSpeed(g_eCtrl.speed_shadow);
	}
	if (g_eCtrl.torque_shadow != g_eCtrl.torque.target) {
		if (PMSM_FOC_Get()->out.n_RunMode == CTRL_MODE_TRQ) {
			eRamp_set_time(&g_eCtrl.torque, g_eCtrl.accl_time, g_eCtrl.dec_time);
		}else {
			eRamp_set_time(&g_eCtrl.torque, g_eCtrl.ebrk_time, g_eCtrl.ebrk_time);
		}
		_eCtrl_set_TgtTorque(g_eCtrl.torque_shadow);
	}
	eRamp_X2_running(&g_eCtrl.current);
	eRamp_X2_running(&g_eCtrl.speed);
	eRamp_X2_running(&g_eCtrl.torque);
}

void eCtrl_Reset(void) {
	_eCtrl_clear_ramp();
	g_eCtrl.is_ebrake = false;
}

void eCtrl_set_accl_time(u16 time) {
	g_eCtrl.accl_time_shadow = time;
}

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

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

static void _eCtrl_set_TgtSpeed(float s) {
	if (_eCtrl_isHwBrk_shutPower()){
		return;
	}
	eRamp_set_X2_target(&g_eCtrl.speed, s);
}

static void _eCtrl_set_TgtTorque(float t) {
	if (_eCtrl_isHwBrk_shutPower() && !g_eCtrl.is_ebrake){
		return;
	}
	eRamp_set_X2_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_Running() && !eCtrl_enable_eBrake(true)) {
			_eCtrl_clear_ramp();
		}
	}
	cpu_exit_critical(mask);
}