// This #include statement was automatically added by the Particle IDE.
#include <neopixel.h>
#include <math.h>

#define PIXEL_COUNT 24
#define PIXEL_PIN D6
#define PIXEL_TYPE WS2812B
#define ActiveDot 8            ///actived dot number
const int dotInterv = PIXEL_COUNT/ActiveDot;

#define HEART 239,74,134
#define ANGER 229,42,9

#define USTrig D1
#define USEcho D2
#define TouchPin D4

int SqsTime = 0;
int SqeTime = 0;
float AngerRate = 1;
float HeartRate = 1;

int TosTime = 0;
int ToeTime = 0;

const String prefixEvent = "DIOT-2018-DARA-TEAMNINE";
String SqEvent = prefixEvent + "-SQUEEZE";
String ToEvent = prefixEvent + "-TOUCH";
const String UUID1 = "78a4530b-5553-42eb-b078-42b6b027bb7c";
const String UUID2 = "8ea39ad8-8112-43cf-bbae-3d1595beca9c";
const String SELFID = UUID1;
const String TARGETID = UUID2;

long lastDist = 0;

const int AngerDuration = 36000;  //12s
const int HeartDuration = 36000;


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

double Cos(String cmd) {
//    int cTime = Time.now();
  float iCos = cmd.toFloat();
  return cos(iCos)*100;
}


int TriggerSQ(String cmd) {
//    int cTime = Time.now();
    int cTime = millis();
    if (ToeTime > cTime){
            TosTime = 0;
            ToeTime = 0;
    }
    SqsTime = cTime;
    SqeTime = cTime + AngerDuration;
    AngerRate = cmd.toInt() * 0.1;
  return 1;
}

int TriggerTO(String cmd) {
//    int cTime = Time.now();
    int cTime = millis();
        if (SqeTime > cTime){
            SqsTime = 0;
            SqeTime = 0;
        }
        TosTime = cTime;
        ToeTime = cTime + HeartDuration;
        HeartRate = cmd.toInt() * 0.1;
  return 1;
}

int dolight(int R, int G, int B, int sTime, int eTime, float emotionRate, float amp, int lBr, int hBr){
    
    if (sTime == 0 || eTime == 0) {  // s or e time undefined
        return -2;
    }

    
//    int cTime = Time.now();
    int cTime = millis();
    if (eTime < cTime || sTime > cTime) {  // finished or not yet
        return -1;
    }
    
    float breathCount = ((cTime-sTime) * 0.0001 * amp);     //360s = 1 round
//    Particle.publish("IOT-PROJECT3-LOG", "BrCount: "+String(breathCount));
    float cosCount = cos(breathCount + 3.1415926) * 100.00;
    float breathBr = map(cosCount,-100,100,lBr,hBr) * emotionRate;
//    Particle.publish("IOT-PROJECT3-LOG", "Br: "+String(breathBr));
    strip.setBrightness(breathBr);
    
    for (int i=0;i<PIXEL_COUNT;i+=dotInterv){
        strip.setPixelColor(i, R,G,B);
    }
    strip.show();
    
    return breathCount;
}

void SqHandler(const char *event, const char *data)
{
//    int cTime = Time.now();
    int cTime = millis();
    String sData = String(data);
    if (cTime > (SqeTime - 2000)) {
        if (sData == TARGETID){
            if (ToeTime > cTime){
                TosTime = 0;
                ToeTime = 0;
            }
            SqsTime = cTime;
            SqeTime = cTime + AngerDuration;
            AngerRate = 1;
        }
        else {                           //if (sData == SELFID)
            if (ToeTime > cTime){
                TosTime = 0;
                ToeTime = 0;
            }
            SqsTime = cTime;
            SqeTime = cTime + AngerDuration;
            AngerRate = 0.1;
        }
    } else{
        Particle.publish("IOT-PROJECT3-LOG", "EVENT-OMITTED-SQ"+sData);
    }
    
}

void ToHandler(const char *event, const char *data)
{
//    int cTime = Time.now();
    int cTime = millis();
    String sData = String(data);
    if (cTime > (ToeTime - 2000)) {
        if (sData == TARGETID){
            if (SqeTime > cTime){
                SqsTime = 0;
                SqeTime = 0;
            }
            TosTime = cTime;
            ToeTime = cTime + HeartDuration;
            HeartRate = 1;
        }
        else {
            if (SqeTime > cTime){
                SqsTime = 0;
                SqeTime = 0;
            }
            TosTime = cTime;
            ToeTime = cTime + HeartDuration;
            HeartRate = 0.1;
        }
    } else{
        Particle.publish("IOT-PROJECT3-LOG", "EVENT-OMITTED-TO"+sData);
    }
    
}

long ReadUS() {
    
  long duration, distance;
  
  digitalWrite(USTrig, LOW);  // Added this line
  delayMicroseconds(2); // Added this line
  digitalWrite(USTrig, HIGH);
  delayMicroseconds(10); // Added this line
  digitalWrite(USTrig, LOW);
  duration = pulseIn(USEcho, HIGH);
  distance = (duration/2) / 29.1;

    return distance;    
}



bool isSqueeze(){
    long cDist = ReadUS();
    if (lastDist == 0) {
        lastDist = cDist;
        return FALSE;
    }
    
    if (abs(cDist - lastDist) > (max(cDist,lastDist)*0.2)) {  //or 2
        lastDist = cDist;
        return TRUE;
    } else {
        lastDist = cDist;
        return FALSE;
    }
    
    return FALSE;
}


bool isTouch(){
    if (digitalRead(TouchPin) == HIGH){
        delay(200);
        if (digitalRead(TouchPin) == HIGH){
            return TRUE;
        }
    }
    
    return FALSE;
}

void setup() {
    
  strip.begin();
  strip.setPixelColor(0, 0,100,255);
  strip.show();
  
  Particle.function("TriggerSQ", TriggerSQ);
  Particle.function("TriggerTO", TriggerTO);
  Particle.function("Cos", Cos);
  
  Particle.subscribe(SqEvent, SqHandler);
  Particle.subscribe(ToEvent, ToHandler);
  
  pinMode(USTrig, OUTPUT);
  pinMode(USEcho, INPUT);

//  pinMode(TouchPin, INPUT_PULLUP); 
    pinMode(TouchPin, INPUT); 
}

void loop() {
//    int cTime = Time.now();
    int cTime = millis();
    
    if (min(SqsTime,TosTime)>cTime || max(SqeTime,ToeTime)<cTime){  // finished or  not yet
        strip.setBrightness(0);
        strip.show();
    }
    
    if (SqsTime < cTime && SqeTime > cTime) {
        dolight(ANGER,SqsTime,SqeTime,AngerRate,24,5,255);
    }
    
    if (TosTime < cTime && ToeTime > cTime) {
        dolight(HEART,TosTime,ToeTime,HeartRate,6,5,255);
    }
    
    if (isSqueeze()){
        Particle.publish(SqEvent,SELFID);
    }
    
    if (isTouch()){
        Particle.publish(ToEvent,SELFID);
    }
    
    delay(100);

}