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

// IMPORTANT: Set pixel COUNT, PIN and TYPE
#define PIXEL_PIN D1
#define PIXEL_COUNT 24
#define PIXEL_TYPE SK6812RGBW

CapTouch Touch(D4, D5); // A2 receving pin, A4 sending pin

int touchstatus = -1; // 0 [touched side], -1[not touched]
int touchstatus2 = -1; // 0 [touched side], -1[not touched]
int TSCheck = 0; //Varible to control publish time

//int ledpin = D0;
int pixel_position = 0;
Adafruit_NeoPixel strip = Adafruit_NeoPixel(PIXEL_COUNT, PIXEL_PIN, PIXEL_TYPE);

int servoDelayI = 200; //Servo Delay time
int servoDelayO = 200;
int handServoStatic = 91; //Servo Neutral Position Reggie's 91,  Dan's 92
int handServoPin1 = A5; //servo pin
int ServoStatus = 0;
Servo handServo1; //Define Servo

int handSerUpLim = 110;  //Servo Speed, more than 93 counterclockwise, less than 93 clockwise  max:113  Min:67
int handSerDownLim = 70;
//int handcurrentspeed = 92;

void TheOtherSide (const char *event, const char *data){ //Gather the event name and the data of it
  // if this event contains this device ID
  // it was published by this device
  // otherwise it was published by the paired device

  String eventName = String( event );
  if(  eventName.indexOf( System.deviceID() ) > 0 ){
    return;
  } else {
    touchstatus2 = String(data).toInt();
  }
}


void setup(){
  Serial.begin(9600);
  Touch.setup();
  pinMode(D7, OUTPUT); //Reveal if touched
//  pinMode(ledpin, OUTPUT);
  Particle.subscribe("doit2017/H2H/Touchstatus", TheOtherSide); // Subscribe to this event and activate the function
  handServo1.attach(handServoPin1);
  handServo1.write(handServoStatic); //
  strip.begin();
  strip.show(); // Initialize all pixels to 'off'
}

void loop(){
  checkForTouch();
  if (TSCheck != touchstatus){
    String S_Touchstatus = String(touchstatus); //String touchstatus
    String deviceId = System.deviceID();
    Particle.publish("doit2017/H2H/Touchstatus/" + deviceId, S_Touchstatus);
  }
  if ((touchstatus == 0) && (touchstatus2 == -1)){
    Serial.println("");
    Serial.println("We got attacked!!!!");
    solid();
    if (ServoStatus == 0){
      HandMoveIn();
      ServoStatus = -1;
    }
  } else if ((touchstatus == 0) && (touchstatus2 == 0)){
    Serial.println("");
    Serial.println("Don't make a move");
    breathe();
  } else if ((touchstatus == -1) && (touchstatus2 == 0)){
    Serial.println("");
    Serial.println("Save our ally");
    solid();
    if (ServoStatus == 0){
      HandMoveOut();
      ServoStatus = 1;
    }
  } else {
    Serial.println("");
    Serial.println("SAFE and SOUND!!!");
    LedOff();
    if (ServoStatus == -1){
      HandMoveOut();
      ServoStatus = 0;
    } else if (ServoStatus == 1){
      HandMoveIn();
      ServoStatus = 0;
    }

    // stop light, stop move, count how long has not been touch and return back to neutral.
  }
  TSCheck = touchstatus;
  delay (500);

}

int checkForTouch(){
  Serial.println("CheckedIfTouched");
  CapTouch::Event touchEvent = Touch.getEvent();
  if (touchEvent == CapTouch::TouchEvent) {
  digitalWrite(D7, HIGH);
  touchstatus = 0;
  } else if (touchEvent == CapTouch::ReleaseEvent){ //
  digitalWrite(D7, LOW);
  touchstatus = -1;
  }
}

void solid(){

  //Turn LEDs on solide white
  uint16_t k;
     for(k=0; k< strip.numPixels(); k++){
     strip.setPixelColor(k, 255, 255, 255 );
     strip.show();
   }
delay(1000);
}

void LedOff(){
  uint16_t j;
  for(j=0; j< strip.numPixels(); j++){
    strip.setPixelColor(j, 0, 0, 0 );
    strip.show();
  }
}

int HandMoveIn(){
  handServo1.write(handSerUpLim);
  delay(servoDelayI);
  handServo1.write(handServoStatic);
}

int HandMoveOut(){
  handServo1.write(handSerDownLim);
  delay(servoDelayO);
  handServo1.write(handServoStatic);
}

float breathe( ){
//breathing LED fading in and out

  float val = (exp(sin(millis()/3700.0*M_PI)) - 0.36787944)*108.0;
  Serial.println( val );

  uint16_t i;
  uint32_t c = strip.Color(val, val, val);

  for(i=0; i< strip.numPixels(); i++) {
    strip.setPixelColor(i, c);
  }
  strip.show();

}