/*
 * MSP432 Signal Pass-through with triple buffering
 *
 * Copyright (c) Dr. Xinrong Li @ UNT
 * All rights reserved.
 */
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>  //Exact-width integer types
#include <ti\devices\msp432p4xx\driverlib\driverlib.h>  //Driver library

#define CHANNEL1 0
#define CHANNEL2 1
#define CHANNEL3 2
#define CHANNEL4 3

#define THROTTLE 3
#define ROLL     2
#define PITCH    1
#define YAW      0

#define PI    3.14159265358979323846

#define X           0     // X axis
#define Y           1     // Y axis
#define Z           2     // Z axis
#define FREQ        250   // Sampling frequency
#define DCO_FREQ    48e6
#define SSF_GYRO    65.5  // Sensitivity Scale Factor of the gyro from datasheet


int         initialized = 0;
int         gyro_raw[3] = {0,0,0};
long     gyro_offset[3] = {0,0,0};
float     gyro_angle[3] = {0,0,0};

int          acc_raw[3] = {0,0,0};
float      acc_angle[3] = {0,0,0};
long   acc_total_vector;//total 3D acceleration vector

float errors[3];                     // Measured errors (compared to instructions) : [Yaw, Pitch, Roll]
float error_sum[3]      = {0, 0, 0}; // Error sums (used for integral component) : [Yaw, Pitch, Roll]
float previous_error[3] = {0, 0, 0}; // Last errors (used for derivative component) : [Yaw, Pitch, Roll]

unsigned long pulse_length_esc1 = 1000;
unsigned long pulse_length_esc2 = 1000;
unsigned long pulse_length_esc3 = 1000;
unsigned long pulse_length_esc4 = 1000;

float measures [3] = {0,0,0,};
float instruction[4];

void initDevice(void);
void calculateAngles(void);
void calculateGyroAngles(void);
void calculateAccelerometerAngles(void);
void resetGyroAngles(void);
void getFlightInstruction(void);
void calculateErrors(void);
void pidController(void);
void initGPIO(void);
void initUART(void);

uint8_t BlueTooth = 0;
uint8_t Temp;
uint8_t PWM;

void main(void)
{
    // 1. First, read raw values from IMU
        //read and store the values to
          //accelRaw=[x,y,z]
          //gyroRaw=[x,y,z]
    // 2. Calculate angles from gyro & accelerometer's values
    calculateAngles();

    // 3. takes user input map it in the throttle range of 1ms-2ms
    getFlightInstruction();
    // 4. takes the difference between the user input and measured valued
    calculateErrors();
    // 5. Calculate motors speed with PID controller
    pidController();
    // 6. Apply motors speed to PCA pwm board
}

void calculateAngles(void)
{
calculateGyroAngles();
calculateAccelerometerAngles();

    if (initialized==1)
    {
        // Correct the drift of the gyro with the accelerometer
        gyro_angle[X] = gyro_angle[X] * 0.9996 + acc_angle[X] * 0.0004;
        gyro_angle[Y] = gyro_angle[Y] * 0.9996 + acc_angle[Y] * 0.0004;
    }
    else
    {
        // At very first start, init gyro angles with accelerometer angles
        resetGyroAngles();
        initialized = 1;
    }

    // To dampen the pitch and roll angles a complementary filter is used
    measures[ROLL]  = measures[ROLL]  * 0.9 + gyro_angle[X] * 0.1;
    measures[PITCH] = measures[PITCH] * 0.9 + gyro_angle[Y] * 0.1;
    measures[YAW]   = -gyro_raw[Z] / SSF_GYRO; // Store the angular motion for this axis
}

void calculateGyroAngles(void)
{
    // Subtract offsets
    gyro_raw[X] -= gyro_offset[X];
    gyro_raw[Y] -= gyro_offset[Y];
    gyro_raw[Z] -= gyro_offset[Z];

    // Angle calculation using integration
    gyro_angle[X] += (gyro_raw[X] / (FREQ * SSF_GYRO));
    gyro_angle[Y] += (-gyro_raw[Y] / (FREQ * SSF_GYRO)); // Change sign to match the accelerometer's one

    // Transfer roll to pitch if IMU has yawed
    gyro_angle[Y] += gyro_angle[X] * sin(gyro_raw[Z] * (PI / (FREQ * SSF_GYRO * 180)));
    gyro_angle[X] -= gyro_angle[Y] * sin(gyro_raw[Z] * (PI / (FREQ * SSF_GYRO * 180)));
}

void calculateAccelerometerAngles(void)
{
    // Calculate total 3D acceleration vector : sqrt(X² + Y² + Z²)
    acc_total_vector = sqrt(pow(acc_raw[X], 2) + pow(acc_raw[Y], 2) + pow(acc_raw[Z], 2));

    // To prevent asin to produce a NaN, make sure the input value is within [-1;+1]
    if (abs(acc_raw[X]) < acc_total_vector)
    {
        acc_angle[X] = asin((float)acc_raw[Y] / acc_total_vector) * (180 / PI); // asin gives angle in radian. Convert to degree multiplying by 180/pi
    }

    if (abs(acc_raw[Y]) < acc_total_vector)
    {
        acc_angle[Y] = asin((float)acc_raw[X] / acc_total_vector) * (180 / PI);
    }
}

void resetGyroAngles(void)
{
    gyro_angle[X] = acc_angle[X];
    gyro_angle[Y] = acc_angle[Y];
}
void getFlightInstruction(void)
{
    // * - Roll     : from -33° to 33°
    //* - Pitch    : from -33° to 33°
    //* - Yaw      : from -180°/sec to 180°/sec
    //* - Throttle : from 1000µs to 1800µs

    instruction[YAW]     =0;
    instruction[PITCH]   =0;
    instruction[ROLL]    =0;
    //need to translate no input user input into pulse length
    if(BlueTooth==127)
    {
        instruction[THROTTLE]= PWM+50;//throttle
        if(instruction[THROTTLE]<=1000)
        {
            instruction[THROTTLE]=1000;
        }
    }
    if(BlueTooth==120)
    {
        instruction[THROTTLE] = PWM-50;//throttle
        if(instruction[THROTTLE]>=1800)
        {
            instruction[THROTTLE]=1800;
        }
    }
    BlueTooth=0;

};

void calculateErrors(void)
{
    errors[YAW]   = instruction[YAW]   - measures[YAW];
    errors[PITCH] = instruction[PITCH] - measures[PITCH];
    errors[ROLL]  = instruction[ROLL]  - measures[ROLL];
}

void pidController(void)
{
    float Kp[3]        = {4.0, 1.3, 1.3};    // P coefficients in that order : Yaw, Pitch, Roll
        float Ki[3]        = {0.02, 0.04, 0.04}; // I coefficients in that order : Yaw, Pitch, Roll
        float Kd[3]        = {0, 18, 18};        // D coefficients in that order : Yaw, Pitch, Roll
        float delta_err[3] = {0, 0, 0};          // Error deltas in that order   : Yaw, Pitch, Roll
        float yaw_pid      = 0;
        float pitch_pid    = 0;
        float roll_pid     = 0;

        // Initialize motor commands with throttle
        pulse_length_esc1 = instruction[THROTTLE];
        pulse_length_esc2 = instruction[THROTTLE];
        pulse_length_esc3 = instruction[THROTTLE];
        pulse_length_esc4 = instruction[THROTTLE];

        // Do not calculate anything if throttle is 0
        if (instruction[THROTTLE] >= 1012) {
            // Calculate sum of errors : Integral coefficients
            error_sum[YAW]   += errors[YAW];
            error_sum[PITCH] += errors[PITCH];
            error_sum[ROLL]  += errors[ROLL];

            // Calculate error delta : Derivative coefficients
            delta_err[YAW]   = errors[YAW]   - previous_error[YAW];
            delta_err[PITCH] = errors[PITCH] - previous_error[PITCH];
            delta_err[ROLL]  = errors[ROLL]  - previous_error[ROLL];

            // Save current error as previous_error for next time
            previous_error[YAW]   = errors[YAW];
            previous_error[PITCH] = errors[PITCH];
            previous_error[ROLL]  = errors[ROLL];

            // PID = e.Kp + (integral)e.Ki + (delta)e.Kd
            yaw_pid   = (errors[YAW]   * Kp[YAW])   + (error_sum[YAW]   * Ki[YAW])   + (delta_err[YAW]   * Kd[YAW]);
            pitch_pid = (errors[PITCH] * Kp[PITCH]) + (error_sum[PITCH] * Ki[PITCH]) + (delta_err[PITCH] * Kd[PITCH]);
            roll_pid  = (errors[ROLL]  * Kp[ROLL])  + (error_sum[ROLL]  * Ki[ROLL])  + (delta_err[ROLL]  * Kd[ROLL]);

            // Calculate pulse duration for each ESC
            pulse_length_esc1 = instruction[THROTTLE] + roll_pid + pitch_pid - yaw_pid;
            pulse_length_esc2 = instruction[THROTTLE] - roll_pid + pitch_pid + yaw_pid;
            pulse_length_esc3 = instruction[THROTTLE] + roll_pid - pitch_pid + yaw_pid;
            pulse_length_esc4 = instruction[THROTTLE] - roll_pid - pitch_pid - yaw_pid;
        }

        //sets the minimum and maximum throttle values
        //keeps pwms within esc operating range
         if (pulse_length_esc1 <= 1100)
         {
             pulse_length_esc1 =1100;
         }
         else
         {
             pulse_length_esc1 =2000;
         }
         if (pulse_length_esc2 <= 1100)
         {
             pulse_length_esc2 =1100;
         }
         else
         {
             pulse_length_esc2 =2000;
         }
         if (pulse_length_esc3 <= 1100)
         {
             pulse_length_esc3 =1100;
         }
         else
         {
             pulse_length_esc3 =2000;
         }
         if (pulse_length_esc4 <= 1100)
         {
             pulse_length_esc4 =1100;
         }
         else
         {
             pulse_length_esc4 =2000;
         }
}

void initDevice(void)
{
    WDT_A_holdTimer();  //stop Watchdog timer

    //Change VCORE to 1 to support a frequency higher than 24MHz.
    //See data sheet for Flash wait-state requirement for a given frequency.
    PCM_setPowerState(PCM_AM_LDO_VCORE1);
    FlashCtl_setWaitState(FLASH_BANK0, 1);
    FlashCtl_setWaitState(FLASH_BANK1, 1);

    //Enable FPU for DCO Frequency calculation.
    FPU_enableModule();
    FPU_enableLazyStacking(); // Required to use FPU within ISR.

    //Only use DCO nominal frequencies: 1.5, 3, 6, 12, 24, 48MHz.
    CS_setDCOFrequency(DCO_FREQ);

    //Divider: 1, 2, 4, 8, 16, 32, 64, or 128.
    //SMCLK used by UART and ADC14.

    CS_initClockSignal(CS_SMCLK, CS_DCOCLK_SELECT, CS_CLOCK_DIVIDER_16);

}

void initGPIO(void)
{
    //                       - P3.2: IN  ( UART: UCA2RXD ).
    //                       - P3.3: OUT ( UART: UCA2TXD ).
   // P3->SEL0    |=   ( BIT2 | BIT3 );
    GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P3, GPIO_PIN2, GPIO_PRIMARY_MODULE_FUNCTION);
    // P3->SEL1    &=  ~( BIT2 | BIT3 );
    GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P3, GPIO_PIN3, GPIO_PRIMARY_MODULE_FUNCTION);
}

void initUART(void)
{
    EUSCI_A2->CTLW0   |=   EUSCI_A_CTLW0_SWRST;
    //Configuration for 3MHz SMCLK, 9600 baud rate.
    //Calculated using the online calculator that TI provides at:
    //http://software-dl.ti.com/msp430/msp430_public_sw/mcu/msp430/MSP430BaudRateConverter/index.html

    const eUSCI_UART_Config config2 =
    {
        EUSCI_A_UART_CLOCKSOURCE_SMCLK, //SMCLK Clock Source
        19, //BRDIV = 19
        9, //UCxBRF = 8
        85, //UCxBRS = 0
        EUSCI_A_UART_NO_PARITY, //No Parity
        EUSCI_A_UART_LSB_FIRST, //MSB First
        EUSCI_A_UART_ONE_STOP_BIT, //One stop bit
        EUSCI_A_UART_MODE, //UART mode
        EUSCI_A_UART_OVERSAMPLING_BAUDRATE_GENERATION //Oversampling
    };

    // Configure GPIO pins for UART. RX: P1.2, TX:P1.3.
    GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P1, GPIO_PIN2|GPIO_PIN3, GPIO_PRIMARY_MODULE_FUNCTION);

    UART_initModule(EUSCI_A2_BASE, &config2);
    UART_enableModule(EUSCI_A2_BASE);                                    // Initialize eUSCI

//EUSCI_A2->IFG     &=  ~EUSCI_A_IFG_RXIFG;  //EUSCI_A_IFG_RXIFG=1 Clear eUSCI RX interrupt flag
UART_clearInterruptFlag(EUSCI_A2_BASE,~EUSCI_A_IFG_RXIFG);

//EUSCI_A2->IE      |=   EUSCI_A_IE_RXIE;                                           // Enable USCI_A2 RX interrupt
UART_enableInterrupt(EUSCI_A2_BASE,EUSCI_A_IE_RXIE);

//see website fore more explanation on NVIC: https://www.keil.com/pack/doc/CMSIS/Core/html/group__NVIC__gr.html#ga5bb7f43ad92937c039dee3d36c3c2798
NVIC_EnableIRQ    ( EUSCIA2_IRQn );   //for cortex-M based device
NVIC_SetPriority  ( EUSCIA2_IRQn, 16 );//for cortex-M based device
}

void EUSCIA2_IRQHandler (void)
{
    uint8_t Mask = 0x7f;
    //Bluetooth transmits a packet that can be set to 9 bits that is read LSB first.
    //check to see if setting UART to LSB manipulates the Data to become MSB or manipulation is required
    //Check to see the default HC-06 packet size setting packets vary from 5-9 bits and register is 8 bits
    // UART_clearInterruptFlag(EUSCI_A2_BASE,~EUSCI_A_IFG_RXIFG);

    if (EUSCI_A2->IFG & EUSCI_A_IFG_RXIFG)
    {

        Temp = UART_receiveData(EUSCI_A2_BASE);
        BlueTooth = (Temp & Mask);
        //BlueTooth = EUSCI_A2->RXBUF;
        UART_clearInterruptFlag(EUSCI_A2_BASE,~EUSCI_A_IFG_RXIFG);

    }
}