/*

 Circuit:
 ICM-20602 6-axis MotionTracking attached to port pins 7, 10 - 13:
 CS:  port pin 7 => DUT pin 5 
 MOSI: port pin 11 => DUT pin 4
 MISO: Port pin 12 => DUT pin 3
 SCK: port pin 13 => DUT pin 2

*/

#include <SPI.h>

// Memory register addresses:
const byte XG_OFFS_TC_H = 0x04; 
const byte XG_OFFS_TC_L = 0x05; 
const byte YG_OFFS_TC_H = 0x07; 
const byte YG_OFFS_TC_L = 0x08; 
const byte ZG_OFFS_TC_H = 0x0A; 
const byte ZG_OFFS_TC_L = 0x0B; 
const byte SELF_TEST_X_ACCEL = 0x0D; 
const byte SELF_TEST_Y_ACCEL = 0x0E; 
const byte SELF_TEST_Z_ACCEL = 0x0F; 
const byte XG_OFFS_USRH = 0x13; 
const byte XG_OFFS_USRL = 0x14; 
const byte YG_OFFS_USRH = 0x15; 
const byte YG_OFFS_USRL = 0x16; 
const byte ZG_OFFS_USRH = 0x17; 
const byte ZG_OFFS_USRL = 0x18; 
const byte SMPLRT_DIV = 0x19; 
const byte CONFIG = 0x1A; 
const byte GYRO_CONFIG = 0x1B; 
const byte ACCEL_CONFIG = 0x1C; 
const byte ACCEL_CONFIG_2 = 0x1D; 
const byte LP_MODE_CFG = 0x1E; 
const byte ACCEL_WOM_X_THR = 0x20; 
const byte ACCEL_WOM_Y_THR = 0x21; 
const byte ACCEL_WOM_Z_THR = 0x22; 
const byte FIFO_EN = 0x23; 
const byte FSYNC_INT = 0x36; 
const byte INT_PIN_CFG = 0x37; 
const byte INT_ENABLE = 0x38; 
const byte FIFO_WM_INT_STATUS = 0x39; 
const byte INT_STATUS = 0x3A; 
const byte ACCEL_XOUT_H = 0x3B; 
const byte ACCEL_XOUT_L = 0x3C; 
const byte ACCEL_YOUT_H = 0x3D; 
const byte ACCEL_YOUT_L = 0x3E; 
const byte ACCEL_ZOUT_H = 0x3F; 
const byte ACCEL_ZOUT_L = 0x40; 
const byte TEMP_OUT_H = 0x41; 
const byte TEMP_OUT_L = 0x42; 
const byte GYRO_XOUT_H = 0x43; 
const byte GYRO_XOUT_L = 0x44; 
const byte GYRO_YOUT_H = 0x45; 
const byte GYRO_YOUT_L = 0x46; 
const byte GYRO_ZOUT_H = 0x47; 
const byte GYRO_ZOUT_L = 0x48; 
const byte SELF_TEST_X_GYRO = 0x50; 
const byte SELF_TEST_Y_GYRO = 0x51; 
const byte SELF_TEST_Z_GYRO = 0x52; 
const byte FIFO_WM_TH1 = 0x60; 
const byte FIFO_WM_TH2 = 0x61; 
const byte SIGNAL_PATH_RESET = 0x68; 
const byte ACCEL_INTEL_CTRL = 0x69; 
const byte USER_CTRL = 0x6A; 
const byte PWR_MGMT_1 = 0x6B; 
const byte PWR_MGMT_2 = 0x6C; 
const byte I2C_IF = 0x70; 
const byte FIFO_COUNTH = 0x72; 
const byte FIFO_COUNTL = 0x73; 
const byte FIFO_R_W = 0x74; 
const byte WHO_AM_I = 0x75; 
const byte XA_OFFSET_H = 0x77; 
const byte XA_OFFSET_L = 0x78; 
const byte YA_OFFSET_H = 0x7A; 
const byte YA_OFFSET_L = 0x7B; 
const byte ZA_OFFSET_H = 0x7D; 
const byte ZA_OFFSET_L = 0x7E; 
const byte READ = 0b10000000; // read command
const byte WRITE = 0b01111111; // write command

// Pins used for the connection with the sensor:
const int chipSelectPin = 7;
const int fsyncPin = 6;
const int intPin = 5;

void setup() {
  Serial.begin(9600);

  // Start the SPI library:
  SPI.begin();
  SPI.setClockDivider(128);
  SPI.setDataMode(SPI_MODE3);

  // Initialize the data ready and chip select pins:
  pinMode(chipSelectPin, OUTPUT);
  pinMode(fsyncPin, OUTPUT);
  pinMode(intPin, INPUT);

  // Take the chip select low to select the device:
  digitalWrite(chipSelectPin, LOW);
  digitalWrite(fsyncPin, LOW);
}

void loop() {
  writeRegister(PWR_MGMT_1, 0b11000001); // Reset
  delay(10);
  writeRegister(PWR_MGMT_1, 0x01); // Power up
  writeRegister(PWR_MGMT_2, 0b00000111); // Turn off the gyro to save power before the temperature is read.
  //writeRegister(INT_PIN_CFG,0b01100111); // set up INT to tigger from FSYNC; 
  writeRegister(INT_PIN_CFG,0b00100111); // set up INT to tigger from FSYNC;
  writeRegister(INT_ENABLE,0b00001000); // FSYNC enable for the INT; 
  // Bit 7: INT_LEVEL = 0, INT/DRDY active high
  // Bit 6: INT_OPEN = 1, INT/DRDY open drain
  // Bit 5: LATCH_INT_EN = 1, INT/DRDY level held
  // Bit 4: INT_RD_CLEAR = 0, read INT_STATUS to clear int
  // Bit 3: FSYNC_INT_LEVEL = 0, FSYNC pin is active high
  // Bit 2: FSYNC_INT_MODE_EN = 1, FSYNC generates an interrupt

  delay(10);
  unsigned int who_am_i_t = readRegister(WHO_AM_I, 1);
  unsigned long TEMP_OUT_t = readRegister(TEMP_OUT_H, 1) * 256 + readRegister(TEMP_OUT_L, 1);
  writeRegister(PWR_MGMT_2, 0b00000000); // Turn on the gyro
  delay(100);
  signed int Accel_Xout_t = ((readRegister(ACCEL_XOUT_H, 1) << 8) | readRegister(ACCEL_XOUT_L, 1));
  signed int Accel_Yout_t = ((readRegister(ACCEL_YOUT_H, 1) << 8) | readRegister(ACCEL_YOUT_L, 1));
  signed int Accel_Zout_t = ((readRegister(ACCEL_ZOUT_H, 1) << 8) | readRegister(ACCEL_ZOUT_L, 1));
  unsigned int GYRO_XOUT_t = (readRegister(GYRO_XOUT_H, 1) << 8) | readRegister(GYRO_XOUT_L, 1);
  unsigned int GYRO_YOUT_t = (readRegister(GYRO_YOUT_H, 1) << 8) | readRegister(GYRO_YOUT_L, 1);
  unsigned int GYRO_ZOUT_t = (readRegister(GYRO_ZOUT_H, 1) << 8) | readRegister(GYRO_ZOUT_L, 1);

  // Display the data:
  Serial.println("\twho am I = " + String(who_am_i_t) + " TEMP = " + String((double)TEMP_OUT_t / 326.8 + 25.0) +
                 " A_X = " + String(Accel_Xout_t) + " A_Y = " + String(Accel_Yout_t) + " A_Z = " +
                 String(Accel_Zout_t));

//  Serial.println("\twho am I = " + String(who_am_i_t) + " TEMP = " + String((double)TEMP_OUT_t / 326.8 + 25.0) +
//                 " A_X = " + String(Accel_Xout_t) + " A_Y = " + String(Accel_Yout_t) + " A_Z = " +
//                 String(Accel_Zout_t) + " G_X = " + String(GYRO_XOUT_t) + " G_Y = " + String(GYRO_YOUT_t) +
//                 " G_Z = " + String(GYRO_ZOUT_t));


  // Wait for some time before reading again:
  //delay(1);
  //Serial.println("INT = "+String(digitalRead(intPin)));//1
  //Serial.println("INT_STATUS = "+String(readRegister(INT_STATUS, 1)));//0
  //Serial.println("INT = "+String(digitalRead(intPin)));//1
  digitalWrite(fsyncPin, HIGH);// tigger
  delay(20);
  digitalWrite(fsyncPin, LOW);
  delay(5);
  Serial.println("INT = "+String(digitalRead(intPin)));//1
  readRegister(INT_STATUS, 1);
  Serial.println("INT = "+String(digitalRead(intPin)));//1
  //Serial.println("FSYNC_INT = "+String(readRegister(FSYNC_INT, 1)));//128
  //Serial.println("FSYNC_INT = "+String(readRegister(FSYNC_INT, 1)));//0

  
}

unsigned int readRegister(byte thisRegister, int bytesToRead) {
  byte inByte = 0;           // incoming byte from the SPI
  unsigned int result = 0;   // result to return
  byte dataToSend = thisRegister | READ;
  digitalWrite(chipSelectPin, LOW);
   SPI.transfer(dataToSend);
  result = SPI.transfer(0x00);
  bytesToRead--;
  if (bytesToRead > 0) {
    result = result << 8;
    inByte = SPI.transfer(0x00);
     result = result | inByte;
    bytesToRead--;
  }
  digitalWrite(chipSelectPin, HIGH);
  return (result);
}

void writeRegister(byte thisRegister, byte thisValue) {

  // now combine the register address and the command into one byte:
  byte dataToSend = thisRegister & WRITE;

  // take the chip select low to select the device:
  digitalWrite(chipSelectPin, LOW);

  SPI.transfer(dataToSend); //Send register location
  SPI.transfer(thisValue);  //Send value to record into register

  // take the chip select high to de-select:
  digitalWrite(chipSelectPin, HIGH);
}