/*
 * Copyright (c) 2020, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "ti_msp_dl_config.h"
#include "meter.h"


int main(void)
{
    EM_message_code code;

    SYSCFG_DL_init();
        /* Get calibrated ADC offset - workaround for ADC_ERR_06 */
    gADCOffset = DL_ADC12_getADCOffsetCalibration(ADC12_0_ADCMEM_0_REF_VOLTAGE_V);
  
    // Initialize the Energy Measurement Library
    code = EM_configureEnergyMeasurement(&gEmSWConfig, (EM_SW_Lib_Result_Handle *) &gEmSWResult);
    if(code != EM_message_code_no_error)
    {
        // Error occurred
        while(1);
    }
    V_dc_estimate = _IQ19toIQ23(_IQ19div(DEFAULT_V_DC_ESTIMATE_A, DEFAULT_V_RMS_SCALE_FACTOR_A));
    I_dc_estimate = _IQ19toIQ23(_IQ19div(DEFAULT_I_DC_ESTIMATE_A, DEFAULT_I_RMS_SCALE_FACTOR_A));
    datasendflag = 0;
    NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN);
    NVIC_EnableIRQ(UART_0_INST_INT_IRQN);
    NVIC_EnableIRQ(ADC12_0_INST_INT_IRQN);
    DL_TimerG_startCounter(TIMER_0_INST);
    while (1) {
            __WFE();
              // EM Lib Foreground process
        EM_foregroundProc(&gemResultStatus);
    }
}


void ADC12_0_INST_IRQHandler(void)
{

//DL_GPIO_setPins(GPIO_GRP_0_PORT, GPIO_GRP_0_PIN_0_PIN );
    int32_t tempI;
    int32_t tempV;
    switch (DL_ADC12_getPendingInterrupt(ADC12_0_INST)) {
        case DL_ADC12_IIDX_MEM1_RESULT_LOADED:
            phAISamples[phAWriteIIdx] = ((int16_t)DL_ADC12_getMemResult(ADC12_0_INST, DL_ADC12_MEM_IDX_0) - 2048 + gADCOffset) << 12;
            phAVSamples[phAWriteVIdx] = ((int16_t)DL_ADC12_getMemResult(ADC12_0_INST, DL_ADC12_MEM_IDX_1) -2048 + gADCOffset) << 12;
            //phAVSamples[phAWriteVIdx] = (temp -2048) << 12;
                        // Set the Ptrs to voltage and current

             tempI = metrology_dcFilter(&I_dc_estimate, phAISamples[phAWriteIIdx]);
             tempV = metrology_dcFilter(&V_dc_estimate, phAVSamples[phAWriteIIdx]);

            //tempI = EM_currentDCFilter(gEmSWResult.phaseBGResults[EM_PH_A_IDX].iDCEstimate, phAISamples[phAWriteIIdx]);
            phAISamples[phAWriteIIdx] = tempI;
            //tempV = EM_voltageDCFilter(gEmSWResult.phaseBGResults[EM_PH_A_IDX].vDCEstimate, phAVSamples[phAWriteVIdx]);
            phAVSamples[phAWriteVIdx] = tempV;
          

            emPhaseConfiguration[EM_PH_A_IDX].voltageSamplePtr = &phAVSamples[phAWriteIIdx];
            emPhaseConfiguration[EM_PH_A_IDX].currentSamplePtr = &phAISamples[phAWriteIIdx];
                        // Increase Index and Mask
            phAWriteIIdx = (phAWriteIIdx+1) & (HAL_ADC_I_MASK);
            phAWriteVIdx = (phAWriteVIdx+1) & (HAL_ADC_V_MASK);

                        // Process the samples
            EM_perSampleProc(gEmSWResult.phaseMetrologyPing[EM_PH_A_IDX],
                             &gEmSWConfig.meterConfig->phaseConfigPtr[EM_PH_A_IDX],
                             &gEmSWResult.phaseBGResults[EM_PH_A_IDX]);

            if(gEmSWResult.phaseMetrologyPing[EM_PH_A_IDX]->cycleCount == gEmSWConfig.algorithmConfig->mainNomFreq)
            {
                phaseDataReady |= HAL_ADC_PHASE_A_DATA_READY;
                EM_perDataChunk(&gEmSWConfig, (EM_SW_Lib_Result_Handle *) &gEmSWResult, EM_PH_A_IDX);

            }
            else if (gEmSWResult.phaseMetrologyPing[EM_PH_A_IDX]->sampleCount > (EM_SAMPLING_FREQ_HZ))
            {
                phaseDataReady |= HAL_ADC_PHASE_A_DATA_READY;
                EM_perDataChunk(&gEmSWConfig, (EM_SW_Lib_Result_Handle *) &gEmSWResult, EM_PH_A_IDX);

            }                 
   
            break;
        default:
            break;
    }
//DL_GPIO_clearPins(GPIO_GRP_0_PORT, GPIO_GRP_0_PIN_0_PIN );
}

void UART_writeBuffer(uint8_t *pData, uint8_t ui8Cnt)
{
    uint8_t res;
    while (ui8Cnt--) {
        DL_UART_transmitDataBlocking(UART_0_INST, *pData);
        pData++;
    }
}

void UART_0_INST_IRQHandler(void)
{
    volatile uint8_t gEchoData = 0;
    switch (DL_UART_Main_getPendingInterrupt(UART_0_INST)) {
        case DL_UART_MAIN_IIDX_RX:
            gEchoData = DL_UART_Main_receiveData(UART_0_INST);
            if(gEchoData == 0x22)
            {
               // DL_UART_Main_transmitData(UART_0_INST, 0x11);
                datasendflag = 1;
            }

            break;
        default:
            break;
    }
}


void EM_perSampleProc(
        EM_Metrology *metro,
        EM_Phase_Configuration *phase,
        EM_Phase_BG_Results *bgData)
{
    // // VRMS
     metro->vDotProd += int64mpy(*phase->voltageSamplePtr, *phase->voltageSamplePtr);

    // // IRMS
     metro->iDotProd += int64mpy(*phase->currentSamplePtr, *phase->currentSamplePtr);

    // // Active Power
    metro->actPowDotProd += int64mpy(*phase->voltageSamplePtr, *phase->currentSamplePtr);

    // // Reactive Power
    EM_calcReactivePowerMultAcc(metro, phase, bgData);

    // // Frequency
    EM_calcFreqPeriod(metro, phase, bgData);

    // // EM_perSample
        // Update previous voltage sample variable
    bgData->vPrevSample = *phase->voltageSamplePtr;

    // Increment the sample count
    metro->sampleCount++;
}


int64_t int64mpy(int32_t Input1, int32_t Input2)
{
    int32_t uLow,vLow;
    int32_t uHigh,vHigh;
    int64_t result;

    /* Implement MULHS, high side of 64-bit 32bitx32bit multiplication */
    uLow = Input1 & 0xFFFF;
    vLow = Input2 & 0xFFFF;

    uHigh = Input1 >> 16;
    vHigh = Input2 >> 16;

    result = (uLow*vLow);
    result += (int64_t)(uHigh*vLow + uLow*vHigh) <<16 ;
    result += (int64_t)(uHigh*vHigh) << 32;

    return (int64_t)result;
}
void EM_calcReactivePowerMultAcc(EM_Metrology *metro,
                                 EM_Phase_Configuration *phase,
                                 EM_Phase_BG_Results *bgData)
{
     // Voltage shifted by 90 degrees
    int16_t vCorrected;
    // Feedback Voltage Difference used for the IIR filter
    int32_t fbDelta;
    int16_t voltage,voltage2;
    _iq23 voltage_c;

    // Voltage Shifted by 90 degrees calculated using IIR Filter (all pass 90 degree filter)
    voltage = (int16_t)(*phase->voltageSamplePtr >> 12);
    voltage2 = (int16_t)(bgData->vPrevSample >> 12);

    fbDelta =  ((int32_t)voltage - (int32_t) bgData->reactPowPrevV90);

    vCorrected = ((int32_t)voltage2 + (int32_t) _IQ30mpy(metro->reactPowGConst,fbDelta));

    //voltage_c = (uint16_t)vCorrected << 12;
    voltage_c = vCorrected << 12;

    metro->reactPowDotProd += int64mpy(voltage_c, *phase->currentSamplePtr);

    // Update Prev V90 for IIR filter
    bgData->reactPowPrevV90 = vCorrected;
}


void EM_perDataChunk(EM_SW_Lib_Config_Handle *config,
                     EM_SW_Lib_Result_Handle *result,
                     uint16_t phaseIdx)
{
    uint32_t tempMainPeriod;
    uint32_t tempPrevMainPeriod;
    uint32_t cycleSampleCount;
    uint16_t prevSampleCount;
    _iq30 oneDivSC;

    // Switch the metro pointers Ping <==> Pong
    EM_Metrology *metroTempPtr;
    metroTempPtr = result->phaseMetrologyPing[phaseIdx];
    result->phaseMetrologyPing[phaseIdx] =  result->phaseMetrologyPong[phaseIdx];
    result->phaseMetrologyPong[phaseIdx] = metroTempPtr;

    // Read Previous sample cycle's main period
    tempMainPeriod = result->phaseMetrologyPong[phaseIdx]->mainPeriod;
    tempPrevMainPeriod = result->phaseMetrologyPing[phaseIdx]->prevMainPeriod;
    cycleSampleCount = result->phaseMetrologyPong[phaseIdx]->cycleSampleCount;
    prevSampleCount = result->phaseMetrologyPing[phaseIdx]->prevSampleCount;
    oneDivSC = result->phaseMetrologyPing[phaseIdx]->oneDivSC;

    // Copy Variables shared

    // Update background mainPeriod with previous background mainPeriod
    result->phaseMetrologyPing[phaseIdx]->mainPeriod = tempMainPeriod;

    // Update background cycleSampleCount with previous background cycleCount so interpolation
    // maintains accuracy
    result->phaseMetrologyPing[phaseIdx]->cycleSampleCount = cycleSampleCount;

    // Update foreground prevMainPeriod with previous foreground prevMainPeriod to avoid
    // calculating the G constant for the reactive G constant (if the period did not change)
    result->phaseMetrologyPong[phaseIdx]->prevMainPeriod = tempPrevMainPeriod;

    // Update the foreground prevSampleCount with previous foreground prevSampleCount to avoid
    // calculating oneDivSC
    result->phaseMetrologyPong[phaseIdx]->prevSampleCount = prevSampleCount;

    // Update the foreground oneDivSC with previous foreground oneDivSC
    result->phaseMetrologyPong[phaseIdx]->oneDivSC = oneDivSC;
}

void EM_foregroundProc(uint8_t *resultStatus)
{
	*resultStatus = HAL_ADC_PHASE_NO_DATA_READY;
	
	// Check if the background data is ready to process
    while(phaseDataReady !=  HAL_ADC_PHASE_NO_DATA_READY)
    {
        // Check if Phase A is ready
        if(phaseDataReady & HAL_ADC_PHASE_A_DATA_READY)
        {

            EM_processForegroundData(EM_PH_A_IDX);
            // Phase Results Available
            *resultStatus |= HAL_ADC_PHASE_A_DATA_READY;
            // Clear Phase Data Ready Flag
            phaseDataReady  &= ~(HAL_ADC_PHASE_A_DATA_READY);

        }

    }
}
void EM_calcFreqPeriod(EM_Metrology *metro,
                       EM_Phase_Configuration *phase,
                       EM_Phase_BG_Results *bgData)
{
    _iq23    v_delta;
    int32_t tempV;

    uint8_t k;
    int32_t divRemain;
    uint16_t divQuot;

    v_delta = *phase->voltageSamplePtr - bgData->vPrevSample;
    if(_IQ23abs(v_delta) > MAX_PER_SAMPLE_VOLTAGE_SLEW)
    {
       
        return;
    }

    //  if(*phase->voltageSamplePtr < 0)
    //      DL_GPIO_togglePins(GPIO_GRP_0_PORT, GPIO_GRP_0_PIN_0_PIN );
    // Check if there was a negative to positive transition in the voltage channel
    if (*phase->voltageSamplePtr >= 0 &&
         bgData->vPrevSample < 0)
    {
        // Apply limit to the sample count, to avoid spikes or dying power lines disturbing
        // the frequency reading too much.
        // Note: The main should be < 40Hz or >70Hz to fail this test.
        if (metro->cycleSampleCount
                > *EMSWLIB_EM_HAL_ADC_minFreqCount
                && metro->cycleSampleCount
                        < *EMSWLIB_EM_HAL_ADC_maxFreqCount)
        {

                        /* Interpolate the zero crossing by successive approx   */
            _iq23 diffVoltage = bgData->vPrevSample - *phase->voltageSamplePtr;
            _iq23 approxVoltage = 0;
            uint16_t samples = 0;
            if(*phase->voltageSamplePtr == 0)
            {
                samples = 256;
            }
            else
            {
                for (int i = 0; i<8; i++)
                {
                    samples <<= 1;
                    diffVoltage >>= 1;
                    approxVoltage += diffVoltage;

                    if(approxVoltage > bgData->vPrevSample)
                    {
                        approxVoltage -= diffVoltage;
                    }
                    else
                    {
                        samples |= 1;
                    }
                }
            }
            
            // Create a low pass filter for main_period by averaging
            // current calculation and previous; to stabilize the main_period
            // and remove noise.
            metro->mainPeriod += metro->cycleSampleCount + samples;
            metro->mainPeriod = metro->mainPeriod >> 1;
            metro->cycleSampleCount= 256-samples;
            metro->cycleCount++;
            
        }
        else
        {
            metro->cycleSampleCount = 0;
        }
    }
    else
    {
        metro->cycleSampleCount += 256;
    }
}

void EM_processForegroundData(uint8_t phaseIdx)
{

    // Calculate one over sample count
    EM_updateOneDivSC(gEmSWResult.phaseMetrologyPong[phaseIdx]);

    // // Calculate Frequency
    EM_calcFreq(gEmSWResult.phaseMetrologyPong[phaseIdx],
                &gEmSWResult.phaseResults[phaseIdx],
                gEmSWConfig.algorithmConfig);

    // Update the React Power G Constant
    EM_updateReactPowConst(gEmSWResult.phaseMetrologyPong[phaseIdx],
                           &gEmSWResult.phaseResults[phaseIdx],
                           gEmSWConfig.algorithmConfig);

    // // Calculate VRMS
    EM_calcVRMSDivSqrt(gEmSWResult.phaseMetrologyPong[phaseIdx],
                       &gEmSWConfig.meterConfig->phaseConfigPtr[phaseIdx],
                       &gEmSWResult.phaseResults[phaseIdx]);

    // // Calculate IRMS
    EM_calcIRMSDivSqrt(gEmSWResult.phaseMetrologyPong[phaseIdx],
                       &gEmSWConfig.meterConfig->phaseConfigPtr[phaseIdx],
                       &gEmSWResult.phaseResults[phaseIdx]);

    // // Calculate VPeak
    // EM_calcVPeak(&gEmSWResult.phaseResults[phaseIdx],
    //             gEmSWConfig.algorithmConfig);

    // // Calculate IPeak
    // EM_calcIPeak(&gEmSWResult.phaseResults[phaseIdx],
    //             gEmSWConfig.algorithmConfig);

    // // Calculate Active Power
    EM_calcActivePowerDiv(gEmSWResult.phaseMetrologyPong[phaseIdx],
                          &gEmSWConfig.meterConfig->phaseConfigPtr[phaseIdx],
                          &gEmSWResult.phaseResults[phaseIdx],
                          gEmSWConfig.algorithmConfig);

    // Calculate Reactive Power
    EM_calcReactivePowerDiv(gEmSWResult.phaseMetrologyPong[phaseIdx],
                            &gEmSWConfig.meterConfig->phaseConfigPtr[phaseIdx],
                            &gEmSWResult.phaseResults[phaseIdx],
                            gEmSWConfig.algorithmConfig);

    // // Calculate Apparent Power
    EM_calcApparentPower(&gEmSWResult.phaseResults[phaseIdx]);

    // // Calculate Power Factor
     EM_calcPowerFactor(&gEmSWResult.phaseResults[phaseIdx]);

    // // Calculate Active Energy
    // EM_calcActiveEnergy(gEmSWResult.phaseMetrologyPong[phaseIdx],
    //                     gEmSWConfig.algorithmConfig,
    //                     &gEmSWResult.phaseResults[phaseIdx]);

    // // Calculate Reactive Energy
    // EM_calcReactiveEnergy(gEmSWResult.phaseMetrologyPong[phaseIdx],
    //                       gEmSWConfig.algorithmConfig,
    //                       &gEmSWResult.phaseResults[phaseIdx]);

    // // Calculate Apparent Energy
    // EM_calcApparentEnergy(gEmSWResult.phaseMetrologyPong[phaseIdx],
    //                       gEmSWConfig.algorithmConfig,
    //                       &gEmSWResult.phaseResults[phaseIdx]);

    // // Clear current metrology results
  
     EM_clearMetrology(gEmSWResult.phaseMetrologyPong[phaseIdx]);

    if(datasendflag ==1)
    {
        uint8_t UART_TX_buffer[50];
        UART_TX_buffer[0] = 0x80;
        *(uint32_t *)&UART_TX_buffer[1] = gEmSWResult.phaseResults[phaseIdx].VRMS ;
        *(uint32_t *)&UART_TX_buffer[5] = gEmSWResult.phaseResults[phaseIdx].IRMS ;
        *(uint16_t *)&UART_TX_buffer[9] = gEmSWResult.phaseResults[phaseIdx].frequency ;
        *(uint64_t *)&UART_TX_buffer[11] = gEmSWResult.phaseResults[phaseIdx].activePower ;
        *(uint64_t *)&UART_TX_buffer[19] = gEmSWResult.phaseResults[phaseIdx].reactivePower ;
        *(uint64_t *)&UART_TX_buffer[27] = gEmSWResult.phaseResults[phaseIdx].apparentPower ;
        *(uint32_t *)&UART_TX_buffer[35] = gEmSWResult.phaseResults[phaseIdx].powerFactor ;

        UART_writeBuffer(UART_TX_buffer,39);

    }

}
uint8_t calcCRC8(uint8_t *pData, uint16_t len)
{
    uint16_t crc8_loop = 0;
    uint8_t crc8 = 0;

    /*  Calculate CRC */
    for (crc8_loop = 0; crc8_loop < len; crc8_loop++)
    {
        if (crc8_loop == 0)
        {
            crc8 = Crc8(*pData,0x00);
        }
        else
        {
            crc8 = (crc8^(*pData))&0xFF;
            crc8 = Crc8(crc8,0x00);
        }
    }
    return crc8;

}

/**
* @brief    crc8Table: CRC table with X^8+X^2+X^1+1
* @param
* @retval
*/
uint8_t Crc8( uint8_t inCrc, uint8_t inData )
{
    uint8_t i,crc;
    crc = inCrc ^ inData;
    for ( i = 0; i < 8; i++ )
    {
        if (( crc & 0x80 ) != 0 )
        {
            crc <<= 1;
            crc ^= 0x07;
        }
        else
        {
            crc <<= 1;
        }
    }
    return crc;
}
void EM_updateOneDivSC(EM_Metrology *metro)
{
    int16_t sampleCount = metro->sampleCount;

    if (sampleCount != metro->prevSampleCount)
    {
        // Update 1/sample_count if the previously used sample count doesn't match
        metro->oneDivSC = _IQ30div(1, metro->sampleCount);
        metro->prevSampleCount = sampleCount;
    }
}

void EM_calcFreq(EM_Metrology *metro, EM_Phase_Results *result,
                 EM_Algorithm_Configuration *algo)
{
    // If there were less than 40 interpolation during one second
    if(metro->cycleCount < 40)
    {
        result->frequency = 0x00;

    }
    // Only calculate frequency if the periods are not equal.
    else if (metro->mainPeriod != metro->prevMainPeriod)
    {
        result->frequency = (int32_t) algo->freqSampling * 256L * 100L
                / metro->mainPeriod;
    }

}


void EM_updateReactPowConst(EM_Metrology *metro, EM_Phase_Results *result,
                            EM_Algorithm_Configuration *algo)
{
    int16_t tempFreqDelta;
    uint16_t coefIdx;

    // If the mainPeriod was the same as the previous mainPeriod then don't
    // calculate a new G constant
    if (metro->mainPeriod != 0 && metro->mainPeriod == metro->prevMainPeriod)
    {
        return;
    }
    else
    {
        metro->prevMainPeriod = metro->mainPeriod;
    }

    // Check if frequency is less than min
    if (result->frequency < algo->reactPowIIRMinFreq)
    {
        metro->reactPowGConst = algo->reactPowIIRTable[0];
    }
    else
    {
        // Calculate difference between current freq and the minimum freq
        tempFreqDelta = result->frequency - algo->reactPowIIRMinFreq;
        coefIdx = tempFreqDelta >> algo->reactPowIIRShift;

        // Check if the coefficient is less than the length of the IIR buffer
        if (coefIdx < algo->reactPowIIRLength)
        {
            metro->reactPowGConst = algo->reactPowIIRTable[coefIdx];
        }
        else
        {
            metro->reactPowGConst =
                    algo->reactPowIIRTable[algo->reactPowIIRLength - 1];
        }
    }
}

void EM_calcVRMSDivSqrt(EM_Metrology *metro, EM_Phase_Configuration *phase,
                        EM_Phase_Results *result)
{
    uint64_t tempV;

    // VRMS Calculation
    // Sqrt (phaseMetrology[phaseIdx].V_dot_prod.V_sq / sample count) * K v,ph

    // Divide by the number of samples
    tempV = mul64u_64_q30((uint32_t *) &metro->vDotProd, metro->oneDivSC);
    //tempV = metro->vDotProd/metro->sampleCount;
    // Find square root of the voltage
    tempV = isqrt64(tempV) >> 32;


    // Multiply tempV by voltage scaling factor
    result->VRMS = _IQ24mpy((uint32_t)tempV, phase->phaseCalibPtr->voltageSF);
}

void EM_calcIRMSDivSqrt(EM_Metrology *metro, EM_Phase_Configuration *phase,
                        EM_Phase_Results *result)
{
    uint64_t tempI;

    // IRMS Calculation
    // Sqrt (phaseMetrology[phaseIdx].I_dot_prod.V_sq / sample count) * K i,ph

    // Divide by the number of samples
        // Divide by the number of samples
    tempI = mul64u_64_q30((uint32_t *) &metro->iDotProd, metro->oneDivSC);

    // Find square root of the current and convert into 32 bit number
        // Find square root of the voltage
    tempI = isqrt64(tempI) >> 32;

    // Multiply tempV by voltage scaling factor
    result->IRMS = _IQ24mpy(phase->phaseCalibPtr->currentSF, (uint32_t)tempI);
}
uint64_t mul64u_64_q30(uint32_t *x, uint32_t y)
{
    uint32_t x_hi = x[1];
    uint32_t x_lo = x[0];
    uint64_t result;

    result = ((((uint64_t) y * (uint64_t) ((uint32_t) x_lo)) >> 30)
            + (((uint64_t) y * (uint64_t) ((uint32_t) x_hi)) << 2));

    return result;
}
uint64_t isqrt64(uint64_t h)
{
    uint64_t x = 0;
    uint64_t y = 0;
    int i;
    /* The answer is calculated as a 64 bit value, where the last
       32 bits are fractional.
       Calling with negative numbers is not a good idea :-) */
    for (i = 0;  i < 64;  i++)
    {
        x = (x << 1) | 1;
        if (y < x)
            x -= 2;
        else
            y -= x;
        x++;
        y <<= 1;
        if ((h & 0x8000000000000000ULL))
            y |= 1;
        h <<= 1;
        y <<= 1;
        if ((h & 0x8000000000000000ULL))
            y |= 1;
        h <<= 1;
    }
    return  x;
}

void EM_calcActivePowerDiv(EM_Metrology *metro, EM_Phase_Configuration *phase,
                           EM_Phase_Results *result,
                           EM_Algorithm_Configuration *algo)
{
    EM_Power_t activePower;

    // Active Power Calculation
    // (phaseMetrology[phaseIdx].AP_dot_prod.P_accum / sample count) x K ACT,ph

    // Divide by the number of samples
    activePower = mul64_64_q30((uint32_t *) &metro->actPowDotProd,
                               metro->oneDivSC);

    //        uint8_t UART_TX_buffer[10];
    // *(uint64_t *)&UART_TX_buffer[0] = activePower;
    //  UART_writeBuffer(UART_TX_buffer,8);
    activePower = -activePower >> 23;
    // Multiply tempP by Active Power scaling factor
    activePower = mul64_64_q30((uint32_t *) &activePower,
                               phase->phaseCalibPtr->activePowerSF);

    //        uint8_t UART_TX_buffer[10];
    // *(uint64_t *)&UART_TX_buffer[0] = activePower;
    //  UART_writeBuffer(UART_TX_buffer,8);

    if (algo->residualPowerCutoff == 0)
    {
        result->activePower = activePower;
    }
    else if (llabs(activePower) > algo->residualPowerCutoff)
    {
        result->activePower = activePower;
    }
    else
    {
        result->activePower = 0;
    }


}
uint64_t mul64_64_q30(uint32_t *x, uint32_t y)
{
    int32_t x_hi = x[1];
    uint32_t x_lo = x[0];
    uint64_t result;

    result = ((((int64_t) y * (int64_t) ((uint32_t) x_lo)) >> 30)
            + ((uint64_t) ((int64_t) y * (int64_t) x_hi) << 2));

    return result;
}

void EM_calcReactivePowerDiv(EM_Metrology *metro, EM_Phase_Configuration *phase,
                             EM_Phase_Results *result,
                             EM_Algorithm_Configuration *algo)
{
    int64_t reactivePower;

    // Reactive Power Calculation
    // (phaseMetrology[phaseIdx].RP_dot_prod.P_accum / sample count) x K REACT,ph

    // Divide by the number of samples
    reactivePower = mul64_64_q30((uint32_t *) &metro->reactPowDotProd,
                                 metro->oneDivSC);

    reactivePower = -reactivePower>>23;
    // Multiply tempP by Reactive Power scaling factor
    reactivePower = mul64_64_q30((uint32_t *) &reactivePower,
                                 phase->phaseCalibPtr->reactivePowerSF);

    result->reactivePower = reactivePower;                        
}
void EM_calcApparentPower(EM_Phase_Results *result)
{
    int64_t p;
    int64_t x;
    int shift;
    /* Calculate apparent (VA) power in 0.01W increments */
    p = llabs(result->activePower);
    x = llabs(result->reactivePower);
    /* Justify for optimal accuracy */
    shift = 0;
    while ((p & 0xFFFFFFFFC0000000) || (x & 0xFFFFFFFFC0000000))
    {
        shift++;
        p >>= 1;
        x >>= 1;
    }
    x = isqrt64(p * p + x * x);
    x >>= (32 - shift);
    result->apparentPower = x;
}

EM_message_code EM_configureEnergyMeasurement(
        EM_SW_Lib_Config_Handle *config,
        EM_SW_Lib_Result_Handle *result)
{
    EM_message_code code = EM_message_code_no_error;
    uint32_t energyConstant;

    // Initialize the G Constant for the IIR Filter
    code = EM_initReactPowGConstant(config,result);

    if(code != EM_message_code_no_error)
    {
        return code;
    }

    // Initialize 1/sample_count for each phase
    code = EM_initOneDivSC(config,result);

    if(code != EM_message_code_no_error)
    {
        return code;
    }

    // Initialize square root 2 constant
    config->algorithmConfig->sqrtTwoConst = _IQ29sqrt(_IQ29(2.0));

    // Calculate 3600 seconds in one hour * sampling frequency
    energyConstant = (uint32_t) 3600* (uint32_t) config->algorithmConfig->freqSampling;

    // Initialize energy calculation constant (1/(3600 * sampling frequency))
    config->algorithmConfig->energyCalcConst = _IQ30div(1,energyConstant);

    if(config->algorithmConfig->reactPowIIRLength > 0
            && config->algorithmConfig->reactPowIIRTable == 0)
    {
        return EM_message_code_reactPowIIRTable_error;
    }

    // Check for meter configuration errors
    if(config->meterConfig->phaseLength == 0)
    {
        return EM_message_code_phaseLength_error;
    }

    if(config->meterConfig->phaseConfigPtr == 0)
    {
        return EM_message_code_phaseConfig_error;
    }

    return code;
}

EM_message_code EM_initReactPowGConstant(EM_SW_Lib_Config_Handle *config,
                                         EM_SW_Lib_Result_Handle *result)
{
    uint8_t i;
    EM_message_code code = EM_message_code_no_error;

    // Check if the IIR coefficient table was initialized correctly
    if(config->algorithmConfig->reactPowIIRLength > 0 &&
            config->algorithmConfig->reactPowIIRTable != 0)
    {
        // Initialize 1/sample_count for each phase
        for(i=0;i<config->meterConfig->phaseLength;i++)
        {
            result->phaseResults[i].frequency = (uint16_t) config->algorithmConfig->mainNomFreq * 100;
            EM_updateReactPowConst(result->phaseMetrologyPing[i],
                                   &result->phaseResults[i],
                                   config->algorithmConfig);
            EM_updateReactPowConst(result->phaseMetrologyPong[i],
                                   &result->phaseResults[i],
                                   config->algorithmConfig);
        }
    }
    else if(config->algorithmConfig->reactPowIIRLength > 0 &&
            config->algorithmConfig->reactPowIIRTable == 0)
    {
        code = EM_message_code_reactPowLength_error;
    }

    return code;
}
EM_message_code EM_initOneDivSC(EM_SW_Lib_Config_Handle *config,
                                EM_SW_Lib_Result_Handle *result)
{
    uint8_t i;
    EM_message_code code = EM_message_code_no_error;

    if(config->algorithmConfig->freqSampling != 0)
    {
        // Initialize 1/sample_count for each phase
        for(i=0;i<config->meterConfig->phaseLength;i++)
        {
            result->phaseMetrologyPing[i]->oneDivSC = _IQ30div(1, config->algorithmConfig->freqSampling);
            result->phaseMetrologyPing[i]->prevSampleCount = config->algorithmConfig->freqSampling;
        }
    }
    else
    {
        code = EM_message_code_samplingFrequency_error;
    }

    return code;
}

void EM_clearMetrology(EM_Metrology *metro)
{
    metro->actPowDotProd = 0;
    metro->iDotProd = 0;
    metro->reactPowDotProd = 0;
    metro->vDotProd = 0;
    metro->prevSampleCount = metro->sampleCount;
    metro->sampleCount = 0;
    metro->cycleCount = 0;
}

void EM_calcPowerFactor(EM_Phase_Results *result)
{
    int64_t p;
    int64_t x;
    int32_t num;
    int32_t den;

    // Power Factor = Active Power / Apparent Power
    p = llabs(result->activePower);
    x = llabs(result->apparentPower);

    if (p && x)
    {
        /* Justify for optimal accuracy */
        while ((p & 0x4000000000000000) == 0 && (x & 0x4000000000000000) == 0)
        {
            p <<= 1;
            x <<= 1;
        }
        num = p >> 32;
        den = x >> 48;
        num /= den;
        num *= 10000;
        num >>= 16;
        /* Don't let a little imprecision cause strange answers */
        if (num > 10000)
            num = 10000;
    }
    else
    {
        num = 0;
    }

    result->powerFactor = num;
}



