#include <math.h>
#include <array>
#include "neopixel.h"

#define PIXEL_PIN D4
#define PIXEL_COUNT 16
#define PIXEL_TYPE WS2812

//Define variables

String emotion=NULL;
int accel_x=A0;
int accel_y=A1;
int accel_z=A2;
int touchPin=A3;
int hugPin=A4;
/*String angry="";
String sad="It's not sad";
String happy="It's not happy";*/

int map_x=0;
int map_y=0;
int map_z=0;
double distance=0;

int ledPin = D0;
int ledPin2 = D1;
int ledPin3 = D2;
int eyePin=D3;
int vibratorPin=D5;

double accel=0.0;
int accel_values[5];
int accel_count=0;
int hug_value=0;
int tear=0;

bool isHappy=false;
bool isSad=false;

int val;
int ledValue = 0;
int blinkNumS = 3;

Adafruit_NeoPixel strip = Adafruit_NeoPixel(PIXEL_COUNT, PIXEL_PIN, PIXEL_TYPE);

void setup()
{
    Serial.begin(9600);

    setUpHardware();

    setupParticleCloud();

    strip.begin();
    strip.show();
}

void setUpHardware()
{
    pinMode(eyePin, OUTPUT);
    pinMode(vibratorPin, OUTPUT);

    pinMode(touchPin, INPUT);
    pinMode(hugPin, INPUT);

    pinMode(ledPin, OUTPUT);
    pinMode( ledPin2, OUTPUT );
    pinMode( ledPin3, OUTPUT );
}

void setupParticleCloud()
{
    Particle.subscribe("cmu/diot2017/angryteddy",angryfunc);
    Particle.subscribe("cmu/diot2017/happyteddy",happyfunc);
    Particle.subscribe("cmu/diot2017/sadteddy",sadfunc);

    Particle.variable("pplate",hug_value);
    Particle.variable("accel",accel);
    Particle.variable("angry",angry);
    Particle.variable("sad",sad);
    Particle.variable("tear",tear);
    Particle.variable("happy",happy);
    Particle.variable("distance",distance);
}

void loop()
{
  senseShake();
  senseHug();
  senseTear();

  if(isAngry())
    {
      Particle.publish("cmu/diot2017/angryteddy");
      delay(5000);
    }
  else if(isHappy)
    Particle.publish("cmu/diot2017/happyteddy");
  else if(isSad)
    Particle.publish("cmu/diot2017/sadteddy");
  else
    do_nothing();

  angry="Out of string";

  delay(500);
}

//SENSING FUNCTIONS
void senseShake()
{
  int x = analogRead(accel_x);
  int y = analogRead(accel_y);
  int z = analogRead(accel_z);

  map_x = map(x,0,4095,0,1000);
  map_y = map(y,0,4095,0,1000);
  map_z = map(z,0,4095,0,1000);

  distance = sqrt(map_x*map_x + map_y*map_y + map_z*map_z);

  accel_values[accel_count++]=distance;
}

void senseHug()
{
  hug_value = analogRead(hugPin);

  if(hug_value>3000)
    isHappy=true;
  else
    {
      isHappy=false;
      happy="It's not happy";
    }
}

void senseTear()
{
  tear = analogRead(touchPin);

  if(tear>3000)
  {
    sad = "It's sad";
    isSad = true;
  }
  else
  {
    isSad = false;
    sad="It's not sad";
  }
}

bool isAngry()
{
  double sum=0;
  double average=0;
  double stddev;

  if(accel_count==4)
  {
    average = averageof(accel_values);
    stddev = staddev(accel_values);
    accel=stddev;
    accel_count=0;
  }

  if(stddev>75)
    return true;
  else
    return false;
}

//ACTUATING FUNCTIONS
void sadfunc(const char* value, const char* event)
{
  for(int j = 0; j < 3; j++)
  {
    digitalWrite( ledPin2, LOW );
    digitalWrite( ledPin3, LOW );
    digitalWrite( ledPin, HIGH );
    delay(500);
    digitalWrite( ledPin, LOW );
    digitalWrite( ledPin3, LOW );
    digitalWrite( ledPin2, HIGH );
    delay(500);
    digitalWrite( ledPin, LOW );
    digitalWrite( ledPin2, LOW );
    digitalWrite( ledPin3, HIGH );
    delay(500);
  }
  digitalWrite( ledPin3, LOW );
  digitalWrite( ledPin, LOW );
  digitalWrite( ledPin2, LOW );
}

void happyfunc(const char* value, const char* event)
{
  //uint16_t i;
   /*int i;*/
   /*redo =random(255);
   greeno = random(255);
   blueo = random (255);*/
   happy = "It's happy";

   int i;
   for(i=0; i< strip.numPixels(); i++) {
     strip.setPixelColor(i, 255, 120, 0);
   }
     strip.show();
     delay (20);
      int x = 3;
      for (int ii = 1 ; ii <252 ; ii = ii + x){
        strip.setBrightness(ii);
        strip.show();
        delay(5);
       }
       x = 3;
      for (int ii = 252 ; ii > 3 ; ii = ii - x){
        strip.setBrightness(ii);
        strip.show();
        delay(3);
        }
      delay(10);
      x = 6;
     for (int ii = 1 ; ii <255 ; ii = ii + x){
        strip.setBrightness(ii);
        strip.show();
        delay(2);
        }
      x = 6;
      for (int ii = 255 ; ii > 1 ; ii = ii - x){
        strip.setBrightness(ii);
        strip.show();
        delay(3);
      }
     delay (50);
}

void angryfunc(const char* value, const char* event)
{

  //Add vibratory motion and led for eyes
  for(int i=0;i<7;i++)
  {
    angry="Inside angry loop";
    digitalWrite(vibratorPin, HIGH);
    digitalWrite(eyePin, HIGH);
    delay(500);

    digitalWrite(vibratorPin, LOW);
    digitalWrite(eyePin, LOW);
    delay(500);
  }
}

void do_nothing()
{}

//HELPER FUNCTIONS

//Calculate average of values in an integer array
double averageof(int values[])
{
  /*int sum=0;*/
  double avg=0.0;

  for(int i=0;i<sizeof(values);i++)
  {
      avg = avg + values[i]/sizeof(values);
  }

  return avg;
}

//Calculate standard deviation of values in an integer array
double staddev(int values[])
{
  double average=averageof(values);
  int avg=(int)(average);
  int summation=0;

  for(int i=0;i<sizeof(values);i++)
  {
    summation+=pow(values[i]-avg,2);
  }

  double deviation = sqrt(summation/sizeof(values));
}