// Accelerometer module using some of Eddy's code


// Inlude the iic header file
//#include "iic.h"

// Define functions to be used
//void accelInit(void);
//void obtainAccelData(int *axraw, int *ayraw, int *azraw);




// 1.30am 23/5/18   Working accelerometer, displaying values to 1 decimal place on the LCD display
                    // Just need to display accelerometer under value




#define accel_wr 0xA6               // iic address for write
#define accel_rd 0xA7               // iic address for read
//#define ADXL345_TO_READ 6


#define ADXL345_POWER_CTL 0x2D      // The power control register
#define ADXL345_DATAX0 0x32         // The register for the first accelerometer data
#define ADXL345_DATA_FORMAT 0x31    // The register to control the presentation of data

// Registers for user set offset adjustments in 2s complement format
#define ADXL345_OFSX 0x1E
#define ADXL345_OFSY 0x1F
#define ADXL345_OFSZ 0x20

#define BUFF_SIZE   100

int buff[BUFF_SIZE];                // Create a buffer
int axraw[BUFF_SIZE];               // Array for the raw data of the x acceleration
int ayraw[BUFF_SIZE];               // Array for the raw data of the y acceleration
int azraw[BUFF_SIZE];               // Array for the raw data of the z acceleration

int lookup[800]= {0, 0, 0, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6, 7, 7, 7, 7, 7, 8, 8, 8, 8, 8, 8, 9, 9, 9, 9, 9, 9, 10, 10, 10, 10, 10, 11, 11, 11, 11, 11, 11, 12, 12, 12, 12, 12, 12, 13, 13, 13, 13, 13, 14, 14, 14, 14, 14, 14, 15, 15, 15, 15, 15, 15, 16, 16, 16, 16, 16, 17, 17, 17, 17, 17, 17, 18, 18, 18, 18, 18, 19, 19, 19, 19, 19, 19, 20, 20, 20, 20, 20, 21, 21, 21, 21, 21, 21, 22, 22, 22, 22, 22, 23, 23, 23, 23, 23, 23, 24, 24, 24, 24, 24, 25, 25, 25, 25, 25, 25, 26, 26, 26, 26, 26, 27, 27, 27, 27, 27, 28, 28, 28, 28, 28, 28, 29, 29, 29, 29, 29, 30, 30, 30, 30, 30, 31, 31, 31, 31, 31, 32, 32, 32, 32, 32, 32, 33, 33, 33, 33, 33, 34, 34, 34, 34, 34, 35, 35, 35, 35, 35, 36, 36, 36, 36, 36, 37, 37, 37, 37, 37, 38, 38, 38, 38, 38, 39, 39, 39, 39, 39, 40, 40, 40, 40, 40, 41, 41, 41, 41, 41, 42, 42, 42, 42, 42, 43, 43, 43, 43, 43, 44, 44, 44, 44, 45, 45, 45, 45, 45, 46, 46, 46, 46, 46, 47, 47, 47, 47, 47, 48, 48, 48, 48, 49, 49, 49, 49, 49, 50, 50, 50, 50, 51, 51, 51, 51, 51, 52, 52, 52, 52, 53, 53, 53, 53, 53, 54, 54, 54, 54, 55, 55, 55, 55, 55, 56, 56, 56, 56, 57, 57, 57, 57, 58, 58, 58, 58, 58, 59, 59, 59, 59, 60, 60, 60, 60, 61, 61, 61, 61, 62, 62, 62, 62, 62, 63, 63, 63, 63, 64, 64, 64, 64, 65, 65, 65, 65, 66, 66, 66, 66, 67, 67, 67, 67, 68, 68, 68, 68, 69, 69, 69, 70, 70, 70, 70, 71, 71, 71, 71, 72, 72, 72, 72, 73, 73, 73, 73, 74, 74, 74, 75, 75, 75, 75, 76, 76, 76, 76, 77, 77, 77, 78, 78, 78, 78, 79, 79, 79, 80, 80, 80, 80, 81, 81, 81, 82, 82, 82, 82, 83, 83, 83, 84, 84, 84, 85, 85, 85, 85, 86, 86, 86, 87, 87, 87, 88, 88, 88, 88, 89, 89, 89, 90, 90, 90, 91, 91, 91, 92, 92, 92, 93, 93, 93, 94, 94, 94, 95, 95, 95, 96, 96, 96, 97, 97, 97, 98, 98, 98, 99, 99, 99, 100, 100, 100, 101, 101, 101, 102, 102, 102, 103, 103, 104, 104, 104, 105, 105, 105, 106, 106, 106, 107, 107, 108, 108, 108, 109, 109, 110, 110, 110, 111, 111, 111, 112, 112, 113, 113, 113, 114, 114, 115, 115, 115, 116, 116, 117, 117, 117, 118, 118, 119, 119, 120, 120, 120, 121, 121, 122, 122, 123, 123, 123, 124, 124, 125, 125, 126, 126, 127, 127, 128, 128, 128, 129, 129, 130, 130, 131, 131, 132, 132, 133, 133, 134, 134, 135, 135, 136, 136, 137, 137, 138, 138, 139, 139, 140, 140, 141, 141, 142, 142, 143, 143, 144, 144, 145, 146, 146, 147, 147, 148, 148, 149, 149, 150, 151, 151, 152, 152, 153, 153, 154, 155, 155, 156, 156, 157, 158, 158, 159, 159, 160, 161, 161, 162, 163, 163, 164, 164, 165, 166, 166, 167, 168, 168, 169, 170, 170, 171, 172, 173, 173, 174, 175, 175, 176, 177, 177, 178, 179, 180, 180, 181, 182, 183, 183, 184, 185, 186, 186, 187, 188, 189, 190, 190, 191, 192, 193, 194, 195, 195, 196, 197, 198, 199, 200, 201, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 213, 214, 215, 216, 217, 218, 219, 220, 221, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 236, 237, 238, 239, 240, 241, 243, 244, 245, 246, 248, 249, 250, 251, 253, 254, 255, 256, 258, 259, 261, 262, 263, 265, 266, 267, 269, 270, 272, 273, 275};


// DELAY?




//  FUNCTIONS
// -----------



// Function for initialistaion of acceleromter
void accelInit(void){

    int  res1;

     res1=iicstart(accel_wr);                       // Gives the bus address to write to start function
     res1=iictransmit(ADXL345_POWER_CTL);   // Uses function to set power control register
     res1=iictransmit(0x08);                        // Enable measurement mode

     res1=iictransmit(ADXL345_DATA_FORMAT); // Uses function to set data format register
     res1=iictransmit(0x08);                        // Set data format to full resolution and +/- 2g range

     iicstop();                             // Function to stop bus communication
}


// Function to obtain the raw data from the readings of the accelerometer
void obtainAccelData(int *axraw, int *ayraw, int *azraw){

    uint8_t i = 0;      // unsigned short
    //uint8_t buff[] = {0,0,0,0,0,0,0}; // buffer
    int res1;

     res1=iicstart(accel_wr);               // Gives the bus address to write to start function
     res1=iictransmit(ADXL345_DATAX0 );     // Gives function address of first data register (X0)
     res1= iicrestart(accel_rd);            // Sets to read
     iicswrcv();                            // Sets to receive mode

     for(i=0; i<4  ;i++) {                  // Loop to receive data values
        buff[i]=iicreceive();                   // Store data received in a buffer
     }

     buff[i]= iicreceivem1();               // Functions use last 2 values??
     buff[i+1]= iicreceivelast();

    // WORK OUT HOW THESE WORK
    *axraw = ((buff[1] << 8) | buff[0]);    // 2nd byte of data is left shifted 8 bits then OR'd with first byte of data
    *ayraw = ((buff[3] << 8) | buff[2]);
    *azraw = ((buff[5] << 8) | buff[4]);


}



int accelToDegrees(int *axraw, int *azraw) {

    int i;
    int index;
    int isNeg = 0;
    int accelz = *azraw;
    int accelx = *axraw;
    long int ratio =  (100*accelz/accelx);// For some reason working when multiply by 100 but not 1000  (FIX LATER)


    // if the ratio is negative make positive but set flag for later
  if (ratio<0){
      isNeg = 1;        // Set flag to show negative
      ratio = ratio*-1; // Make positive
    }

    // Loop through the lookup table and compare vals, store index
    for (i=0; i<=700; i++){
      if (ratio>= lookup[i]){
            index = i;
        }
    }

    // Now check if the original value was negative in the beginning
    if (isNeg == 1){
      index = index*-1;
    }

    return index;; // normally return index but for now return the lookup table value

    // Now use the index to calculate the degree
  // Display index for now


}