// For: https://forum.arduino.cc/t/inittimerssafe-messing-with-analogread-values/1001600/
// Added library files from: https://github.com/terryjmyers/PWM
// Changed #include <PWM.h> to #include "PWM.h"
// Changed library files to be used locally here.

#include "pca.h"
#include "PWM.h"

// Global Variables
int xPin = A2, yPin = A1, zPin = A0;
float xVal, yVal, zVal;
float xOffset, yOffset, zOffset;
int g = 9.81;
float t0 = 0, t, dt;

float input[3] = {0, 0 , 0}, output[3] = {0, 0, 0};

Eloquent::ML::Port::PCA pca;

void setup() 
{
  Serial.begin(9600);
  zeroAvg(); //finding avg to get rid of DC bias
//  Serial.print(xOffset);
//  Serial.print(" ");
//  Serial.print(yOffset);
//  Serial.print(" ");
//  Serial.println(zOffset);
  t0 = millis();

  InitTimersSafe();

//  Serial.println(Timer0_GetFrequency());
//  Serial.println(Timer1_GetFrequency());
}

void loop() 
{
  delay(20);
  xVal = ((analogRead(xPin)/156.0)*g)- xOffset; //reading accelerometer data and getting rid of bias
  yVal = ((analogRead(yPin)/174.0)*g)- yOffset;
  zVal = ((analogRead(zPin)/192.0)*g)- zOffset;

  dt = millis() - t0;
  t = (t0 + dt)/1000;
  t0 = t;
  
//  Serial.print("Acc: "); // printing accel values
//  Serial.print(xVal, 7);
//  Serial.print(",");
//  Serial.print(yVal, 7);
//  Serial.print(",");
//  Serial.print(zVal, 7);
//  Serial.print(",");
//  Serial.println(t);

  input[0] = {xVal};
  input[1] = {yVal};
  input[2] = {zVal};

//  Serial.print("PCA Inputs: "); // printing accel values
//  Serial.print(input[0], 7);
//  Serial.print(",");
//  Serial.print(input[1], 7);
//  Serial.print(",");
//  Serial.print(input[2], 7);
//  Serial.print(",");
//  Serial.println(t);

  pca.transform(input, output);

//  Serial.print("PCA: "); // printing PCA values
  Serial.print(output[0], 7);
//  Serial.print(",");
//  Serial.print(output[1], 7);
//  Serial.print(",");
//  Serial.print(output[5], 7); //idk why but this one seems to give reasonable readings
  Serial.print(",");
  Serial.println(t); 

  delay(200); // so we can actually read the data
}

// function for finding zero mean
void zeroAvg()
{
  int k = 0;
  float xSum = 0, ySum = 0, zSum = 0;
  
  while (k < 1000) //average over 1000 iterations
  {

   xVal = (analogRead(xPin)/156.0)*g;
   yVal = (analogRead(yPin)/174.0)*g;
   zVal = (analogRead(zPin)/192.0)*g;
   
   xSum = xSum + xVal;
   ySum = ySum + yVal;
   zSum = zSum + zVal;
   k++;
  }

  xOffset = xSum/1000; // calculating offset values
  yOffset = ySum/1000;
  zOffset = zSum/1000;
}