//http://projectlab.engphys.ubc.ca/coursearchive/apsc150-electronics-2013/accelerometer-adxl335-on-gy-61-breakout-board/
//http://bildr.org/2011/04/sensing-orientation-with-the-adxl335-arduino/
//A gyro is great at measuring rotation, but has no understanding of orientation.
//An accelerometer is good at determining orientation, but has no ability to keep track during rotation.
#include "math.h"
//Analog read pins
const int xPin = A0;
const int yPin = A1;
const int zPin = A2;
/////////servo stuff/////////
int monitorlight = D7;  //Light for testing
const int pwr_off = 0;  //Sets analog signal to 0
int servoPin = D3;
Servo myServo;
int servoPos = 0;
int pos1= 90;
int pos2 = 50;
int quickness = 25;
//////Shake Variables//////////
int shake_type = 1;
int shake_iterations = 5;
int count = 0;
int thres_count = 2;
int last_zaverage = 2000;
int thres_shake = 3;
bool shake = FALSE;
bool event_being_pub = FALSE;
//The minimum and maximum values that came from
//the accelerometer while standing still
//You very well may need to change these
int minVal = 0;
int maxVal = 10000;
//to hold the caculated values
double x;
double y;
double z;
//variables for low pass filter
const int numReadings = 100;
int readIndex = 0;
int xtotal = 0;
int xaverage = 0;
int xreadings[numReadings];
int ytotal = 0;
int yaverage = 0;
int yreadings[numReadings];
int ztotal = 0;
int zaverage = 0;
int zreadings[numReadings];
void setup(){
  Serial.begin(9600);
  Particle.variable("window", &numReadings, INT);
  Particle.variable("count", &count, INT);
  Particle.variable("pos1", &pos1, INT);
  Particle.variable("pos2", &pos2, INT);
  Particle.variable("quickness", &quickness, INT);
  Particle.function("window size", window_size);
  Particle.subscribe("bz/new_shake",event_driven);
  pinMode(D7, OUTPUT);
  for (int thisReading = 0; thisReading < numReadings; thisReading++)
  {
      xreadings[thisReading] = 0;
      yreadings[thisReading] = 0;
      zreadings[thisReading] = 0;
  }
  pinMode(monitorlight, OUTPUT);  //The light on the photon is set up for testing
  myServo.attach( servoPin );
  //Register our Particle to control the servo
    Particle.function("servo", servoControl);
    // Keep a cloud variable for the current position
   Particle.variable(  "servoPos" , &servoPos , INT );
   Particle.function(  "pos1", position1);
   Particle.function(  "pos2", position2);
   Particle.function(  "quickness", quick);
   /*Particle.function("shake_iterations", shake_iterate);
   Particle.function("shake_type", shaking_type);*/
  // myServo.write(pos1);
}
void loop(){
  //initialize the arrays for the rolling average that will serve as allow pass filter
  //read the analog values from the accelerometer
  int xRead = analogRead(xPin);
  int yRead = analogRead(yPin);
  int zRead = analogRead(zPin);
        // subtract the last reading:
      xtotal = xtotal - xreadings[readIndex];
      ytotal = ytotal - yreadings[readIndex];
      ztotal = ztotal - zreadings[readIndex];
      // read from the sensor:
      xreadings[readIndex] = analogRead(xPin);
      yreadings[readIndex] = analogRead(yPin);
      zreadings[readIndex] = analogRead(zPin);
      // add the reading to the total:
      xtotal = xtotal + xreadings[readIndex];
      ytotal = ytotal + yreadings[readIndex];
      ztotal = ztotal + zreadings[readIndex];
      // advance to the next position in the array:
      readIndex = readIndex + 1;
      // if we're at the end of the array...
      if (readIndex >= numReadings) {
      // ...wrap around to the beginning:
      readIndex = 0;
      }
      // calculate the average:
      xaverage = xtotal / numReadings;
      yaverage = ytotal / numReadings;
      zaverage = ztotal / numReadings;
      // send it to the computer as ASCII digit
      //output reading to serial connection
      send_info_to_serial(xRead, xaverage, yRead, yaverage, zRead, zaverage);
  delay(1);//just here to slow down the serial output - Easier to read
  //save filtered signal to check for delta
  count++;
  if (count == thres_count)
  { last_zaverage = zaverage;
    count = 0;
  }
  //check for change in accelerometer
  if ((abs(zaverage-last_zaverage) > thres_shake) && (millis() > 10000))
    shake = TRUE;
  //This triggers the hippo_shake
  if (shake == TRUE)
  {
    hippo_shake(shake_type, shake_iterations);
    /*if (event_being_pub == FALSE)
    {*/
    /*Particle.publish("bz/dad_play", "1");
    event_being_pub = TRUE;
    delay(6000);*/
    /*}*/
  }
}
////////////////////Functions/////////////////
int window_size( String speed ) //must return type int, and take an argument type string
{
  int window_convert = speed.toInt();
  int thres_shake = constrain(window_convert,0,600);
  return 1;
}
void hippo_shake(int shake_type, int shake_iterations)
  {
    for (int p=1; p< shake_iterations; p++)
    {
      myServo.write(pos1);
      delay(quickness);
      myServo.write(pos2);
      delay(quickness);
      shake = FALSE;
    }
  }
void event_driven(const char *event, const char *data)
{
  String hello = data;
  int cmd_speed = hello.toInt();
}
void send_info_to_serial(int xRead, int xaverage, int yRead, int yaverage, int zRead, int zaverage)
{
  Serial.print(" ");
  Serial.print("x: ");
  Serial.print(xRead);
  Serial.print(" ");
  Serial.print(xaverage);
  Serial.print(" | y: ");
  Serial.print(yRead);
  Serial.print(" ");
  Serial.print(yaverage);
  Serial.print(" | z: ");
  Serial.print(zRead);
  Serial.print(" ");
  Serial.println(zaverage);
}
int servoControl(String command)
{
    // Convert
   int newPos = command.toInt();
   // Make sure it is in the right range
   // And set the position
   servoPos = constrain( newPos, 0 , 180);
   // Set the servo
   myServo.write( servoPos );
   // done
   return 1;
}
/////////////////variable changes for tuning/////////////////
int position1(String command)
{    // Convert
   int newPos1 = command.toInt();
   // Make sure it is in the right range
   // And set the position
   pos1 = constrain( newPos1, 0 , 180);
   return 1;
}
int position2(String command)
{    // Convert
   int newPos2 = command.toInt();
   // Make sure it is in the right range
   // And set the position
   pos2 = constrain( newPos2, 0 , 180);
   return 1;
}
int quick(String command)
{    // Convert
   quickness = command.toInt();
   return 1;
}
int shake_iterate(String command)
{    // Convert
   shake_iterations = command.toInt();
   return 1;
}
int shaking_type(String command)
{    // Convert
   shake_type = command.toInt();
   return 1;
}