#include "app/nv_storage.h"
#include "bsp/fmc_flash.h"
#include "libs/crc16.h"
#include "libs/logger.h"
#include "foc/motor/motor_param.h"
#include "foc/foc_config.h"

static motor_params_t m_params;
static foc_params_t   foc_params;

motor_params_t *nv_get_motor_params(void) {
	return &m_params;
}
foc_params_t *nv_get_foc_params(void) {
	return &foc_params;
}


void nv_save_hall_table(s32 *hall_table) {
	memcpy((char *)m_params.hall_table, (char *)hall_table, sizeof(m_params.hall_table));
	nv_save_motor_params();
}

void nv_save_angle_offset(float offset) {
	m_params.offset = offset;
	nv_save_motor_params();
}


static void nv_default_motor_params(void) {
	m_params.poles = MOTOR_POLES;
	m_params.r = MOTOR_R;
	m_params.ld = MOTOR_Ld;
	m_params.lq = MOTOR_Lq;
	m_params.offset = MOTOR_ENC_OFFSET;//180;//(69.0f);
	//m_params.offset = (360-128);
	m_params.est_pll_band = 100;
	m_params.pos_lock_pll_band = 100;
	m_params.flux_linkage = 0.0f;
}

static void nv_default_foc_params(void) {
	foc_params.s_maxDCVol = CONFIG_RATED_DC_VOL;
	foc_params.s_minDCVol = CONFIG_MIN_DC_VOL;
	foc_params.s_maxiDC = 30;
	//foc_params.s_maxIdq = 200;
	//foc_params.s_minIdq = -200;
	foc_params.s_PhaseCurrLim = 150;
	foc_params.s_maxRPM = 8000;
	foc_params.s_maxEpmRPM = 200;
	foc_params.s_maxEpmPhaseCurrLim = 10;
	foc_params.s_maxTorque = foc_params.s_PhaseCurrLim;
	foc_params.s_PhaseCurreBrkLim = 0.0f;
	foc_params.n_currentBand = CONFIG_CURRENT_BANDWITH;
	foc_params.n_modulation = 1.0f;
	foc_params.n_PhaseFilterCeof = 0.2f;
	foc_params.n_brkShutPower = 1;
	foc_params.s_LimitiDC = 200.0f;
	foc_params.s_iDCeBrkLim = 15.0f;
	
	foc_params.pid_conf[PID_D_id].kp = (CONFIG_CURRENT_BANDWITH * MOTOR_Ld);
	foc_params.pid_conf[PID_D_id].ki = (MOTOR_R/MOTOR_Ld);
	foc_params.pid_conf[PID_D_id].kb = 0;
	
	foc_params.pid_conf[PID_Q_id].kp = (CONFIG_CURRENT_BANDWITH * MOTOR_Lq);
	foc_params.pid_conf[PID_Q_id].ki = (MOTOR_R/MOTOR_Lq);
	foc_params.pid_conf[PID_Q_id].kb = 0;

	foc_params.pid_conf[PID_TRQ_id].kp = 0.13f;
	foc_params.pid_conf[PID_TRQ_id].ki = 0.3f;
	foc_params.pid_conf[PID_TRQ_id].kb = 1.0f;

	foc_params.pid_conf[PID_Spd_id].kp = 0.13f;
	foc_params.pid_conf[PID_Spd_id].ki = 0.08f;
	foc_params.pid_conf[PID_Spd_id].kb = 1.0f;

	foc_params.pid_conf[PID_Pow_id].kp = 0.5f;
	foc_params.pid_conf[PID_Pow_id].ki = 100.0f;
	foc_params.pid_conf[PID_Pow_id].kb = 0;

	foc_params.pid_conf[PID_Lock_id].kp = (0.1f);
	foc_params.pid_conf[PID_Lock_id].ki = (0.5f);
	foc_params.pid_conf[PID_Lock_id].kb = 0;

}

void nv_save_motor_params(void) {
	u16 crc = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
	m_params.crc16 = crc;
	fmc_write_data(motorParam_idx_0, (u8 *)&m_params, sizeof(m_params));
	fmc_write_data(motorParam_idx_1, (u8 *)&m_params, sizeof(m_params));
}
void nv_read_motor_params(void) {
	fmc_read_data(motorParam_idx_0, (u8 *)&m_params, sizeof(m_params));
	u16 crc0 = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
	if (crc0 != m_params.crc16) {
		sys_debug("mp 0 error\n");
		fmc_read_data(motorParam_idx_1, (u8 *)&m_params, sizeof(m_params));
		crc0 = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
		if (crc0 != m_params.crc16) {
			nv_default_motor_params();
			nv_save_motor_params();
			return;
		}
		fmc_write_data(motorParam_idx_0, (u8 *)&m_params, sizeof(m_params));
	}else {
		fmc_read_data(motorParam_idx_1, (u8 *)&m_params, sizeof(m_params));
		crc0 = crc16_get((u8 *)&m_params, sizeof(m_params) - 2);
		if (crc0 != m_params.crc16) {
			sys_debug("mp 1 error\n");
			fmc_read_data(motorParam_idx_0, (u8 *)&m_params, sizeof(m_params));
			fmc_write_data(motorParam_idx_1, (u8 *)&m_params, sizeof(m_params));
		}
	}
}
void nv_save_foc_params(void) {
	u16 crc = crc16_get((u8 *)&foc_params, sizeof(foc_params) - 2);
	foc_params.crc16 = crc;
	fmc_write_data(focParam_idx_0, (u8 *)&foc_params, sizeof(foc_params));
	fmc_write_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
}

void nv_read_foc_params(void) {
	fmc_read_data(focParam_idx_0, (u8 *)&foc_params, sizeof(foc_params));
	u16 crc0 = crc16_get((u8 *)&foc_params, sizeof(foc_params) - 2);
	if (crc0 != foc_params.crc16) {
		sys_debug("fp 0 error\n");
		fmc_read_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
		crc0 = crc16_get((u8 *)&foc_params, sizeof(foc_params) - 2);
		if (crc0 != foc_params.crc16) {
			nv_default_foc_params();
			nv_save_foc_params();
			return;
		}
		fmc_write_data(focParam_idx_0, (u8 *)&foc_params, sizeof(foc_params));
	}else {
		fmc_read_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
		crc0 = crc16_get((u8 *)&foc_params, sizeof(foc_params) - 2);
		if (crc0 != foc_params.crc16) {
			sys_debug("fp 1 error\n");
			fmc_read_data(focParam_idx_0, (u8 *)&foc_params, sizeof(foc_params));
			fmc_write_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
		}
	}
}

void nv_set_pid(u8 id, pid_conf_t *pid) {
	foc_params.pid_conf[id] = *pid;
	nv_save_foc_params();
}

void nv_get_pid(u8 id, pid_conf_t *pid) {
	*pid = foc_params.pid_conf[id];
}

void nv_set_hwbrake_mode(u8 mode) {
	foc_params.n_brkShutPower = mode;
}

void nv_set_throttle_vol(float min, float max) {
	foc_params.n_minThroVol = min;
	foc_params.n_maxThroVol = max;
}

void nv_set_ebrake_current(float phase_curr, float dc_curr) {
	foc_params.s_PhaseCurreBrkLim = phase_curr;
	foc_params.s_iDCeBrkLim = dc_curr;
}

void nv_storage_init(void) {
	nv_read_motor_params();
	nv_read_foc_params();
	sys_debug("encoder_off = %f\n", m_params.offset);
	nv_default_motor_params();
	nv_default_foc_params();
	nv_save_foc_params();
	nv_save_motor_params();
}