#include <SPI.h>
#include <SD.h>
#define SS_ADXL 6 //slave select for adxl372
#define SPI_SPEED 10000000 //10MHz top speed for SCLK
#define debug 0 //if serial connection for debug

#include "adxl372.h"
#define adxl_int1_pin 19
#define adxl_int2_pin 15
#define samples_after_trigger 4000
#define samples_total 6000

#define frequency_debug_pin 11
#define VBATPIN A7

File logfile; //define a log file
#define cardSelect 4 //slave select for micro sd card
#include "sd_utils.h"

#define LED_RED 13
#define LED_GRN 8

int16_t z_accel[samples_total];
int8_t dt[samples_total];
uint32_t this_time, last_time;
uint32_t pointer = 0; 
uint32_t data_start = 0;

void setup() {
  if(debug){
    Serial.begin(115200);
    while (!Serial) {} //wait for serial connection
    Serial.println("\r\nADXL372 logger");    
  }
  pinMode(SS_ADXL, OUTPUT);
  digitalWrite(SS_ADXL, HIGH); //unselect adxl, must do before talking to sd card
  pinMode(adxl_int1_pin, INPUT); //impact detection
  pinMode(adxl_int2_pin, INPUT); //data ready
  
  pinMode(LED_RED,OUTPUT); pinMode(LED_GRN,OUTPUT);
  digitalWrite(LED_RED,LOW); digitalWrite(LED_GRN,LOW);
  
  SPI.begin(); //this gives about 100kHz sampling rate on z data.
  //I couldn't get this to take my user defined clock speed for some reason...
  if (!init_sd_card()){
    while(1){
      delay(100); digitalWrite(LED_GRN,LOW);
      delay(100); digitalWrite(LED_GRN,HIGH);
    }
  }
  if (!init_adxl372()){
    while(1){
      delay(100); digitalWrite(LED_RED,LOW);
      delay(100); digitalWrite(LED_RED,HIGH);
    }    
  }
  //if debug, simply report values
  if (debug){
    while(1){
      float average = 0;
      for(int i=0; i<samples_total; ++i){
        read_z_accel();
        average += z_accel[pointer];
      }
      Serial.print("ADXL LSB: "); Serial.println( average/samples_total/16. );
      Serial.print("g value: "); Serial.println( (200./4096)*average/samples_total/16. );
      delay(10);
    }
    
  }
  //else
  //wake up in "armed" mode: writing into cyclic buffer as fast a possible.
  //when trigger, continue writing for samples_after_trigger.
  //when done, turn on green led, create file on SD card, and write out.
  //then turn off adxl and wait.
  digitalWrite(LED_RED,HIGH);
  unsigned long last_read_time = 0;
  readbyte(adxl_status2); //reset activity interrupt by reading status2
  while(1){
    read_z_accel();
    //if impact detection, break out of loop.
    if ( digitalRead( adxl_int1_pin ) ) {
      data_start = pointer; //keep track of data starting point.
      break; 
    }
  }
  for(int i=0; i<samples_after_trigger; i++){
    read_z_accel();
  }
  shutdown_adxl372();
  digitalWrite(LED_RED,LOW);
  
  if (debug) Serial.println("Triggered");
  
  int ind=0;
  logfile.print("data_start,"); logfile.println(data_start);
  logfile.print("battery_voltage,"); logfile.println( 2*(3.3/1024)*analogRead(VBATPIN) );
  for(int i=0; i<samples_total; i++){
    digitalWrite(LED_GRN,HIGH);
    //start with oldest sample
    ind = (i+data_start+samples_after_trigger+1)%samples_total;
    logfile.print(dt[ind]);
    //logfile.print(times[ind]-times[data_start]);
    logfile.print(",");
    logfile.println(z_accel[ind]);
    if (i%100==0) logfile.flush();
    digitalWrite(LED_GRN,LOW);
    delay(1);
  }
  logfile.flush();
  if (debug) Serial.println("Done Writing");
  //Serial.println( ((float)z_accel[pointer])/40 );    

}
void loop() {}

void read_z_accel() {
  while(!digitalRead(adxl_int2_pin) ){} //wait for data ready
  //digitalWrite( frequency_debug_pin, !digitalRead(frequency_debug_pin));
  pointer = (pointer + 1)%samples_total;
  digitalWrite(SS_ADXL, LOW);
  SPI.transfer( (adxl_z_data << 1) | 0b000000001 );
  z_accel[pointer] = ((SPI.transfer(0x00) << 8) | SPI.transfer(0x00));// >> 4;
  this_time = micros();
  dt[pointer] = this_time-last_time;
  last_time = this_time;
  digitalWrite(SS_ADXL, HIGH);
}