#include "mbed.h"
#include "IMUTest.h"

#define RAW2ANG (1.0/91.02222)
#define RAW2ANGDOT (1.0/16.0)
#define NELEMS(x)  (sizeof(x) / sizeof((x)[0]))

//Serial pc(USBTX, USBRX);
Serial sdCard(D1, D0);
Serial um7serial(A0, A1);
DigitalOut myled(LED1);

// read a float from 4 bytes in buffer,
// starting at offset, MSB first
float readFloat(uint8_t * buffer, int offset) {
  float val=0;
 
  unsigned long result=0;
  result |= ((unsigned long)(buffer[offset]) << 0x18);
  result |= ((unsigned long)(buffer[offset+1]) << 0x10);
  result |= ((unsigned long)(buffer[offset+2]) << 0x08);
  result |= ((unsigned long)(buffer[offset+3]));
  memcpy(&val,&result,4);
 
  return val;
}

void readIMU(){
    int packet_length = 7 + 4*5;
    uint8_t rx_data[packet_length];
    
    int i=0;
    while(i<packet_length){
        if(um7serial.readable()){ 
//            pc.printf("Data received: ");
            rx_data[i] = um7serial.getc();
//            pc.printf("%c",rx_data[i]);
            i++;
        }
    }
//    pc.printf("\n");

    UM7_packet new_packet;
    // Call the parse_serial_data function to handle the incoming serial data. The serial data should
    // be placed in ’rx_data’ and the length in ‘packet_length’ before this function is called.
    int datastatus = parse_serial_data( rx_data, packet_length, &new_packet );
//    pc.printf("%i\n", datastatus);
    if( !datastatus){
    // We got a good packet!
//        pc.printf("Is it 0x70?: %i\n", new_packet.Address==0x70);
//        pc.printf("Is it 0x71?: %i\n", new_packet.Address==0x71);
//        pc.printf("Is it 0x72?: %i\n", new_packet.Address==0x72);
//        pc.printf("Is it 0x73?: %i\n", new_packet.Address==0x73);
//        pc.printf("Is it 0x74?: %i\n", new_packet.Address==0x74);
//        pc.printf("Size of data packet: %i\n\n", NELEMS(new_packet.data));
//        pc.printf("data_length: %i\n\n", new_packet.data_length);
//        pc.printf("Address: %hhx\n\n", new_packet.Address);

        if( new_packet.Address == 0x70 ){
            rollRaw = 256*new_packet.data[0] + new_packet.data[1];
            roll = rollRaw*RAW2ANG;
            pitchRaw = 256*new_packet.data[2] + new_packet.data[3];
            pitch = pitchRaw*RAW2ANG;
            yawRaw = 256*new_packet.data[4] + new_packet.data[5];
            yaw = yawRaw*RAW2ANG;
            rollDotRaw = 256*new_packet.data[8] + new_packet.data[9];
            rollDot = rollDotRaw*RAW2ANGDOT;
            pitchDotRaw = 256*new_packet.data[10] + new_packet.data[11];
            pitchDot = pitchDotRaw*RAW2ANGDOT;
            yawDotRaw = 256*new_packet.data[12] + new_packet.data[13];
            yawDot = yawDotRaw*RAW2ANGDOT;
            eulerTime = readFloat(&new_packet.data[0], 16);
        }
    }
}

void IMUTest()
{
//    pc.baud(115200);
    sdCard.baud(115200);
    um7serial.baud(115200);
    
//    pc.printf("IMU Test\n");
    
    while(1){
        readIMU();
        /*
        pc.printf("roll : %f\r\n", roll);
        pc.printf("pitch: %f\r\n", pitch);
        pc.printf("yaw  : %f\r\n", yaw);
        pc.printf("rollDot : %f\r\n", rollDot);
        pc.printf("pitchDot: %f\r\n", pitchDot);
        pc.printf("yawDot  : %f\r\n", yawDot); 
        pc.printf("eulerTime  : %f\r\n", eulerTime); 
        pc.printf("\n");
        */
//        pc.printf("%f, %f, %f, %f, %f, %f, %f\n",roll, pitch, yaw, rollDot, pitchDot, yawDot, eulerTime);
        sdCard.printf("%f, %f, %f, %f, %f, %f, %f\n",roll, pitch, yaw, rollDot, pitchDot, yawDot, eulerTime);
        wait(0.5);
    }
}
