#include "LPD8806.h"
#include "SPI.h"

// Example to control LPD8806-based RGB LED Modules in a strip using an accelerometer
// Based on the LEDbeltKit.pde sketch provided by Adafruit
/*****************************************************************************/

#if defined(USB_SERIAL) || defined(USB_SERIAL_ADAFRUIT)
// this is for teensyduino support
int dataPin = 2;
int clockPin = 1;
#else 
// these are the pins we use for the LED belt kit using
// the Leonardo pinouts
int dataPin = 16;
int clockPin = 15;
#endif

// Set the first variable to the NUMBER of pixels. 32 = 32 pixels in a row
// The LED strips are 32 LEDs per meter but you can extend/cut the strip
LPD8806 strip = LPD8806(32, dataPin, clockPin);

const int pinAnalogXInput = 2;
const int pinAnalogYInput = 3;
const int pinAnalogZInput = 4;
int npix = strip.numPixels();

void setup() {
  // Start up the LED strip
  strip.begin();

  // Update the strip, to start they are all 'off'
  strip.show();

  Serial.begin(300);

  Serial.println(dataPin);
  Serial.println(clockPin);
  Serial.println(npix);
}

int responseDiff(int diffy, int inputVal);
int oldX=332;
int oldY=332;
int oldZ=332;
int diffX, diffY, diffZ;
int bval1, bval2, bval3;
int slowvar = 0;
int s2 = 0;
int inc = 1;
float th = 0;
float a1,a2,a3,a4,a5,a6;

int L1 = 3; // You may need to change this

int actpix = 0;

void loop() {
  
  // The basic idea here is to measure the "jerk", the derivative of acceleration.
  // The directional jerks are used to increase or decrease the bval variables,
  // which are a measure of activity in each direction.

  int intAnalogXReading = analogRead(pinAnalogXInput);
  int intAnalogYReading = analogRead(pinAnalogYInput);
  int intAnalogZReading = analogRead(pinAnalogZInput);

  diffX = abs(oldX - intAnalogXReading);
  diffY = abs(oldY- intAnalogYReading);
  diffZ = abs(oldZ - intAnalogZReading);
  
  bval1 = responseDiff(diffX, bval1);
  bval2 = responseDiff(diffY, bval2);
  bval3 = responseDiff(diffZ, bval3);
  if (bval1 > 127) bval1 = 127;
  if (bval2 > 127) bval2 = 127;
  if (bval3 > 127) bval3 = 127;
  if (bval1 < 3) bval1 = 2;
  if (bval2 < 3) bval2 = 2;
  if (bval3 < 3) bval3 = 2;
  
  //  the a1,...,a6 variables are just for some interesting colors
  uint16_t j;
  th = 6.28*float(s2)/2500.0;
  a1 = (1 + cos(th))/2.0;
  a2 = (1 - cos(th))/2.0;
  a3 = (1 + cos(.618*th))/2.0; // Using the golden ratio since its very irrational
  a4 = (1 - cos(.618*th))/2.0;
  a5 = (1 + cos(1.618*th))/2.0;
  a6 = (1 - cos(1.618*th))/2.0;
  for (j=0; j < npix; j=j+8) {
    strip.setPixelColor((j+slowvar/320) % 32, strip.Color(max(1,bval1*a1), bval2*a2, bval3*a6/2));
    strip.setPixelColor((j+slowvar/320 + 1) % 32, strip.Color(bval1*a6, max(1,bval2*a1), bval3*a2));
    strip.setPixelColor((j+slowvar/320 + 2) % 32, strip.Color(bval1*a2, bval2*a6, max(1,bval3*a1)));
    strip.setPixelColor((j+slowvar/320 + 3) % 32, strip.Color(max(1,bval2*a3), bval1*a4, bval3*a3));
    strip.setPixelColor((j+slowvar/320 + 4) % 32, strip.Color(bval2*a5, max(1,bval1*a1), bval3*a4));
    strip.setPixelColor((j+slowvar/320 + 5) % 32, strip.Color(bval3*a4/2, bval2*a6, max(1,bval1*a5)));
    strip.setPixelColor((j+slowvar/320 + 6) % 32, strip.Color(max(1,bval3*a3), bval2*a5/2, bval1*a6));
    strip.setPixelColor((j+slowvar/320 + 7) % 32, strip.Color(bval2*a2, max(1,bval3*a4), bval2*a1/2));
    strip.show();
    slowvar = slowvar + inc;
    inc = 6 + max(max(bval1, bval3),bval2)/2;
    if (slowvar > 640000) slowvar = 0;
  }
  s2 = s2 + 1;
  if (s2 > 200000) s2 = 0;
  
  oldX = intAnalogXReading;
  oldY = intAnalogYReading;
  oldZ = intAnalogZReading;

  //strip.show();
  delay(2);
}

int responseDiff(int diffy, int inputVal) {
  int outVal = inputVal;
  float inDiff = float(diffy);
  if (diffZ < L1){
    outVal = 3*outVal/4; // exponential decay if there is little movement
  }
  else{
    outVal = outVal + round(2.5 + 0.6*(inDiff - 20.0)/(1 + abs(inDiff/10.0 - 2.1)));
  }
  return outVal;
}