//Accel Interrupt
#include "driverlib.h"
#include <gpio.h>
#include <intrinsics.h>
#include <msp430fr5xx_6xxgeneric.h>
#include <stdint.h>
#include <stdio.h>
#include <msp430.h>

#define ACCEL_THRESHOLD 4000
volatile int16_t accelValue = 0;

//Address if the BMI160
#define SLAVE_ADDR 0x69
//Maximum I2C buffer size
#define MAX_BUFFER_SIZE 20

//Number of FFT samples
#define SAMPLES 10
//#define TimeStampSample 10
#pragma PERSISTENT(input)
int16_t input[SAMPLES] = {0}; //Store samples
volatile uint32_t cycleCount;

#define UP              0x0010                          // Timer_A Up mode
#define CONTINUOUS      0x0020                          // Timer_A Continuous mode
#define ACLK            0x0100                          // Timer_A SMCLK source
#define DEVELOPMENT     0x5A80                          // Stop the watchdog timer
#define BOUNCE_DELAY    0xA000                          // Delay for Button Bounce
#define MS_10           100                             // Approximate value to count for 10ms
#define SMCLK           0x0200                          // Timer_A SMCLK source
void timer_init(void)
{
    TA0CCTL0 = CCIE; //Enable counter interrupt
    TA0CCR0 = 0; //Initially stop Timer by starting at 0
    TA0CTL = TASSEL__SMCLK | MC__UP | ID__8; //Set clock source to ACLK, the count mode to Continuous, and the clock divider to 8
    TA0EX0 = TAIDEX_7; //Set expansion clock divider to 8

    TA1CCTL0 = CCIE;                          // TACCR0 interrupt enabled
    TA1CCR0 = 0;                              // Set count target to 0 by default
    // Set timer clock speed to 4.5898 ticks/s, or 16524 ticks/hour
    TA1CTL = TASSEL__ACLK | MC__STOP | ID__8; //Set clock source to ACLK, the count mode to Continuous, and the clock divider to 8
    TA1EX0 = TAIDEX_7; //Set expansion clock divider to 8
}
int delay(int count)
{
    if(TA1CTL & TAIFG)                                  // If Timer_1 is done counting
    {
        count = count-1;                                        // Decrement count
        TA1CTL = TA1CTL & (~TAIFG);                             // Reset Timer_1
    }
    return count;                                       // Return the value of count
}
void UART_transmitString( char *pStr ) //Transmits a string over UART0
{
    while( *pStr )
    {
        while(!(UCA0IFG&UCTXIFG));
        UCA0TXBUF = *pStr;
        pStr++;
    }
}
typedef enum I2C_ModeEnum{
    IDLE_MODE,
    NACK_MODE,
    TX_REG_ADDRESS_MODE,
    RX_REG_ADDRESS_MODE,
    TX_DATA_MODE,
    RX_DATA_MODE,
    SWITCH_TO_RX_MODE,
    SWITHC_TO_TX_MODE,
    TIMEOUT_MODE
} I2C_Mode;

I2C_Mode MasterMode = IDLE_MODE;

uint8_t TransmitRegAddr = 0; //Register address for transmission
uint8_t ReceiveBuffer[MAX_BUFFER_SIZE] = {0}; //Buffer for received values
uint8_t RXByteCtr = 0; //Count received bytes
uint8_t ReceiveIndex = 0; //Index of received data
uint8_t TransmitBuffer[MAX_BUFFER_SIZE] = {0}; //Buffer for transmitted values
uint8_t TXByteCtr = 0; //Count transmitted bytes
uint8_t TransmitIndex = 0; //Index of transmitted data

void CopyArray(uint8_t *source, uint8_t *dest, uint8_t count)
{
    uint8_t copyIndex = 0;
    for (copyIndex=0; copyIndex<count; copyIndex++)
    {
        dest[copyIndex]=source[copyIndex];
    }
}

I2C_Mode I2C_Master_ReadReg(uint8_t dev_addr, uint8_t reg_addr, uint8_t count)
{
    //printf("R\n");
    /* Initialize state machine */
    MasterMode = TX_REG_ADDRESS_MODE;
    TransmitRegAddr = reg_addr;
    RXByteCtr = count;
    TXByteCtr = 0;
    ReceiveIndex = 0;
    TransmitIndex = 0;

    /* Initialize slave address and interrupts */
    UCB1I2CSA = dev_addr;
    UCB1IFG &= ~(UCTXIFG + UCRXIFG);       // Clear any pending interrupts
    UCB1IE &= ~UCRXIE;                       // Disable RX interrupt
    UCB1IE |= UCTXIE;                        // Enable TX interrupt

    UCB1CTLW0 |= UCTR + UCTXSTT;             // I2C TX, start condition
    __bis_SR_register(LPM0_bits + GIE);              // Enter LPM0 w/ interrupts
 //   UCB1IE &= ~UCRXIE;                       // Disable RX interrupt


    return MasterMode;

}

I2C_Mode I2C_Master_WriteReg(uint8_t dev_addr, uint8_t reg_addr, uint8_t *reg_data, uint8_t count)
{
    /* Initialize state machine */
    MasterMode = TX_REG_ADDRESS_MODE;
    TransmitRegAddr = reg_addr;

    //Copy register data to TransmitBuffer
    CopyArray(reg_data, TransmitBuffer, count);

    TXByteCtr = count;
    RXByteCtr = 0;
    ReceiveIndex = 0;
    TransmitIndex = 0;

    /* Initialize slave address and interrupts */
    UCB1I2CSA = dev_addr;
    UCB1IFG &= ~(UCTXIFG + UCRXIFG);       // Clear any pending interrupts
    UCB1IE &= ~UCRXIE;                       // Disable RX interrupt
    UCB1IE |= UCTXIE;                        // Enable TX interrupt

    UCB1CTLW0 |= UCTR + UCTXSTT;             // I2C TX, start condition
    __bis_SR_register(LPM0_bits + GIE);              // Enter LPM0 w/ interrupts
    //printf("W\n");
    return MasterMode;
}

//******************************************************************************
// BMI160 Functions ************************************************************
//******************************************************************************
void bmi160_init(char FOC_axis)
{
    uint8_t writeData[1];
    //Read Chip ID, which is D1
    I2C_Master_ReadReg(SLAVE_ADDR, 0x00, 1);
    if(ReceiveBuffer[0] != 0xD1)
    {
        UART_transmitString(" Incorrect sensor chip ID ");
        printf("Incorrect sensor chip ID\n");
    }




    //Configure the accelerometer
    writeData[0]=0b00101000; //Set acc_us to 0 for off, and acc_bwp must then be 010. Set acc_odr to 1011(800Hz),1100(1600Hz),1000(100Hz),0001(25/32Hz)
    I2C_Master_WriteReg(SLAVE_ADDR, 0x40, writeData, 1);
    //Check if configuration worked
    I2C_Master_ReadReg(SLAVE_ADDR, 0x40, 1);
    if(ReceiveBuffer[0] != writeData[0])
    {
        UART_transmitString(" Accelerometer config failed ");
        printf("Accelerometer config failed\n");
    }
    //Set the range of the accelerometer
    writeData[0]=0b1000; //0b0011 for 2g, 0b0101 for 4g, 0b1000 for 8g
    I2C_Master_WriteReg(SLAVE_ADDR, 0x41, writeData, 1);
    //Check if range is set
    I2C_Master_ReadReg(SLAVE_ADDR, 0x41, 1);
    if(ReceiveBuffer[0] != writeData[0])
    {
        UART_transmitString(" Accelerometer range set failed ");
        printf("Accelerometer range set failed\n");
    }


    //Set the Accelerometer to normal power mode
    writeData[0] = 0x11;
    I2C_Master_WriteReg(SLAVE_ADDR, 0x7E, writeData, 1);
    //Read power mode status of sensors
    I2C_Master_ReadReg(SLAVE_ADDR, 0x03, 1);
    if(ReceiveBuffer[0] != 0x10)
    {
        UART_transmitString(" Accelerometer not on ");
        printf("Accelerometer not on\n");
    }

}

void initGPIO()
{
    // I2C pins (P4.0 is SDA, P4.1 is SCL)
    P4SEL1 |= BIT0 | BIT1;
    P4SEL0 &= ~(BIT0 | BIT1);

    // Configure P3.4 and P3.5 to UART (Primary, TX and RX respectively) for NeoCortec
    P3SEL0 |= BIT4 | BIT5;                    // USCI_A1 UART operation
    P3SEL1 &= ~(BIT4 | BIT5);                 // SEL1 is 0 and SEL0 is 1 for primary operation, inverse for secondary

    // Configure P2.0 and P2.1 to UART (Primary, TX and RX respectively) for PC
    P2SEL0 |= BIT0 | BIT1;                    // USCI_A0 UART operation
    P2SEL1 &= ~(BIT0 | BIT1);                 // SEL1 is 0 and SEL0 is 1 for primary operation, inverse for secondary

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

void initClockTo16MHz()
{
    FRCTL0 = FRCTLPW | NWAITS_1;

    // Clock System Setup
    CSCTL0_H = CSKEY_H;                     // Unlock CS registers
    CSCTL1 = DCOFSEL_0;                     // Set DCO to 1MHz

    // Set SMCLK = MCLK = DCO, ACLK = VLOCLK (9.4kHz)
    CSCTL2 = SELA__VLOCLK | SELS__DCOCLK | SELM__DCOCLK;

    // Per Device Errata set divider to 4 before changing frequency to
    // prevent out of spec operation from overshoot transient
    CSCTL3 = DIVA__4 | DIVS__4 | DIVM__4;   // Set all corresponding clk sources to divide by 4 for errata
    CSCTL1 = DCOFSEL_4 | DCORSEL;           // Set DCO to 16MHz

    // Delay by ~10us to let DCO settle. 60 cycles = 20 cycles buffer + (10us / (1/4MHz))
    __delay_cycles(60);
    CSCTL3 = DIVA__32 | DIVS__1 | DIVM__1;  // Set ACLK to 239.75Hz, SMCLK to 16MHz, and MCLK to 16MHz
    CSCTL0_H = 0;                           // Lock CS registers
}

void initI2C()
{
    UCB1CTLW0 = UCSWRST;                      // Enable SW reset
    UCB1CTLW0 |= UCMODE_3 | UCMST | UCSSEL__SMCLK | UCSYNC; // I2C master mode, SMCLK
    UCB1BRW = 160;                            // fSCL = ACLK/160 = ~100kHz
    UCB1I2CSA = SLAVE_ADDR;                   // Slave Address
    UCB1CTLW0 &= ~UCSWRST;                    // Clear SW reset, resume operation
    UCB1IE |= UCNACKIE;
}

void UART_init(void)
{
    // Configure USCI_A1 for UART mode
    UCA1CTLW0 = UCSWRST;                      // Put eUSCI in reset
    UCA1CTLW0 |= UCSSEL__SMCLK;               // CLK = SMCLK
    UCA1BR0 = 8;                              // Clock prescaler set to 8
    UCA1BR1 = 0x00;                           // High byte empty, low byte is 8
    UCA1MCTLW |= UCOS16 | UCBRF_10 | 0xF700;  // Over-sampling on, first modulation register set to 10, second modulation register set to 0xF7 (247) for high byte, 0 for low byte
    UCA1CTLW0 &= ~UCSWRST;                    // Initialize eUSCI
    UCA1IE |= UCRXIE;                         // Enable USCI_A1 RX interrupt

    // Configure USCI_A0 for UART mode
    UCA0CTLW0 = UCSWRST;                      // Put eUSCI in reset
    UCA0CTLW0 |= UCSSEL__SMCLK;               // CLK = SMCLK
    UCA0BR0 = 8;                              // Clock prescaler set to 8
    UCA0BR1 = 0x00;                           // High byte empty, low byte is 8
    UCA0MCTLW |= UCOS16 | UCBRF_10 | 0xF700;  // Over-sampling on, first modulation register set to 10, second modulation register set to 0xF7 (247) for high byte, 0 for low byte
    UCA0CTLW0 &= ~UCSWRST;                    // Initialize eUSCI
    UCA0IE |= UCRXIE;                         // Enable USCI_A0 RX interrupt
}
void goToSleep(void)
{
    // Enable global interrupts
    __bis_SR_register(GIE);
    // Put MSP430 into LPM3 mode
    __bis_SR_register(LPM3_bits);
    // After waking up, the program continues from here
}

int main(void)
{
    WDTCTL = WDTPW;   // Stop watchdog timer
    //Initialize all peripherals
    initClockTo16MHz();
    initGPIO();
    UART_init();
    initI2C();
    timer_init();
    bmi160_init('Z');
    TA0CTL   = TA0CTL | (SMCLK + CONTINUOUS);           // SMCLK:  Counts faster than ACLK
    TA0CCTL0 = CCIE;                                    // Timer_0 interrupt
    TA1CTL   = TA1CTL | (ACLK  + UP         );          // Count up from 0 with ACLK
    TA1CCR0  = MS_10;                                   // Duration approximatley 10ms
    //_BIS_SR(GIE);
    __bis_SR_register(GIE);
    int i=0;
    // Activate all interrupts
    while(1)
    {       goToSleep();
            if (accelValue > ACCEL_THRESHOLD)
                    {
                        // Display accelerometer value
                        printf("Accelerometer value: %d\n", accelValue);
                    }

          /*
           printf("Reading samples\n");
           //Read SAMPLES amount of data from the BMI160
           for(i=0;i<SAMPLES;i++)
           {
               I2C_Master_ReadReg(SLAVE_ADDR, 0x16, 2); //Read the acceleration value from the BMI160 registers
               input[i]= ReceiveBuffer[0] | (ReceiveBuffer[1] << 8); //Store the value in an array
               printf("%d\n", (unsigned)(input[i]));
                     }
           */

    }
}

 //I2C Interrupt
 #pragma vector = USCI_B1_VECTOR
 __interrupt void USCI_B1_ISR(void)
 {
   //Must read from UCB1RXBUF
   uint8_t rx_val = 0;
   switch(__even_in_range(UCB1IV, USCI_I2C_UCBIT9IFG))
   {
     case USCI_NONE:          break;         // Vector 0: No interrupts
     case USCI_I2C_UCALIFG:   break;         // Vector 2: ALIFG
     case USCI_I2C_UCNACKIFG:                // Vector 4: NACKIFG
       UCB1CTLW0 |= UCTXSTT;                 // Re-send start if NACK
       break;
     case USCI_I2C_UCSTTIFG:  break;         // Vector 6: STTIFG
     case USCI_I2C_UCSTPIFG:  break;         // Vector 8: STPIFG
     case USCI_I2C_UCRXIFG3:  break;         // Vector 10: RXIFG3
     case USCI_I2C_UCTXIFG3:  break;         // Vector 12: TXIFG3
     case USCI_I2C_UCRXIFG2:  break;         // Vector 14: RXIFG2
     case USCI_I2C_UCTXIFG2:  break;         // Vector 16: TXIFG2
     case USCI_I2C_UCRXIFG1:  break;         // Vector 18: RXIFG1
     case USCI_I2C_UCTXIFG1:  break;         // Vector 20: TXIFG1
     case USCI_I2C_UCRXIFG0:                 // Vector 22: RXIFG0
         rx_val = UCB1RXBUF;
         if (RXByteCtr)
         {
           ReceiveBuffer[ReceiveIndex++] = rx_val;
           RXByteCtr--;
         }

         if (RXByteCtr == 1)
         {
           UCB1CTLW0 |= UCTXSTP;
         }
         else if (RXByteCtr == 0)
         {
           UCB1IE &= ~UCRXIE;
           MasterMode = IDLE_MODE;
           __bic_SR_register_on_exit(CPUOFF);      // Exit LPM0
         }
         break;
     case USCI_I2C_UCTXIFG0:                 // Vector 24: TXIFG0
         switch (MasterMode)
         {
           case TX_REG_ADDRESS_MODE:
               UCB1TXBUF = TransmitRegAddr;
               if (RXByteCtr)
                   MasterMode = SWITCH_TO_RX_MODE;   // Need to start receiving now
               else
                   MasterMode = TX_DATA_MODE;        // Continue to transmision with the data in Transmit Buffer
               break;

           case SWITCH_TO_RX_MODE:
               UCB1IE |= UCRXIE;              // Enable RX interrupt
               UCB1IE &= ~UCTXIE;             // Disable TX interrupt
               UCB1CTLW0 &= ~UCTR;            // Switch to receiver
               MasterMode = RX_DATA_MODE;    // State state is to receive data
               UCB1CTLW0 |= UCTXSTT;          // Send repeated start
               if (RXByteCtr == 1)
               {
                   //Must send stop since this is the N-1 byte
                   while((UCB1CTLW0 & UCTXSTT));
                   UCB1CTLW0 |= UCTXSTP;      // Send stop condition
               }
               break;

           case TX_DATA_MODE:
               if (TXByteCtr)
               {
                   UCB1TXBUF = TransmitBuffer[TransmitIndex++];
                   TXByteCtr--;
               }
               else
               {
                   //Done with transmission
                   UCB1CTLW0 |= UCTXSTP;     // Send stop condition
                   MasterMode = IDLE_MODE;
                   UCB1IE &= ~UCTXIE;                       // disable TX interrupt
                   __bic_SR_register_on_exit(CPUOFF);      // Exit LPM0
               }
               break;

           default:
               __no_operation();
               break;
         }
         break;
     default: break;
   }
 }



#pragma vector = TIMER0_A0_VECTOR
__interrupt void Timer_A0_ISR(void)
{
    // Read accelerometer value
    I2C_Master_ReadReg(SLAVE_ADDR, 0x16, 2);
    accelValue = ReceiveBuffer[0] | (ReceiveBuffer[1] << 8);
    printf("Interrupt value: %d\n", accelValue);
    // Wake up the MSP430
    __bic_SR_register_on_exit(LPM3_bits);
}