#include "ParaMge_F4.h"
#include "BMCL_Config.h"
#include "BMCL_crcLib.h"

#include "stmflashF4.h"
#include "stddef.h"
#include "stdlib.h"
#include "string.h"

// BMCL CONFIG variable
// uint32_t BMCL_mot_Calibration = BMCL_CONFIG_MOT_CAL_STA_EMPTY;

// _PARAMGE_CONFIG_AIO dataAIO_Ent = {0};
// _PARAMGE_CONFIG_AIO *dataAIO = &dataAIO_Ent;
_PARAMGE_CONFIG_AIO dataAIO = {0};

//=======CRC Funtion Pack
static uint16_t aioData_CRC_Check(_PARAMGE_CONFIG_AIO *aioData)
{
    // 校验时跳过校验位
    aioData = (uint32_t)aioData + offsetof(_PARAMGE_CONFIG_AIO, version);
    // 读取数据进行校验
    uint16_t dataCRC = BMCL_CONFIG_EEPROM_CRC_INIVAULE;
    dataCRC = crc16_xmodem_Update(aioData, sizeof(_PARAMGE_CONFIG_AIO) - offsetof(_PARAMGE_CONFIG_AIO, version), dataCRC);
    return dataCRC;
}

int Para_Init()
{
    uint32_t needToWriteCaliData = 0;

    memset(&dataAIO, 0x00, sizeof(_PARAMGE_CONFIG_AIO));
    // // 重置校准状态,如果未校准的枚举不是0,下面的语句会有用的
    // dataAIO.motComSetting[0].motDevSetting.mge_Calibration = BMCL_CONFIG_MGE_CAL_STA_EMPTY;
    // dataAIO.motComSetting[0].motDevSetting.mot_Calibration = BMCL_CONFIG_MOT_CAL_STA_EMPTY;
    paraMge_ReadALL();

    // ! 目前电机校准和磁编码器的校准是一起的,以电机校准标志为主
    // if (dataAIO->motDevSetting.mge_Calibration != BMCL_CONFIG_MGE_CAL_STA_CALED)
    // {
    switch (dataAIO.motComSetting[0].motDevSetting.mge_Calibration)
    {
    case BMCL_CONFIG_MOT_CAL_STA_EMPTY:
        // NOTE: 下面是校准流程,开机不校准,故注释
        // motor_PreLoadALL(); // Input the PreSet , then to cali the motor & mge
        // motor_ParaReadALL(&dataAIO);
        // TODO: 先注释,开机先不校准
        // motor_Align_Sensor(); // 开始校准
        // // get the cali info
        // dataAIO.motComSetting[0].motDevSetting.mge_direction = mge_direction;
        // dataAIO.motComSetting[0].motDevSetting.mge_absOffset = mge_Get_abOffset();
        // dataAIO.motComSetting[0].motDevSetting.mge_mot_eleAngleOffset = motor_Get_ZeroEAngleOffset();
        // dataAIO.motComSetting[0].motDevSetting.mge_Calibration = BMCL_CONFIG_MGE_CAL_STA_CALED;
        // dataAIO.motComSetting[0].motDevSetting.mot_Calibration = BMCL_CONFIG_MOT_CAL_STA_CALS1;
        // dataAIO.version = BMCL_CONFIG_VERSION;
        // needToWriteCaliData = 1;
        BMCL_mot_Calibration = BMCL_CONFIG_MOT_CAL_STA_EMPTY;
        break;
    case BMCL_CONFIG_MOT_CAL_STA_CALS1:
        BMCL_mot_Calibration = BMCL_CONFIG_MOT_CAL_STA_CALS1;
        // mge_direction = dataAIO.motComSetting[0].motDevSetting.mge_direction;
        // mge_Set_abOffset(dataAIO.motComSetting[0].motDevSetting.mge_absOffset);
        // motor_Set_ZeroEAngleOffset(dataAIO.motComSetting[0].motDevSetting.mge_mot_eleAngleOffset);
        break;
    case BMCL_CONFIG_MOT_CAL_STA_CALS2:
        BMCL_mot_Calibration = BMCL_CONFIG_MOT_CAL_STA_CALS2;
        // mge_direction = dataAIO.motComSetting[0].motDevSetting.mge_direction;
        // mge_Set_abOffset(dataAIO.motComSetting[0].motDevSetting.mge_absOffset);
        // motor_Set_ZeroEAngleOffset(dataAIO.motComSetting[0].motDevSetting.mge_mot_eleAngleOffset);
        // motor_LoadALL(&dataAIO);
        break;
    }
    // }

    // Check the dataAIO if need write to the flash
    // if (needToWriteCaliData == 1)
    // { // need to do it
        // verify the data
        // paraMge_SaveALL();
    // }
    // free(dataAIO);
    return BMCL_mot_Calibration;
}

/// @brief paraMge 内部函数 返回指定电机的电机公共模板结构体的指针
/// @param MOT __PARAMGE_INDEX_MOT枚举，选择电机
/// @return 返回结构体指针，NULL - 输入非法
static _PARAMGE_CONFIG_MOTCOM *Get_SelectedMOT_4MOTCOM(__PARAMGE_INDEX_MOT MOT)
{
    switch (MOT)
    {
    case __PARAMGE_INDEX_MOT_P:
        return &(dataAIO.motComSetting[0]);
        break;
    case __PARAMGE_INDEX_MOT_R:
        return &(dataAIO.motComSetting[1]);
        break;
    case __PARAMGE_INDEX_MOT_Y:
        return &(dataAIO.motComSetting[2]);
        break;
    default:
        return NULL;
        break;
    }
}

/**
 * @brief paraMge 获取GenPID的参数
 * @param[in] index GenPID索引 : 要获取的GenPID的索引
 * @param[out] dest GenPID模板指针 : 返回获取的GenPID
 * @return 返回读取结果状态
 */
int paraMge_GetGenPID(__PARAMGE_INDEX_GENPID index, __PARAMGE_TEMP_GENPID *dest, __PARAMGE_INDEX_MOT MOT)
{
    _PARAMGE_CONFIG_MOTCOM *motComSetting = NULL;
    motComSetting = Get_SelectedMOT_4MOTCOM(MOT);
    if (motComSetting == NULL)
        return -1;

    switch (index)
    {
    case __PARAMGE_INDEX_GENPID_ANGPID:
        dest->config = motComSetting->angle_PID.config;
        break;
    case __PARAMGE_INDEX_GENPID_VELPID:
        dest->config = motComSetting->Vel_PID.config;
        break;
    default:
        return -1;
        break;
    }
}

/**
 * @brief paraMge 写入GenPID的参数,该操作不写入Flash中,仅缓存
 * @param index GenPID索引 : 要写入的GenPID的索引
 * @param src GenPID模板指针 : 写入的GenPID的指针
 * @return 返回写入结果状态
 */
int paraMge_WriteGenPID(__PARAMGE_INDEX_GENPID index, __PARAMGE_TEMP_GENPID *src, __PARAMGE_INDEX_MOT MOT)
{
    _PARAMGE_CONFIG_MOTCOM *motComSetting = NULL;
    motComSetting = Get_SelectedMOT_4MOTCOM(MOT);
    if (motComSetting == NULL)
        return -1;

    switch (index)
    {
    case __PARAMGE_INDEX_GENPID_ANGPID:
        motComSetting->angle_PID.config = src->config;
        break;
    case __PARAMGE_INDEX_GENPID_VELPID:
        motComSetting->Vel_PID.config = src->config;
        break;
    default:
        return -1;
        break;
    }
}

int paraMge_GetKPF(__PARAMGE_INDEX_KPF index, __PARAMGE_TEMP_KPF *dest, __PARAMGE_INDEX_MOT MOT)
{
    _PARAMGE_CONFIG_MOTCOM *motComSetting = NULL;
    motComSetting = Get_SelectedMOT_4MOTCOM(MOT);
    if (motComSetting == NULL)
        return -1;

    switch (index)
    {
    case __PARAMGE_INDEX_KPF_VEL:
        dest->config = motComSetting->kfp_velP.config;
        break;
    case __PARAMGE_INDEX_KPF_ANG:
        dest->config = motComSetting->kfp_angP.config;
        break;
    case __PARAMGE_INDEX_KPF_SSMQ:
        dest->config = motComSetting->kfp_ssmq.config;
        break;
    case __PARAMGE_INDEX_KPF_PMOTANG:
        dest->config = motComSetting->kfp_motAngP.config;
        break;
    default:
        return -1;
        break;
    }
}

int paraMge_WriteGenKPF(__PARAMGE_INDEX_KPF index, __PARAMGE_TEMP_KPF *src, __PARAMGE_INDEX_MOT MOT)
{
    _PARAMGE_CONFIG_MOTCOM *motComSetting = NULL;
    motComSetting = Get_SelectedMOT_4MOTCOM(MOT);
    if (motComSetting == NULL)
        return -1;

    switch (index)
    {
    case __PARAMGE_INDEX_KPF_VEL:
        motComSetting->kfp_velP.config = src->config;
        break;
    case __PARAMGE_INDEX_KPF_ANG:
        motComSetting->kfp_angP.config = src->config;
        break;
    case __PARAMGE_INDEX_KPF_SSMQ:
        motComSetting->kfp_ssmq.config = src->config;
        break;
    case __PARAMGE_INDEX_KPF_PMOTANG:
        motComSetting->kfp_motAngP.config = src->config;
        break;
    default:
        return -1;
        break;
    }
}

int paraMge_GetINIF(__PARAMGE_INDEX_INIF index, __PARAMGE_TEMP_INTF *dest, __PARAMGE_INDEX_MOT MOT)
{
    _PARAMGE_CONFIG_MOTCOM *motComSetting = NULL;
    motComSetting = Get_SelectedMOT_4MOTCOM(MOT);
    if (motComSetting == NULL)
        return -1;

    switch (index)
    {
    case __PARAMGE_INDEX_INTF_0:
        dest->inter_Range = motComSetting->interFilter.inter_Range;
        break;
    case __PARAMGE_INDEX_INTF_1:
        dest->inter_Range = motComSetting->interFilter1.inter_Range;
        break;
    default:
        return -1;
        break;
    }
}

int paraMge_WriteINIF(__PARAMGE_INDEX_INIF index, __PARAMGE_TEMP_INTF *src, __PARAMGE_INDEX_MOT MOT)
{
    _PARAMGE_CONFIG_MOTCOM *motComSetting = NULL;
    motComSetting = Get_SelectedMOT_4MOTCOM(MOT);
    if (motComSetting == NULL)
        return -1;

    switch (index)
    {
    case __PARAMGE_INDEX_INTF_0:
        motComSetting->interFilter.inter_Range = src->inter_Range;
        break;
    case __PARAMGE_INDEX_INTF_1:
        motComSetting->interFilter1.inter_Range = src->inter_Range;
        break;
    default:
        return -1;
        break;
    }
}

int paraMge_GetMOT(__PARAMGE_TEMP_MOT *dest, __PARAMGE_INDEX_MOT MOT)
{
    _PARAMGE_CONFIG_MOTCOM *motComSetting = NULL;
    motComSetting = Get_SelectedMOT_4MOTCOM(MOT);
    if (motComSetting == NULL)
        return -1;

    dest->mge_Calibration = motComSetting->motDevSetting.mge_Calibration;
    dest->mge_direction = motComSetting->motDevSetting.mge_direction;
    dest->mge_absOffset = motComSetting->motDevSetting.mge_absOffset;
    dest->mge_mot_eleAngleOffset = motComSetting->motDevSetting.mge_mot_eleAngleOffset;

    dest->mot_Calibration = motComSetting->motDevSetting.mot_Calibration;
    dest->mot_FOC_Udc = motComSetting->motDevSetting.mot_FOC_Udc;
    dest->mot_FOC_ULimitOnUdc = motComSetting->motDevSetting.mot_FOC_ULimitOnUdc;
    dest->mot_FOC_PP = motComSetting->motDevSetting.mot_FOC_PP;
    dest->mot_LimitAngle_S = motComSetting->motDevSetting.mot_LimitAngle_S;
    dest->mot_LimitAngle_B = motComSetting->motDevSetting.mot_LimitAngle_B;
    dest->mot_CenterAngle = motComSetting->motDevSetting.mot_CenterAngle;
}

int paraMge_WriteMOT(__PARAMGE_TEMP_MOT *src, __PARAMGE_INDEX_MOT MOT)
{
    _PARAMGE_CONFIG_MOTCOM *motComSetting = NULL;
    motComSetting = Get_SelectedMOT_4MOTCOM(MOT);
    if (motComSetting == NULL)
        return -1;

    motComSetting->motDevSetting.mge_Calibration = src->mge_Calibration;
    motComSetting->motDevSetting.mge_direction = src->mge_direction;
    motComSetting->motDevSetting.mge_absOffset = src->mge_absOffset;
    motComSetting->motDevSetting.mge_mot_eleAngleOffset = src->mge_mot_eleAngleOffset;

    motComSetting->motDevSetting.mot_Calibration = src->mot_Calibration;
    motComSetting->motDevSetting.mot_FOC_Udc = src->mot_FOC_Udc;
    motComSetting->motDevSetting.mot_FOC_ULimitOnUdc = src->mot_FOC_ULimitOnUdc;
    motComSetting->motDevSetting.mot_FOC_PP = src->mot_FOC_PP;
    motComSetting->motDevSetting.mot_LimitAngle_S = src->mot_LimitAngle_S;
    motComSetting->motDevSetting.mot_LimitAngle_B = src->mot_LimitAngle_B;
    motComSetting->motDevSetting.mot_CenterAngle = src->mot_CenterAngle;
}

static __PARAMGE_DEF_STORAGE_STA paraMge_CrcVerify(uint32_t opADDR)
{
    uint16_t dataCRC = BMCL_CONFIG_EEPROM_CRC_INIVAULE;
    uint32_t opADDR_temp = opADDR;
    uint32_t dataCache;
    opADDR_temp += offsetof(_PARAMGE_CONFIG_AIO, version);
    while (opADDR_temp < (opADDR + sizeof(_PARAMGE_CONFIG_AIO)))
    {
        dataCache = stmflash_read_word(opADDR_temp);
        dataCRC = crc16_xmodem_Update(&dataCache, sizeof(uint32_t), dataCRC);
        opADDR_temp += sizeof(uint32_t);
    }
    uint32_t crcSum_P = offsetof(_PARAMGE_CONFIG_AIO, crcSum) + opADDR;
    uint16_t flashCRC = (uint16_t)stmflash_read_word(crcSum_P);
    if (dataCRC == flashCRC)
    {
        return __PARAMGE_DEF_STORAGE_STA_VAILD; // 储存有效
    }
    else
    {
        return __PARAMGE_DEF_STORAGE_STA_NONE; // 没有储存
    }
}

/**
 * @brief paraMge 从Flash中读取到缓冲区
 * @return 读取状态
 */
__PARAMGE_DEF_STORAGE_STA paraMge_ReadALL()
{
    uint32_t opADDR1 = BMCL_CONFIG_EEPROM_ADDR;
    uint32_t opADDR2 = BMCL_CONFIG_EEPROM_ADDR2;
    __PARAMGE_DEF_STORAGE_STA SavedState1 = paraMge_CrcVerify(opADDR1);
    __PARAMGE_DEF_STORAGE_STA SavedState2 = paraMge_CrcVerify(opADDR2);
    if ((SavedState1 == __PARAMGE_DEF_STORAGE_STA_VAILD) && (SavedState2 == __PARAMGE_DEF_STORAGE_STA_VAILD))
    {
        if (paraMge_GetSaveTimes(opADDR1) >= paraMge_GetSaveTimes(opADDR2))
        {
            // 数据有效,将数据存入大结构体
            stmflash_read(opADDR1, &dataAIO, sizeof(_PARAMGE_CONFIG_AIO) / sizeof(uint32_t));
            return __PARAMGE_DEF_STORAGE_STA_VAILD; // 储存有效
        }
        else
        {
            // 数据有效,将数据存入大结构体
            stmflash_read(opADDR2, &dataAIO, sizeof(_PARAMGE_CONFIG_AIO) / sizeof(uint32_t));
            return __PARAMGE_DEF_STORAGE_STA_VAILD; // 储存有效
        }
    }
    else if (SavedState1 == __PARAMGE_DEF_STORAGE_STA_VAILD)
    {
        // 数据1有效,将数据存入大结构体
        stmflash_read(opADDR1, &dataAIO, sizeof(_PARAMGE_CONFIG_AIO) / sizeof(uint32_t));
        return __PARAMGE_DEF_STORAGE_STA_VAILD; // 储存有效
    }
    else if (SavedState2 == __PARAMGE_DEF_STORAGE_STA_VAILD)
    {
        // 数据2有效,将数据存入大结构体
        stmflash_read(opADDR2, &dataAIO, sizeof(_PARAMGE_CONFIG_AIO) / sizeof(uint32_t));
        return __PARAMGE_DEF_STORAGE_STA_VAILD; // 储存有效
    }
    else
    {
        // 数据无效,重置校准状态为未校准
        memset(&dataAIO, 0x00, sizeof(__PARAMGE_TEMP_MOT));
        dataAIO.motComSetting[0].motDevSetting.mge_Calibration = BMCL_CONFIG_MGE_CAL_STA_EMPTY;
        dataAIO.motComSetting[0].motDevSetting.mot_Calibration = BMCL_CONFIG_MOT_CAL_STA_EMPTY;
        // motor_PreLoadALL();                    // 载入默认值
        return __PARAMGE_DEF_STORAGE_STA_NONE; // 没有储存
    }
}

/**
 * @brief paraMge 从缓冲区写入到Flash中
 * @return 返回写入保存状态
 */
__PARAMGE_DEF_SAVE_STA paraMge_SaveALL()
{
    uint32_t opADDR1 = BMCL_CONFIG_EEPROM_ADDR;
    uint32_t opADDR2 = BMCL_CONFIG_EEPROM_ADDR2;

    __PARAMGE_DEF_STORAGE_STA SavedState1 = paraMge_CrcVerify(opADDR1);
    __PARAMGE_DEF_STORAGE_STA SavedState2 = paraMge_CrcVerify(opADDR2);
    if ((SavedState1 == __PARAMGE_DEF_STORAGE_STA_VAILD) && (SavedState2 == __PARAMGE_DEF_STORAGE_STA_VAILD))
    {
        if (paraMge_GetSaveTimes(opADDR1) < paraMge_GetSaveTimes(opADDR2))
        {
            // 数据1更旧,则将数据存入区域1
            dataAIO.SaveTimes = paraMge_GetSaveTimes(opADDR2) + 1;
            dataAIO.version = BMCL_CONFIG_VERSION;
            dataAIO.crcSum = aioData_CRC_Check(&dataAIO);
            stmflash_write(opADDR1, &dataAIO, sizeof(_PARAMGE_CONFIG_AIO) / sizeof(uint32_t));
            if (paraMge_CrcVerify(opADDR1))
            {
                return __PARAMGE_DEF_SAVE_STA_SUCCEED; // 保存成功
            }
            else
            {
                dataAIO.SaveTimes = paraMge_GetSaveTimes(opADDR2);
                return __PARAMGE_DEF_SAVE_STA_FAIL;
            }
        }
        else
        {
            //  数据2更旧,则将数据存入区域2
            dataAIO.SaveTimes = paraMge_GetSaveTimes(opADDR1) + 1;
            dataAIO.version = BMCL_CONFIG_VERSION;
            dataAIO.crcSum = aioData_CRC_Check(&dataAIO);
            stmflash_write(opADDR2, &dataAIO, sizeof(_PARAMGE_CONFIG_AIO) / sizeof(uint32_t));
            if (paraMge_CrcVerify(opADDR2))
            {
                return __PARAMGE_DEF_SAVE_STA_SUCCEED; // 保存成功
            }
            else
            {
                dataAIO.SaveTimes = paraMge_GetSaveTimes(opADDR1);
                return __PARAMGE_DEF_SAVE_STA_FAIL;
            }
        }
    }
    else if (SavedState1 == __PARAMGE_DEF_STORAGE_STA_VAILD)
    {
        // 仅数据1有效,将数据存入区域2
        dataAIO.SaveTimes = paraMge_GetSaveTimes(opADDR1) + 1;
        dataAIO.version = BMCL_CONFIG_VERSION;
        dataAIO.crcSum = aioData_CRC_Check(&dataAIO);
        stmflash_write(opADDR2, &dataAIO, sizeof(_PARAMGE_CONFIG_AIO) / sizeof(uint32_t));
        if (paraMge_CrcVerify(opADDR2))
        {
            return __PARAMGE_DEF_SAVE_STA_SUCCEED; // 保存成功
        }
        else
        {
            dataAIO.SaveTimes = paraMge_GetSaveTimes(opADDR1);
            return __PARAMGE_DEF_SAVE_STA_FAIL;
        }
    }
    else if (SavedState2 == __PARAMGE_DEF_STORAGE_STA_VAILD)
    {
        // 仅数据2有效,将数据存入区域1
        dataAIO.SaveTimes = paraMge_GetSaveTimes(opADDR2) + 1;
        dataAIO.version = BMCL_CONFIG_VERSION;
        dataAIO.crcSum = aioData_CRC_Check(&dataAIO);
        stmflash_write(opADDR1, &dataAIO, sizeof(_PARAMGE_CONFIG_AIO) / sizeof(uint32_t));
        if (paraMge_CrcVerify(opADDR1))
        {
            return __PARAMGE_DEF_SAVE_STA_SUCCEED; // 保存成功
        }
        else
        {
            dataAIO.SaveTimes = paraMge_GetSaveTimes(opADDR2);
            return __PARAMGE_DEF_SAVE_STA_FAIL;
        }
    }
    else
    {
        // 均无效则先将数据存入区域1
        dataAIO.SaveTimes = 1;
        dataAIO.version = BMCL_CONFIG_VERSION;
        dataAIO.crcSum = aioData_CRC_Check(&dataAIO);
        stmflash_write(opADDR1, &dataAIO, sizeof(_PARAMGE_CONFIG_AIO) / sizeof(uint32_t));
        if (paraMge_CrcVerify(opADDR1))
        {
            return __PARAMGE_DEF_SAVE_STA_SUCCEED; // 保存成功
        }
        else
        {
            dataAIO.SaveTimes = 0;
            return __PARAMGE_DEF_SAVE_STA_FAIL;
        }
    }
}

/**
 * @brief paraMge 返回保存次数
 * @return 保存次数
 */
uint32_t paraMge_GetSaveTimes(uint32_t opADDR)
{
    uint32_t flashSaveTimes, SaveTimes_P;
    SaveTimes_P = offsetof(_PARAMGE_CONFIG_AIO, SaveTimes) + opADDR;
    flashSaveTimes = (uint16_t)stmflash_read_word(SaveTimes_P);
    return flashSaveTimes;
}