/*****************************************************************************/
/*                                                                           */
/* FILENAME                                                                  */
/* 	 main.c                                                                  */
/*                                                                           */
/* DESCRIPTION                                                               */
/*   TMS320C5515 USB Stick Application 20 Spectrum Analyser.                 */
/*   Take microphone input and send to headphones with ring modulation.      */
/*                                                                           */
/* REVISION                                                                  */
/*   Revision: 1.00	                                                         */
/*   Author  : Richard Sikora                                                */
/*---------------------------------------------------------------------------*/
/*                                                                           */
/* HISTORY                                                                   */
/*   Revision: 1.00                                                          */
/*   15th September 2010. Created from TMS320C5505 USB Stick code.           */
/*                                                                           */
/*****************************************************************************/
/*
 * Copyright (C) 2010 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.
 *
*/

#include "stdio.h"
#include "usbstk5515.h"
#include "usbstk5515_led.h"
#include "aic3204.h"
#include "PLL.h"
//#include "bargraph.h"
#include "oled.h"
#include "pushbuttons.h"
#include "stereo.h"
#include "dsplib.h"
#include "sinewaves.h"
#include "math.h"

#include "IIR_filters_fourth_order.h"

#include "IIR_low_pass_filters.h"

Int16 left_input;
Int16 right_input;
Int16 left_output;
Int16 right_output;
Int16 mono_input;

int PSKxPSK;
int BP_PSKxPSK;
Int16 iloczyn = 10000;
Int16 nosna = 100;
Int16 feedback = 10000;
Int16 out = 1000;




Int16 PSK;
Int16 nosna_prostokat = 0;
Int16 nosna_sin = 0;
Int16 PSK_x_nosna;
Int16 PSK_x_puls;
Int16 dem_PSK;
Int16 dem_PSK_bity;

int fo = 6000;
int B3dB = 500;

signed int bufor[4];
unsigned int index = 0;

signed int wsp1[6] = {0, 0, 0, 0, 0, 0 };
signed int wsp2[6] = {0, 0, 0, 0, 0, 0 };

#define SAMPLES_PER_SECOND 48000
#define GAIN_IN_dB 0

//unsigned long int i = 0;

unsigned int Step = 0; 
unsigned int LastStep = 99;


/* Numerator coefficients */
#define B0 0
#define B1 1
#define B2 2

/* Denominator coefficients */
#define A0 3
#define A1 4
#define A2 5 

void wsp_IIR_BP(int fo, int B3dB, signed int *wsp)
{
    double b = cos(2.0*3.1415927*B3dB/SAMPLES_PER_SECOND);
    double delta = 4.0 - 4.0*b*b;
    double beta = cos(2.0*3.1415927*fo/SAMPLES_PER_SECOND);
    double alfa = (2.0 - sqrt(delta))/(2.0*b);
	int K = (int)((1.0 - alfa)*(1<<14));	
		
	wsp[B0] = K ;   // B0
    wsp[B1] = 0;    // B1/2
    wsp[B2] = -K;   // B2
    
    wsp[A0] = 32767;    								// A0 = 1.0
    wsp[A1] = (int)((-beta*(1.0 + alfa))*(1 << 14));    // A1/2
    wsp[A2] = (int)(alfa*(1L<<15));   					// A2	
}     

void wsp_IIR_BS(int fo, int B3dB, signed int *wsp)
{	
	double b = cos(2.0*3.1415927*B3dB/SAMPLES_PER_SECOND);
    double delta = 4.0 - 4.0*b*b;
    double beta = cos(2.0*3.1415927*fo/SAMPLES_PER_SECOND);
    double alfa = (2.0 - sqrt(delta))/(2.0*b);
    int K = (int)((1.0 + alfa)*(1<<14));
	
	wsp[B0] = K ;   			// B0
    wsp[B1] = (int)(-beta* K);  // B1/2
    wsp[B2] = K ;				// B2
    
    wsp[A0] = 32767;    								// A0 = 1.0
    wsp[A1] = (int)((-beta*(1.0 + alfa))*(1 << 14));    // A1/2
    wsp[A2] = (int)(alfa*(1L<<15));   					// A2	
}

//koniec projektowania



//int j = 0;
//int k = 0;
//int m = 0;
//int display_counter = 0;
//int waveform_counter;
//long delay;

//DATA input_buffer[1024]; /* Must be declared as DATA for dsplib compatibility */

//DATA buffer2[1024];
//int display_buffer[96];
//int buffer1[96];

 
/* ------------------------------------------------------------------------ *
 *                                                                          *
 *  main( )                                                                 *
 *                                                                          *
 * ------------------------------------------------------------------------ */
void main( void ) 
{
    /* Initialize BSL */
    USBSTK5515_init( );
	
	/* Initialize PLL */
	pll_frequency_setup(120);

    /* Initialise hardware interface and I2C for code */
    aic3204_hardware_init();
    
    /* Initialise the AIC3204 codec */
	aic3204_init(); 
	
	/* Turn off the 4 coloured LEDs */
	USBSTK5515_ULED_init();
	
	/* Initialise the OLED LCD display */
	oled_init();
	SAR_init();
	/* Flush display buffer */
    oled_display_message("                 ", "                   ");
    
    printf("\n\nRunning Project Spectrum Analyser\n");
    printf( "<-> Audio Loopback from Microphone In --> to Headphones/Lineout\n\n" );

	/* Setup sampling frequency and  gain for microphone */
    set_sampling_frequency_and_gain(SAMPLES_PER_SECOND, GAIN_IN_dB);

    oled_display_message("Application 20     ", "Spectrum Analyser  ");

	/* New. Add descriptive text */ 
	puts("\n Bargraph at 6dB intervals");
    puts("\n Press SW1 for DOWN, SW2 for UP, SW1 + SW2 for reset\n");
    puts(" Step 1   = Straight through, no signal processing. Set levels");
    puts(" Step 2   = Waveform view");
    puts(" Step 3   = FFT 1024 Display. Calculate power and display as bargraph");
    puts(" Step 4   = FFT 512 Display. Calculate power and display as bargraph");

      
	/* Default to XF LED off */
	asm(" bclr XF");
	
	fo = 6000;
    B3dB = 500;
    wsp_IIR_BP(2*fo, B3dB, wsp1);

    
 	for ( ;; )
 	{

     aic3204_codec_read(&left_input, &right_input); // Configured for one interrupt per two channels.

     PSK = left_input;
 	 PSKxPSK = ((long)PSK * PSK) >> 15;	 
 	 PSKxPSK = _shrs(PSKxPSK, -3);
 	 
     BP_PSKxPSK =  fourth_order_IIR_direct_form_IA(wsp1, PSKxPSK);
      
     // PLL 
     iloczyn = ((long)BP_PSKxPSK * feedback) >> 15;		// detektor fazy     
     out = second_order_IIR_direct_form_I(IIR_low_pass_1200Hz, iloczyn); //filtr dolnoprzepustowy       
     nosna = generate_sinewave_1(fo + (out >>2), 10000);   	// VCO
     feedback = ((long)nosna * nosna) >> 15;				// kwadrat nośnej
     feedback -= 12207/4;									// usunięcie składowej stałej
      
      // opóźnienie nośnej
     nosna_sin = bufor[index];		// opóźniona nośna
     bufor[index] = nosna;				// opóźnienie nośnej o pi/2
     if(fo==3000) index = ++index % 4;  // index = ++index % length;
     if(fo==6000) index = ++index % 2;  // index = ++index % length;
     if(nosna_sin >= 0 ) nosna_prostokat = 30000; else nosna_prostokat = -30000;
           
     PSK_x_puls  = ((long)PSK * nosna_prostokat) >> 15;      
	 PSK_x_nosna = ((long)PSK * nosna_sin) >> 14;
	 
      									// filtry dolnoprzepustowe    
     //dem_PSK = fourth_order_IIR_direct_form_II(IIR_low_pass_2400Hz, PSK_x_nosna); 
	 dem_PSK = fourth_order_IIR_direct_form_II(IIR_low_pass_4000Hz, PSK_x_nosna);	
	 //dem_PSK = fourth_order_IIR_direct_form_II(IIR_low_pass_4000Hz, PSK_x_puls);
	 //dem_PSK = fourth_order_IIR_direct_form_II(IIR_low_pass_4800Hz, PSK_x_nosna);
	 //dem_PSK = fourth_order_IIR_direct_form_II(IIR_low_pass_9600Hz, PSK_x_nosna);
	
	 if(dem_PSK > 0) dem_PSK_bity = 20000; else dem_PSK_bity = -20000;    
          
     
     Step = pushbuttons_read(9);
         
	  if ( Step == 1 )
        {
         if ( Step != LastStep )
          {
           oled_display_message("L = PSK           ", "P = PSK * PSK     ");
           LastStep = Step;
          }	
         left_output = PSK;      // Straight trough. No processing.
	     right_output = PSKxPSK;

	    } 
	  
      
	    if ( Step == 2)
        {
         if ( Step != LastStep )
          {
           oled_display_message("L = BP PSK * PSK  ", "P = PSK * PSK     ");
           LastStep = Step;
          }	
         left_output = BP_PSKxPSK;     // -60dB
	     right_output = PSKxPSK;	// -80dB

	    } 
	      if ( Step == 3)
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = nosna sin     ", "P = kwadrat filtr.");          
            LastStep = Step;
          }         
         left_output = nosna_sin;
	     right_output = BP_PSKxPSK;	// -80dB
	    }
	    
	    if ( Step == 4)
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = PSK           ", "P = nosna sin     ");          
            LastStep = Step;
          }         
         left_output = PSK;
	     right_output = nosna_sin;	// -80dB
	    }
	     
	    if ( Step == 5)
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = nosna puls    ", "P = nosna sin     ");          
            LastStep = Step;
          }         
         left_output = nosna_prostokat;
	     right_output = nosna_sin;	// -80dB
	    } 
	    
	    if ( Step == 6)
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = PSK           ", "P = PSK_x_nosna    ");          
            LastStep = Step;
          }         
         left_output = PSK;
	     right_output = PSK_x_nosna;	// -80dB
	    } 
	    
	    if ( Step == 7)
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = PSK x puls    ", "P = PSK_x_nosna    ");          
            LastStep = Step;
          }         
         left_output = PSK_x_puls;
	     right_output = PSK_x_nosna;	// -80dB
	    } 
	    
	    if ( Step == 8)
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = dem psk filtr ", "P = PSK_x_nosna    ");          
            LastStep = Step;
          }         
         left_output = dem_PSK;
	     right_output = PSK_x_nosna;	// -80dB
	    } 
	    
	    if ( Step == 9)
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = dem psk filtr ", "P = dem psk bity  ");          
            LastStep = Step;
          }         
         left_output = dem_PSK;
	     right_output = dem_PSK_bity;	// -80dB
	    }  
	    
	    
     aic3204_codec_write(left_output, right_output);

 	}

   /* Disable I2S and put codec into reset */ 
    aic3204_disable();

    printf( "\n***Program has Terminated***\n" );
    
    oled_display_message("PROGRAM HAS        ", "TERMINATED        ");
    
    SW_BREAKPOINT;
}

/* ------------------------------------------------------------------------ *
 *                                                                          *
 *  End of main.c                                                           *
 *                                                                          *
 * ------------------------------------------------------------------------ */
