#include <SPI.h>
#include <Arduino.h>

 
const int slaveSelectPina = 10;
int slaveSelectPin = 10;
const int slaveSelectPinb = 9;
int mode = -1;
int temp;
int XData, YData, ZData, Temperature;
int tstart;
int timevals[100], axa[100], aya[100], aza[100];
int axb[100], ayb[100], azb[100];
int delayval = 1;
byte accelseta = 0x05;
byte accelsetb = 0x05;

void setup(){
    Serial.begin(115200);
    begin_SPI();  // Set up SPI 
    beginMeasure();            // Switch ADXL362 to measure mode  	
  // check serial communication - acknowldegement routine
    Serial.println('a'); // sending a character to PC
    char a = 'b';
    while (a !='a')
    {
      //wait for a specific character from the pc
    a = Serial.read();
    }
}

void loop(){
    
  if (Serial.available() > 0 ) 
 {
   mode = Serial.read();

   if (mode=='P'){ // Switch accelerometers
      if (slaveSelectPin == slaveSelectPina) {
        slaveSelectPin = slaveSelectPinb;
      }  
      else {
        slaveSelectPin = slaveSelectPina;
      }
   }
   if (mode=='S'){ // Cycle the accelerometer sensitivity
       if (slaveSelectPin==slaveSelectPina){
         if (accelseta==0x05){
           accelseta = 0x45;  // +/-4g sensitivity
         }
         else if (accelseta==0x45){
           accelseta = 0x85;  // +/- 8g sensitivity
         }
         else {
           accelseta = 0x05;  // +/- 2g sensitivity
         }
        SPIwriteOneRegister(slaveSelectPin,0x2C,accelseta); // Write to FILTER_CTL_REG
       }
       else {
        if (accelsetb==0x05){
           accelsetb = 0x45;  // +/-4g sensitivity
         }
         else if (accelsetb==0x45){
           accelsetb = 0x85;  // +/- 8g sensitivity
         }
         else {
           accelsetb = 0x05;  // +/- 2g sensitivity
         }
         SPIwriteOneRegister(slaveSelectPin,0x2C,accelsetb); // Write to FILTER_CTL_REG
       }       
       // Modify FILTER_CTL_REG to toggle sensitivity 
 
       delay(10);   
   }
   //    Increase the delay value in the history loop
    if (mode =='D'){
      delayval = 2*delayval;
      if (delayval>65) {
        delayval = 64;
      }
    }
   //    Reduce the delay value in the history loop
    if (mode =='C'){
      delayval = delayval/2;
      if (delayval<1) {
        delayval = 1;
      }
    }    
   if (mode =='V')  // Read instantaneous accelerometer values off current slave select pin
   {
      digitalWrite(slaveSelectPin, LOW);
      SPI.transfer(0x0B);  // read instruction
      SPI.transfer(0x0E);  // Start at XData Reg
      XData = SPI.transfer(0x00);
      XData = XData + (SPI.transfer(0x00) << 8);
      YData = SPI.transfer(0x00);
      YData = YData + (SPI.transfer(0x00) << 8);
      ZData = SPI.transfer(0x00);
      ZData = ZData + (SPI.transfer(0x00) << 8);
 //     Temperature = SPI.transfer(0x00);
 //     Temperature = Temperature + (SPI.transfer(0x00) << 8);
      digitalWrite(slaveSelectPin, HIGH);   	 
      Serial.println(XData,DEC);
      Serial.println(YData,DEC);
      Serial.println(ZData,DEC);
      delay(5);
   }
   // Record and play back an acceleration history off both accelerometers
   if (mode =='H'){
      tstart = millis();    
      for (int i=0; i<100; i++) {
        digitalWrite(slaveSelectPina, LOW);
        SPI.transfer(0x0B);  // read instruction
        SPI.transfer(0x0E);  // Start at XData Reg
        XData = SPI.transfer(0x00);
        XData = XData + (SPI.transfer(0x00) << 8);
        YData = SPI.transfer(0x00);
        YData = YData + (SPI.transfer(0x00) << 8);
        ZData = SPI.transfer(0x00);
        ZData = ZData + (SPI.transfer(0x00) << 8);        
        digitalWrite(slaveSelectPina, HIGH);
        timevals[i] = millis()-tstart;
        axa[i] = XData;
        aya[i] = YData;
        aza[i] = ZData;
        digitalWrite(slaveSelectPinb, LOW);
        SPI.transfer(0x0B);  // read instruction
        SPI.transfer(0x0E);  // Start at XData Reg
        XData = SPI.transfer(0x00);
        XData = XData + (SPI.transfer(0x00) << 8);
        YData = SPI.transfer(0x00);
        YData = YData + (SPI.transfer(0x00) << 8);
        ZData = SPI.transfer(0x00);
        ZData = ZData + (SPI.transfer(0x00) << 8);        
        digitalWrite(slaveSelectPinb, HIGH);        
        axb[i] = XData;
        ayb[i] = YData;
        azb[i] = ZData;        
        delay(delayval);
      }
    for (int i=0; i<100; i++) {
      Serial.println(timevals[i],DEC);
      Serial.println(axa[i],DEC);
      Serial.println(aya[i],DEC);
      Serial.println(aza[i],DEC);
      Serial.println(axb[i],DEC);
      Serial.println(ayb[i],DEC);
      Serial.println(azb[i],DEC);
    }   
  }
 }
}
void begin_SPI() {
  // Setup SPI protocol, issue device soft reset
    pinMode(slaveSelectPina, OUTPUT);
    pinMode(slaveSelectPinb, OUTPUT);
    SPI.begin();
    SPI.setDataMode(SPI_MODE0);	//CPHA = CPOL = 0    MODE = 0
    delay(10); 
  // soft reset
    SPIwriteOneRegister(slaveSelectPina,0x1F, 0x52);  // Write to SOFT RESET, "R"
    delay(10);
    SPIwriteOneRegister(slaveSelectPinb,0x1F, 0x52);  // Write to SOFT RESET, "R"
    delay(10);
}
void beginMeasure() {
  // First accelerometer
  byte temp = SPIreadOneRegister(slaveSelectPina,0x2D);	// read Reg 2D before modifying for measure mode
  // turn on measurement mode
  byte tempwrite = temp | 0x02;			// turn on measurement bit in Reg 2D
  SPIwriteOneRegister(slaveSelectPina,0x2D, tempwrite); // Write to POWER_CTL_REG, Measurement Mode
  delay(10);
  tempwrite = accelseta;			// Modify ODR bits for 400Hz output data rate
  SPIwriteOneRegister(slaveSelectPina,0x2C, tempwrite); // Write to FILTER_CTL_REG
  delay(10);
  // Second accelerometer
  temp = SPIreadOneRegister(slaveSelectPinb,0x2D);	// read Reg 2D before modifying for measure mode
  // turn on measurement mode
  tempwrite = temp | 0x02;			// turn on measurement bit in Reg 2D
  SPIwriteOneRegister(slaveSelectPinb,0x2D, tempwrite); // Write to POWER_CTL_REG, Measurement Mode
  delay(10);
  tempwrite = accelsetb;			// Modify ODR bits for 400Hz output data rate
  SPIwriteOneRegister(slaveSelectPinb,0x2C, tempwrite); // Write to FILTER_CTL_REG
  delay(10);
}

byte SPIreadOneRegister(int slaveSelectPin, byte regAddress){
  byte regValue = 0;
  
  digitalWrite(slaveSelectPin, LOW);
  SPI.transfer(0x0B);  // read instruction
  SPI.transfer(regAddress);
  regValue = SPI.transfer(0x00);
  digitalWrite(slaveSelectPin, HIGH);

  return regValue;
}

void SPIwriteOneRegister(int slaveSelectPin, byte regAddress,byte regValue){
  
  digitalWrite(slaveSelectPin, LOW);
  SPI.transfer(0x0A);  // write instruction
  SPI.transfer(regAddress);
  SPI.transfer(regValue);
  digitalWrite(slaveSelectPin, HIGH);
}