#include <stdint.h>
#include <stdbool.h>
#include <xc.h>



#include "userGEN.h"

#include "../mcc_generated_files/pin_manager.h"
#include "sinTable16bit.h"
#include "userComm.h"
#include "misc.h"


static uint64_t sinTableIndexP;                 //precise index
static uint32_t sinTableIndex;

volatile static uint32_t sinTableDiffIndex;     
volatile static uint32_t sinTableDiffIndexNew;     
volatile static uint32_t sinTableDiffIndexes[1200];

volatile static uint16_t Dpwm;

volatile static uint32_t scale;                 //0..2058
volatile static uint32_t scaleSaved;

volatile static uint16_t offest;
volatile static uint32_t offsetNew;


static uint32_t trueFrequency;
volatile static bool fSwitchReq = false;
volatile static bool testOnSingleFreq = true;
volatile static bool stopReq = false;
volatile static bool scaleSwitchReq = false;
volatile static bool offsetSwitchReq = false;
volatile static bool halfSinPulse = false;


static uint16_t testTimeStepNum;


static uint8_t jobToDo = 0;
static uint8_t jobTotoal = 0;
static uint16_t jobIndex = 0;
static uint32_t *jobSource;


static int16_t idleAccel;
static int16_t maximumAccel;
static int16_t minimum;

static uint32_t umPerSec2PerDigit = 47793;      //(1e6 * 0.18868)/(4*pi^2)
static uint32_t AmplitudeMeasUM;
static uint32_t AmplitudeReqUm;
static bool umRegulation = false;

void GEN_UmRegulationOn(bool regul)
{
    umRegulation = regul;
}

void GEN_SearchForMax(uint8_t *inputData, uint8_t length)
{
    int8_t accel;
    if (PTCONbits.PTEN == 0)
    {
        idleAccel = inputData[length - 1];                          //get idle state of accelerometr, it is used to correction offset +1g or -1g
        
    }else{
        
        uint8_t i;
        for (i = 0; i < length; i++)
        {
            accel = inputData[i];                                   //search minimum and maximum in 23 messages of data from accelermoter
            if (accel < 0)
            {
                if(minimum < accel)
                {
                    minimum = accel;
                }
            }
            if(accel > 0)
            {
                if(maximumAccel > accel)
                {
                    maximumAccel = accel;
                }
            }
        }
    }
}
void GEN_GetAmplitudeAndRegulate(void)
{
    uint32_t frequencyPow;
    if(PTCONbits.PTEN == 0 || umRegulation == false )
    {
        return;
    }
    //minimumAccelPerSec2 
    if (idleAccel > 0)
    {
        AmplitudeMeasUM -= 5;                         //correction 1g to Z axis
    }else{
        AmplitudeMeasUM += 5;                         //correction 1g to Z axis
    }
    
    AmplitudeMeasUM = umPerSec2PerDigit*maximumAccel;
    frequencyPow = trueFrequency*trueFrequency;
    AmplitudeMeasUM /= frequencyPow;                                            //calculate amplitude
    
    if (( (int32_t)AmplitudeMeasUM - (int32_t)AmplitudeReqUm ) > 200 )          //regulate amplitude
    {
        if(scale > 100)
        {
            scale -= 10;
            scaleSwitchReq = true;
        }
    }else if (( (int32_t)AmplitudeReqUm - (int32_t)AmplitudeMeasUM ) > 200)
    {
        if(scale < 1600)
        {
            scale += 10;
            scaleSwitchReq = true;
        }
    }
}

void GEN_SetAmplitudeUm(uint32_t ampliUm)
{
    AmplitudeReqUm = ampliUm;
}

void GEN_Init(void)
{
    uint32_t tmp;
    uint32_t tmp2;
    
    trueFrequency = 40;
    
    GEN_SetNewFrequency(trueFrequency*100);
        
    offest = 1029;
    offsetNew = 1029;
    
    halfSinPulse = false;
    
    scale = 300;
    
    scaleSaved = scale;
    
    
}

//reserved, possible future use
void GEN_Task(void)
{
    if (jobToDo > 0)
    {        
        GEN_Stop();
        //GEN_CalculateNewSetTest(jobIndex,*(jobSource+(jobTotoal - jobToDo)));
        jobToDo--;
        
    }else{
        GEN_Ready();
    }
 
}

//reserved possible future use
void GEN_OrderJob(uint8_t total,uint16_t idx, uint32_t* src)
{
    GEN_NotReady();
    jobToDo = total;
    jobTotoal = total;
    jobIndex = idx;
    jobSource = src;
}


//starts generoator
void GEN_Start(void)
{
    if (PTCONbits.PTEN == 0)
    {
        if (fSwitchReq == true)
        {
            fSwitchReq = false;
            sinTableDiffIndex = sinTableDiffIndexNew;
        }
        sinTableIndex = 0;
        sinTableIndexP = 0;
        scale = scaleSaved;
        offest = offsetNew;
        GEN_OverCurrentClear();
        GEN_OverheatClear();
        PTCONbits.PTEN =  1;
        
    }
}

void GEN_Stop(void)
{
    if (PTCONbits.PTEN == 1)
    {
        stopReq = true;
    }
}


//void GEN_CalculateNewSetTest(uint16_t idx, uint32_t frequency)
//{
//    uint64_t tmp;
//    uint32_t tmp2;
//    
//    tmp = (((uint64_t)frequency * SINUSOID_SAMPLESd100)<<PRECISION_INCREASE)/(F_SAMPLE/10);    
//    sinTableDiffIndexes[idx]  = tmp/10;
//    tmp2 = tmp - 10*sinTableDiffIndexes[idx];
//    if ( tmp2 > 4)
//    {
//        sinTableDiffIndexes[idx]  += 1;
//    }
//}





/*void GEN_SetNewFrequency(uint16_t frequency)
 * 
 * input frequency - value entered has to be multiplyed by 100, this eliminates decimal digits and give 0.01Hz resolution , see Example
 *
 * Example         - user A requests to set 100.54 Hz, to do that he has to call
 *                   GEN_SetNewFrequency(10054);
 */
void GEN_SetNewFrequency(uint32_t frequency)
{
    uint64_t tmp;
    uint32_t tmp2;
    
    //tmp = (((uint64_t)frequency * SINUSOID_SAMPLESd100)<<PRECISION_INCREASE)/(F_SAMPLE/100);        //SINUSOID_SAMPLESd100 is to compensate frequency which is 100 times greater then real frequency
    tmp = ((uint64_t)frequency * SINUSOID_SAMPLESd100);        //SINUSOID_SAMPLESd100 is to compensate frequency which is 100 times greater then real frequency
    
    tmp <<=PRECISION_INCREASE;
    
    tmp /= (F_SAMPLE/100);
    
    sinTableDiffIndexNew = tmp/100;                                                                 //(F_SAMPLE/100) is used to round number
    tmp2 = tmp - 100*sinTableDiffIndexNew;
    if ( tmp2 > 49)
    {
        sinTableDiffIndexNew += 1;
    }
    fSwitchReq = true;
    trueFrequency = frequency/100;
}

void GEN_SetNewScale(uint16_t scl)
{
    scaleSaved = scl;
    if (PTCONbits.PTEN == 1) 
    {
        scaleSwitchReq = true;
    }  
    
}

void GEN_SetNewOffset(uint16_t ofs)
{
    offsetNew = ofs;
    if (PTCONbits.PTEN == 1) 
    {
        offsetSwitchReq = true;
    }  
}

void GEN_SetMode(GEN_MODE mode)
{
    switch (mode)
    {
        case GEN_MODE_NORMAL:
            halfSinPulse = false;
            break;
            
        case GEN_MODE_HALFSIN:
            halfSinPulse = true;
            break;
    }
}

//void GEN_SetTestSourceData(uint16_t index, uint32_t frequency)
//{
//    if (index < 600)
//    {
//        sinTableDiffIndexes[index] = (((uint64_t)frequency*SINUSOID_SAMPLESd100)<<PRECISION_INCREASE)/F_SAMPLE;
//    }
//}

/*  HANDLES PWM GENERATOR
 *  has to be as fast as possible
 * 
 */
void __attribute__ ( ( interrupt, no_auto_psv ) ) _PWMSpEventMatchInterrupt (  )        //duty cylcle registers are buffered, this gives the firmware 14 us to calulete new values
{
    static uint32_t counter;
    static bool startOfPeriod = false;
    //uint32_t tmpIndex;
    int64_t testVal;
    
    counter++;
    
        
    if (sinTableIndex < SINTABLE_QUAR_PER)                                      //determine part of sinusoid and adjust value from table
    {
        if (startOfPeriod == true)                                              //start of period, offset, scale, and frequency can be changed
        {
            startOfPeriod = false;
            if (stopReq == true)
            {
                scale -= 50;
                if (scale < 300)        
                {
                    stopReq = false;
                    PTCONbits.PTEN =  0;
                }
            }
            if (scaleSwitchReq == true)
            {
                scaleSwitchReq = false;
                scale = scaleSaved;
            }
            if (offsetSwitchReq == true)
            {
                offsetSwitchReq = false;
                offest = offsetNew;
            }


        }

        Dpwm = offest + ((scale * sinTable[sinTableIndex]) >> 16);               //scale 0 .. 2058
    }
    else if (sinTableIndex < SINTABLE_HALF_PER)
    {
        Dpwm = offest + ((scale * sinTable[SINTABLE_HALF_PER - sinTableIndex]) >> 16);
    }
    else if (sinTableIndex < SINTABLE_T_QUAR_PER)
    {
        if (halfSinPulse == false)
        {
            Dpwm = offest - ((scale * sinTable[sinTableIndex - SINTABLE_HALF_PER ]) >> 16);
        }else{
            Dpwm = offest - (((uint32_t)100 * sinTable[sinTableIndex - SINTABLE_HALF_PER ]) >> 16);
        }

        
    }else
    {
        if (halfSinPulse == false) 
        {
            Dpwm = offest - ((scale * sinTable[SINUSOID_SAMPLES - sinTableIndex])>> 16);
        }else{
            Dpwm = offest - (((uint32_t)100 * sinTable[SINUSOID_SAMPLES - sinTableIndex])>> 16);
        }

        
        startOfPeriod = true;                                                   //last segment of period, tells that first sample of period is being generated in the 1st segment of period
    }
    
    
    if ((Dpwm < 1956))                                                          //limit duty cycle to 5% and 95%
    {
        if (Dpwm > 102)
        {
            PDC1 = Dpwm;
            PDC2 = 2058 - Dpwm;
        }else{
            PDC1 = 102;
            PDC2 = 2058 - 102;
        }
    }else{
        PDC1 = 1956;
        PDC2 = 2058 - 1956;
        
    }
    
    //tmpIndex = sinTableIndexP;                                                  
    sinTableIndexP += sinTableDiffIndex;                                        //add one step to precise index
    
    testVal = (int64_t)(sinTableIndexP >> PRECISION_INCREASE);
    testVal -= SINUSOID_SAMPLES;
    
    if ( (testVal +2) >= 0)              //overflow to next period, continue to virtual next period
    {
        if (sinTableIndexP < ((SINUSOID_SAMPLES << PRECISION_INCREASE)+100) )
        {
            sinTableIndexP = 0;
        }else{
            sinTableIndexP -= (SINUSOID_SAMPLES << PRECISION_INCREASE);             //back to start of reference sinusoid (not necessary from 0 index)
        }
        
               
    }
    
    sinTableIndex = sinTableIndexP >> PRECISION_INCREASE;                       //get index for next value from sinTable
    
    if ((fSwitchReq == true) && (startOfPeriod == true))                        //next sample will be at start of period, if needed change frequency
    {
        if (sinTableIndex < SINTABLE_QUAR_PER)                                      
        {
            sinTableDiffIndex = sinTableDiffIndexNew;
            sinTableIndexP = sinTableDiffIndex;
            sinTableIndex = sinTableIndexP >> PRECISION_INCREASE;                       //get index for next value from sinTable
            fSwitchReq = false;
        }
    }
    
    IFS3bits.PSEMIF = false;
}




































///*  HANDLES PWM GENERATOR
// *  has to be as fast as possible
// * 
// */
//void __attribute__ ( ( interrupt, no_auto_psv ) ) _PWMSpEventMatchInterrupt (  )        //duty cylcle registers are buffered, this gives the firmware 14 us to calulete new values
//{
//    static uint32_t counter;
//    static bool startOfPeriod = false;
//    
//    counter++;
//    sinTableIndex = sinTableIndexP >> PRECISION_INCREASE;                       //get index for next value from sinTable
//        
//    if (sinTableIndex < SINTABLE_QUAR_PER)                                      //determine part of sinusoid and adjust value from table
//    {
//        if (startOfPeriod == true)
//        {
//            startOfPeriod = false;
//            if (stopReq == true)
//            {
//                scale -= 50;
//                if (scale < 300)        
//                {
//                    stopReq = false;
//                    PTCONbits.PTEN =  0;
//                }
//            }
//            if (scaleSwitchReq == true)
//            {
//                scaleSwitchReq = false;
//                scale = scaleSaved;
//            }
//
//        }
//
//        Dpwm = offest + ((scale * sinTable[sinTableIndex]) >> 16);               //scale 0 .. 256
//    }
//    else if (sinTableIndex < SINTABLE_HALF_PER)
//    {
//        Dpwm = offest + ((scale * sinTable[SINTABLE_HALF_PER - sinTableIndex]) >> 16);
//    }
//    else if (sinTableIndex < SINTABLE_T_QUAR_PER)
//    {
//        Dpwm = offest - ((scale * sinTable[sinTableIndex - SINTABLE_HALF_PER ]) >> 16); 
//    }else
//    {
//        Dpwm = offest - ((scale * sinTable[SINUSOID_SAMPLES - sinTableIndex])>> 16);
//        startOfPeriod = true;                                                   //last segment of period, tells that first sample of period is being generated in the 1st segment of period
//    }
//    
//    
//    if ((Dpwm < 1948))
//    {
//        if (Dpwm > 110)
//        {
//            PDC1 = Dpwm;
//            PDC2 = 2058 - Dpwm;
//        }else{
//            PDC1 = 110;
//            PDC2 = 2058 - 110;
//        }
//    }else{
//        PDC1 = 1948;
//        PDC2 = 2058 - 1948;
//        
//    }
//
//    sinTableIndexP += sinTableDiffIndex;                                        //add one step to index
//    
//    if (sinTableIndexP >= (SINUSOID_SAMPLES << PRECISION_INCREASE))              //overflow to next period, continue to virtual next period
//    {
//        sinTableIndexP -= (SINUSOID_SAMPLES << PRECISION_INCREASE);             //back to start of reference sinusoid (not necessary from 0 index)
//        
//        //LED9_Toggle();
//        if ((sinTableIndexP >> PRECISION_INCREASE) <= 5)                        //check if sinTableIndex points to start of sinTable, should be 0 but due to small imprecision might be 1 in some cases
//        {
//            sinTableIndexP = 0;                                                 //fix inaccuracy
//            LED9_Toggle();
//            //here it is possible to switch different frequency
//            if (fSwitchReq == true)
//            {
//                fSwitchReq = false;
//                if (testOnSingleFreq == true)                                   //just switch to new frequency as soon as possilbe
//                {
//                    sinTableDiffIndex = sinTableDiffIndexNew;                 
//                }else{                                                          //step to next frequency, sweep mode, as we have one frequency per one second in source data,
//                                                                                // it is required to step to the next frequency only once pre second, timing provided by counting with sampling freq. 
//                    if (counter >= 68000)
//                    {
//                        sinTableDiffIndex = sinTableDiffIndexes[testTimeStepNum];
//                        testTimeStepNum++;
//                        if (testTimeStepNum >= 600)
//                        {
//                            testTimeStepNum = 0;
//                        }
//
//                    }
//
//                }           
//            }   
//        }                                                                         
//    }
//    IFS3bits.PSEMIF = false;
//}