//**************************************************************************
//    Calibration Routines for the FE427
//
//    Stefan Schauer
//    Texas Instruments Freising
//    Date        Comments
//    =====================
//    01/09/19    Code Starts for FE427

//**************************************************************************
#include "device.h"
#include "calibration.h"
#include "emeter.h"
#include "parameter.h"
#include "emeter-toolkit.h"

#include <math.h>

#ifdef withCalibration

unsigned char Cal_Mode = 0;    // Calibration Mode


void CalPlus(void)
{
    if (Cal_Mode == 1) s_parameters.pSET_INTRPTLEVL.l ++;  // 
    if (Cal_Mode == 2) s_parameters.pSET_PHASECORR1 ++;    // 
    if (Cal_Mode == 3) s_parameters.pSET_PHASECORR2 ++;    // 

    if (Cal_Mode == 4) s_parameters.pSET_V1OFFSET ++;  // 
    if (Cal_Mode == 5) s_parameters.pSET_I1OFFSET ++;    // 
    if (Cal_Mode == 6) s_parameters.pSET_I2OFFSET ++;    // 

    if (Cal_Mode == 0xA) s_parameters.VRatio += s_parameters.VRatio/1000;  // inc by 0.1%
    if (Cal_Mode == 0xB) s_parameters.IRatio += s_parameters.IRatio/1000;  // inc by 0.1% 
    if (Cal_Mode == 0xC) s_parameters.EnergyRatio += s_parameters.EnergyRatio/1000;  // inc by 0.1%

    // Init. Embedded Signal Processing parameters
    init_esp_parameter(0);
    // Init. event generation and start measurement
    start_measurement();
}

void CalMinus(void)
{
    if (Cal_Mode == 1) s_parameters.pSET_INTRPTLEVL.l --;
    if (Cal_Mode == 2) s_parameters.pSET_PHASECORR1 --;
    if (Cal_Mode == 3) s_parameters.pSET_PHASECORR2 --;

    if (Cal_Mode == 4) s_parameters.pSET_V1OFFSET --;  // 
    if (Cal_Mode == 5) s_parameters.pSET_I1OFFSET --;    // 
    if (Cal_Mode == 6) s_parameters.pSET_I2OFFSET --;    // 

    if (Cal_Mode == 0xA) s_parameters.VRatio -= s_parameters.VRatio/1000;  // dec by 0.1%
    if (Cal_Mode == 0xB) s_parameters.IRatio -= s_parameters.IRatio/1000;  // dec by 0.1% 
    if (Cal_Mode == 0xC) s_parameters.EnergyRatio -= s_parameters.EnergyRatio/1000;  // dec by 0.1%

    // Init. Embedded Signal Processing parameters
    init_esp_parameter(0);
    // Init. event generation and start measurement
    start_measurement();
}

void CalMode(unsigned char mode)
{
    float fvalue;
//    unsigned int value;
    unsigned int i = 0;
    unsigned char saved_tx_mode;
    
    Cal_Mode = mode;
    if (Cal_Mode == 0)
    {
//        float fvalue;
        saved_tx_mode = TX_Mode; // save tx_mode
        // Init. Embedded Signal Processing parameters
        s_parameters.IRatio = 1;
        s_parameters.VRatio = 1;
        s_parameters.EnergyRatio = 1;
        s_parameters.pSET_PHASECORR1 = (int) 0;
        s_parameters.pSET_PHASECORR2 = (int) 0;
        s_parameters.pSET_STARTCURR_INT = 0;
        s_parameters.pSET_STARTCURR_FRAC = 0;
//        s_parameters.pSET_INTRPTLEVL.l = 0x80000000;
        init_esp_parameter(0);
        // Init. event generation and start measurement
        start_measurement();
        while ((i++) < 3) {LPM0; } // wait for values
        fvalue = (float)((((float)POWERFCT)/POW_2_14));
        fvalue = acos(fvalue)*180/__PI;  // convert to degree
        fvalue = calPhi - fvalue;
        fvalue = (fvalue/calFreq*(POW_2_20/360));
        s_parameters.pSET_PHASECORR1 = (int) fvalue;
//        s_parameters.pSET_PHASECORR2 = (int) 0;
        
        #define CalTime 5       
        CalCyclCnt = CalTime * 50;    // Set Cycles to CalTime * 1 sec (@50Hz)
        // Init. event generation and start measurement
        start_calibration();

        while (OP_Mode != done) { LPM0; } // wait for stable values
        s_parameters.VRatio = (float) calVoltage * 1000 / V1RMS;
        #define scale (float)( (float)1000 * (float)1000 * (float)calCosPhi * (float)CalTime / (float)16 )
        s_parameters.EnergyRatio = (float) ( (float)calCurrent *  (float)calVoltage * scale / (float)energy.l );
        s_parameters.IRatio = (float) calCurrent * 1000 / IRMS_HI;
        s_parameters.IRatio = (float) (s_parameters.EnergyRatio / s_parameters.VRatio);

//        s_parameters.pSET_STARTCURR_INT = (unsigned int)(IRMS_HI * defStartCurrent / calCurrent);
//        s_parameters.pSET_STARTCURR_FRAC = (unsigned int);
//        s_parameters.pSET_INTRPTLEVL.l = (unsigned long) 0;
//        s_parameters.pSET_INTRPTLEVL.h = (unsigned long) 0;
    
        // Init. Embedded Signal Processing parameters
        init_esp_parameter(0);
        // Init. event generation and start measurement
        start_measurement();
        TX_Mode = saved_tx_mode;

    }

    if (Cal_Mode == 9)
    {
        // save Parameters
        flash_clr((int *) (&s_parameters_flash));
        {
           unsigned int i = 0;
           unsigned int ssize = sizeof(s_parameters);
           while (i < ssize)
           {
    //                    flash_write_int16((int16_t *)(&s_parameters_flash + i), ((int)((int *)((int)&s_parameters + i))) );
              char * ptemp = ((char *) &s_parameters);
              flash_write_int8((int8_t *)(((char *)&s_parameters_flash + i)), (int) ptemp[i]);
              i++;
           }
        }
        // Init. Embedded Signal Processing parameters
        init_esp_parameter(1);
        // Init. event generation and start measurement
        start_measurement();
    
        _EINT();  // Interrupts has been disabled during Flash write/erase
    }
}
#endif // withCalibration