/*hardware interface*/

#include "hardware.h"
#include "structs.h"

extern struct results_struct results;
extern int g_wave_step;
extern int iv_sel;
extern uint32_t wav_len;
extern uint32_t wav_periods;
extern double voltage_saved[WAV_LEN_MAX];
extern double current_saved[WAV_LEN_MAX];
extern int g_cal_autorange_mode;


void init(struct config_struct *config, struct cal_struct *cal, struct cal_struct *cal_flash_pt)
{
    /*clock configuration - default, with some settings derived from configuration bits*/
    
    /*configure ports*/
    ANSELA = 0x00000000;
    ANSELB = 0x00000000;
    ANSELC = 0x00000000;
    PWR_EN_TRIS = 0x0;
    SIPO_SER_TRIS = 0x0;
    SIPO_CLK_TRIS = 0x0;
    SIPO_LATCH_TRIS = 0x0;
    SCL_OUT_TRIS = 0x0;
    SDA_IN_TRIS = 0x1;
    SCS_OUT_TRIS = 0x0;
    REF_OSC_TRIS = 0x0;
    TX_TRIS = 0x0;
    RX_TRIS = 0x1;
    INT_TRIS = 0x1;
    CURR_VOLT_SW_TRIS = 0x0;
    PWR_EN_LAT = 0x0;
    
    /*configure adc*/
    ANSELAbits.ANSA0 = 0x1;     //analog input
    ANSELAbits.ANSA1 = 0x1;     //analog input
    TRISAbits.TRISA0 = 0x1;     //analog input
    TRISAbits.TRISA1 = 0x1;     //analog input
    AD1CON1bits.SIDL = 0x01;    //disable adc in idle mode
    AD1CON1bits.FORM = 0x5;     //signed integer 32bit NOT WORKING
    AD1CON1bits.SSRC = 0x5;     //timer1 period match starts convertion
    AD1CON1bits.MODE12 = 0x1;   //12bit mode
    AD1CON1bits.ASAM = 0x1;     //sampling immediately after conversion
    AD1CON2bits.VCFG = 0x2;     //reference Vref+ pin and VSS
    AD1CON2bits.OFFCAL = 0x0;   //disables offset calibration mode
    AD1CON2bits.BUFREGEN = 0x0; //buffer as fifo
    AD1CON2bits.CSCNA = 0x0;    //scan inputs disable
    AD1CON2bits.SMPI = 0x0;     //result in buffer 0
    AD1CON2bits.BUFM = 0x0;     //buffer configured as 16-word
    AD1CON3bits.ADRC = 0x0;     //clock from pheripheral bus clock
    AD1CON3bits.EXTSAM = 0x1;   //extended sampling time (TODO - is it needed?)
    AD1CON3bits.ADCS = 0x3;     //conversion clock Tad
    AD1CON5bits.ASEN = 0x0;     //disable auto-scan
    AD1CON5bits.BGREQ = 0x0;    //bandgap disabled
    AD1CON5bits.ASINT = 0x0;    //no interrupt?
    AD1CON5bits.WM = 0x0;       //threshold (comparison) disabled
    AD1CHSbits.CH0NA = 0x0;     //negative input vss
    AD1CHSbits.CH0SA = 0x1;     //input - AN1
    IPC3bits.AD1IP = 0x4;       //interrupt priority 4
    IEC0bits.AD1IE = 0x1;       //interrupt enable
    AD1CON1bits.ON = 0x1;       //turn adc on
    
    /*configure timer triggering external adc*/
    T1CONbits.TCS = 0x0;        //internal clock
    T1CONbits.TCKPS = 0x1;      //prescale 1/8 to match adc period
    PR1 = 25;                   //period 26 to get 1/10.4us (for 100kHz, to be overwritten for different test frequencies)
    T1CONbits.SIDL = 0x1;       //stop in idle
    T1CONbits.ON = 0x1;         //turn on timer

    /*configure generator*/
    CCP1CON1bits.TRIGEN = 0x0;  //triggered operation disabled
    CCP1CON1bits.SYNC = 0x00;   //rolls over at match with period register
    CCP1CON1bits.CLKSEL = 0x0;  //clock is Fsys
    CCP1CON1bits.TMRPS = 0x1;   //prescale 1:4 to get 5MHz (can be divide to get 100kHz and lower down to 100Hz on 16-bit period register)
    CCP1CON1bits.T32 = 0;       //16bit mode
    CCP1CON1bits.CCSEL = 0x0;   //output compare / PWM mode
    CCP1CON1bits.MOD = 0x3;     //toggle output on match
    CCP1CON2bits.OCAEN = 0x1;   //OCA pin controlled by output A of MCCP1
    CCP1CON3bits.OUTM = 0x0;    //Steerable Single Output mode
    CCP1PR = 24;                //period register (5MHz / 25 = 200kHz toggle)
    CCP1RA = 24;                //compare register
    CCP1CON1bits.ON = 0x1;      //start timer
    
    /*configure uart*/
    U1MODEbits.CLKSEL = 0x0;    //clock is the PBCLK (1:1 with SYSCLK)
    U1MODEbits.SLPEN = 0x1;     //active when sleep
    U1MODEbits.SIDL = 0x0;      //continues in idle mode
    U1MODEbits.UEN = 0x0;       //rts/cts not used
    U1MODEbits.WAKE = 0x1;      //wake-up on start bit
    U1MODEbits.LPBACK = 0x0;    //no loopback
    U1MODEbits.ABAUD = 0x0;     //no baud measurement
    U1MODEbits.BRGH = 0x0;      //standard speed mode
    U1MODEbits.PDSEL = 0x0;     //8-bit, no parity
    U1MODEbits.STSEL = 0x0;     //1 stop bit
    U1STAbits.URXEN = 0x1;      //receiver enable bit
    U1STAbits.UTXEN = 0x1;      //transmit enable bit
    U1STAbits.ADDEN = 0x0;      //address detect disable
    U1BRG = 64;                 //baud rate 20M/(16*(64+1)) = 19230.8
    U1MODEbits.ACTIVE = 0x1;    //is active
    U1MODEbits.ON = 0x1;        //enable uart
    
    /*configure analog*/
    config->amp_sel = 0x0;
    config->fq_sel = 0x0;
    config->v_gain = 0;
    config->i_gain = 0;
    config->v_i_gain = config->v_gain;
    config->do_the_cal = 0;
    config->comp_open = 0;
    config->comp_short = 0;
    
    /*initialize calibration data from flash, reset calibration if data seems to be incorrect (out of 90% - 110% gain and +/-0.1rad)*/
    if(
    0.9 > cal_flash_pt->cal_i10 || 1.1 < cal_flash_pt->cal_i10 ||
    0.9 > cal_flash_pt->cal_i100 || 1.1 < cal_flash_pt->cal_i100 ||
    0.9 > cal_flash_pt->cal_i1000 || 1.1 < cal_flash_pt->cal_i1000 ||
    0.9 > cal_flash_pt->cal_i10000 || 1.1 < cal_flash_pt->cal_i10000 ||
    0.9 > cal_flash_pt->cal_u1 || 1.1 < cal_flash_pt->cal_u1 ||
    0.9 > cal_flash_pt->cal_u10 || 1.1 < cal_flash_pt->cal_u10 ||
    0.9 > cal_flash_pt->cal_u100 || 1.1 < cal_flash_pt->cal_u100 ||
    -0.1 > cal_flash_pt->ph_cal_i10 || 0.1 < cal_flash_pt->ph_cal_i10 ||
    -0.1 > cal_flash_pt->ph_cal_i100 || 0.1 < cal_flash_pt->ph_cal_i100 ||
    -0.1 > cal_flash_pt->ph_cal_i1000 || 0.1 < cal_flash_pt->ph_cal_i1000 ||
    -0.1 > cal_flash_pt->ph_cal_i10000 || 0.1 < cal_flash_pt->ph_cal_i10000 ||
    -0.1 > cal_flash_pt->ph_cal_u1 || 0.1 < cal_flash_pt->ph_cal_u1 ||
    -0.1 > cal_flash_pt->ph_cal_u10 || 0.1 < cal_flash_pt->ph_cal_u10 ||
    -0.1 > cal_flash_pt->ph_cal_u100 || 0.1 < cal_flash_pt->ph_cal_u100
    )
    {
        cal->cal_i10 = 1.0;
        cal->cal_i100 = 1.0;
        cal->cal_i1000 = 1.0;
        cal->cal_i10000 = 1.0;
        cal->cal_u1 = 1.0;
        cal->cal_u10 = 1.0;
        cal->cal_u100 = 1.0;
        cal->ph_cal_i10 = 0;
        cal->ph_cal_i100 = 0;
        cal->ph_cal_i1000 = 0;
        cal->ph_cal_i10000 = 0;
        cal->ph_cal_u1 = 0;
        cal->ph_cal_u10 = 0;
        cal->ph_cal_u100 = 0;
        nvm_nop((void*)cal_flash_pt);
        erase_cal((void*)0x9D00D000);
        nvm_nop((void*)cal_flash_pt);
        write_cal((void*)0x9D00D000, (void*)cal);
    }
    cal->cal_i10 = cal_flash_pt->cal_i10;
    cal->cal_i100 = cal_flash_pt->cal_i100;
    cal->cal_i1000 = cal_flash_pt->cal_i1000;
    cal->cal_i10000 = cal_flash_pt->cal_i10000;
    cal->cal_u1 = cal_flash_pt->cal_u1;
    cal->cal_u10 = cal_flash_pt->cal_u10;
    cal->cal_u100 = cal_flash_pt->cal_u100;
    cal->ph_cal_i10 = cal_flash_pt->ph_cal_i10;
    cal->ph_cal_i100 = cal_flash_pt->ph_cal_i100;
    cal->ph_cal_i1000 = cal_flash_pt->ph_cal_i1000;
    cal->ph_cal_i10000 = cal_flash_pt->ph_cal_i10000;
    cal->ph_cal_u1 = cal_flash_pt->ph_cal_u1;
    cal->ph_cal_u10 = cal_flash_pt->ph_cal_u10;
    cal->ph_cal_u100 = cal_flash_pt->ph_cal_u100;
    
    /*set interrupts*/
    INTCONbits.MVEC = 1;    
    __builtin_enable_interrupts();
}


void config_hardware(struct config_struct *config)
{
    double volt_max = -100000000;
    double volt_min = 100000000;
    double curr_max = -100000000;
    double curr_min = 100000000;
    double mean_i = 0;
    double mean_v = 0;
    int32_t i_pkfind;
    static int32_t v_gain_adj_cnt = 0;
    static int32_t i_gain_adj_cnt = 0;
    
    //--------------------------------------------------------------------------
    if(config->fq_sel == 0x0)       //test frequency 100kHz
    {
        wav_len = 25;               //buffer is 25bits long
        wav_periods = 250;          //averaged 250 times
        PR1 = 25;                   //prescale 1/8, sampling Fs=(20MHz/8)/26 (period 10.4us) - 1 sample per period
        g_wave_step = 1;
        CCP1PR = 24;                //prescale 1/4, period register 5MHz / 25 = 200kHz toggle
        CCP1RA = CCP1PR;            //prescale 1/4, compare register
    }
    else if(config->fq_sel == 0x1)  //test frequency 10kHz
    {
        wav_len = 25;               //buffer is 25bits long
        wav_periods = 250;          //averaged 250 times
        PR1 = 29;                   //prescale 1/8, sampling Fs=(20MHz/8)/30 (period 12us) - ? samples per period
        g_wave_step = 3;
        CCP1PR = 249;               //prescale 1/4, period register 5MHz / 250 = 20kHz toggle
        CCP1RA = CCP1PR;            //prescale 1/4, compare register
    }
    else if(config->fq_sel == 0x2)  //test frequency 1kHz
    {
        wav_len = 100;              //buffer is 100bits long
        wav_periods = 100;          //averaged 50 times
        PR1 = 24;                   //prescale 1/8, sampling Fs=(20MHz/8)/25 (period 10us)
        g_wave_step = 1;
        CCP1PR = 2499;              //prescale 1/4, period register 5MHz / 2500 = 2kHz toggle
        CCP1RA = CCP1PR;            //prescale 1/4, compare register
    }
    else if(config->fq_sel == 0x3)  //test frequency 100Hz
    {
        wav_len = 250;              //buffer is 250bits long
        wav_periods = 30;           //averaged 30 times
        PR1 = 99;                   //prescale 1/8, sampling Fs=(20MHz/8)/100 (period 40us)
        g_wave_step = 1;
        CCP1PR = 24999;             //prescale 1/4, period register 5MHz / 25000 = 200Hz toggle
        CCP1RA = CCP1PR;            //prescale 1/4, compare register
    }
    //--------------------------------------------------------------------------
    
    //----------------------------------------
    /*Autorange*/
    //----------------------------------------
    for(i_pkfind = 0; i_pkfind < wav_len; ++i_pkfind)
    {
        if(volt_max < results.voltage_saved[i_pkfind])
            volt_max = results.voltage_saved[i_pkfind];
        if(volt_min > results.voltage_saved[i_pkfind])
            volt_min = results.voltage_saved[i_pkfind];
        if(curr_max < results.current_saved[i_pkfind])
            curr_max = results.current_saved[i_pkfind];
        if(curr_min > results.current_saved[i_pkfind])
            curr_min = results.current_saved[i_pkfind];
        mean_i += results.current_saved[i_pkfind];
        mean_v += results.voltage_saved[i_pkfind];
    }
    //----------------------------------------
    //adjust voltage gain if signal not within desired range
    if((volt_max - volt_min > (2.5 / 3.0 * ADC_RES) * wav_periods) || (mean_v / wav_len > 1 / 3.0 * ADC_RES * wav_periods) || (mean_v / wav_len < -1 / 3.0 * ADC_RES * wav_periods))   //compare raw, accumulated signal value with maximum allowed range of adc reading (2.5Vpp) scaled by ADC resolution (from ADC raw data), number of accumulated periods (from averaging) and wave length (from dft)   //gain decreased also when it's something wrong with mean value   
        ++v_gain_adj_cnt;
    else if(volt_max - volt_min < (0.225 / 3.0 * ADC_RES) * wav_periods)
        --v_gain_adj_cnt;
    else
        v_gain_adj_cnt = 0;
    
    if(v_gain_adj_cnt > RANGE_ADJ_FILTER)                   //if not in range many times then adjust
    {
        if(config->v_gain > 0)
            --(config->v_gain);                             //change voltage gain if needed
        v_gain_adj_cnt = 0;
        config->v_i_gain = config->v_gain;                  //set default voltage gain for current measurement equal to the gain for voltage measurement
    }
    else if(v_gain_adj_cnt < -RANGE_ADJ_FILTER)
    {
        if(config->v_gain < 2)
            ++(config->v_gain);                             //change voltage gain if needed
        v_gain_adj_cnt = 0;
        if(config->i_gain > 0)
            config->v_i_gain = config->v_gain;              //set default voltage gain for current measurement equal to the gain for voltage measurement
        if(g_cal_autorange_mode)                         //for phase calibration of voltage amplifier force voltage gain 1x when current is measured (while still allowing for 10x and 100x while voltage measurement)
            config->v_i_gain = 0;
    }
    //----------------------------------------
    //the same for current
    if((curr_max - curr_min > (2.5 / 3.0 * ADC_RES) * wav_periods) || (mean_i / wav_len > 1 / 3.0 * ADC_RES * wav_periods) || (mean_i / wav_len < -1 / 3.0 * ADC_RES * wav_periods))    //gain decreased also when it's something wrong with mean value
        ++i_gain_adj_cnt;
    else if(curr_max - curr_min < (0.225 / 3.0 * ADC_RES) * wav_periods)
        --i_gain_adj_cnt;
    else
        i_gain_adj_cnt = 0;

    if(i_gain_adj_cnt > RANGE_ADJ_FILTER)               //if not in range many times then adjust
    {
        if((config->i_gain == 0) && (config->v_i_gain > 0)) //if overdriven in shunt mode then decrease voltage gain (current gain is unchanged 1V/100mA)
            --(config->v_i_gain);
        else if((config->v_i_gain > config->v_gain))        //tend to decrease voltage amplifier gain while current measurement to the same as while voltage measurement (if current has the extra voltage gain)
            --(config->v_i_gain);
        else if(config->i_gain > 0)                         //finally decrease current gain
            --(config->i_gain);
        i_gain_adj_cnt = 0;
    }
    else if(i_gain_adj_cnt < -RANGE_ADJ_FILTER)
    {
        if(config->i_gain < 3)
            ++(config->i_gain);                             //increase current gain
        else if(config->v_i_gain < 2)
            ++(config->v_i_gain);                           //if doesn't help then add some extra voltage gain
        i_gain_adj_cnt = 0;
    }

    
    //----------------------------------------
    
    config->irr = I_GAIN_IRR(config->i_gain);           //determine hardware configuration bits from gain level (from autorange)
    if(!iv_sel)
        config->vrr = V_GAIN_VRR(config->v_gain);       //hardware configuration bits for voltage signal path or part of current signal path
    else
        config->vrr = V_GAIN_VRR(config->v_i_gain);
    
    //TEMP: IV converter hardware debug
    static int tmp_test_iv_conv = 0;   
    TRISBbits.TRISB2 = 0x1;
    CNPUBbits.CNPUB2 = 0x1;
    if (PORTBbits.RB2 == 0)
        ++tmp_test_iv_conv;
    if (tmp_test_iv_conv == 1)
        config->irr = 0x02;
    if (tmp_test_iv_conv == 2)
        config->irr = 0x04;
    if (tmp_test_iv_conv == 3)
        config->irr = 0x08;
    if (tmp_test_iv_conv == 4)
        config->irr = 0x10;
    if (tmp_test_iv_conv == 5)
        config->irr = 0x03;
    if (tmp_test_iv_conv == 6)
        config->irr = 0x05;
    if (tmp_test_iv_conv == 7)
        config->irr = 0x09;
    if (tmp_test_iv_conv == 8)
        tmp_test_iv_conv = 0;
    //END TEMP: IV converter hardware debug
    
    send_hw_conf(*config);
}


void send_hw_conf(struct config_struct config)
{
    int i_sipo;

    //set either voltage or current measurement
    CURR_VOLT_SW_LAT = ~iv_sel;
    
    //--------------------------------------------------------------------------
    //configure hardware using external shift registers
    SIPO_CLK_LAT = 0x0;
    SIPO_LATCH_LAT = 0x0;
    for(i_sipo = 0; i_sipo < 16; ++i_sipo)
    {
        switch (i_sipo)
        {
        case 3:
            SIPO_SER_LAT = (~config.irr >> 4) & 0x1;
            break;
        case 4:
            SIPO_SER_LAT = (~config.irr >> 3) & 0x1;
            break;
        case 5:
            SIPO_SER_LAT = (~config.irr >> 2) & 0x1;
            break;
        case 6:
            SIPO_SER_LAT = (~config.irr >> 1) & 0x1;
            break;
        case 7:
            SIPO_SER_LAT = (config.irr >> 0) & 0x1;
            break;
        case 9:
            SIPO_SER_LAT = (config.vrr >> 2) & 0x1;
            break;
        case 10:
            SIPO_SER_LAT = (config.vrr >> 1) & 0x1;
            break;
        case 11:
            SIPO_SER_LAT = (config.vrr >> 0) & 0x1;
            break;            
        case 12:
            SIPO_SER_LAT = (config.fq_sel >> 1) & 0x1;
            break;
        case 13:
            SIPO_SER_LAT = (config.fq_sel >> 0) & 0x1;
            break;
        case 14:
            SIPO_SER_LAT = (config.amp_sel >> 1) & 0x1;
            break;
        case 15:
            SIPO_SER_LAT = (config.amp_sel >> 0) & 0x1;
            break;
        default:
            break;
        }
        SIPO_CLK_LAT = 0x1;
        SIPO_CLK_LAT = 0x0;
    }
    SIPO_LATCH_LAT = 0x1;
}

void calibration(struct config_struct *config, struct cal_struct *cal, struct cal_struct *cal_flash_pt)
{   
    static double tmp_v_mod_at_gain1x;
        int zzz=0;
        double zzzd = 0;
        
    if(config->fq_sel == 0x3)                                                           //gain calibration possible with 100Hz test wave
    {
        if(config->v_gain == 0)                                                         //for voltage gain 1x calibrate current gain for different gains, the calibrated range is selected depending of measured reference resistance
        {
            if((results.z_mod < 10000.0 * 1.1) && (results.z_mod > 10000.0 * 0.9))
                cal->cal_i10000 = results.z_mod / 10000.0;
            if((results.z_mod < 1000.0 * 1.1) && (results.z_mod > 1000.0 * 0.9))
                cal->cal_i1000 = results.z_mod / 1000;
            if((results.z_mod < 100.0 * 1.1) && (results.z_mod > 100.0 * 0.9))
            {
                cal->cal_i100 = results.z_mod / 100;
                g_cal_autorange_mode = 1;                                               //force voltage gain 1x while current measurement, next two steps are calibrating voltage amplifier gain on 10x and 100x gain (test level will be decreased and the voltage amplifier is 1x for current and 10x/100x for voltage measurement)
            }
            if((results.z_mod < 10.0 * 1.1) && (results.z_mod > 10.0 * 0.9))            //calibration in shunt mode (low impedances measurements)
                cal->cal_i10 = results.z_mod / 10.0;
        }
        if(config->v_gain == 1)                                                         //for a voltage gain 10x...
        {
            if((results.z_mod < 100.0 * 1.1) && (results.z_mod > 100.0 * 0.9))          //...calibrate the voltage gain 10x (using voltage saved for 1x, current gain and measured component are the same)
                cal->cal_u10 = 100 / results.z_mod;
        }
        if(config->v_gain == 2)                                                         //for a voltage gain 100x...
        {
            if((results.z_mod < 100.0 * 1.1) && (results.z_mod > 100.0 * 0.9))          //...calibrate the voltage gain 100x (using voltage saved for 1x, current gain and measured component are the same)
                cal->cal_u100 = 100 / results.z_mod;
            g_cal_autorange_mode = 0;
        }
    }
    else if(config->fq_sel == 0x0)                                                      //phase calibration possible with 100kHz test wave
    {
        if(config->v_gain == 0)                                                         //for a voltage gain 1x...
        {
            if((results.z_mod < 10600.0 * 1.1) && (results.z_mod > 10600.0 * 0.9))      //capacitor 150pF low ESR
                cal->ph_cal_i10000 = PI / 2 + results.z_arg;
            if((results.z_mod < 1060.0 * 1.1) && (results.z_mod > 1060.0 * 0.9))        //capacitor 1.5nF low ESR
                cal->ph_cal_i1000 = PI / 2 + results.z_arg;
            if((results.z_mod < 100.0 * 1.1) && (results.z_mod > 100.0 * 0.9))          //resistor 100R (low reactance)
            {
                cal->ph_cal_i100 = results.z_arg;
                g_cal_autorange_mode = 1;                                            //force voltage gain 1x while current measurement, next two steps are calibrating voltage amplifier phase shifts on 10x and 100x gain (test level will be decreased and the voltage amplifier is 1x for current and 10x/100x for voltage measurement)
            }
            if((results.z_mod < 10.0 * 1.1) && (results.z_mod > 10.0 * 0.9))            //resistor 10R (low reactance)
                cal->ph_cal_i10 = results.z_arg;
        }
        if(config->v_gain == 1)                                                         //for a voltage gain 10x...
        {
            if((results.z_mod < 100.0 * 1.1) && (results.z_mod > 100.0 * 0.9))          //resistor 100R (low reactance)
                cal->ph_cal_u10 = -results.z_arg;                                       //calibrate phase for 10x voltage gain
        }
        if(config->v_gain == 2)                                                         //for a voltage gain 100x...
        {
            if((results.z_mod < 100.0 * 1.1) && (results.z_mod > 100.0 * 0.9))          //resistor 100R (low reactance)
                cal->ph_cal_u100 = -results.z_arg;                                      //calibrate phase for 100x voltage gain
            g_cal_autorange_mode = 0;
        }
    }
    //short circuit to clear calibration in nonvolatile memory
    if(results.z_mod > 500000)
    {
        cal->cal_i10 = 1.0;
        cal->cal_i100 = 1.0;
        cal->cal_i1000 = 1.0;
        cal->cal_i10000 = 1.0;
        cal->cal_u1 = 1.0;
        cal->cal_u10 = 1.0;
        cal->cal_u100 = 1.0;
        cal->ph_cal_i10 = 0.0;
        cal->ph_cal_i100 = 0.0;
        cal->ph_cal_i1000 = 0.0;
        cal->ph_cal_i10000 = 0.0;
        cal->ph_cal_u1 = 0.0;
        cal->ph_cal_u10 = 0.0;
        cal->ph_cal_u100 = 0.0;
    }
    //open circuit to save calibration to nonvolatile memory
    if(results.z_mod < 1)
    {
        nvm_nop((void*)cal_flash_pt);
        erase_cal((void*)cal_flash_pt);
        nvm_nop((void*)cal_flash_pt);
        write_cal((void*)cal_flash_pt, (void*)cal);
    }
    config->do_the_cal = 0x0;
}

unsigned int erase_cal(void* address)
{
unsigned  int res;
NVMADDR = (unsigned int)KVA_TO_PA(address);// Set NVMADDR to the Start Address of page to erase
res = NVMUnlock(0x4004);// Unlock and Erase Page
return res;// Return Result
}

unsigned int write_cal (void* address, void* data)
{
    unsigned  int res;    
    NVMADDR = (unsigned int)KVA_TO_PA(address);   // Set NVMADDR to Start Address of row to program
    NVMSRCADDR = (unsigned int)KVA_TO_PA(data);   // Set NVMSRCADDR to the SRAM data buffer Address
    res = NVMUnlock(0x4003);            // Unlock and Write Row
    return res;                         // Return Result
}

unsigned int nvm_nop (void* address)
{
    unsigned  int res;    
    NVMADDR = (unsigned int)KVA_TO_PA(address);   // Set NVMADDR to Start Address of row to program
    res = NVMUnlock(0x4000);            // Unlock and nop
    return res;                         // Return Result
}

unsigned  int  NVMUnlock (unsigned int nvmop)
{
    unsigned int  status;
    asm volatile ("di %0" : "=r" (status));    // Suspend or Disable all Interrupts
    NVMCON = nvmop;    // Enable Flash Write/Erase Operations and Select Flash operation to perform
    NVMKEY = 0xAA996655;    // Write Keys
    NVMKEY = 0x556699AA;    // Write Keys
    NVMCONSET = 0x8000;    // Start the operation using the Set Register
    while (NVMCON & 0x8000);    // Wait for operation to complete
    if(status & 0x00000001) asm volatile ("ei");    // Restore Interrupts
    else asm volatile ("di");
    NVMCONCLR = 0x0004000;    // Disable NVM write enable
    return (NVMCON & 0x3000);    // Return WRERR and LVDERR Error Status Bits 
}