#include <msp430.h> 
#include <Camera_Motor_EVM.h>
#include <UART.h>

void Init_MCU(void);
void Init_UART(void);

/**
 * Global Variables
 */

enum Command_Action Next_Action = None;

//Iris Motor
enum Direction IrisMotorDirection   =   IRIS_MOTOR_DIRECTION;
unsigned int g_uiIRISTimeoutCounter =   IRIS_MOTOR_RUN_TIME_SECONDS * 1000;
//Focus Motor
unsigned int g_uiFocusMotorPulseCount =  0;       /*Number of pulses before timeout*/
enum Direction FocusMotorDirection   =  FOCUS_MOTOR_DIRECTION;
enum DRV8428_Step_Size FocusMotorStepSetting =  FOCUS_MOTOR_STEP_SIZE;
int g_iFocusMotorPositionCounter =  0;                                  // Counter for current stepper motor position
int g_iFocusMotorPIcount =  0;                                          // Count at which the photointerrupted fired
//Tilt Motor
unsigned int g_uiTiltMotorPulseCount =          0;       /*Number of pulses before timeout*/
enum Direction TiltMotorDirection    =          TILT_MOTOR_DIRECTION;
enum DRV8428_Step_Size TiltMotorStepSetting =   TILT_MOTOR_STEP_SIZE;
int g_iTiltMotorPositionCounter =  0;                                  // Counter for current stepper motor position
int g_iTiltMotorPIcount =  0;                                          // Count at which the photointerrupted fired
//Global flags
unsigned int g_uiFocusMotorTimeoutFlag = 0;
unsigned int g_uiTiltMotorTimeoutFlag  = 0;
unsigned int g_uiIrisMotorTimeoutFlag  = 0;

int g_StepperPositionUpdate = 0;



/**
 * main.c
 */
int main(void)
{
    Init_MCU();
    Init_UART();

	Init_Iris_Motor();
	Init_Focus_Motor();
	Init_Tilt_Motor();

	Start_IRIS_Motor();     // Automatically disabled by timer

	while(1){

	    Next_Action = UartCommandProcess();

	    switch(Next_Action){
            case UpdateFocus:
                // Disable the Focus motor
                FOCUS_DISABLE;

                //update direction
                if(g_StepperPositionUpdate > 0){
                    FocusMotorDirection = Clockwise;
                }
                else{
                    FocusMotorDirection = Counter_Clockwise;
                    g_StepperPositionUpdate= 0 - g_StepperPositionUpdate;  //convert negative number to positive for # of steps
                }

                Focus_Motor_Update_Direction();

                //Set # of Steps to take
                g_uiFocusMotorPulseCount = (unsigned int)g_StepperPositionUpdate;

                //Start Motor
                Start_Focus_Motor();
                break;

            case UpdateTilt:
                // Disable the Focus motor
                TILT_DISABLE;

                //update direction
                if(g_StepperPositionUpdate > 0){
                    TiltMotorDirection = Clockwise;
                }
                else{
                    TiltMotorDirection = Counter_Clockwise;
                    g_StepperPositionUpdate= 0 - g_StepperPositionUpdate;  //Flip to positive number
                }

                Tilt_Motor_Update_Direction();

                //Set # of Steps to take
                g_uiTiltMotorPulseCount = (unsigned int)g_StepperPositionUpdate;

                //Start Motor
                Start_Tilt_Motor();
                break;

            case None:
                break;

            default:
                break;
	    }


	    LPM0;                       //Enter LPM0 for low power waiting
        __no_operation();

	}
}



void Init_MCU(void)
{
    WDTCTL = WDTPW + WDTHOLD;   // Stop Watch Dog Timer

    // Configure two FRAM waitstate as required by the device datasheet for MCLK
    // operation at 24MHz(beyond 8MHz) _before_ configuring the clock system.
    FRCTL0 = FRCTLPW | NWAITS_2;

    //Configure DCO for 24MHz operation
    __bis_SR_register(SCG0);                           // disable FLL
    CSCTL3 |= SELREF__REFOCLK;                         // Set REFO as FLL reference source
    CSCTL0 = 0;                                        // clear DCO and MOD registers
    CSCTL1 |= DCORSEL_7;                               // Set DCO = 24MHz
    CSCTL2 = FLLD_0 + 731;                             // DCOCLKDIV = 24MHz
    __delay_cycles(3);
    __bic_SR_register(SCG0);                           // enable FLL
    while(CSCTL7 & (FLLUNLOCK0 | FLLUNLOCK1));         // FLL locked

    CSCTL4 = SELMS__DCOCLKDIV | SELA__REFOCLK;         // set default REFO(~32768Hz) as ACLK source, ACLK = 32768Hz
                                                       // default DCOCLKDIV as MCLK and SMCLK source (= 24MHz)

    // Disable the GPIO power-on default high-impedance mode
    // to activate previously configured port settings
    PM5CTL0 &= ~LOCKLPM5;

    // Enable Global Interrupts
    _BIS_SR(GIE);
}






/**
 * Interrupts
 */

//Focus Motor PWM ISR
// TimerB0 Interrupt Vector (TBIV) handler
#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=TIMER0_B1_VECTOR
__interrupt void TIMER0_B1_ISR(void)
#elif defined(__GNUC__)
void __attribute__ ((interrupt(TIMER0_B1_VECTOR))) TIMER0_B1_ISR (void)
#else
#error Compiler not supported!
#endif
{
    switch(__even_in_range(TB0IV,TB0IV_TBIFG))
    {
        case TB0IV_NONE:
            break;                               // No interrupt
        case TB0IV_TBCCR1:
            if(g_uiFocusMotorPulseCount == 0){
                g_uiFocusMotorTimeoutFlag = 1;
                FOCUS_DISABLE;                  // Disable the Focus motor
                TB0CTL = MC__STOP;              //Stop the timer
//                LPM3_EXIT;
            }
            else{
                FOCUS_STEP_PIN_HIGH;
            }

            break;
        case TB0IV_TBCCR2:
            break;                               // CCR2 not used
        case TB0IV_TBIFG:                        // overflow
            g_uiFocusMotorPulseCount--;
            FOCUS_STEP_PIN_LOW;

            if(FocusMotorDirection == Clockwise){
                g_iFocusMotorPositionCounter++;
            }
            else{
                g_iFocusMotorPositionCounter--;
            }


            break;
        default:
            break;
    }
}

//TILT motor PWM ISR
// TimerB2 Interrupt Vector (TBIV) handler
#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=TIMER2_B1_VECTOR
__interrupt void TIMER2_B1_ISR(void)
#elif defined(__GNUC__)
void __attribute__ ((interrupt(TIMER0_B1_VECTOR))) TIMER0_B1_ISR (void)
#else
#error Compiler not supported!
#endif
{
    switch(__even_in_range(TB2IV,TB0IV_TBIFG))
    {
        case TB0IV_NONE:
            break;                               // No interrupt
        case TB0IV_TBCCR1:
            if(g_uiTiltMotorPulseCount == 0){   //Check for timeout
                g_uiTiltMotorTimeoutFlag = 1;
                TILT_DISABLE;                  // Disable the Tilt motor
                TB2CTL = MC__STOP;
//                LPM3_EXIT;
            }
            else{
                TILT_STEP_PIN_HIGH;
            }
            break;                               // CCR1 not used

        case TB0IV_TBCCR2:
            break;                               // CCR2 not used
        case TB0IV_TBIFG:
            g_uiTiltMotorPulseCount--;
            TILT_STEP_PIN_LOW;

            if(TiltMotorDirection == Clockwise){
                g_iTiltMotorPositionCounter++;
            }
            else{
                g_iTiltMotorPositionCounter--;
            }

            break;

        default:
            break;
    }
}

//1 ms tick timer - Timeout for IRIS motor
// TimerB3 Interrupt Vector (TBIV) handler
#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=TIMER3_B1_VECTOR
__interrupt void TIMER3_B1_ISR(void)
#elif defined(__GNUC__)
void __attribute__ ((interrupt(TIMER0_B1_VECTOR))) TIMER0_B1_ISR (void)
#else
#error Compiler not supported!
#endif
{
    switch(__even_in_range(TB3IV,TB0IV_TBIFG))
    {
        case TB0IV_NONE:
            break;                               // No interrupt
        case TB0IV_TBCCR1:
            break;
        case TB0IV_TBCCR2:
            break;                               // CCR2 not used
        case TB0IV_TBCCR3:
            break;                               // CCR3 not used
        case TB0IV_TBCCR4:
            break;                               // CCR4 not used
        case TB0IV_TBCCR5:
            break;                               // CCR5 not used
        case TB0IV_TBCCR6:
            break;                               // CCR6 not used
        case TB0IV_TBIFG:                        // overflow ISR
            g_uiIRISTimeoutCounter--;

            if(g_uiIRISTimeoutCounter == 0){    //Check for timeout
                g_uiIrisMotorTimeoutFlag = 1;
                IRIS_BRAKE;// Disable Iris motor
//                LPM3_EXIT;
            }

            break;
        default:
            break;
    }
}


// Port 1 interrupt service routine
#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=PORT1_VECTOR
__interrupt void Port_1(void)
#elif defined(__GNUC__)
void __attribute__ ((interrupt(PORT1_VECTOR))) Port_1 (void)
#else
#error Compiler not supported!
#endif
{

    g_iFocusMotorPIcount = g_iFocusMotorPositionCounter;
    P1IFG &= ~BIT3;                         // Clear P1.3 IFG
}

// Port 3 interrupt service routine
#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=PORT3_VECTOR
__interrupt void Port_3(void)
#elif defined(__GNUC__)
void __attribute__ ((interrupt(PORT1_VECTOR))) Port_1 (void)
#else
#error Compiler not supported!
#endif
{

    g_iTiltMotorPIcount = g_iTiltMotorPositionCounter;
    P1IFG &= ~BIT3;                         // Clear P1.3 IFG
}
