/***************************************************************************
 * This code and information is provided "as is" without warranty of any   *
 * kind, either expressed or implied, including but not limited to the     *
 * implied warranties of merchantability and/or fitness for a particular   *
 * purpose.                                                                *
 *                                                                         *
 * Copyright (C) 2005 Teridian Semiconductor Corp. All Rights Reserved.    *
 ***************************************************************************/
//**************************************************************************
//  DESCRIPTION: 71M652x POWER METER - Temperature Calibration.
// 
//  AUTHOR:  RGV / MTF
//
//  HISTORY: See end of file.
//**************************************************************************
// File: calibration.c
//               
#include "options.h"
// This calibration does not adjust phase, but it can handle
// all equations.  So, select the equations not handled by calphased.c
#if (CAL_SAVE || AUTOCAL) && \
    (EQUATION == _1ELEMENT_3WIRE \
     || EQUATION == _2ELEMENT_4WIRE_DELTA \
     || EQUATION == _2ELEMENT_4WIRE_WYE)
#include <math.h>
#include "mmath.h"
#include "meter.h"
#include "library.h"
#if NV_SELECT == NV_EEPROM
#include "eeprom.h"
#endif 
#if NV_SELECT == NV_FLASH
#include "flash.h"
#endif 
#if NV_SELECT == NV_FLASH || NV_SELECT == NV_EEPROM
#include "ce.h" // disable CE while writing flash
#endif
#include "calibration.h"

#define ZERO  0L
#if M6520
#define NUM_PARMS   0x80                 // Half of CE DATA ram.
#elif TRACE10
#define NUM_PARMS   0x100                // Half of CE DATA ram.
#else
#error unknown device
#endif

/*** External functions referenced by this module ***/
// Refer to include files.

/*** External variables referenced by this module ***/
extern const int32r_t CeData[];    // The default data for the analog front end.
extern const int8r_t NumCeData[2];

/*** Public functions referenced by this module ***/
/* See 'calibration.h' */

/*** Public variables referenced by this module ***/

/*** Private functions referenced by this module ***/
static int32_t adjust_gain (int32_t unitmax, int32_t actual, int32_t measured);
static uint32_t cal_checksum (void);
static void cal_end (void);
static void compensation (void);
bool memcpy_cepr (int32x_t * pDst, uint16_t pSrc, uint16_t len);
void memcpy_prce (uint16_t pDst, int32x_t * pSrc, uint16_t len);

#if AUTOCAL
/*** Private variables referenced by this module ***/
static int16i_t cs;   // Remaining accumulation intervals of calibration.

#if EQUATION == _1ELEMENT_2WIRE || EQUATION == _1ELEMENT_3WIRE 
#define CALIBRATIONS 3   
#elif EQUATION == _2ELEMENT_3WIRE_DELTA 
#define CALIBRATIONS 4   
#elif EQUATION == _2ELEMENT_4WIRE_DELTA || EQUATION == _2ELEMENT_4WIRE_WYE
#define CALIBRATIONS 5   
#elif EQUATION == _3ELEMENT_4WIRE_WYE
#define CALIBRATIONS 6   
#else
#error no such equation
#endif

#if EQUATION != _3ELEMENT_4WIRE_WYE
#define ADJUSTS 2
#else
#define ADJUSTS 3
#endif

static int32x_t cm[ CALIBRATIONS ];

static volatile int32p_t * code sqsum[] = 
#if EQUATION == _1ELEMENT_2WIRE || EQUATION == _1ELEMENT_3WIRE 
{ &v0sqsum, &i0sqsum, &i1sqsum };
#elif EQUATION == _2ELEMENT_3WIRE_DELTA 
{ &v0sqsum, &i0sqsum, &i1sqsum, &v1sqsum };
#elif EQUATION == _2ELEMENT_4WIRE_DELTA
{ &v0sqsum, &i0sqsum, &i1sqsum, &v2sqsum, &i2sqsum };
#elif EQUATION == _2ELEMENT_4WIRE_WYE
{ &v0sqsum, &i0sqsum, &i1sqsum, &v1sqsum, &i2sqsum };
#elif EQUATION == _3ELEMENT_4WIRE_WYE
{ &v0sqsum, &i0sqsum, &i1sqsum, &v1sqsum, &v2sqsum, &i2sqsum };
#else
#error no such equation
#endif

static const int32x_t * code cal[] =
#if EQUATION == _1ELEMENT_2WIRE || EQUATION == _1ELEMENT_3WIRE 
{ &cal_v0, &cal_i0, &cal_i1 };
#elif EQUATION == _2ELEMENT_3WIRE_DELTA 
{ &cal_v0, &cal_i0, &cal_i1, &cal_v1 };
#elif EQUATION == _2ELEMENT_4WIRE_DELTA
{ &cal_v0, &cal_i0, &cal_i1, &cal_v2, &cal_i2 };
#elif EQUATION == _2ELEMENT_4WIRE_WYE
{ &cal_v0, &cal_i0, &cal_i1, &cal_v1, &cal_i2 };
#elif EQUATION == _3ELEMENT_4WIRE_WYE
{ &cal_v0, &cal_i0, &cal_i1, &cal_v1, &cal_v2, &cal_i2 };
#else
#error no such equation
#endif

static const uint16x_t * code unitmax[] =
#if EQUATION == _1ELEMENT_2WIRE || EQUATION == _1ELEMENT_3WIRE 
{ &Vmax, &Imax, &Imax };
#elif EQUATION == _2ELEMENT_3WIRE_DELTA 
{ &Vmax, &Imax, &Imax, &Vmax };
#elif EQUATION == _2ELEMENT_4WIRE_DELTA
{ &Vmax, &Imax, &Imax, &Vmax, &Imax };
#elif EQUATION == _2ELEMENT_4WIRE_WYE
{ &Vmax, &Imax, &Imax, &Vmax, &Imax };
#elif EQUATION == _3ELEMENT_4WIRE_WYE
{ &Vmax, &Imax, &Imax, &Vmax, &Vmax, &Imax };
#else
#error no such equation
#endif

static const uint16x_t * code xcal[] =
#if EQUATION == _1ELEMENT_2WIRE || EQUATION == _1ELEMENT_3WIRE 
{ &Vcal, &Ical, &Ical };
#elif EQUATION == _2ELEMENT_3WIRE_DELTA 
{ &Vcal, &Ical, &Ical, &Vcal };
#elif EQUATION == _2ELEMENT_4WIRE_DELTA
{ &Vcal, &Ical, &Ical, &Vcal, &Ical };
#elif EQUATION == _2ELEMENT_4WIRE_WYE
{ &Vcal, &Ical, &Ical, &Vcal, &Ical };
#elif EQUATION == _3ELEMENT_4WIRE_WYE
{ &Vcal, &Ical, &Ical, &Vcal, &Vcal, &Ical };
#else
#error no such equation
#endif

void cal_begin (void)                   // Begin calibration.
{
    int32x_t *px;
    int8_t i;

    // MPU calibrations are omitted intentionally, because
    // all of them are more like configurations set by a user
    // to adapt the meter.

    // Sets default calibration for CE, including temperature compensations.
    px = &cal_i0;
    i = CALIBRATIONS;
    do  {   memset_ce (px++, UNITY);    } while (--i);

    i = ADJUSTS;
    do  {   memset_ce (px++, ZERO);     } while (--i);

    #if ADC_COMPENSATION
    // Start the calibration process, with 2 accumulation intervals
    // before temperature is collected, to clear the data pipeline.
    // Then, it collects data for another two seconds with the
    // temperature compensation running.
    cs = 4 + Scal;
    #else
    // Start the calibration process, with 2 accumulation intervals
    // before measurement begins, to clear the data pipeline.
    cs = 2 + Scal;
    #endif

    // set the global calibration flag
    Config |= CFG_CAL;
}

/* This the calibration state machine.
 * 1. Let the CE settle, then 
 * 2. Measure temperature for calibration, then 
 * 3. Measure the current for calibration, then 
 * 4. Perform the calibrations.
 * This only calibrates the gains, and assumes that the
 * phase calibrations are made another way.
 * It does not calibrate neutral current,
 * shunts, or other odd arrangements.
 */
void Calibrate (void)
{
    // Scal, Vcal and Ical are defined in 'defaults.c'.
    int8_t cnt;
    int32x_t *pcm;
    int32p_t * code *psqsum;
    int32_t rounding = Scal / 2;        // round to nearest LSB.

    #if ADC_COMPENSATION
    /* Need two (2) accumulation intervals before the
     * raw temperature will be correct.
     * At this point, it can set the nominal temperature,
     * and then begin to collect the data with
     * temperature compensation enabled. */
    if (cs == (Scal + 2))
        compensation ();                // Set temperature compensation values.
    #endif
    
    if (cs == Scal) // On the first accumulation interval of calibration.
        memset_x ((uint8x_t *) cm, 0, sizeof (cm)); // Initialize the data.

    // This code collects values averaged over several 
    // accumulation intervals, without loss of precision or overflow.
    // Collect the voltages and currents 
    // averaged over several accumulation intervals.
    pcm = cm;
    psqsum = sqsum;
    cnt = sizeof (cm) / sizeof (int32x_t);
    do
    {
        *pcm++ += (**psqsum++ + rounding) / Scal;                 
    } while (--cnt);
    
    if (0 == --cs)              // Count accumulation intervals.
    {                           // If the last accumulation interval, calibrate.
        cal_end ();             // Adjust calibration constants.
        cal_save();             // Save the new calibration.
        // Turn ON all the pulse outputs to show it's done.
        // This is a convenient indication because it works with
        // automatic calibration or manual operation.
        DIO &= 0xF3;            // The pulse outputs are DIOs.
        #if M6520
        CONFIG2 = 0x40 | (CONFIG2 & 0x3F); // Force TX_DIS to DIO_2.
        USER0 &= 0x3D;          // Drive the outputs to ground (LEDs on).
        DIR0 |= 0xC4;           // Change the DIOs 2, 6, 7 to outputs.
        #elif TRACE10                  
        USER0 &= 0x3F;          // Drive both outputs to ground (LEDs on).
        DIR0  |= 0xC0;          // Change the DIOs 6, 7 to outputs.
        #else
        #error unknown device
        #endif
        // clear the global calibration flag
        Config &= ~CFG_CAL;
    }
}

#if ADC_COMPENSATION
static void compensation (void)
{
    int32x_t tppmc1, tppmc2;
    int32x_t a, b;
    int32x_t tn;
    #if ENHANCED_TRIM 
    // These 51 bytes are dynamically allocated, so it doesn't waste RAM.
    int8x_t trimbga, trimbgb, trimm;
    int32x_t c, d;
    // temporary values correspond to letters in the design-spreadsheet.
    int32x_t td, tf, th, tj, tl;
    #endif

    // set temp_nom for calibration; note that the calibration
    // is with the temperature compensation on.
    temp_nom = temp_raw;
    tn = temp_nom;
    tppmc1 = 0; // the linear parts per million per degree C
    tppmc2 = 0; // the quadratic parts per million per degree C
    
    // code to add other components' temperature compensations 
    // to PPMC and PPMC2 goes here. tn is the calibration temperature.
    
    // calculate and add in the IC's temperature compensation
    #if ENHANCED_TRIM // compensate H-version (high-precision) parts
    // read a trim value
    trimbgb = Read_Trim (_TRIMBGB); // -128..127
    
    // if the chip has not been trimmed
    if (trimbgb == 0)
    {
    #endif // ENHANCED_TRIM.
        // Set untrimmed defaults for temperature compensations.
        a = -668;               // -6.68 * (ppm/c) * 1000
        b = -341000;            // -0.341 * (ppm/c^2) * 1000000
    #if ENHANCED_TRIM
    }
    else
    {   // Chip is trimmed, so calculate the..
        // ..temperature compensations, per the spreadsheet.
        // Constants are not abstracted because they are used only here..
        trimm   = Read_Trim (_TRIMM);   // -4..3
        trimbga = Read_Trim (_TRIMBGA); // -128..127
        #if M6520
        // c = [(temp_nom/2427.249249) - (bga * 500) - 370000]/900
        // 2427.249249 = (2^16)/(2^3)
        tn = (tn + 256L) / 512L; // tn = tn / 2^9
        td = ((tn * 27L) + 64L) / 128L; 
        th = ((int32_t)trimbga) * -500L;
        tj = th - 370000L;
        tf = td + tj;
        c = (tf + 5L)/9L;
        #elif TRACE11
        // c = [(temp_nom/4.74074) - (bga * 500) - 370000]/900
        // 4.74074 = 128/27
        td = ((tn * 27L) + 64L) / 128L; 
        th = ((int32_t)trimbga) * -500L;
        tj = th - 370000L;
        tf = td + tj;
        c = (tf + 5L)/9L;
        #elif TRACE13
        // Calculate c = [(temp_nom/2) - (bga * 500) - 370000]/900
        td = tn / 2L;
        th = ((int32_t)trimbga) * -500L;
        tj = th - 370000L;
        tf = td + tj;
        c = (tf + 5L)/9L;
        #else
        #error unknown configuration
        #endif
        // Calculate d = (bgb /10) - [(m + 0.5)*0.14]
        td =  ((int32_t)trimbgb) * 100L;
        th = (((int32_t)trimm) * 100L) + 50L;
        tj = ((th * 14L) + 5L) / 10L;
        d = td - tj;
    
        // a = (-0.28c+33)d+0.33c+7.9
        // calculate a
        td = -28L * c;
        tf = (td + (330000L + 5L)) / 10L;
        th = ((tf * d) + 5000L) / 10000L;
        tl = ((33L * c) + 50L) / 100L;
        tn = tl + 790L;
        a = th + tn;
    
        // b = (-0.0002c+0.02)d-0.46
        // Calculate b
        td = -2L * c;
        tf = td + 20000L;
        th = ((tf * d) + 500L) / 1000L;
        b = th - 460000L;
    } // end else chip has a trim
    #endif // ENHANCED_TRIM.
    tppmc1 += ((22463L * a) + 50000L)  / 100000L;    // a * 2.4632
    tppmc2 += ((1150L  * b) + 500000L) / 1000000L;   // b * 1150.1
    
    ppmc1 = tppmc1;
    ppmc2 = tppmc2;
}
#endif // compensation

static void cal_end (void)
{   // Note: Adjustments for shunts have to have different constants, 
    // and possibly a changed IMAX.
    int32x_t        *pcm    = cm;
    int32x_t * code *pcal   = cal;
    int16x_t * code *punit  = unitmax;
    int16x_t * code *pxcal  = xcal;
    int8_t cnt = sizeof (cm) / sizeof (int32x_t);

    do
    {      
        // Adjust the gain on each channel.
        memset_ce (
            *pcal++,