/*
 * control_table.c
 *
 * Mapping various parameters as servo motors to addresses.
 *
 *  Created on: 2023/09/26
 *      Author: BestTechnology
 */

#include "hal_data.h"

#include <string.h>
#include <usart.h>

//#define DBG_TRACE
#include "trace.h"

#include "control_table.h"
#include "global_variables.h"

#define NVM_HEAD    BSP_FEATURE_FLASH_DATA_FLASH_START

#define sizeofrec(d)      (sizeof(d) / sizeof(d[0]))


//-----------------------------
// Control table
//-----------------------------

// Actual control table
TControlTable ControlTable;

// Initial value of control table value
const TControlTable Default_ControlTable = {
    ID: 1,
    Baudrate: br_1M,  //br_115200,
    OffsetAngle: 0,
    ReturnDelayTime: 10,
    AngleCtrlGain: {2000,5000,0,1},
    VelocityCtrlGain: {1100,1500,100,32},
    CurrentCtrlGain: {900,1500,0,2},

    AngleLimit: { LONG_MIN, LONG_MAX },
    AngleFlexibility: { -0,-0,0,0 },
    CurrentFlexibility: { -0,-0,0,0 },

    AngleCorrection: {0, 1000, 2000, 3000, 4000, 5000, 6000, 7000, 8000, 9000,10000,11000,12000,13000,14000,15000,16000,17000,18000,19000,20000,21000,22000,23000,24000,25000,26000,27000,28000,29000,30000,31000,32000,33000,34000,35000,35999},

    MonInfo: {{-1,0,0,0,0,0,0}, {-1,0,0,0,0,0,0}, {-1,0,0,0,0,0,0}, {-1,0,0,0,0,0,0}, {-1,0,0,0,0,0,0}},
};


// Indexes in the control table for users point to elements of this structure.
const TItemInfo ItemInfo[] = {
    // 0
    { &ControlTable.ID,                               dt_uint8,  false, true,  0, 254 },
    { &ControlTable.Baudrate,                         dt_uint8,  false, true,  0, 15 },
    { &ControlTable.OffsetAngle,                      dt_int32,  false, true,  -36000, 36000 },
    { &ControlTable.LED,                              dt_uint8,  false, false, 0, UCHAR_MAX },
    { &ControlTable.ReturnDelayTime,                  dt_uint16, false, false, 0, 1000 },

    // 5
    { &ControlTable.SystemStatus,                     dt_uint32, true,  false, 0, ULONG_MAX },

    // 6
    { &ControlTable.AngleFlexibility.n_compliance,    dt_int32,  false, true, -36000, 0 },
    { &ControlTable.AngleFlexibility.n_deadzone,      dt_int32,  false, true, -36000, 0 },
    { &ControlTable.AngleFlexibility.p_deadzone,      dt_int32,  false, true, 0, 36000 },
    { &ControlTable.AngleFlexibility.p_compliance,    dt_int32,  false, true, 0, 36000 },

    // 10
    { &ControlTable.CurrentFlexibility.n_compliance,  dt_int32,  false, true, -1300, 0 },
    { &ControlTable.CurrentFlexibility.n_deadzone,    dt_int32,  false, true, -1300, 0 },
    { &ControlTable.CurrentFlexibility.p_deadzone,    dt_int32,  false, true, 0, 1300 },
    { &ControlTable.CurrentFlexibility.p_compliance,  dt_int32,  false, true, 0, 1300 },

    // 14
    { 0, dt_uint32, false, false, 0, 0 },
    { 0, dt_uint32, false, false, 0, 0 },
    { 0, dt_uint32, false, false, 0, 0 },
    { 0, dt_uint32, false, false, 0, 0 },
    // 18
    { &ControlTable.AngleLimit.min,                   dt_int32, false, true, LONG_MIN, LONG_MAX },
    { &ControlTable.AngleLimit.max,                   dt_int32, false, true, LONG_MIN, LONG_MAX },

    // 20
    { &ControlTable.AngleCtrlGain.Kp,                 dt_int32,  false, true,  0, 10000 },
    { &ControlTable.AngleCtrlGain.Ki,                 dt_int32,  false, true,  0, 10000 },
    { &ControlTable.AngleCtrlGain.Kd,                 dt_int32,  false, true,  0, 10000 },
    { &ControlTable.AngleCtrlGain.delta_t,            dt_int32,  false, true,  1, 1000 },
    { &ControlTable.VelocityCtrlGain.Kp,              dt_int32,  false, true,  0, 10000 },
    { &ControlTable.VelocityCtrlGain.Ki,              dt_int32,  false, true,  0, 10000 },
    { &ControlTable.VelocityCtrlGain.Kd,              dt_int32,  false, true,  0, 10000 },
    { &ControlTable.VelocityCtrlGain.delta_t,         dt_int32,  false, true,  1, 1000 },
    { &ControlTable.CurrentCtrlGain.Kp,               dt_int32,  false, true,  0, 10000 },
    { &ControlTable.CurrentCtrlGain.Ki,               dt_int32,  false, true,  0, 10000 },
    { &ControlTable.CurrentCtrlGain.Kd,               dt_int32,  false, true,  0, 10000 },
    { &ControlTable.CurrentCtrlGain.delta_t,          dt_int32,  false, true,  1, 1000 },

    // 32
    { &ControlTable.TransitionMode,                   dt_uint8,  false, false,  0, 3 },
    { &ControlTable.TransitionTime,                   dt_int32,  false, false,  0, 300000 },

    // 34
    { &ControlTable.Excitation,                       dt_uint8,  false, false, 0, 15 },

    // 35
    { &ControlTable.TargetAngle,                      dt_int32,  false, false, LONG_MIN, LONG_MAX },
    { &ControlTable.TargetVelocity,                   dt_int32,  false, false, -2000, 2000 },
    { &ControlTable.TargetCurrent,                    dt_int32,  false, false, -1300, 1300 },
    { &ControlTable.TargetPWM,                        dt_int32,  false, false, -1000, 1000 },
    { 0, dt_uint32, false, false, 0, 0 },

    // 40
    { &ControlTable.CurrentAngle,                     dt_int32,  true,  false, 0, 0 },
    { &ControlTable.CurrentVelocity,                  dt_int32,  true,  false, 0, 0 },
    { &ControlTable.CurrentCurrent,                   dt_int32,  true,  false, 0, 0 },
    { &ControlTable.CurrentPWM,                       dt_int32,  true,  false, 0, 0 },
    { &ControlTable.CurrentRawPosition,               dt_uint16, true,  false, 0, 0 },

    // 45
    { &ControlTable.CurrentBusVoltage,                dt_uint16, true,  false, 0, 0 },
    { &ControlTable.CurrentMPUTemp,                   dt_uint16, true,  false, 0, 0 },
    { &ControlTable.CurrentAMPTemp,                   dt_uint16, true,  false, 0, 0 },
    { &ControlTable.CurrentMotorTemp,                 dt_uint16, true,  false, 0, 0 },
    { &ControlTable.AngleViation,                     dt_int32,  true,  false, 0, 0 },
    { &ControlTable.VelocityViation,                  dt_int32,  true,  false, 0, 0 },
    { &ControlTable.CurrentViation,                   dt_int32,  true,  false, 0, 0 },

    { &ControlTable.SysTickTimee,                     dt_uint32, true,  false, 0, 0 },
    { &ControlTable.ControlDurationTime,              dt_uint32, true,  false, 0, 0 },
    { &ControlTable.NoCommTime,                       dt_uint32, true,  false, 0, 0 },

    // 55
    { &ControlTable.AngleCorrection[0],               dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[1],               dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[2],               dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[3],               dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[4],               dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[5],               dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[6],               dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[7],               dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[8],               dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[9],               dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[10],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[11],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[12],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[13],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[14],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[15],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[16],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[17],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[18],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[19],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[20],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[21],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[22],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[23],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[24],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[25],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[26],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[27],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[28],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[29],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[30],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[31],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[32],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[33],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[34],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[35],              dt_uint16, false, true,  0, 36000 },
    { &ControlTable.AngleCorrection[36],              dt_uint16, false, true,  0, 36000 },

    // 92
    { &ControlTable.MonInfo[0].ItemIndex,             dt_int16,  false, true,  -1, 91 },
    { &ControlTable.MonInfo[0].min,                   dt_int32,  false, true,  LONG_MIN, LONG_MAX },
    { &ControlTable.MonInfo[0].max,                   dt_int32,  false, true,  LONG_MIN, LONG_MAX },
    { &ControlTable.MonInfo[0].time,                  dt_uint32, false, true,  0, ULONG_MAX },
    { &ControlTable.MonInfo[0].DetectMode,            dt_uint8,  false, true,  0, UCHAR_MAX },
    { &ControlTable.MonInfo[0].ProcMode,              dt_uint8,  false, true,  0, UCHAR_MAX },
    { &ControlTable.MonInfo[0].DurationTime,          dt_uint32, true,  false, 0, ULONG_MAX },

    { &ControlTable.MonInfo[1].ItemIndex,             dt_int16,  false, true,  -1, 91 },
    { &ControlTable.MonInfo[1].min,                   dt_int32,  false, true,  LONG_MIN, LONG_MAX },
    { &ControlTable.MonInfo[1].max,                   dt_int32,  false, true,  LONG_MIN, LONG_MAX },
    { &ControlTable.MonInfo[1].time,                  dt_uint32, false, true,  0, ULONG_MAX },
    { &ControlTable.MonInfo[1].DetectMode,            dt_uint8,  false, true,  0, UCHAR_MAX },
    { &ControlTable.MonInfo[1].ProcMode,              dt_uint8,  false, true,  0, UCHAR_MAX },
    { &ControlTable.MonInfo[1].DurationTime,          dt_uint32, true,  false, 0, ULONG_MAX },

    { &ControlTable.MonInfo[2].ItemIndex,             dt_int16,  false, true,  -1, 91 },
    { &ControlTable.MonInfo[2].min,                   dt_int32,  false, true,  LONG_MIN, LONG_MAX },
    { &ControlTable.MonInfo[2].max,                   dt_int32,  false, true,  LONG_MIN, LONG_MAX },
    { &ControlTable.MonInfo[2].time,                  dt_uint32, false, true,  0, ULONG_MAX },
    { &ControlTable.MonInfo[2].DetectMode,            dt_uint8,  false, true,  0, UCHAR_MAX },
    { &ControlTable.MonInfo[2].ProcMode,              dt_uint8,  false, true,  0, UCHAR_MAX },
    { &ControlTable.MonInfo[2].DurationTime,          dt_uint32, true,  false, 0, ULONG_MAX },

    { &ControlTable.MonInfo[3].ItemIndex,             dt_int16,  false, true,  -1, 91 },
    { &ControlTable.MonInfo[3].min,                   dt_int32,  false, true,  LONG_MIN, LONG_MAX },
    { &ControlTable.MonInfo[3].max,                   dt_int32,  false, true,  LONG_MIN, LONG_MAX },
    { &ControlTable.MonInfo[3].time,                  dt_uint32, false, true,  0, ULONG_MAX },
    { &ControlTable.MonInfo[3].DetectMode,            dt_uint8,  false, true,  0, UCHAR_MAX },
    { &ControlTable.MonInfo[3].ProcMode,              dt_uint8,  false, true,  0, UCHAR_MAX },
    { &ControlTable.MonInfo[3].DurationTime,          dt_uint32, true,  false, 0, ULONG_MAX },

    { &ControlTable.MonInfo[4].ItemIndex,             dt_int16,  false, true,  -1, 91 },
    { &ControlTable.MonInfo[4].min,                   dt_int32,  false, true,  LONG_MIN, LONG_MAX },
    { &ControlTable.MonInfo[4].max,                   dt_int32,  false, true,  LONG_MIN, LONG_MAX },
    { &ControlTable.MonInfo[4].time,                  dt_uint32, false, true,  0, ULONG_MAX },
    { &ControlTable.MonInfo[4].DetectMode,            dt_uint8,  false, true,  0, UCHAR_MAX },
    { &ControlTable.MonInfo[4].ProcMode,              dt_uint8,  false, true,  0, UCHAR_MAX },
    { &ControlTable.MonInfo[4].DurationTime,          dt_uint32, true,  false, 0, ULONG_MAX },
};

// Write control table back to default values
bool RestoreControlTable2FactoryDefaults (void) {
    __disable_irq();
    memcpy (&ControlTable, &Default_ControlTable, sizeof(TControlTable));
    __enable_irq();
    return SaveCcontroltableToNVM ();
}

//typedef void (*twitm_fp)(int index, int data);
bool write_item (uint16_t idx, int data, twitm_fp fp) {
    if (idx >= sizeofrec(ItemInfo)) return false;
    if (ItemInfo[idx].readonly) return false;
    if (ItemInfo[idx].offset != NULL) {
        switch (ItemInfo[idx].dtype) {
            case dt_int8:
                if ((int8_t)data > (int8_t)ItemInfo[idx].max || (int8_t)data < (int8_t)ItemInfo[idx].min) return false;
                *(int8_t *)ItemInfo[idx].offset = (int8_t)data;
                break;
            case dt_uint8:
                if ((uint8_t)data > (uint8_t)ItemInfo[idx].max || (uint8_t)data < (uint8_t)ItemInfo[idx].min) return false;
                *(uint8_t *)ItemInfo[idx].offset = (uint8_t)data;
                break;
            case dt_int16:
                if ((int16_t)data > (int16_t)ItemInfo[idx].max || (int16_t)data < (int16_t)ItemInfo[idx].min) return false;
                *(int16_t *)ItemInfo[idx].offset = (int16_t)data;
                break;
            case dt_uint16:
                if ((uint16_t)data > (uint16_t)ItemInfo[idx].max || (uint16_t)data < (uint16_t)ItemInfo[idx].min) return false;
                *(uint16_t *)ItemInfo[idx].offset = (uint16_t)data;
                break;
            case dt_int32:
                if ((int32_t)data > (int32_t)ItemInfo[idx].max || (int32_t)data < (int32_t)ItemInfo[idx].min) return false;
                *(int32_t *)ItemInfo[idx].offset = (int32_t)data;
                // !!!!!!!!!!!!!!!!!
                if (idx ==_ITEM_IDX_TARGETANGLE) UpdateTargetValue = true;
                // !!!!!!!!!!!!!!!!!
                break;
            case dt_uint32:
                if ((uint32_t)data > (uint32_t)ItemInfo[idx].max || (uint32_t)data < (uint32_t)ItemInfo[idx].min) return false;
                *(uint32_t *)ItemInfo[idx].offset = (uint32_t)data;
                break;
        }
        if (fp != NULL) fp (idx, data);
    }
    return true;
}

bool read_item (uint16_t idx, Tdatatype *dtype, int32_t *data) {
    if (idx >= sizeofrec(ItemInfo)) return false;
    if (ItemInfo[idx].offset != NULL){
        switch ((*dtype = ItemInfo[idx].dtype)) {
            case dt_int8:
                *data = (int32_t)*(int8_t *)ItemInfo[idx].offset;
                break;
            case dt_uint8:
                *data = (int32_t)*(uint8_t *)ItemInfo[idx].offset;
                break;
            case dt_int16:
                *data = (int32_t)*(int16_t *)ItemInfo[idx].offset;
                break;
            case dt_uint16:
                *data = (int32_t)*(uint16_t *)ItemInfo[idx].offset;
                break;
            case dt_int32:
                *data = (int32_t)*(int32_t *)ItemInfo[idx].offset;
                break;
            case dt_uint32:
                *data = (int32_t)*(uint32_t *)ItemInfo[idx].offset;
                break;
        }
    } else
        *data = 0;
    return true;
}

////////////////////////////////////////////////////////////
// Flash functions
////////////////////////////////////////////////////////////

// Although not particularly necessary to that extent, the CRC was used to determine if a legitimate value was written to DATAFLASH.

static bool DATAFLASH_Write (void *buf, uint32_t size) {
    bool result = false;
    uint32_t crc_c;
    uint8_t wb[size + 2];
    crc_input_t t = { size, 0, wb };
    fsp_err_t err;

DBG ("\nDATAFLASH_Write");
DBG ("\n size=%d",size);
    if (size <= BSP_FEATURE_FLASH_HP_DF_BLOCK_SIZE * 16) {
        if (R_FLASH_HP_Open (&g_flash0_ctrl, &g_flash0_cfg) == FSP_SUCCESS) {
            __disable_irq ();
            memcpy (wb, buf, size);
            __enable_irq ();
            R_CRC_Open (&g_crc0_ctrl, &g_crc0_cfg);
            R_CRC_Calculate (&g_crc0_ctrl, &t , &crc_c);
            R_CRC_Close (&g_crc0_ctrl);
DBG ("\n crc=%d",crc_c);
            *(uint16_t *)&wb[size] = (uint16_t)(crc_c & 0xffff);
DBG ("\n addr=$%08x",BSP_FEATURE_FLASH_DATA_FLASH_START);
            size+=2;
            __disable_irq ();
            result = ((err = R_FLASH_HP_Write (&g_flash0_ctrl, (uint32_t)wb, BSP_FEATURE_FLASH_DATA_FLASH_START, size)) == FSP_SUCCESS);
            __enable_irq ();
            R_FLASH_HP_Close (&g_flash0_ctrl);
DBG ("\n err=%d\n",err);
        }
    }
DBG (" %s\n", result ? "OK":"NG");

    return result;
}

static bool DATAFLASH_IsNormal (uint32_t size) {
    uint32_t crc_f = *(uint16_t *)(BSP_FEATURE_FLASH_DATA_FLASH_START + size);
    uint32_t crc_c;

DBG ("\nDATAFLASH_IsNormal");
    crc_input_t t = { size, 0, (void *)BSP_FEATURE_FLASH_DATA_FLASH_START };
    R_CRC_Open (&g_crc0_ctrl, &g_crc0_cfg);
    R_CRC_Calculate (&g_crc0_ctrl, &t , &crc_c);
    R_CRC_Close (&g_crc0_ctrl);
DBG("\n %d %d", crc_f,crc_c);
DBG("\n %s", (crc_f == crc_c)?"OK":"NG");

    return (crc_f == crc_c);
}

static bool DATAFLASH_Erase_1k (void) {
    bool result = false;

DBG("\nDATAFLASH_Erase_1k");
    if (R_FLASH_HP_Open (&g_flash0_ctrl, &g_flash0_cfg) == FSP_SUCCESS) {
        result = R_FLASH_HP_Erase (&g_flash0_ctrl, BSP_FEATURE_FLASH_DATA_FLASH_START, 16) == FSP_SUCCESS;
DBG("\n %s", result?"OK":"NG");
        R_FLASH_HP_Close (&g_flash0_ctrl);
    }

    return result;
}

// NVMing of control table
bool SaveCcontroltableToNVM (void) {
    bool result = false;
    if (DATAFLASH_Erase_1k ()) {
        result = DATAFLASH_Write ((void *)&ControlTable, sizeof (TControlTable));
    }
    return result;
}

// Reads NVM data into control table
bool LoadCcontroltableFromNVM (void) {
    if (DATAFLASH_IsNormal (sizeof(TControlTable))) {
        for (unsigned int i = 0; i < sizeofrec(ItemInfo); i++) {
            if (ItemInfo[i].nvm && ItemInfo[i].offset != NULL) {
                switch (ItemInfo[i].dtype) {
                    case dt_int8:
                        *(int8_t *)ItemInfo[i].offset = *(int8_t *)(NVM_HEAD + ((uint32_t)ItemInfo[i].offset - (uint32_t)&ControlTable));
                        break;
                    case dt_uint8:
                        *(uint8_t *)ItemInfo[i].offset = *(uint8_t *)(NVM_HEAD + ((uint32_t)ItemInfo[i].offset - (uint32_t)&ControlTable));
                        break;
                    case dt_int16:
                        *(int16_t *)ItemInfo[i].offset = *(int16_t *)(NVM_HEAD + ((uint32_t)ItemInfo[i].offset - (uint32_t)&ControlTable));
                        break;
                    case dt_uint16:
                        *(uint16_t *)ItemInfo[i].offset = *(uint16_t *)(NVM_HEAD + ((uint32_t)ItemInfo[i].offset - (uint32_t)&ControlTable));
                        break;
                    case dt_int32:
                        *(int32_t *)ItemInfo[i].offset = *(int32_t *)(NVM_HEAD + ((uint32_t)ItemInfo[i].offset - (uint32_t)&ControlTable));
                        break;
                    case dt_uint32:
                        *(uint32_t *)ItemInfo[i].offset = *(uint32_t *)(NVM_HEAD + ((uint32_t)ItemInfo[i].offset - (uint32_t)&ControlTable));
                        break;
                }
            }
        }
        return true;
    }
    return false;
}
