//handadc.c
//use state machine to transfer accerelation sensors signals to directions
//horizontal Y-direction requires 5 states; vertical Z-direction requires 7 states 
//LH:left hand     RH:right hand
//RHS
//State machine state names 
#define no_state_z_R 0 
#define psu_up1_z_R 1 
#define psu_down1_z_R 2
#define psu_up12_z_R 3
#define psu_down2_z_R 4 
#define psu_up2_z_R 5
#define psu_down22_z_R 6     
#define val_z_R 2    

//State machine state names 
#define no_state_y_R 0 
#define psu_up1_y_R 1 
#define psu_down1_y_R 2
#define psu_down2_y_R 4 
#define psu_up2_y_R 5

unsigned char Ain_z_R ; 		//raw A to D number
unsigned char PreviousAin_z_R;//use to compare with the next one
unsigned char comp_z_R;   
unsigned char state_z_R; 
unsigned char stable_z_R;
bit flag_tr_up_z_R,flag_tr_down_z_R; 


unsigned char Ain_y_R ; 		//raw A to D number
unsigned char PreviousAin_y_R;
unsigned char comp_y_R;   
unsigned char state_y_R; 

bit stable_y_R; //some parameters used for state machines
bit flag_tr_up_y_R,flag_tr_down_y_R; 
bit flag_tr_sup_z_R, flag_tr_sdown_z_R=1; 


//LHS
//State machine state names 
#define no_state_z_L 0 
#define psu_up1_z_L 1 
#define psu_down1_z_L 2
#define psu_up12_z_L 3
#define psu_down2_z_L 4 
#define psu_up2_z_L 5
#define psu_down22_z_L 6     
#define val_z_L 2    

//State machine state names 
#define no_state_y_L 0 
#define psu_up1_y_L 1 
#define psu_down1_y_L 2
#define psu_down2_y_L 4 
#define psu_up2_y_L 5

unsigned char Ain_z_L ; 		//raw A to D number
unsigned char PreviousAin_z_L;
unsigned char comp_z_L;   
unsigned char state_z_L; 
unsigned char stable_z_L;
bit flag_tr_up_z_L,flag_tr_down_z_L; 

unsigned char Ain_y_L ; 		//raw A to D number
unsigned char PreviousAin_y_L;
unsigned char comp_y_L;   
unsigned char state_y_L; 

bit stable_y_L;
bit flag_tr_up_y_L,flag_tr_down_y_L; 
bit flag_tr_sup_z_L, flag_tr_sdown_z_L=1;

unsigned char axis_flag;
unsigned char LHU,LHR,LHD,LHL;//set hand directions
unsigned char RHU,RHR,RHD,RHL;


void state_machine_y_R(void);  
void state_machine_z_R(void);  

void state_machine_y_L(void);  
void state_machine_z_L(void);  

void handset(void)
{
  if(axis_flag==0)   //RHS y axis 
      {
          
          ADMUX = 0b01100000; //for solder board, use reference coltage of AVCC with external capacitor at AREF pin
          //ADMUX = 0b00100000; //select analog input pin5; use Vref   
          ADCSR = 0b11000111; 
          while (ADCSR.6==1)
          Ain_y_R = ADCH;
 
 	        comp_y_R=Ain_y_R-PreviousAin_y_R;//compare this time with last time      
 	 
 	        if(comp_y_R<=3 | comp_y_R>=252) stable_y_R=1;//stable;no moving
 	        else stable_y_R=0;
 	
 	        if(comp_y_R>=8&comp_y_R<128){//acceleration in positive direction
 	            flag_tr_up_y_R=1;
 	            flag_tr_down_y_R=0;  
          }  
          else if((comp_y_R<=247&comp_y_R>128)){ //acceleration in negative direction 
  	          flag_tr_down_y_R=1;    
  	          flag_tr_up_y_R=0; 
  	      }
  	      else{   //no mpving
  	          flag_tr_up_y_R=0;
  	          flag_tr_down_y_R=0;
  	      }  
  	      state_machine_y_R();//call state machine	
          PreviousAin_y_R=Ain_y_R;
          axis_flag=1;//go to next detection  
      }         
      else if(axis_flag==1){ //RHS Z axis
          ADMUX = 0b01100001; //select analog input pin10 
          ADCSR = 0b11000111; 
          while (ADCSR.6==1); 
          Ain_z_R = ADCH;
    
          comp_z_R=Ain_z_R-PreviousAin_z_R;
 	 
 	        if(comp_z_R<=2 | comp_z_R>=253){if(stable_z_R<=val_z_R) stable_z_R=stable_z_R+1;}
 	        else   stable_z_R=0;
 	     
 	        if(comp_z_R>=10&comp_z_R<128)
 	        {
 	            flag_tr_up_z_R=1;
 	            flag_tr_down_z_R=0;     
 	            if(comp_z_R>=20){        
 	               flag_tr_sup_z_R=1;
 	               flag_tr_sdown_z_R=0;
 	            }	   
          }
  	      else if((comp_z_R<=245&comp_z_R>128)){  

  	          flag_tr_down_z_R=1;    
  	          flag_tr_up_z_R=0; 
  	          if(comp_z_R<=245){        
 	               flag_tr_sup_z_R=0;
 	               flag_tr_sdown_z_R=1;
 	            }
  	      }
  	      else{
  	          flag_tr_up_z_R=0;
  	          flag_tr_down_z_R=0;  
  	          flag_tr_sup_z_R=0;
  	          flag_tr_sdown_z_R=0;
  	      }  
  	      state_machine_z_R();	
          PreviousAin_z_R=Ain_z_R;     
          axis_flag=2; 
      }//end of lse if(axis_flag==1)      
      if(axis_flag==2)   //LHS y_axis 
      {
          ADMUX = 0b01100010; //select analog input pin5  
          ADCSR = 0b11000111; 
          while (ADCSR.6==1);
          Ain_y_L = ADCH;
          
          comp_y_L=Ain_y_L-PreviousAin_y_L;      

 	        if(comp_y_L<=3 | comp_y_L>=252) stable_y_L=1;
 	        else   stable_y_L=0;
 	
 	        if(comp_y_L>=8&comp_y_L<128){
 	            flag_tr_up_y_L=1;
 	            flag_tr_down_y_L=0;  
          }   
          else if((comp_y_L<=247&comp_y_L>128)){  
  	          flag_tr_down_y_L=1;    
  	          flag_tr_up_y_L=0;
  	      }
  	      else{
  	          flag_tr_up_y_L=0;
  	          flag_tr_down_y_L=0;
  	      }  
  	      state_machine_y_L();	
          PreviousAin_y_L=Ain_y_L;
          axis_flag=3;  
     }         
     else if(axis_flag==3){ //LHS Z 
          ADMUX = 0b01100011; //select analog input pin5 
          ADCSR = 0b11000111;
          while (ADCSR.6==1); 
          Ain_z_L = ADCH;
    
          comp_z_L=Ain_z_L-PreviousAin_z_L;      
          
 	        if(comp_z_L<=2 | comp_z_L>=253){
 	            if(stable_z_L<=val_z_L) stable_z_L=stable_z_L+1;
 	        }
 	        else   stable_z_L=0;
 	      	if(comp_z_L>=10&comp_z_L<128)
 	        {
 	            flag_tr_up_z_L=1;
 	            flag_tr_down_z_L=0;     
 	            if(comp_z_L>=20){        
 	               flag_tr_sup_z_L=1;
 	               flag_tr_sdown_z_L=0;
 	            }
          }
  	      else if((comp_z_L<=245&comp_z_L>128)){  
  	          flag_tr_down_z_L=1;    
  	          flag_tr_up_z_L=0; 
  	          if(comp_z_L<=245){        
 	               flag_tr_sup_z_L=0;
 	               flag_tr_sdown_z_L=1;
 	            }
          }
  	      else{
  	          flag_tr_up_z_L=0;
  	          flag_tr_down_z_L=0;  
  	          flag_tr_sup_z_L=0;
  	          flag_tr_sdown_z_L=0;
  	      }  
  	      state_machine_z_L();       
          PreviousAin_z_L=Ain_z_L;     
          axis_flag=0;   
     }//end of else if(axis_flag==3)    
}
void state_machine_y_R(void) 
{
  switch (state_y_R)
  {
     case no_state_y_R: 
          if (flag_tr_up_y_R==1) state_y_R=psu_up1_y_R;
          else if (flag_tr_down_y_R==1) state_y_R=psu_down2_y_R;
          else state_y_R=no_state_y_R;
          RHL=0;
          RHR=0;
          break;
          
     case psu_up1_y_R:
          if (flag_tr_down_y_R==1)state_y_R=psu_down1_y_R;    
          else if(stable_y_R==1) state_y_R=no_state_y_R; 
          else state_y_R=psu_up1_y_R;
          RHL=0;
          break;
     case psu_down1_y_R://ser RHL
          RHL=1;  
          if (flag_tr_up_y_R==1) state_y_R=no_state_y_R; 
          else  if(stable_y_R==1) state_y_R=no_state_y_R; 
          else state_y_R=psu_down1_y_R; 
           
          break;  
     
     
     case psu_down2_y_R:
          if (flag_tr_up_y_R==1) state_y_R=psu_up2_y_R;             
          else  if(stable_y_R==1) state_y_R=no_state_y_R; 
          else state_y_R=psu_down2_y_R; 
          RHR=0;
          break;
     case psu_up2_y_R: //set RHR
          RHR=1;   
          if (flag_tr_down_y_R==1)state_y_R=no_state_y_R;  
          else if(stable_y_R==1) state_y_R=no_state_y_R; 
          else state_y_R=psu_up2_y_R; 
            
          break;  
  }//end switch
}   
void state_machine_z_R(void)//RHS 
{
  switch (state_z_R)
  {
     case no_state_z_R: 
          if (flag_tr_sup_z_R==1) state_z_R=psu_up1_z_R;
          else if (flag_tr_sdown_z_R==1) state_z_R=psu_down2_z_R;
          else state_z_R=no_state_z_R;
          RHU=0;
          RHD=0;
          break;
          
     case psu_up1_z_R:
          if (flag_tr_down_z_R==1)  state_z_R=psu_down1_z_R;              
          else  if(stable_z_R==val_z_R) state_z_R=no_state_z_R; 
          else state_z_R=psu_up1_z_R;
          RHU=0;
          break;  
          
          
     case psu_down1_z_R:  
          if (flag_tr_up_z_R==1) state_z_R=psu_up12_z_R;    
          else  if(stable_z_R==val_z_R) state_z_R=no_state_z_R; 
          else state_z_R=psu_down1_z_R;
          RHU=1;
          break;  
     case psu_up12_z_R://set RHU
          RHU=1; 
          if (flag_tr_down_z_R==1) state_z_R=no_state_z_R;                 
          else  if(stable_z_R==val_z_R) state_z_R=no_state_z_R; 
          else state_z_R=psu_up12_z_R;
          
          break;

     case psu_down2_z_R:
          if (flag_tr_up_z_R==1) state_z_R=psu_up2_z_R;                      
          else  if(stable_z_R==val_z_R) state_z_R=no_state_z_R; 
          else state_z_R=psu_down2_z_R;
          RHD=0;
          break;
          
          
     case psu_up2_z_R://set RHD
          if (flag_tr_down_z_R==1)state_z_R=psu_down22_z_R;  
          else if(stable_z_R==val_z_R) state_z_R=no_state_z_R; 
          else state_z_R=psu_up2_z_R;
          RHD=1;//yohowo
          break;  
     case psu_down22_z_R: //set RHD
          RHD=1;
          if (flag_tr_up_z_R==1) state_z_R=no_state_z_R;                    
          else  if(stable_z_R==val_z_R) state_z_R=no_state_z_R; 
          else state_z_R=psu_down22_z_R;
          
          break;
  }//end switch
}   
void state_machine_y_L(void)//LHS 
{
  switch (state_y_L)
  {
     case no_state_y_L: 
          if (flag_tr_up_y_L==1) state_y_L=psu_up1_y_L;
          else if (flag_tr_down_y_L==1) state_y_L=psu_down2_y_L;
          else state_y_L=no_state_y_L;  
          LHL=0;
          LHR=0;
          break;
         
     case psu_up1_y_L:
          if (flag_tr_down_y_L==1)state_y_L=psu_down1_y_L;    
          else if(stable_y_L==1) state_y_L=no_state_y_L; 
          else state_y_L=psu_up1_y_L;
          LHL=0;
          break;
     case psu_down1_y_L: //set LHL 
          LHL=1;
          if (flag_tr_up_y_L==1)state_y_L=no_state_y_L;
          else  if(stable_y_L==1) state_y_L=no_state_y_L; 
          else state_y_L=psu_down1_y_L;
          break;  
     
     
     case psu_down2_y_L:
          LHR=0;
          if (flag_tr_up_y_L==1) state_y_L=psu_up2_y_L;             
          else  if(stable_y_L==1) state_y_L=no_state_y_L; 
          else state_y_L=psu_down2_y_L; 
          break;
     case psu_up2_y_L://set LHR
          LHR=1;   
          if (flag_tr_down_y_L==1) 
          state_y_L=no_state_y_L;  
          else if(stable_y_L==1) state_y_L=no_state_y_L; 
          else state_y_L=psu_up2_y_L;      
          break;  
  }
}   

void state_machine_z_L(void) 
{
  switch (state_z_L)
  {
     case no_state_z_L: 
          if (flag_tr_sup_z_L==1&PreviousAin_z_L!=0) state_z_L=psu_up1_z_L;
          else if (flag_tr_sdown_z_L==1) state_z_L=psu_down2_z_L;
          else state_z_L=no_state_z_L;
          LHU=0;
          LHD=0;
          break;
     case psu_up1_z_L:
          if (flag_tr_down_z_L==1) state_z_L=psu_down1_z_L;                 
          else  if(stable_z_L==val_z_L) state_z_L=no_state_z_L; 
          else state_z_L=psu_up1_z_L;
          LHU=0;
          break;
     case psu_down1_z_L:  
          if (flag_tr_up_z_L==1) state_z_L=psu_up12_z_L;    
          else  if(stable_z_L==val_z_L) state_z_L=no_state_z_L; 
          else state_z_L=psu_down1_z_L;   
          LHU=0;
          break;  
     case psu_up12_z_L: //set LHU
          LHU=1;
          if (flag_tr_down_z_L==1) state_z_L=no_state_z_L;                
          else  if(stable_z_L==val_z_L) state_z_L=no_state_z_L; 
          else state_z_L=psu_up12_z_L;
          break;
     
     
     case psu_down2_z_L:
          if (flag_tr_up_z_L==1) state_z_L=psu_up2_z_L;                    
          else  if(stable_z_L==val_z_L) state_z_L=no_state_z_L; 
          else state_z_L=psu_down2_z_L;
          LHD=0;
          break;
     case psu_up2_z_L:
          if (flag_tr_down_z_L==1) 
          state_z_L=psu_down22_z_L; 
          else if(stable_z_L==val_z_L) state_z_L=no_state_z_L; 
          else state_z_L=psu_up2_z_L;
          LHD=0;
          break;  
     case psu_down22_z_L://setLHD 
          if (flag_tr_up_z_L==1)  state_z_L=no_state_z_L;                       
          else  if(stable_z_L==val_z_L) state_z_L=no_state_z_L; 
          else state_z_L=psu_down22_z_L;
          LHD=1;
          break; 
  }
}