/******************************************************************************
 *
 * Copyright 2021 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 *
 ******************************************************************************
 * meterliblprt1ph_test.c
 ******************************************************************************/
#include <math.h>

#include "types.h"
#include "Metering3Ph.h"
#include "Calibration3Ph.h"
#include "pin_mux.h"
#include "board.h"

#define _PI   3.14159265358979323846	/* pi */
#define NSAMPLES 1500                  /*!< Sample frequency in Hz                    */
#define I_MAX                 141.421  /*!< Maximal current I-peak in amperes         */
#define U_MAX                 350.000  /*!< Maximal voltage U-peak in volts           */
#define RPH_PHERR_PERHZ                         0.0f
#define YPH_PHERR_PERHZ                         0.0f
#define BPH_PHERR_PERHZ                         0.0f

/* static data definitions                                                    */
static tMETERLIBLPRT3PH_DATA mlib3phdata;
tCalibPoint3Ph CalibPoint;
float FreqDependentPhErr[nVPHASES] = {RPH_PHERR_PERHZ, YPH_PHERR_PERHZ, BPH_PHERR_PERHZ};
static volatile uint16  u16_samplePh[nVPHASES];
static volatile frac32  i24_samplePh[nVPHASES];
static double  time = 0.0, U_ANGLE = (  60.0/180.0)*_PI,
                        I_SHIFT = (  -5.5/180.0)*_PI,
                        DEG_120 = (-120.0/180.0)*_PI;

void RestoreDefCalib(void);
void AppInterfaceInit(void);

#if defined(__ICCARM__)
  #pragma diag_suppress=Pa082
#endif

void FudgeParameters3Ph(void)
{
}

void CorrectAppPhAngle3Ph(uint32 currenttoUse)
{
}

void RestoreDefCalib(void)
{
  CalibStruct3Ph.VCoeff[0] = 2.0201332511E-4;
  CalibStruct3Ph.VCoeff[1] = 2.01980598E-4;
  CalibStruct3Ph.VCoeff[2] = 2.0200338622E-4;
  
  CalibStruct3Ph.ActPowerCoeff[0] = 5.80507517E-11;
  CalibStruct3Ph.ActPowerCoeff[1] = 5.79365895E-11;
  CalibStruct3Ph.ActPowerCoeff[2] = 5.8122021E-11;
  
  CalibStruct3Ph.PhAngle[0] = 5.63219785E-2;
  CalibStruct3Ph.PhAngle[1] = 5.77261448E-2;
  CalibStruct3Ph.PhAngle[2] = 5.62735796E-2;
  
  CalibStruct3Ph.ICoeff[0] = 2.87363377E-7;
  CalibStruct3Ph.ICoeff[1] = 2.868708E-7;
  CalibStruct3Ph.ICoeff[2] = 2.87729477E-7;
  CalibStruct3Ph.ICoeff[3] = 3.75760566E-7;
  
  CalibStruct3Ph.VFCoeff[0] = 9.35506676E-8;
  CalibStruct3Ph.VFCoeff[1] = 7.83038061E-8;
  CalibStruct3Ph.VFCoeff[2] = 4.8104311644E-8;

  CalibStruct3Ph.IFCoeff[0] = 6.80729916E-11;
  CalibStruct3Ph.IFCoeff[1] = 1.2588186044E-10;
  CalibStruct3Ph.IFCoeff[2] = 1.1778295E-10;
  
  CalibStruct3Ph.FrequencyCoeff = 1.00008511;
}

void DoSyncing(void)
{
}

void AppInterfaceInit(void)
{
  mlib3phdata.IMax   = 60.0;
  mlib3phdata.IBasic = 10.0;
  mlib3phdata.MaxPower = (240.0*3.0*100.0);
}

void CalibMemwrite3Ph(void)
{
  CalibStruct3Ph.CalibStatus = CALIBDONE;
}

uint32 NoOfMetDue = 0;
volatile float temp32;
void main (void)
{
  uint8 i;
  BOARD_InitPins();
  BOARD_BootClockRUN();  

  MeterLibLPRT3Ph_InitParams(&mlib3phdata, NSAMPLES, (NSAMPLES*20)/100, FreqDependentPhErr, FALSE);
  mlib3phdata.Frequency = 50.0f;
  AppInterfaceInit();
  RestoreDefCalib();
  
  mlib3phdata.CalibState = CALIBSTATE_PROGRESS;
  /* calibration can be done starting with CALIB_SETUP or CALIB_NEU_SETUP - one at a time */
  CalibStep3Ph  = CALIB_SETUP;
  CalibPoint.Voltage = 240.0;
  CalibPoint.Current = 10.0;
  CalibPoint.PhAngle = 1.04719;
  CalibPoint.PowerFactor = 0.5;
  CalibPoint.Frequency = 50.0;
  
  while (1)
  {
    /* calculate phase voltage and phase current waveforms                    */
    time = time+(1.0/NSAMPLES);
    
    for(i=0; i<nVPHASES; i++)
    {
      temp32 = (((((sin(2*_PI*50.0*time+(i*DEG_120)+U_ANGLE)+1.0)/2)*240.0*sqrt(2)+0x0)/U_MAX));
      u16_samplePh[i] = (uint16)(temp32 * 0xffff);
      i24_samplePh[i] = FRAC24(((sin(2*_PI*50.0*time+(i*DEG_120)+I_SHIFT)*10.0*sqrt(2)+0.0)/I_MAX));
      
      mlib3phdata.VOfstSum[i] += u16_samplePh[i];
      mlib3phdata.VSampsS[i] = u16_samplePh[i] - mlib3phdata.VOffsets[i];
      
      mlib3phdata.ISamps[i] = i24_samplePh[i]; 
      mlib3phdata.IOfstSum[i] += mlib3phdata.ISamps[i];
      mlib3phdata.ISamps[i] -= mlib3phdata.IOffsets[i]; // Previous offset
      ProcSamples3Ph(i);
    }
    
    DoPower3Ph();
    
    if (mlib3phdata.MetDue == TRUE)
    {
      NoOfMetDue++;
      /* Do metering calculations */
      DoMetering3Ph();
      
      if ((mlib3phdata.CalibState == CALIBSTATE_PROGRESS) || (mlib3phdata.CalibState == CALIBSTATE_NEU_PROGRESS)) // will break when CALIBSTATE_COMPLETE
      {
        DoCalibration3Ph(&CalibPoint);
      }
    }
  }
}
