/*
 * UART.c
 *
 *  Created on: Aug 3, 2020
 *      Author: joovi
 */

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

#define UART_RX_BUFF_SIZE   20
#define UART_TX_BUFF_SIZE   3

enum UART_commands {Reset_Count = 0x01, Read_position = 0x02, Read_PI_Position = 0x03, Step_Command = 0x04, Set_Position = 0x05};
enum UART_commands Commands;

unsigned char UartRxBuffer[UART_RX_BUFF_SIZE] = {0};
unsigned int ui_UartRxBufferWriteIndex = 0;
unsigned int ui_UartRxBufferReadIndex = 0;

unsigned char UartTxBuffer[UART_TX_BUFF_SIZE] = {0};
unsigned int ui_UartTxByteCount = 0;



void Init_UART(void)
{
    // Configure UART pins
    P4SEL0 |= BIT2 | BIT3;                    // set 2-UART pin as second function

    // Configure UART
    UCA1CTLW0 |= UCSWRST;                     // Put eUSCI in reset
    UCA1CTLW0 |= UCSSEL__SMCLK;

    // Baud Rate calculation
    UCA1BR0 = 208;                              // 24000000/115200 = 208.333
    UCA1MCTLW = 0x4900;                       // 24000000/115200 - INT(1000000/115200)=0.3333
                                              // UCBRSx value = 0xD6 (See UG)
    UCA1BR1 = 0;
    UCA1CTLW0 &= ~UCSWRST;                    // Initialize eUSCI
    UCA1IE |= UCRXIE;                         // Enable USCI_A0 RX interrupt

}

enum Command_Action UartCommandProcess(void)
{
    int x =0;
    int New_Position =0;
    char MotorSelectFlag = FOCUS_MOTOR_COMMAND_BIT;   // Focus = 0, Tilt = 1
    enum Command_Action Action = None;

    //Figure out which motor the command is for
    if(UartRxBuffer[ui_UartRxBufferReadIndex] & 0x80){
        MotorSelectFlag = TILT_MOTOR_COMMAND_BIT; //
    }

    // switch through the commands
    switch(UartRxBuffer[ui_UartRxBufferReadIndex++] & 0x7F){

        case Reset_Count:
            if(MotorSelectFlag == FOCUS_MOTOR_COMMAND_BIT){   //Focus Motor
                g_iFocusMotorPositionCounter = 0;
            }
            else{  // Tilt Motor
                g_iTiltMotorPositionCounter = 0;
            }
            break;

        case Read_position:
            if(MotorSelectFlag == FOCUS_MOTOR_COMMAND_BIT){ //Focus Motor
                UartTxBuffer[0]= (char)((g_iFocusMotorPositionCounter >> 8) & 0x00FF);
                UartTxBuffer[1]= (char) (g_iFocusMotorPositionCounter & 0x00FF);
            }
            else{ // Tilt Motor
                UartTxBuffer[0]= (char)((g_iTiltMotorPositionCounter >> 8) & 0x00FF);
                UartTxBuffer[1]= (char) (g_iTiltMotorPositionCounter & 0x00FF);
            }

            ui_UartTxByteCount = 1;

            while(!(UCA1IFG & UCTXIFG));
            UCA1TXBUF = UartTxBuffer[0];                   // Load data onto buffer
            UCA1IE |= UCTXIE;

            break;

        case Read_PI_Position:
            if(MotorSelectFlag == FOCUS_MOTOR_COMMAND_BIT){ //Focus Motor
                UartTxBuffer[0]= (char)((g_iFocusMotorPIcount >> 8) & 0x00FF);
                UartTxBuffer[1]= (char) (g_iFocusMotorPIcount & 0x00FF);
            }
            else{ // Tilt Motor
                UartTxBuffer[0]= (char)((g_iTiltMotorPIcount >> 8) & 0x00FF);
                UartTxBuffer[1]= (char) (g_iTiltMotorPIcount & 0x00FF);
            }

            ui_UartTxByteCount = 1;

            while(!(UCA1IFG & UCTXIFG));
            UCA1TXBUF = UartTxBuffer[0];                   // Load data onto buffer
            UCA1IE |= UCTXIE;

            break;

        case Step_Command:
            x= ui_UartRxBufferReadIndex + 2;

            if(x >= UART_RX_BUFF_SIZE){
                x = x - UART_RX_BUFF_SIZE;
            }

            while( !(ui_UartRxBufferWriteIndex == x)); // Wait to recieve 2 bytes with Count info

            if(MotorSelectFlag == FOCUS_MOTOR_COMMAND_BIT){ //Focus Motor
                Action = UpdateFocus;
            }
            else{ // Tilt Motor
                Action = UpdateTilt;
            }

            if(ui_UartRxBufferReadIndex >= UART_RX_BUFF_SIZE){
                ui_UartRxBufferReadIndex = 0;
            }

            g_StepperPositionUpdate = (UartRxBuffer[ui_UartRxBufferReadIndex++] << 8) & 0xFF00;

            if(ui_UartRxBufferReadIndex >= UART_RX_BUFF_SIZE){
                ui_UartRxBufferReadIndex = 0;
            }

            g_StepperPositionUpdate |= UartRxBuffer[ui_UartRxBufferReadIndex++] & 0x00FF;

            break;

        case Set_Position:
                    x= ui_UartRxBufferReadIndex + 2;

                    if(x >= UART_RX_BUFF_SIZE){
                        x = x - UART_RX_BUFF_SIZE;
                    }

                    while( !(ui_UartRxBufferWriteIndex == x)); // Wait to recieve 2 bytes with Count info

                    if(ui_UartRxBufferReadIndex >= UART_RX_BUFF_SIZE){
                        ui_UartRxBufferReadIndex = 0;
                    }

                    New_Position = (UartRxBuffer[ui_UartRxBufferReadIndex++] << 8) & 0xFF00;

                    if(ui_UartRxBufferReadIndex >= UART_RX_BUFF_SIZE){
                        ui_UartRxBufferReadIndex = 0;
                    }

                    New_Position |= UartRxBuffer[ui_UartRxBufferReadIndex++] & 0x00FF;

                    if(MotorSelectFlag == FOCUS_MOTOR_COMMAND_BIT){ //Focus Motor
                        Action = UpdateFocus;
                        g_StepperPositionUpdate = New_Position - g_iFocusMotorPositionCounter;
                    }
                    else{ // Tilt Motor
                        Action = UpdateTilt;
                        g_StepperPositionUpdate = New_Position - g_iTiltMotorPositionCounter;
                    }



                    break;


        default:    // unknown command
            ui_UartRxBufferReadIndex = ui_UartRxBufferWriteIndex;
            __no_operation();
            __no_operation();
            break;
    }

    if(ui_UartRxBufferReadIndex >= UART_RX_BUFF_SIZE){
        ui_UartRxBufferReadIndex = 0;
    }

    return Action;
}


/**
 * Interrupts
 */

//unsigned char UartRxBuffer[UART_RX_BUFF_SIZE] = {0};
//unsigned int ui_UartRxBufferIndex = 0;

#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=USCI_A1_VECTOR
__interrupt void USCI_A1_ISR(void)
#elif defined(__GNUC__)
void __attribute__ ((interrupt(USCI_A0_VECTOR))) USCI_A0_ISR (void)
#else
#error Compiler not supported!
#endif
{
    switch(__even_in_range(UCA1IV,USCI_UART_UCTXCPTIFG))
    {
        case USCI_NONE: break;
        case USCI_UART_UCRXIFG:

            UartRxBuffer[ui_UartRxBufferWriteIndex++] = UCA1RXBUF;

            if(ui_UartRxBufferWriteIndex >= UART_RX_BUFF_SIZE){
                ui_UartRxBufferWriteIndex = 0;
            }

            UCA1IFG &=~ UCRXIFG;            // Clear interrupt

              __bic_SR_register_on_exit(LPM0_bits); // Exit LPM0 on reti
              break;
        case USCI_UART_UCTXIFG:
            UCA1TXBUF = UartTxBuffer[1];                   // Load data onto buffer
            UCA1IE &= ~UCTXIE;

            break;
        case USCI_UART_UCSTTIFG: break;
        case USCI_UART_UCTXCPTIFG: break;
    }
}

