// LUT_Sin C File
// Used resources DAC1, DMA2_Ch3, TIM6 and pin xx
// Write Juha Linna
// (c) 2010 Juha Linna

#include "Errors.h"
#include "stm32f10x_dac.h"
#include "stm32f10x_tim.h"
#include "stm32f10x_dma.h"
#include "stm32f10x_rcc.h"
#include "LUTSin.h"


#define Pi 3.1415926535
//Pii cont 8979323846264338327950288419716939937510


// Inside module variables and tables

u16 pLutDataTbl[10000];
u16 pLutDataTbl_1k[1000];
u16 pLutDataTbl_10k[100];
u16 pLutDataTbl_100k[10];

u16 LutDataReady_1k;
u16 LutDataReady_10k;
u16 LutDataReady_100k;
u32 LutDataTblFreq;

u16 LutDataAmpl_1k;
u16 LutDataAmpl_10k;
u16 LutDataAmpl_100k;
u16 LutDataAmpl;

u16 LutDataOffs_1k;
u16 LutDataOffs_10k;
u16 LutDataOffs_100k;
u16 LutDataOffs;


u16 LutDataSize = 10000;

// Module private functions
float LUTSin_Sin(float Phase);
float LUTSin_Sin2(float Phase);
double LUTSin_xx_2(int n);


/*******************************************************************************
* Function Name  : LUTSin_SetFreq
* Description    : Set LUT Stepping for decided frequency. This calulates new 
*                  new step values for LUT next pos calculation.
*
* Input          : Freq (100 = 100Hz, 500 = 500Hz)
*                  out[t] = sin(t*f*2*pi) * Ampl + Offs                
* Return         : 0
*******************************************************************************/
int LUTSin_Init(long Freq, int Ampl, int Offs)
{

    // Timer scalers
    RCC_HCLKConfig(RCC_SYSCLK_Div1);
    RCC_PCLK1Config(RCC_HCLK_Div2);
    RCC_PCLK2Config(RCC_HCLK_Div1);


    // Enable CLK
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE);

    LutDataTblFreq      = 0;
    LutDataReady_1k     = 0;
    LutDataReady_10k    = 0;
    LutDataReady_100k   = 0;

    LutDataAmpl_1k      = 0;
    LutDataAmpl_10k     = 0;
    LutDataAmpl_100k    = 0;
    LutDataAmpl         = 0;

    LutDataOffs_1k      = 0;
    LutDataOffs_10k     = 0;
    LutDataOffs_100k    = 0;
    LutDataOffs         = 0;

    LUTSin_SetFreq(Freq, Ampl, Offs); 

// Init Timer
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
    TIM6->CR1 = 0;
    TIM6->CR2 = 0x20; // 0;
    TIM6->SR  = 0;
    TIM6->EGR = 0;
    TIM6->PSC = 0;
    TIM6->ARR = 72; // 72 with 72MHz clk -> 1MHz DAC Update
    TIM6->DIER = 0x0100; // Enable DMA request
    TIM6->CR1  = 1;      // Start Timer
    
    //TIM_DMACmd(TIM6, TIM_DMA_Update, ENABLE);

    

// Init DAC
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_DAC, ENABLE);
     
    DAC_InitTypeDef DAC_InitStruct;
    DAC_StructInit(&DAC_InitStruct);
    DAC_InitStruct.DAC_Trigger = DAC_Trigger_T6_TRGO;
    DAC_InitStruct.DAC_OutputBuffer = DAC_OutputBuffer_Enable;
    DAC_Init(DAC_Channel_1, &DAC_InitStruct);
    
    DAC_DMACmd(DAC_Channel_1, ENABLE);
    DAC_Cmd(DAC_Channel_1, ENABLE);

    return GL_OK;

}
    
/*******************************************************************************
* Function Name  : LUTSin_SetFreq
* Description    : Calculate new sintable for 1MHz DAC sampling. 
*
* Input          : Freq (100 = 100Hz, 500 = 500Hz)
* Return         : 0
*******************************************************************************/
int LUTSin_SetFreq(long Freq, int Ampl, int Offs)
{
    // ReInit Sin Table
    int pos, tmp;
    int GenLen = 1000000 / Freq; // 1MHz / 100KHz = 10
    int AmplNeg = Ampl * -1;

    u16 *pDataTbl = 0;
    u16 *pDataReady = 0;
    u16 *pDataAmpl = 0;
    u16 *pDataOffs = 0;
    u16  LocalDataReady = 0;
    float PiStep;

    // Chk if selected freq is already generated
    // there are backs for 100 kHz, 10 kHz and 1 kHz 
    // Warning pointer war :)

    switch(Freq)
    {
        case 1000:   //   1 kHz
            pDataReady  = &LutDataReady_1k;
            pDataTbl    =  pLutDataTbl_1k;      // Pointer to table
            pDataAmpl   = &LutDataAmpl_1k;
            pDataOffs   = &LutDataOffs_1k;
            break;
        case 10000:  //  10 kHz
            pDataReady  = &LutDataReady_10k;
            pDataTbl    =  pLutDataTbl_10k;     // Pointer to table
            pDataAmpl   = &LutDataAmpl_10k;
            pDataOffs   = &LutDataOffs_10k;
            break;
        case 100000: // 100 kHz
            pDataReady  = &LutDataReady_100k;
            pDataTbl    =  pLutDataTbl_100k;    // Pointer to table
            pDataAmpl   = &LutDataAmpl_100k;
            pDataOffs   = &LutDataOffs_100k;
            break;
        default: // Other Freq
            pDataReady  = &LocalDataReady;
            pDataTbl    =  pLutDataTbl;         // Pointer to table
            pDataAmpl   = &LutDataAmpl;
            pDataOffs   = &LutDataOffs;
            if(LutDataTblFreq == Freq)
                LocalDataReady = 1;
            else
                LutDataTblFreq = Freq;
            break;
    }

    
    if( (*pDataAmpl != Ampl) || ( *pDataOffs != Offs)) // Chk if need recreate
    {    
        *pDataReady = 0;
        *pDataAmpl  = Ampl;
        *pDataOffs  = Offs;
    }
    if(*pDataReady == 0) // if not exists then create
    {
        if(GenLen > LutDataSize)
            GenLen = LutDataSize;

        PiStep = (2 * Pi) / GenLen;
        
        for(pos = 0; pos < GenLen; pos++)
        {
            tmp = (int) Ampl * LUTSin_Sin( pos * PiStep );

            if (tmp > Ampl)
                tmp = Ampl;
            if (tmp < AmplNeg)
                tmp = AmplNeg;
            
            pDataTbl[pos] = Offs + (u16) tmp;
        }
    }

    // Init DMA for current data
    DMA_Cmd(DMA2_Channel3, DISABLE);
    DMA_InitTypeDef DMA_DMADAC_Init;
    DMA_StructInit(&DMA_DMADAC_Init);
    DMA_DMADAC_Init.DMA_PeripheralBaseAddr = (u32) DAC + 0x08; //DAC_DHR12R1
    DMA_DMADAC_Init.DMA_MemoryBaseAddr     = (u32) &pDataTbl[0];
    DMA_DMADAC_Init.DMA_DIR                = DMA_DIR_PeripheralDST;
    DMA_DMADAC_Init.DMA_BufferSize         = GenLen;
    //DMA_DMADAC_Init.DMA_PeripheralInc    = DMA_MemoryInc_Disable;
    DMA_DMADAC_Init.DMA_MemoryInc          = DMA_MemoryInc_Enable;
    DMA_DMADAC_Init.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
    DMA_DMADAC_Init.DMA_MemoryDataSize     = DMA_MemoryDataSize_HalfWord;
    DMA_DMADAC_Init.DMA_Mode               = DMA_Mode_Circular;
    DMA_DMADAC_Init.DMA_Priority           = DMA_Priority_High;
    //DMA_DMADAC_Init.DMA_M2M;DMADAC
    DMA_Init(DMA2_Channel3, &DMA_DMADAC_Init);
    DMA_Cmd(DMA2_Channel3, ENABLE);

    return GL_OK;
}

/*******************************************************************************
* Function Name  : LUTSin_Sin
* Description    : Calculate sin value of Phase in radians. 
*                  Trim phase to between -pi/2 < x < pi/2
*                  sin(x) = x - (x^3/!3) + (x^5/!5) - (x^7/!7)  when -pi/2 < x < pi/2
*
* Input          : Phase in radians
* Return         : sin(Phase)
*******************************************************************************/


// Math Sin
float LUTSin_Sin(float Phase) // +/- 2Pi area
{
    int sing = 1, n;
    if (Phase < 0.0)
    {
        Phase *= -1;
        sing = -sing;
    }

    // Add range to +6 Pi
    for (n=0; n<3; n++)
        if (Phase > (2* Pi))
        {
            Phase -= 2* Pi;
        }
        else
        {
            break;
        }

    if(Phase > Pi)
    {
        Phase -= Pi;
        sing = -sing;
    }

    /*
    if(Phase > (Pi / 2))
    {
        Phase = Pi - Phase;
    }
    */

    return LUTSin_Sin2(Phase) * sing;

}
float LUTSin_Sin2(float Phase)
{
    int n;
    float out = 0, tmp;
    tmp = Phase;
    for(n = 0; n < 4; n++)
    {
        out += tmp/LUTSin_xx_2(1+n*2);
        tmp *= -Phase*Phase;
    }

    return out;

}



double LUTSin_xx_2(int n)
{
    double x = 1;
    int y;
    for(y = 2; y <= n; y++)
    {
        x *= y;
    }
    
    return x;
}