#include "../HAL/Biopositioning.h"
#include "../HAL/hal_cc2530znp_target_board.h"
#include "../HAL/hal_vti_cma3000_D01_accelerometer.h"



#include "../ZNP/znp_interface.h"
#include "../ZNP/application_configuration.h"
#include "../ZNP/af_zdo.h"
#include "znp_example_utils.h"   //for handleReturnValue() and polling()

#define TEST_CLUSTER 0x77  

// Global variables for Motor Control

unsigned char motor_state;
unsigned char clockwise;
unsigned char motor_enable;
unsigned char motor_count_ready;
unsigned int motor_global_count;
unsigned char zposbuf[2];
unsigned char zposdebugbuf[4];

#pragma vector=TIMERB0_VECTOR
__interrupt void Timer_B (void)
{
    if (clockwise == 0)
    { 
        if ( motor_count_ready == 0 )
        {
           anticw_steppersequence();
        }
        else 
        {
            clockwise = 0xff;
        }
    }
    else 
    {
        if (motor_count_ready == 1)
        {  
          clockwise_steppersequence();
          motor_global_count++;
        }      
    }
    toggleLed(0);
}

// Clockwise and Anti clockwise control from the Push Button P1.2
#pragma vector=PORT1_VECTOR
__interrupt void PORT1_ISR(void)
{
    // This routine will check the interrupt for both the rising edge and the falling edge 
    if (P1IFG & BIT2)
    { 
      //clockwise = ~clockwise;
      
      if (motor_enable == 0)
      { 
          clockwise = 0; // When motor is started first it will go to base position then will start the upwards motion
          motor_enable = 1;
          motor_global_count = 0;
          TBCTL = MC_1|ID_3|TBSSEL_2|TBCLR; // Set up and start Timer B
          
          // Change the type of the interrupt and wait for the other one to come
          P1IES &= ~BIT2;   //Interrupt on low-to-high transition
      }
      else
      {
          motor_enable = 0;
          motor_count_ready = 0; // Make the system ready to take new count 
          TBCTL = MC_0|ID_3|TBSSEL_2|TBCLR; // Set up and start Timer B
          
           // Change the type of the interrupt back to deafault mode and freez the overall count for second motor and disable the timer and reset all the flags
          P1IES = BIT2;   //Interrupt on high-to-low transition
          zposSync();
          zposSyncDebug();
          printf ("Final Value of Count 0x%0c%0c%0c%0c\r\n",zposdebugbuf[0],zposdebugbuf[1],zposdebugbuf[2],zposdebugbuf[3]);
          afSendData(DEFAULT_ENDPOINT,DEFAULT_ENDPOINT,0, TEST_CLUSTER,zposbuf , 2);
      } 
     }
    P1IFG = 0;                          // clear the interrupt
}

// Controlling the Limit Switch 
#pragma vector=PORT2_VECTOR
__interrupt void PORT2_ISR(void)
{
    if (P2IFG & LIMIT_PIN)                
    {
        motor_count_ready = 1;
    }
    
    P2IFG &= ~LIMIT_PIN;                          // clear the interrupt
}


void initAccelTimer(void)
{
	TACTL = MC_2|ID_3|TASSEL_2|TACLR;
    while (!(TACTL & TAIFG));
	TACTL &= ~TAIFG;
	TACTL |= MC_0;
	handleAccelerometer();
}
/** Accelerometer interrupt service routine. */

void handleAccelerometer(void)
{
    unsigned char interruptStatus;
    char* interruptReason;
    unsigned char accelerationValue;
    unsigned char Xdata, Ydata, Zdata;


    toggleLed(0); 
//  interruptStatus = readAccelerometerRegister(ACCEL_INT_STATUS);  //will clear the interrupt
    delayUs(ACCELEROMETER_DELAY_BETWEEN_OPERATIONS_US);  
    
 	 Zdata = readAccelerometerRegister(ACCEL_DOUTZ);	 
	 delayUs(ACCELEROMETER_DELAY_BETWEEN_OPERATIONS_US);


    printf("Interrupt: %c \r\n", Zdata);
    P1IFG = 0;
    delayUs(ACCELEROMETER_DELAY_BETWEEN_OPERATIONS_US); //in case this interrupt is immediately followed by another one
}
	
void initStepperTimer(void )
{
    TBCCR0 = 1500; // Upper limit of count for TAR
    TBCCTL0 = CCIE;
    TBCTL = MC_0|ID_3|TBSSEL_2|TBCLR; // Set up and start Timer B
}

void stepperinit(void)
{
    P4DIR |= MOTOR_BLUE | MOTOR_GREEN | MOTOR_RED;
    P2DIR |= MOTOR_BLACK;
    P4OUT &= ~(MOTOR_BLUE | MOTOR_GREEN | MOTOR_RED);
    P2OUT &= ~MOTOR_BLACK;
    motor_state = 0;
    clockwise = 0xff;
    motor_enable = 0; 
    motor_count_ready = 0;
}

// Can make all these as atomic operation, this will help in synchrnozing well
void clockwise_steppersequence(void)
{ 
   P4OUT &= ~(MOTOR_BLUE | MOTOR_GREEN | MOTOR_RED);
   P2OUT &= ~MOTOR_BLACK;
   
   if (motor_state == 0)
   {  
      P4OUT |= MOTOR_BLUE;
   }
   else if (motor_state == 1)
   {
      P4OUT |= MOTOR_GREEN;
   }  
   else if (motor_state == 2)
   {
      P4OUT |= MOTOR_RED;
   } 
   else if (motor_state == 3)
   {
      P2OUT |= MOTOR_BLACK;
   } 
   
   if (motor_state  != 3)
        motor_state++;
   else
        motor_state = 0;
   
}

void anticw_steppersequence(void)
{ 
   P4OUT &= ~(MOTOR_BLUE | MOTOR_GREEN | MOTOR_RED);
   P2OUT &= ~MOTOR_BLACK;
   
   if (motor_state == 3)
   {  
      P4OUT |= MOTOR_BLUE;
   }
   else if (motor_state == 2)
   {
      P4OUT |= MOTOR_GREEN;
   }  
   else if (motor_state == 1)
   {
      P4OUT |= MOTOR_RED;
   } 
   else if (motor_state == 0)
   {
      P2OUT |= MOTOR_BLACK;
   } 
   
   if (motor_state  != 3)
        motor_state++;
   else
        motor_state = 0;
   
}

void InitLimit(void)
{
    P2DIR &= ~LIMIT_PIN; 
    P2IE  |= LIMIT_PIN;   //Enable Button Interrupt
    P2IES |= LIMIT_PIN;   //Interrupt on high-to-low transition
    P2REN |= LIMIT_PIN;   //enable resistor for button
    P2OUT |= LIMIT_PIN;   //make resistor a PULL-UP
    P2IFG &= ~LIMIT_PIN;

}

void zposSync(void)
{
   volatile unsigned int temp;
   
   temp = motor_global_count & 0x00ff;
  // zposbuf[1] = (motor_global_count & 0x00ff);		  
   zposbuf[0] = temp;
   temp = motor_global_count & 0xff00;
   //zposbuf[0] = ((motor_global_count & 0xff00)>>8);		  
   zposbuf[1] = (temp>>8);		  

}

void zposSyncDebug(void)
{
   volatile unsigned char temp;
   temp = ((motor_global_count & 0xF000) >> 12);
   zposdebugbuf[0] = TO_HEX(temp); 	  
   temp = ((motor_global_count & 0x0F00)>>8);
   zposdebugbuf[1] = TO_HEX(temp);
   temp = ((motor_global_count & 0x00F0)>>4);
   zposdebugbuf[2] = TO_HEX(temp);
   temp = (motor_global_count & 0x000F);
   zposdebugbuf[3] = TO_HEX(temp);		  

}


