#include "foc/core/torque.h"
#include "foc/foc_config.h"
#include "foc/motor/motor.h"
#include "foc/core/e_ctrl.h"
#include "foc/core/PMSM_FOC_Core.h"
#include "app/nv_storage.h"
#include "libs/logger.h"
#include "prot/can_foc_msg.h"

/*
通过查表获取对应扭矩和速度时的Id和IQ的分配
*/
static torque_lut_t *_trq_tbl = NULL;
static torque_manager_t g_trq_mn;
void torque_init(void) {
	_trq_tbl = nv_get_trq_tlb();
	torque_reset();
}

void torque_reset(void) {
	memset(&g_trq_mn, 0, sizeof(g_trq_mn));
}

void torque_get_idq(float torque, float rpm, DQ_t *dq_out) {
	if ((_trq_tbl == NULL) || (torque < 0 || rpm < 0)) {
		dq_out->d = 0;
		dq_out->q = torque;
		return;
	}
	int trq_idx = (int)torque / TBL_TRQ_INTVAL;
	int rpm_idx = (int)rpm / TBL_SPD_INTVAL;
	if (trq_idx >= MAX_TRQ_POINTS) {
		trq_idx = MAX_TRQ_POINTS -1;
	}
	if (rpm_idx >= MAX_SPD_POINTS) {
		rpm_idx = MAX_SPD_POINTS -1;
	}
	s16 d = _trq_tbl->dq[trq_idx][rpm_idx].d;
	s16 q = _trq_tbl->dq[trq_idx][rpm_idx].q;
	if (trq_idx < MAX_TRQ_POINTS - 1) {
		trq_idx += 1;
	}
	if (rpm_idx < MAX_SPD_POINTS - 1) {
		rpm_idx += 1;
	}
	s16 d_delta = _trq_tbl->dq[trq_idx][rpm_idx].d - d;
	s16 q_delta = _trq_tbl->dq[trq_idx][rpm_idx].q - q;
	float comp_ceof = 0.5f * ((torque - torque/TBL_TRQ_INTVAL*TBL_TRQ_INTVAL)/(float)TBL_TRQ_INTVAL + (rpm - rpm/TBL_SPD_INTVAL*TBL_SPD_INTVAL)/(float)TBL_SPD_INTVAL);

	dq_out->d = d + d_delta * comp_ceof;
	dq_out->q = q + q_delta * comp_ceof;

}

/* 获取油门开度 */
static float throttle_ration(float f_throttle) {
	if (f_throttle <= nv_get_foc_params()->n_minThroVol) {
		return 0;
	}
	float delta = f_throttle - (nv_get_foc_params()->n_minThroVol);

	int ration = (delta * 1000.0f) / (nv_get_foc_params()->n_maxThroVol - nv_get_foc_params()->n_minThroVol);
	
	return ((float)ration)/1000.0f;
}

float throttle_to_speed(float f_throttle) {
	return (PMSM_FOC_GetSpeedLimit() * throttle_ration(f_throttle));
}

float throttle_to_torque(float f_throttle) {
	return PMSM_FOC_GetTorqueLimit() * throttle_ration(f_throttle);
}

#define REAL_DQ_CEOF 0.9f
#define FINAL_DQ_CEFO 1.1F
#define START_RPM_FOR_AUDOHOLD 10.0F
#if 1
void torque_mode_process(void) {
	float ref_trq = PMSM_FOC_GetTorqueLimit() * g_trq_mn.thro_value;
	if ((mc_throttle_released()) && eCtrl_enable_eBrake(true)) {
		g_trq_mn.thro_value = 0;
		g_trq_mn.torque_prev = 0;
		g_trq_mn.torque_base = 0;
		return;
	}
	
	if (!mc_throttle_released()) {
		if (PMSM_FOC_GetSpeed() <= START_RPM_FOR_AUDOHOLD) {
			ref_trq  = MAX(eCtrl_get_FinalTorque() * FINAL_DQ_CEFO, ref_trq );
			g_trq_mn.torque_base = ref_trq;
		}
		PMSM_FOC_Set_Torque(ref_trq );
		g_trq_mn.torque_prev = ref_trq;
	}else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
		float real_trq = PMSM_FOC_Get_Real_Torque() * REAL_DQ_CEOF;
		float ref_now = min(real_trq, eCtrl_get_RefTorque());
		eCtrl_reset_Torque(ref_now);
		PMSM_FOC_Set_Torque(0);
		g_trq_mn.thro_value = 0;
		g_trq_mn.torque_prev = 0;
		g_trq_mn.torque_base = 0;
	}
}
#else
void torque_mode_process(void) {
	float thro_curr = g_trq_mn.thro_value;
	float thro_prev = g_trq_mn.thro_prev;
	float trq_ref = PMSM_FOC_GetTorqueLimit() * thro_curr;

	if ((thro_curr == 0.0f) && eCtrl_enable_eBrake(true)) {
		g_trq_mn.thro_value = 0;
		g_trq_mn.accl = false;
		g_trq_mn.thro_prev = 0.0f;
		return;
	}
	float real_trq = eCtrl_get_FinalTorque() * REAL_DQ_CEOF;
	if (trq_ref > 0) {
		if (PMSM_FOC_GetSpeed() <= 10.0f) {
			g_trq_mn.accl_ref = MAX(real_trq, trq_ref); //不能小于autohold产生的扭矩
			trq_ref = g_trq_mn.accl_ref;
		}else {
			if (thro_curr > thro_prev) {
				if (!g_trq_mn.accl){
					g_trq_mn.accl_ref = eCtrl_get_RefTorque();
				}
				if (g_trq_mn.accl_ref > PMSM_FOC_GetTorqueLimit()) {
					g_trq_mn.accl_ref = PMSM_FOC_GetTorqueLimit();
				}
				trq_ref = g_trq_mn.accl_ref + thro_curr * (PMSM_FOC_GetTorqueLimit() - g_trq_mn.accl_ref);
				g_trq_mn.accl = true;
			}else if (thro_curr < thro_prev){
				if (g_trq_mn.accl) {
					g_trq_mn.accl_ref = min(real_trq, eCtrl_get_RefTorque());
					eCtrl_reset_Torque(g_trq_mn.accl_ref);
				}
				trq_ref = thro_curr * g_trq_mn.accl_ref;
				g_trq_mn.accl = false;
			}else {
				if (g_trq_mn.accl) {
					trq_ref = g_trq_mn.accl_ref + thro_curr * (PMSM_FOC_GetTorqueLimit() - g_trq_mn.accl_ref);
				}else {
					trq_ref = thro_curr * g_trq_mn.accl_ref;
				}
			}
		}
		PMSM_FOC_Set_Torque(trq_ref);
	}
	else if (eCtrl_get_FinalTorque() != 0){
		float ref_now = min(real_trq, eCtrl_get_RefTorque());
		eCtrl_reset_Torque(ref_now);
		PMSM_FOC_Set_Torque(0);
		g_trq_mn.thro_value = 0;
	}
	g_trq_mn.thro_prev = thro_curr;
}

#endif
void speed_mode_process(void) {
	float speed_Ref = g_trq_mn.spd_ref;
	PMSM_FOC_Set_Speed(speed_Ref);
}

#define THRO_REF_LP_CEOF 0.2f

void throttle_process(u8 run_mode, float f_throttle) {
	if (run_mode == CTRL_MODE_TRQ) {
		float thro_value = throttle_ration(f_throttle);
		g_trq_mn.thro_value = LowPass_Filter(g_trq_mn.thro_value, thro_value, THRO_REF_LP_CEOF);
		if ((g_trq_mn.count++ % 20) == 0) {
			torque_mode_process();
		}
	
	}else if (run_mode == CTRL_MODE_SPD) {
		float spd_ref = throttle_to_speed(f_throttle);
		g_trq_mn.spd_ref = LowPass_Filter(g_trq_mn.spd_ref, spd_ref, THRO_REF_LP_CEOF);
		if ((g_trq_mn.count++ % 20) == 0) {
			speed_mode_process();
		}
	}else if (run_mode == CTRL_MODE_CURRENT_BRK) {
		eCtrl_reset_Torque(0);
		if (eCtrl_get_FinalCurrent() < 0.0001f && PMSM_FOC_GetSpeed() < CONFIG_MIN_RPM_EXIT_EBRAKE) {
			eCtrl_enable_eBrake(false);	
		}
		if (!mc_throttle_released() || (mc_throttle_released() && (PMSM_FOC_GetSpeed() == 0.0f))) {
			eCtrl_enable_eBrake(false);
		}
	}
}

/*
void torque_manager(u8 run_mode, float f_throttle) {
	if ((g_trq_mn.count++ % 20) != 0) {
		return;
	}
	if (run_mode == CTRL_MODE_SPD) {
		float speed_Ref = throttle_to_speed(f_throttle);
		PMSM_FOC_Set_Speed(speed_Ref);
	}else if (run_mode == CTRL_MODE_TRQ) {
		if (mc_throttle_released()) {
			eCtrl_enable_eBrake(true);
			PMSM_FOC_Set_Torque(0);
			g_trq_mn.torque_prev = 0;
		}else {
			float torque = throttle_to_torque(f_throttle);
			eCtrl_enable_eBrake(false);
			PMSM_FOC_Set_Torque(torque);
			g_trq_mn.torque_prev = torque;
		}
	}else if (run_mode == CTRL_MODE_CURRENT_BRK) {
		if (!mc_throttle_released() || (mc_throttle_released() && (PMSM_FOC_GetSpeed() == 0.0f))) {
			eCtrl_enable_eBrake(false);
		}
	}
} */