#include "app/nv_storage.h"
#include "bsp/fmc_flash.h"
#include "libs/crc16.h"
#include "foc/motor/motor_param.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_hall_offset(s16 offset) {
	m_params.hall_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.encoder_cpr = 4096;
	m_params.est_pll_band = 200;
	m_params.flux_linkage = 0.0f;
	m_params.hall_offset = 0;
}

static void nv_default_foc_params(void) {
	foc_params.s_maxvDC = 16;
	foc_params.s_maxiDC = 10;
	foc_params.s_maxIdq = 20;
	foc_params.s_minIdq = -20;
	foc_params.s_maxRPM = 8200;
	foc_params.s_maxTorque = 15;
	foc_params.n_currentBand = 500;
	foc_params.n_modulation = 1.0f;
	foc_params.n_PhaseFilterCeof = 0.2f;
	foc_params.spd_kp = 0.001f;
	foc_params.spd_ki = 0.01;
	foc_params.trq_kp = 0.001f;
	foc_params.trq_ki = 0.01;
	foc_params.fw_kp = 0.1f;
	foc_params.fw_ki = 0.01;
	foc_params.fw_baseSpd = 5000;
}

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) {
		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_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) {
		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_write_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
	}
}


void nv_storage_init(void) {
	nv_read_motor_params();
	nv_read_foc_params();
}