/*
 * File:		twr-k20d72m-oobe.c
 * Purpose:		Main process
 *
 */

#include "common.h"
#include "tsi.h"
#include "buzzer.h"

#include "angle_cal.h"
#include "hal_dev_mma8451.h"
#include "hal_i2c.h"
#include "TWR_K20D72M_DEF.h"
#include "fft_test.h"
#include "median.h"
#include "protocol.h"
#include "rtc.h"
#include "adc_test.h"
#include "irda_test.h"
#include "cmt_test.h"
#include "task_mgr.h"

/* Function prototype */
void accel_init(void);
void vfn_test_accel(void);
void accel_read(void);
/*do nothing*/
void vfn_null(void);
void  switch_test(void);

/* global variables */
signed short resultx, resulty, resultz;

/********************************************************************/
void main (void)
{
  	static char aux_fall_input,aux_clap_detected,auxclapmed,aux_tone_detected;
    static char fall_counter;
	
  	printf("--- TWR-K20D72M OOBE ---\n");
	
	TWR_GPIO_init();
	printf("\n TWRK20D72M Demo Software Rev 1.0 \n");                
    printf("\n >"); 
    uart_app_init();
	buzzer_ftm_init(); //buzzer init
    PIT_init();
    rtc_init();
    accel_init();
    adc_test_init();
    tsi_test_init();
    irda_test_init();
        
    tone_filter_init();
	
 	next_task(vfn_null);
    ti_fall = 3000;  //waits a least 3 seconds before detect falls
         
    cmt_init();
    EnableInterrupts;
    tone(_C7 ,300);
    while(tone_enable){};
    tone(_A5,300);
    while(tone_enable){};

    cmt_init();	
	
	while(1)
	{
	  	ptr_next_task();  // do the actual function 
		tsi_test_service();
		vfn_test_accel();
		switch_test();
	  
		/* tone detection */
		if (input_rise(tone_detected,&aux_tone_detected) && !tone_enable && !ti_tone_wait)
		{
			ti_tone_wait = 800;
			tone(_A5,180);
			printf("\n\r tone detected %3i\n\r ",tone_counter);
		} 
		else if (tone_detected==1 && !ti_tone_wait) 
			tone_detected=0; 
              
		// Clap detection 
		if (input_rise(clap_detected,&aux_clap_detected) && !tone_enable && !ti_tone_wait)
		{
			ti_tone_wait = 400;
			tone(_A4,100);
			printf("\n\r clap/snap detected  %3i \n\r ",clap_counter);
		} 
		else if (clap_detected==1 && !ti_tone_wait) 
			clap_detected=0; 
  
		/*fall detection */ 
		if (input_rise(fall_input,&aux_fall_input))
		{
			printf("\n\r i'm falling!!! %i \n\r",fall_counter++);
			ti_delay = 1500;
			tone(_A6,400);
		}
         
		if (input_rise(base_time125m&0x1,&auxclapmed))   
		{
			if (clap_energy>65535*2) 
				clap_energy= 65535*2;
			energy_scaled = (clap_energy/2);
			median_energy = median(energy_scaled, &arr_medianasClap[0]);
		}
	} 
}
/********************************************************************/

void accel_init(void)
{
    unsigned char tmp;
    printf("****Accelerometor init ****\n");
    //Initialize I2C modules
    hal_i2c_init();
   //Configure MMA8451 sensor //MMA8451Q
    tmp = hal_dev_mma8451_read_reg(0x2a);
    hal_dev_mma8451_write_reg(0x2a,tmp|0x01);
}

/*do nothing*/
void vfn_null(void)
{

}

void  switch_test(void)
{
  static char aux_sw1, aux_sw2, sw1_count, sw2_count;
        if (input_rise(SW1_ON,&aux_sw1))
           {
              printf("\n\r push_button (SW3)  %i \n\r",sw1_count++);
              tone(_C6,100);                    
          }
  
  
        if (input_rise(SW2_ON,&aux_sw2))
           {
              printf("\n\r push_button (SW2) %i \n\r",sw2_count++);
              tone(_E6,100);                    
          }
        
        if (ti_test_leds==0)
        {   
/*          
         if (SW1_ON) LED1_ON; else LED1_OFF;
         if (SW2_ON) LED2_ON; else LED2_OFF;    
         if (ELECTRODE1_ON) LED3_ON; LED3_OFF;
         if (ELECTRODE2_ON) LED4_ON; LED4_OFF;
  */
         if (SW1_ON) LED3_ON; else LED3_OFF;
         if (SW2_ON) LED4_ON; else LED4_OFF;    
         //if (ELECTRODE1_ON) LED1_ON; LED1_OFF;
         //if (ELECTRODE2_ON) LED2_ON; LED2_OFF;
        }
        
}        

void vfn_test_accel(void)
{
	if (ti_accel_sampling==0)
	{
		accel_read();              
		ti_accel_sampling = 10;            
	} 
}

void tone_accel_demo(void)
{
	static int note,notez;
	note  = yz_angle/100;  //9-9
    
	if (!ti_delay && !tone_enable) buzzer_off();
	if (note != notez && xz_mag > 20 && !ti_delay)
	{
		if (note <-9) note=-9;
		if (note >9)  note =9;

		if (notez < note) notez++;
		if (notez > note) notez--;
		set_out_module(C_MAJ_SCALE[9 - notez]); 
		ti_delay = 75;
		buzzer_on();
	}     
}

void accel_read(void)
{
	if((hal_dev_mma8451_read_reg(0x00)&0xf) != 0)
	{
		resultx   = hal_dev_mma8451_read_reg(0x01)<<8;
		resultx  |= hal_dev_mma8451_read_reg(0x02);
		resultx >>= 8;

		resulty   = hal_dev_mma8451_read_reg(0x03)<<8;
		resulty  |= hal_dev_mma8451_read_reg(0x04);
		resulty >>= 8;

		resultz   = hal_dev_mma8451_read_reg(0x05)<<8;
		resultz  |= hal_dev_mma8451_read_reg(0x06);
		resultz >>= 8; 
            
		angle_calculation(); //-900  to  900            
		detect_fall_detection();
            
		tone_accel_demo();
	}      
}
