#include <Mega32.h>
#include <stdio.h>
#include <delay.h>
#include <math.h>
#include <txrx.c>


#define t1 5     
#define t2 50
#define t3 60
#define data_length 1
#define tx_id 3
#define threshold


//the three task subroutines
void task1(void);  	//
void task2(void);	//
void task3(void);	//

void initialize(void);
void calc_transmit(void);  //Computes the one byte of data that will be sent

unsigned char time1, time2, time3;	//timeout counters

char Ain, Ain2, flex, Flex_Analog, fourhz, mux_set, good, avg_count; 		//raw A to D number//signed char ax, ay;
float ax, vx, xx, ay, vy, xy, avg, dev, prev_avg, prev_vx, prev2_vx, prev3_vx, prev2_avg, prev3_avg;
float ax_buffer[10];
char move, count_stop, right_flag, left_flag;
signed char x, x_prev, x_prev2;
char count_task1;
int x_sum;
int time;
char send_value, trigger, reset, pause, level;

//Wireless stuff

char data[data_length];
unsigned char packet_count;



//**********************************************************
//timer 0 compare ISR
interrupt [TIM0_COMP] void timer0_compare(void)
{
  //Decrement the three times if they are not already zero
  if (time1>0)	--time1;
  if (time2>0)  --time2;
  if (time3>0)	--time3;
}


void generate_data(char count)
{
	data[0] = count;  //Single byte to be transmitted
}


void main(void)
{
    initialize();

    while (1)
    {
        if (time1 == 0) //Called at 200Hz
        {
            task1();
 				}



  	if (time2==0)
  		{        
  	 		time2 = t2;
  		}


    }
}  //endmain




//////////////////////////////
//    Reads A/D converter ////
//////////////////////////////
void task1(void)
{
    switch (mux_set)   //Takes X and (prob Z) Y at 100Hz offset by 1 Hz
    {
        case 0:
        ADMUX = 0b01100000;   //Set mux for first conversion on channel 0
        ADCSR.6=1;            //Start first conversion
        mux_set = 1;          //Go to next switch
        break;

        case 1:
        Ain = ADCH;          //Read first conversion
        ADMUX = 0b01100001;  //Set for channel 1
        ADCSR.6=1;           //Start second conversion
        mux_set = 2;         //Next switch
        break;

        case 2:
        Ain2 = ADCH;         //Read second conversion
        mux_set = 0;         //Next switch
        flex = ~PIND.3;     //Button on finger
        trigger = ~PINA.4;   //Button on glove
        reset = ~PINA.5;                     
        //pause = ~PINA.6;
        pause = ~PINA.7;
        x_sum = x_sum + Ain;    //Sum is used next for averaging
        tx_me(data,data_length,tx_id);  //Sends wireless data serially
        break;
    }

    time1 = t1;  //Reset timer


    count_task1++;  //Used to keep track of when averaging must occur
    
    if (count_task1>=12)
    {
	    	calc_transmit();  //Figure out what data to transmit
        count_task1 = 0;
        x_sum = 0;
    }
    
}





//////////////////////////////////////
/////  Calculates serial variable/////
//////////////////////////////////////
void calc_transmit(void)   //this is called at about 14hz
{
  x_sum = x_sum - 280;
  x_sum = x_sum/32;   //Averaging
  x = x_sum;

	send_value = 0x00;  //Default of no movement
  if ((x_prev>=-3 && x<=-4) && right_flag==0 && flex==1 && trigger==1)       //ROTATE RIGHT
  {
    right_flag = 1;
    send_value = send_value|0x01;
  }
  if (right_flag==1 && ((flex==1 && trigger==1 && x>=-2) || (trigger==0)))
  {
  	right_flag = 0;
  	if (flex==1 && trigger==1 && x==-2) send_value = send_value|0x02; //Rotate back left
  }
  
  
//   if ((x!=-4 && x!=-5) && (x_prev==-4||x_prev==-5))
//   {
//   	right_flag = 0;
//   }
//   if ((x_prev==-4||x_prev==-5) && x==-3 && flex==1 && trigger==1)         //ROTATE BACK LEFT
//   {
//   //  send_value = send_value|0x02;
//   }


  if ((x_prev<=3 && x>=4) && left_flag==0 && flex==1 && trigger==1)        //ROTATE LEFT
  {
    left_flag = 1;
    send_value = send_value|0x02;
  }
  if (left_flag==1 && ((flex==1 && trigger==1 && x<=2) || (trigger==0))) 
  {
  	left_flag = 0;
  	if (flex==1 && trigger==1 && x==2) send_value = send_value|0x01;
  }
//  if ((x_prev==4||x_prev==5) && x==3 && flex==1 && trigger==1)                        //ROTATE BACK RIGHT
//  {
//    send_value = send_value|0x01;
//  }


  if ((x>x_prev) && flex==1 && trigger==0)    //Move left
  {
      send_value = send_value|0x08;
  }

  if ((x<x_prev) && flex==1 && trigger==0)   //Move right
  {
      send_value = send_value|0x04;
  }



  if ((Ain2<=110)&&(flex==0) && (trigger == 1))  //Slam down
  {
    send_value = send_value|0x10;
  }
  
  //if (trigger==1) send_value = send_value|0x20;    //For debugging
  //if (flex==1) send_value = send_value|0x40;       //For debugging
  
  if (reset==1) send_value = 0x03;
  if (pause==1) send_value = 0x0C;
  if (level==1) send_value = 0x0B;

 generate_data(send_value);   //Put the data to be send into the "data" array
 
 
 
 
  good = 0;
  x_prev2 = x_prev;
  x_prev = x;



}





///////////////////////////
//   Initialize      //////
///////////////////////////

void initialize(void)
{



   mux_set = 0;
   good = 0;
   avg_count = 0;
   avg = 0;
   dev = 0;
   move = 0;
   count_stop = 0;
   right_flag = 0;
   left_flag = 0;
   count_task1 = 0;
   x_sum = 0;
   x_prev = 0;
   x_prev2 = 0;
   x = 0;
   time = 0;
   send_value = 0;      
   trigger = 0;

   xx = 0;
   
   //wireless stuff
   
   txrx_init(1,0,207,0);   //Sets up wireless and serial
   packet_count = 1;
   generate_data(packet_count);   //Generates first data


   //init the A to D converter
   //channel zero/ left adj /EXTERNAL Aref
   //enable ADC and set prescaler to 1/128*16MHz=125,000
   //and clear interupt enable
   //and start a conversion


   ADCSR = 0b11000111;

   //set up timer 0
   TIMSK=2;		//turn on timer 0 cmp match ISR
   OCR0 = 249;  	//set the compare re to 250 time ticks
  	//prescalar to 64 and turn on clear-on-match
   TCCR0=0b00001011; 


   DDRA.4 = 0;    //Button on glove
   PORTA.4 = 1;   //Pull up resistor on
	DDRD.3 = 0;   //Button on finger
	PORTD.3 = 1;  //Pull up resistor on
  PORTA.5 = 1;
  PORTA.6 = 1;
  PORTA.7 = 1;
  DDRA.5 = 0;
  DDRA.6 = 0;
  DDRA.7 = 0;
   //crank up the ISRs
  #asm
  	sei
  #endasm
}