//#############################################################################
//
// FILE:   empty_driverlib_main.c
//
// TITLE:  Empty Project
//
// Empty Project Example
//
// This example is an empty project setup for Driverlib development.
//
//#############################################################################
//
//
// $Copyright:
// Copyright (C) 2022 Texas Instruments Incorporated - http://www.ti.com
//
// 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.
// $
//#############################################################################

//
// Included Files
//
#include "driverlib.h"
#include "device.h"
#include "board.h"
#include "c2000ware_libraries.h"
#include "math.h"

#include "rampgen.h"
#include "abc_dq0_neg.h"
#include "abc_dq0_pos.h"
#include "dq0_abc.h"
#include "spll_3ph_ddsrf.h"
#include "spll_3ph_srf.h"

#define TINV_PI                 ((float32_t)3.141592653589)
#define TINV_ISR1_FREQUENCY_HZ  19200
#define TINV_AC_FREQ_HZ         ((float32_t)60)
#define TWO_PI                  6.2831853

RAMPGEN TINV_rgen;

ABC_DQ0_POS TINV_vGrid_dq0_pos;
ABC_DQ0_NEG TINV_vGrid_dq0_neg;

DQ0_ABC TINV_vInv_dq0;
SPLL_3PH_DDSRF TINV_spll_3ph_1;

volatile float32_t TINV_angle_radians;
float32_t TINV_angleSPLL_radians = 0;
float32_t TINV_sine_A;
float32_t TINV_sine_B;
float32_t TINV_sine_C;
float32_t TINV_cosine_A;
float32_t TINV_cosine_B;
float32_t TINV_cosine_C;
float32_t TINV_sine;
float32_t TINV_cosine;

float32_t TINV_vGrid_A_sensed_pu;
float32_t TINV_vGrid_B_sensed_pu;
float32_t TINV_vGrid_C_sensed_pu;

float32_t theta = 0;
float32_t GoalF = 60;
void TINV_runTransformOnSensedSignals(void);
void TINV_generateInternalReference(void);
void TINV_runSPLL(float32_t v_p_d, float32_t v_n_d,float32_t v_p_q, float32_t v_n_q);
void theta_generator(float *baseTheta);
//
// Main
//
void main(void)
{

    //
    // Initialize device clock and peripherals
    //
    Device_init();

    //
    // Disable pin locks and enable internal pull-ups.
    //
    Device_initGPIO();

    //
    // Initialize PIE and clear PIE registers. Disables CPU interrupts.
    //
    Interrupt_initModule();

    //
    // Initialize the PIE vector table with pointers to the shell Interrupt
    // Service Routines (ISR).
    //
    Interrupt_initVectorTable();

    //
    // PinMux and Peripheral Initialization
    //
    Board_init();
    //
    // C2000Ware Library initialization
    //
    C2000Ware_libraries_init();
    //
    // RAMPGEN initialization
    //
    RAMPGEN_reset(&TINV_rgen);
    RAMPGEN_config(&TINV_rgen,
                   TINV_ISR1_FREQUENCY_HZ,
                   TINV_AC_FREQ_HZ);

    // DQ transform initialization
    ABC_DQ0_POS_reset(&TINV_vGrid_dq0_pos);
    ABC_DQ0_NEG_reset(&TINV_vGrid_dq0_neg);

    // SPLL initialization
    SPLL_3PH_DDSRF_init(TINV_AC_FREQ_HZ,
                        (float32_t)(1.0 / TINV_ISR1_FREQUENCY_HZ),
                        (float32_t)(0.00188141f),
                        (float32_t)(-0.99623717f),
                        &TINV_spll_3ph_1);

    TINV_spll_3ph_1.lpf_coeff.b0 = 333.807f;
    TINV_spll_3ph_1.lpf_coeff.b1 = -333.674f;

    //
    // Enable Global Interrupt (INTM) and real time interrupt (DBGM)
    //
    EINT;
    ERTM;

    while(1)
    {
        
    }
}

uint16_t DACA_Out;
uint16_t DACC_Out;

__interrupt void cpuTimer0ISR(void)
{
    ABC_DQ0_POS_run(&TINV_vGrid_dq0_pos,
            TINV_vGrid_A_sensed_pu,
            TINV_vGrid_B_sensed_pu,
            TINV_vGrid_C_sensed_pu,
            TINV_sine, TINV_cosine);

    ABC_DQ0_NEG_run(&TINV_vGrid_dq0_neg,
            TINV_vGrid_A_sensed_pu,
            TINV_vGrid_B_sensed_pu,
            TINV_vGrid_C_sensed_pu,
            TINV_sine, TINV_cosine);

    TINV_runSPLL(TINV_vGrid_dq0_pos.d,
                TINV_vGrid_dq0_neg.d,
                TINV_vGrid_dq0_pos.q,
                TINV_vGrid_dq0_neg.q);

    TINV_sine = sinf(TINV_angleSPLL_radians);
    TINV_cosine = cosf(TINV_angleSPLL_radians);

    //DACA_Out = (uint16_t)((float)theta * 650);
    DACA_Out = (uint16_t)((float)TINV_angle_radians * 650);
    DACC_Out = (uint16_t)((float)TINV_angleSPLL_radians * 650);

    DAC_setShadowValue(DAC_A_BASE,DACA_Out);
    DAC_setShadowValue(DAC_C_BASE,DACC_Out);


    Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP1);
}

uint16_t ReSetPLLTheta = 0;
__interrupt void cpuTimer1ISR(void)
{
    if(ReSetPLLTheta == 1)
    {
        GPIO_writePin(Debug,1);
        ReSetPLLTheta = 0;
        TINV_angleSPLL_radians = 0;
        SPLL_3PH_DDSRF_reset(&TINV_spll_3ph_1);
    }
    TINV_generateInternalReference();

    GPIO_writePin(Debug,0);
    Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP1);
}

void TINV_runTransformOnSensedSignals(void)
{
    //
    // Run ABC DQ0 POS on the grid voltage
    // with the locked PLL angle
    //
    ABC_DQ0_POS_run(&TINV_vGrid_dq0_pos,
            TINV_vGrid_A_sensed_pu,
            TINV_vGrid_B_sensed_pu,
            TINV_vGrid_C_sensed_pu,
            TINV_sine, TINV_cosine);

    //
    // Run ABC DQ0 NEG on the grid voltage
    // with the locked PLL angle
    //
    ABC_DQ0_NEG_run(&TINV_vGrid_dq0_neg,
            TINV_vGrid_A_sensed_pu,
            TINV_vGrid_B_sensed_pu,
            TINV_vGrid_C_sensed_pu,
            TINV_sine, TINV_cosine);
}

void TINV_generateInternalReference(void)
{
    //
    // Generate the RAMP TINV_angle
    //
    RAMPGEN_run(&TINV_rgen);

    //
    // Compute TINV_angle and sine and cosine
    //
    TINV_angle_radians = TINV_rgen.out * TINV_PI * 2.0f;

    //
    // Use the TINV_angle value to compute the sine value
    //
    TINV_sine_A = sinf(TINV_angle_radians);
    TINV_cosine_A = cosf(TINV_angle_radians);

    TINV_sine_B = sinf(TINV_angle_radians - TINV_PI * 2.0f / 3.0f);
    TINV_cosine_B = cosf(TINV_angle_radians - TINV_PI * 2.0f / 3.0f);

    TINV_sine_C = sinf(TINV_angle_radians - TINV_PI * 4.0f / 3.0f);
    TINV_cosine_C = cosf(TINV_angle_radians - TINV_PI * 4.0f / 3.0f);

    TINV_vGrid_A_sensed_pu = TINV_sine_A;
    TINV_vGrid_B_sensed_pu = TINV_sine_B;
    TINV_vGrid_C_sensed_pu = TINV_sine_C;
}

void TINV_runSPLL(float32_t v_p_d, float32_t v_n_d,
                                float32_t v_p_q, float32_t v_n_q)
{
    /*
    SPLL_3PH_SRF_run(v_p_q, &TINV_spll_3ph_2);

    TINV_angleSPLL_radians = TINV_spll_3ph_2.theta[1];
    */
    SPLL_3PH_DDSRF_run(&TINV_spll_3ph_1,
                           v_p_d, v_n_d,
                           v_p_q, v_n_q);
    TINV_angleSPLL_radians = TINV_spll_3ph_1.theta[1];

}

//
// End of File
//