#makewithmaxim VR glove

#include "mbed.h"
#include "max32630fthr.h"
#include "USBSerial.h"
#include "bmi160.h"
#include "USBMSD_SD.h"

#define         RED "\[\033[0;31m\]"
#define   LIGHT_RED "\[\033[1;31m\]"
#define       GREEN "\[\033[0;32m\]"
#define LIGHT_GREEN "\[\033[1;32m\]"
#define        BLUE "\[\033[0;34m\]"
#define  LIGHT_BLUE "\[\033[1;34m\]"
#define      YELLOW "\[\033[1;33m\]"
#define       WHITE "\[\033[1;37m\]"
#define  LIGHT_GRAY "\[\033[0;37m\]"

MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);

USBMSD_SD sd(P0_5, P0_6, P0_4, P0_7);  // mosi, miso, sclk, cs

// Hardware serial port over DAPLink
Serial daplink(P2_1, P2_0);
SPI spi(P5_1, P5_2, P5_0); // mosi, miso, sclk
DigitalOut cs(P4_0);

// Virtual serial port over USB
//USBSerial microUSB;

DigitalOut rLED(LED1);
DigitalOut gLED(LED2);
DigitalOut bLED(LED3);
AnalogIn adc1(AIN_2);       //1.8 volt input
//DigitalOut out1(P5_0);      //Digital output test

//Init bluetooth stuff
Serial pan1326b(P0_1, P0_0); 
DigitalOut pan1326b_CTS(P0_2);
DigitalIn pan1326b_RTS(P0_3);
DigitalOut pan1326b_RST(P1_6);
DigitalOut pan1326b_CLK(P1_7);

//prototypes 
void dumpImuRegisters(BMI160 &imu);
void printRegister(BMI160 &imu, BMI160::Registers reg);
void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg);
int Get_MCP3208(int chan);
void get_fingers(int *thumb, int *index, int *middle, int *ring, int *pinky);
void Init_CC2564(void)

//------------- Functions ----------------------------------
//See 6.4.2 Host Controller Interface for CC256X Dual Mode Bluetooth Controller
void Init_CC2564(void){
    pan1326b.baud(115200);

}

void CC2564_send_msg(char *str){
    if(pan1326b_RTS == 0){
    
    }
    else{
        daplink.printf("RTS was high, terminate sending message\r\n");
    }
}

void get_fingers(int *thumb, int *index, int *middle, int *ring, int *pinky){
    *thumb = Get_MCP3208(1);
    *index = Get_MCP3208(2);
    *middle = Get_MCP3208(3);
    *ring = Get_MCP3208(4);
    *pinky = Get_MCP3208(5); 
}

int Get_MCP3208(int chan)
{
    unsigned char ch_result[3];
    char cword[3];
    typedef union
    {
        int intd;
        char bytes[4];
    }A2D_result;

    A2D_result A2D_result_1;

    cword[0] = 0x06; //Start bit, single
    cword[0] += (chan >> 2);    // + MSB of chan
    cword[0] &= 0x07;
    cword[1] = (chan << 6); //2 LSB's of chan

    cs=0;

    ch_result[0] = spi.write(cword[0]);
    ch_result[1] = spi.write(cword[1]);
    ch_result[2] = spi.write(cword[2]);

    cs=1;

    ch_result[1] &= 0x0f;
    A2D_result_1.bytes[1] = ch_result[1];
    A2D_result_1.bytes[0] = ch_result[2];
    A2D_result_1.bytes[2] = 0;
    A2D_result_1.bytes[3] = 0;
    return A2D_result_1.intd;
}

// main() runs in its own thread in the OS
// (note the calls to Thread::wait below for delays)
int main()
{
    int c;

    wait(.2);
    spi.format(8, 0);
    spi.frequency(1000000);
    daplink.baud(115200);
    //daplink.printf("%s",WHITE); //Turn cursor white
    daplink.printf("%s\r",LIGHT_GRAY); //Turn cursor light grey
    daplink.printf("daplink serial port\r\n");
//    microUSB.printf("micro USB serial port\r\n");
    rLED = LED_ON;
    gLED = LED_ON;
    bLED = LED_OFF;

    rLED = LED_OFF;

    I2C i2cBus(P5_7, P6_0);
    i2cBus.frequency(400000);
    BMI160_I2C imu(i2cBus, BMI160_I2C::I2C_ADRS_SDO_LO);
    
    //--------------------------

    while(1) {
        daplink.printf("\033[H");  //home
        daplink.printf("\033[0J");  //erase from cursor to end of screen
    
        uint32_t failures = 0;
    
        if(imu.setSensorPowerMode(BMI160::GYRO, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
        {
            daplink.printf("Failed to set gyroscope power mode\r\n");
            failures++;
        }
        wait_ms(100);
    
        if(imu.setSensorPowerMode(BMI160::ACC, BMI160::NORMAL) != BMI160::RTN_NO_ERROR)
        {
            daplink.printf("Failed to set accelerometer power mode\r\n");
            failures++;
        }
        wait_ms(100);
    
    
        BMI160::AccConfig accConfig;
        //example of using getSensorConfig
        if(imu.getSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
        {
            daplink.printf("ACC Range = %d\n", accConfig.range);
            daplink.printf("ACC UnderSampling = %d\r\n", accConfig.us);
            daplink.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp);
            daplink.printf("ACC OutputDataRate = %d\r\n\n", accConfig.odr);
        }
        else
        {
            daplink.printf("Failed to get accelerometer configuration\n");
            failures++;
        }
    
        //example of setting user defined configuration
        accConfig.range = BMI160::SENS_4G;
        accConfig.us = BMI160::ACC_US_OFF;
        accConfig.bwp = BMI160::ACC_BWP_2;
        accConfig.odr = BMI160::ACC_ODR_8;
        if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
        {
            daplink.printf("ACC Range = %d\n", accConfig.range);
            daplink.printf("ACC UnderSampling = %d\r\n", accConfig.us);
            daplink.printf("ACC BandWidthParam = %d\r\n", accConfig.bwp);
            daplink.printf("ACC OutputDataRate = %d\r\n\n", accConfig.odr);
        }
        else
        {
            daplink.printf("Failed to set accelerometer configuration\n");
            failures++;
        }
    
        BMI160::GyroConfig gyroConfig;
        if(imu.getSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR)
        {
            daplink.printf("GYRO Range = %d\r\n", gyroConfig.range);
            daplink.printf("GYRO BandWidthParam = %d\r\n", gyroConfig.bwp);
            daplink.printf("GYRO OutputDataRate = %d\r\n\n", gyroConfig.odr);
        }
        else
        {
            daplink.printf("Failed to get gyroscope configuration\r\n");
            failures++;
        }
    
        wait(1.0);
        printf("\033[H");  //home
        printf("\033[0J");  //erase from cursor to end of screen
    
        if(failures == 0)
        {
            float imuTemperature;
            BMI160::SensorData accData;
            BMI160::SensorData gyroData;
            BMI160::SensorTime sensorTime;
        
            while(1)
            {
                unsigned int val;
                //val = Get_MCP3208(0);
                            
                imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
                imu.getTemperature(&imuTemperature);
            
                daplink.printf("\033[H");  //home
            
                daplink.printf("%s\r",RED); //Turn cursor red
                daplink.printf("ACC xAxis = %s%4.3f\r\n", "\033[K", accData.xAxis.scaled);
                daplink.printf("ACC yAxis = %s%4.3f\r\n", "\033[K", accData.yAxis.scaled);
                daplink.printf("ACC zAxis = %s%4.3f\r\n\n", "\033[K", accData.zAxis.scaled);
            
                daplink.printf("%s\r",BLUE); //Turn cursor blue
                daplink.printf("GYRO xAxis = %s%5.1f\r\n","\033[K", gyroData.xAxis.scaled);
                daplink.printf("GYRO yAxis = %s%5.1f\r\n", "\033[K", gyroData.yAxis.scaled);
                daplink.printf("GYRO zAxis = %s%5.1f\r\n\n", "\033[K", gyroData.zAxis.scaled);
                            
                daplink.printf("%s\r",GREEN); //Turn cursor green
                daplink.printf("Sensor Time = %s%f\r\n", "\033[K", sensorTime.seconds);
                daplink.printf("Sensor Temperature = %s%5.3f\r\n", "\033[K", imuTemperature);
                        
                float ad_val = adc1.read();
            
                daplink.printf("%s\r",LIGHT_GRAY); //Turn cursor light grey
                daplink.printf("ADC = %s%5.3f\r\n", "\033[K", ad_val);
                daplink.printf("\033[0J");  //erase from cursor to end of screen             
            
                daplink.printf("%s\r",YELLOW); //Turn cursor yellow
                int thumb, index, middle, ring, pinky;
                get_fingers(&thumb, &index, &middle, &ring, &pinky);
                daplink.printf("%d,%d,%d,%d,%d\r\n", thumb,index,middle,ring,pinky);
            
                gLED = !gLED;
            }
        }
        else
        {
            while(1)
            {
                rLED = !rLED;
                wait(0.25);
            }
        }
    }
}