/*****************************************************************************/
/*                                                                           */
/* 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 "oled.h"
#include "pushbuttons.h"
#include "stereo.h"
#include "dsplib.h"
#include "sinewaves.h"
#include "math.h"
#include "combfilters.h"
#include "gsm.h"

#include "IIR_filters_fourth_order.h"
#include "IIR_low_pass_filters.h"

#include "IIR.h"

signed int wspLP_lib[6] = {0, 0, 0, 0, 0, 0 };
signed int x2[3] = { 0, 0, 0 };  // próbki wejściowe
signed int y2[3] = { 0, 0, 0 };  // próbki wyjściowe
                                       
signed int x1[3] = { 0, 0, 0 };  // próbki wejściowe
signed int y1[3] = { 0, 0, 0 };  // próbki wyjściowe   

#define SAMPLES_PER_SECOND 48000
#define GAIN_IN_dB 0

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

int iloczyn1 = 1000;
int iloczyn2 = 1000;

int iloczyn1_2 = 1000;
int iloczyn2_2 = 1000;

int nosna_cos = 100;
int feedback = 100;
long feedbackL = 100;

int out1 = 10;
int out2 = 10;

int out1D = 10;
int out2D = 10;

int out1_3 = 10;
int out2_3 = 10;

int out1x2 = 10;
long out1x2long = 10;

DATA PSK;
int nosna_sin = 0;
int PSK_x_nosna_cos;
int PSK_x_puls;
int dem_PSK;
int dem_PSK_bity1;
int dem_PSK_bity2;

int fo = 12000;
double f3dB_F = 1;	// filtr sygnału sterujacego VCO 0.5
int  prog = 5000;

short int *fazaVCO;

// filtr dopasowany
int R = 4000;
int N = 8;
int invN = 1;
int bufor1[48], bufor2[48];
int index = 0, i;
long suma1 = 0, suma2 = 0;

double ALFA_F, ALFA;
double omega3dB;

const signed int *wspLP;

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

//koniec projektowania


/* ------------------------------------------------------------------------ *
 *                                                                          *
 *  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\nLaboratorium FCiPS\n");
    printf( "Ćwiczenie 6 Demodulatory z pętlą PLL\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 */ 
	printf("\nDemodulator QAM");
    printf("Częstotliwość nośna = %5d Hz\n", fo);
    printf("Szybkość transmisji symboli R = %5d Bd\n", R);
//    printf("Filtry dolnoprzepustowe IIR 2 stopnia\n");
      
	/* Default to XF LED off */
	asm(" bclr XF");
	
	// projekty filtrów LP 1 rzędu
	omega3dB = 2*3.1415926*f3dB_F/SAMPLES_PER_SECOND;
    ALFA_F = (1.0 - sin(omega3dB))/cos(omega3dB);

    //wspLP = IIR_low_pass_4800Hz;
    wspLP = IIR_low_pass_2400Hz;
      
    wsp_LP(2000, SAMPLES_PER_SECOND, wspLP_lib);   
    //wsp_LP(5, SAMPLES_PER_SECOND, wspLP_out);
        
    N = SAMPLES_PER_SECOND / R;
    invN = (1L<<15)/N;
    
 	for ( ;; )
 	{

     aic3204_codec_read(&left_input, &right_input); // Configured for one interrupt per two channels.
     
     PSK = left_input;   
  
     		//PLL 
	 VCO90QAM(fo, feedbackL, 30000, &nosna_sin, &nosna_cos);
	       
     iloczyn1 = _smpyr(PSK, nosna_cos) >> 16;		// detektor fazy 1   
     iloczyn2 = _smpyr(PSK, nosna_sin) >> 16;		// detektor fazy 2 
     
     // filtry dopasowane
     
     bufor1[index] = iloczyn1;
     bufor2[index] = iloczyn2;
     
     //bufor1[index] = out1;
     //bufor2[index] = out2;
     
     suma1 = (long)bufor1[0]; 
     suma2 = (long)bufor2[0];
         
     for(i = 1; i < N; i++)
     {
     	suma1 += (long)bufor1[i]; 
     	suma2 += (long)bufor2[i];
     } 
     
     index++;
     if(index >= N) index = 0;
     
     out1 = out1D = (suma1 * invN) >> 15;
     out2 = out2D = (suma2 * invN) >> 15;
     
     // koniec filtrów dopasowanych

     //out1 = second_order_IIR_direct_form_I (wspLP, iloczyn1); //filtr dolnoprzepustowy  
     //out2 = second_order_IIR_direct_form_IA(wspLP, iloczyn2); //filtr dolnoprzepustowy 
     
     
     // Filtry dolnoprzepustowe
     out1  = IIR_I_order(x1, y1, wspLP_lib, iloczyn1);
     out2  = IIR_I_order(x2, y2, wspLP_lib, iloczyn2);
       
     //out1_3 = ((long)out1 * out1) >> 13;
     //out1_3 = ((long)out1 * out1_3) >> 13;    
     
     //out2_3 = ((long)out2 * out2) >> 13;
     //out2_3 = ((long)out2 * out2_3) >> 13;
     							// im większe przesunięcie tym mniejszy sygnał
     out1_3 = L_crshft_r((long)out1 * out1, 13);
     out1_3 = L_crshft_r((long)out1 * out1_3, 13);    
     
     out2_3 = L_crshft_r((long)out2 * out2, 13);
     out2_3 = L_crshft_r((long)out2 * out2_3, 13);
         
     // tylko do pomiaru     
     iloczyn1_2 = ((long)out1_3 * out2) >> 13;
     iloczyn2_2 = ((long)out2_3 * out1) >> 13;

     out1x2long = _lsmpy(out1_3, out2);
     out1x2long = _smas(out1x2long, out2_3, out1);
     out1x2 = L_crshft_r(out1x2long, 13);				
     
     //feedback = out1x2;
          
     feedbackL = LP_VCO_QAM(out1x2, ALFA_F) >> 13; 		//filtr dolnoprzepustowy
     feedback = feedbackL >> 5;

     	// Koniec PLL

     Step = pushbuttons_read(9);
         
	  if ( Step == 1 )
        {
         if ( Step != LastStep )
          {
           oled_display_message("L = PSK           ", "P = nosna cos     ");
           printf("\nSprawdzenie poziomu sygnałów, regulacja\n");
           printf("Obserwacja sygnału wejściowego i odtworzonej nośnej\n");
           printf("Sygnał wejściowy powinien mieć wartość maksymalną mniejszą od nośnej o kilkanaście procent\n");
           printf("Kanał lewy:  left input   -> out\n");
           printf("Kanał prawy: nośna sinus  -> out\n");
           LastStep = Step;
          }	
         left_output = PSK;      // Straight trough. No processing.
	     right_output = nosna_sin;
	    } 
	    
	    if ( Step == 2)
        {
         if ( Step != LastStep )
          {
           oled_display_message("L = right input   ", "P = nosna sin     ");
           printf("\nOcena jakości synchronizacji, obserwacja krzywej Lissajous\n");
           printf("\nPrzy braku synchronizacji wyregulować poziom sygnału wejściowego QAM\n");
           printf("Kanał lewy:  nośna sinus -> out\n");
           printf("Kanał prawy: right input -> out\n");
           LastStep = Step;
          }	
         left_output =  nosna_sin;    // -60dB
	     right_output = right_input;	// -80dB
	    }
	        
	    if ( Step == 3)        
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = iloczynin 1   ", "P = iloczynin 2   ");
            printf("\nIloczyny sygnału QAM razy nośna synfazowa i kwadraturowa\n");
            printf("Kanał lewy:  QAM * cos -> out\n");
            printf("Kanał prawy: QAM * sin -> out\n");          
            LastStep = Step;
          }         
         left_output = iloczyn1;
	     right_output = iloczyn2;	// -80dB
	    }
	    
	    if ( Step == 4)
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = out1          ", "P = out2          ");
            printf("\nObwiednie sygnału po filtrach dolnoprzepustowych\n");
            printf("Kanał lewy:  obwiednia synfazowa -> out\n");
            printf("Kanał prawy: obwiednia kwadraturowa -> out\n");          
            LastStep = Step;
          }         
         left_output = out1;
	     right_output = out2;	// -80dB
	    }
	     
	    if ( Step == 5)
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = feedback      ", "P = out1 x out2   ");
            printf("\nSterowanie generatorem VCO\n");
            printf("Kanał lewy:  Sygnał błędu przed filtracją -> out\n");
            printf("Kanał prawy: Sygnał błędu po filtracji -> out\n");                       
            LastStep = Step;
          }         
         left_output = out1x2;
	     right_output = feedback;	// -80dB
	    } 
	    
	    if ( Step == 6)
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = out1 D        ", "P = out2 D        ");
            printf("\nZdemodulowane sygnały po filtrach filtrem dopsowanym\n");
            printf("Kanał lewy:  sygnał synfazowy -> out\n");
            printf("Kanał prawy: sygnał kwadraturowy -> out\n");          
            LastStep = Step;
          }         
         left_output = out1D;
	     right_output = out2D;	// -80dB
	    } 
	    
	    if ( Step == 7)
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = iloczyn1 2    ", "P = iloczyn2 2    ");
            printf("\nIloczyny przed sumatorem\n");
            printf("Kanał lewy:  iloczyn górny -> out\n");
            printf("Kanał prawy: iloczyn dolny -> out\n");           
            LastStep = Step;
          }         
         left_output = iloczyn1_2;
	     right_output = iloczyn2_2;	// -80dB
	    } 
	    
	    if ( Step == 8)
        {
         if ( Step != LastStep )
          {
            oled_display_message("L = dem psk bity  ", "P = dem PSK bity       ");          
            LastStep = Step;
          }         
         left_output = out1_3;// dem_PSK_bity1;
	     right_output = out2_3;// dem_PSK_bity2;	// -80dB
	    } 
	    
	    if ( Step == 9)
        {
          if ( Step != LastStep )
          {	 
            oled_display_message("patrz na konsole  ", "i wpisz fo dla VCO");
            printf("Podaj częstotliwość VCO [Hz] fo = ");
    		scanf("%d", &fo);
    		
    		do
    		{
	    		printf("Podaj liczbę symboli na s (minimum = 1000) [Bd] SR = ");
	    		scanf("%d", &R);   		
	    		// filtr dopasowany
	    		
	    		N = SAMPLES_PER_SECOND / R;
	    		if (N >= 48) printf("Przekroczone parametry: SAMPLES_PER_SECOND / R > 48\n");
    		}
    		while(N > 48);
    		
    		invN = (1L<<15)/N;
    		
    		// projekty filtrów LP 1 rzędu
    		wsp_LP(R >> 1, SAMPLES_PER_SECOND, wspLP_lib);   
         
            LastStep = Step;
          }         
         left_output = PSK;      
	     right_output = nosna_cos;
	    }  
	    
	    
     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                                                           *
 *                                                                          *
 * ------------------------------------------------------------------------ */
