int i = 0;

int xPin = A1;
int yPin = A2;
int zPin = A3;
int sPin = D0;
int ledPin = D1;
int wentUp = 0;

//Led pulsing variables
int brightness = 0;
int fade = 5;

int xReading, yReading, zReading;
int x, y, z; // Readings

int xBase, yBase, zBase;
int clinkThreshold = 1; // 30% of the range of values is half the threshold
int tapThreshold = 5;  // 50% of the range of values is half the threshold
int xLowThC, yLowThC, zLowThC;
int xHighThC, yHighThC, zHighThC;

int zLowThT;
int zHighThT;

int timerTapCount, timerStartTap, timerStartClink;
int timerLimitTapCount = 5;
int timerLimitTap = 20;
int timerLimitClink = 10;

// OUR DEVICE'S STATE
bool tapped, clinked;
int numTaps;

// OTHER PERSON'S STATE
bool otherTapped, otherClinked;

// LIGHTS
bool clinkingLights, tappingLights;

int maxX = 0;
int minX = 5000;
int maxY = 0;
int minY = 5000;
int maxZ = 0;
int minZ = 5000;

void tapClinkHandler(const char *event, const char *data)
{
  i++;
  Serial.print(i);
  Serial.print(" ");
  Serial.print(event);
  Serial.print(", data: ");
  // So data could be either NULL, tap, or clink
  if(data) {
    Serial.println(data);
    if(strcmp(data, "tap")==0)
    {
      // SOLENOID TAPPING CODE
      otherTapped = 1;

        Particle.publish("In SOleniod");
      // Have we also tapped?
      if(tapped == 1)
      {
        // Clink state activated: CLINKING LEDs

        // Clink timer starts
        Serial.println("WE ARE IN CLINK MODE");
        clinkingLights = 1;
        tappingLights = 0;
        timerStartClink = Time.now();
        fade = 0;
        brightness = 0;
      } else {
        Serial.println("THE OTHER PERSON HAS CLINKED IT'S TIME FOR ME TO CLINK");
        tappingLights = 1;
        timerStartTap = Time.now();
        for(int i = 0; i < 5; i++){
            analogWrite(sPin, 220);
            delay(100);
            analogWrite(sPin, 0);
            delay(100);
        }
       
        // Tap state activated: TAPPING LEDs
      }
    } else if (strcmp(data, "clink") == 0)
    {
        // This should only be sent when both are in tapping State
        if(tapped == 0)
        {
          // This is invalid, this state shouldn't be entered
          Serial.println("The other person has clinked but we haven't tapped.");
          return;
        }

        if(clinked == 1)
        {
          Serial.println("WE BOTH CLINKED SUCCESS");
          // SOLENOID CLINKS CODE
          // reset all states
          tapped = 0;
          clinked = 0;
          otherTapped = 0;
          otherClinked = 0;
          clinkingLights = 0;
          tappingLights = 0;
          analogWrite(sPin,255);
          delay(500);
          analogWrite(sPin, 0);
        } else {
          Serial.println("The other person has clinked but I haven't");
          // Set a variable for the other person being in a clink state
          otherClinked = 1;
          timerStartClink = Time.now();
          // So that when we clink finally, the solenoid clinks
        }
    } else {
      Serial.println("What is this state?");
    }
  } else {
    Serial.println("Null");
  }
}


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

  xReading = analogRead(xPin);
  xBase = map(xReading, 1954, 2184, 0, 10);

  yReading = analogRead(yPin);
  yBase = map(yReading, 1557, 2214, 0, 10);

  zReading = analogRead(zPin);
  zBase = map(zReading, 2329, 2486, 0, 10);

  xLowThC = xBase - clinkThreshold;
  xHighThC = xBase + clinkThreshold;
  yLowThC = yBase - clinkThreshold;
  yHighThC = yBase + clinkThreshold;
  zLowThC = zBase - clinkThreshold;
  zHighThC = zBase + clinkThreshold;

  zLowThT = zBase - tapThreshold;
  zHighThT = zBase + tapThreshold;

  Particle.subscribe("TAP_CLINK_CMU_IOT1", tapClinkHandler);
  
  pinMode(ledPin, OUTPUT);
  pinMode(sPin, OUTPUT);
    Serial.print("ZHigh: ");
    Serial.println(zHighThT);
    Serial.print("ZLow: ");
    Serial.println(zLowThT);
}

// Idea: get maximum and minimum range of motion and
// then map the values into that range from 0 to 100
void loop()
{
  xReading = analogRead(xPin);
  x = map(xReading, 1954, 2184, 0, 10);
  if(xReading > maxX) maxX = xReading;
  if(xReading < minX) minX = xReading;

  yReading = analogRead(yPin);
  y = map(yReading, 1557, 2214, 0, 10);
  if(yReading > maxY) maxY = yReading;
  if(yReading < minY) minY = yReading;

  zReading = analogRead(zPin);
  z = map(zReading, 2329, 2486, 0, 10);
  if(zReading > maxZ) maxZ = zReading;
  if(zReading < minZ) minZ = zReading;

  
  // For taps, we need to have a time limit for when each successive tap can
  // be counted, if it exceed like 5 seconds? then it resets the number of taps
  if(numTaps > 0 and numTaps < 3)
  {
    if((Time.now() - timerTapCount) >= timerLimitTapCount)
    {
      // If taps aren't done successively, then reset the number of taps
      Serial.println("Your taps have been reset");
      numTaps = 0;
    }
  }


  if(tappingLights == 1)
  {
    if((Time.now() - timerStartTap) <= timerLimitTap)
    {
      // LED LIGHT CODE maybe fade up one second
      // And fade down one second

    } else {
      // Reset all states
      Serial.println(timerStartTap - Time.now());
      Serial.println(timerLimitTap);
      tapped = 0;
      clinked = 0;
      otherTapped = 0;
      otherClinked = 0;
      tappingLights = 0;
      clinkingLights = 0;
      numTaps = 0;
    }
  } else if (clinkingLights == 1)
  {
    if((Time.now() - timerStartClink) <= timerLimitClink)
    {
      // LED LIGHT CODE FOR CLINK
    } else {
      // Time's up - reset all states
      tapped = 0;
      clinked = 0;
      otherTapped = 0;
      otherClinked = 0;
      tappingLights = 0;
      clinkingLights = 0;
      numTaps = 0;
    }
  }


  if(clinked == 0 and tapped == 1 and otherTapped == 1)
  {
    if(otherClinked == 1)
    {
      if(z <= zLowThC and (y <= yLowThC or x <= xLowThC))
      {
        //Time.now() - timerStart) >= timerLimit
        Serial.println("WE BOTH CLINKED SUCCESS");
        Particle.publish("TAP_CLINK_CMU_IOT2", "clink");
        // SOLENOID CLINK CODE
        // Reset all states
        analogWrite(sPin,255);
        delay(500);
        analogWrite(sPin, 0);
        tapped = 0;
        clinked = 0;
        otherTapped = 0;
        otherClinked = 0;
        clinkingLights = 0;
        tappingLights = 0;
        numTaps = 0;
      }
    } else {
      if(z <= zLowThC and (y <= yLowThC or x <= xLowThC))
      {
        clinked = 1;
        Serial.println("You clinked!");
        Particle.publish("TAP_CLINK_CMU_IOT2", "clink");
      }
    }
  } else if(tapped == 0)
  {
    // We only want to publish to particle once
    if(otherTapped == 1)
    {
      if (z >= zHighThT or z <= zLowThT)
      {
        numTaps++;
        timerTapCount = Time.now();
        Serial.println("Tap1");
        Serial.println(numTaps);
        if(numTaps == 3)
        {
          tapped = 1;
          Serial.println("You tapped and the other person tapped!");
          Particle.publish("TAP_CLINK_CMU_IOT2", "tap");
          // Now we need to start the clinking lights
          clinkingLights = 1;
          tappingLights = 0;
          // Reset numTaps
          numTaps = 0;
          timerStartTap = Time.now();
          timerStartClink = Time.now();
        }
      }
    } else {
        //Serial.println("13");
      if (z >= zHighThT or z <= zLowThT)
      {
        numTaps++;
        Serial.println("Tap2");
        Serial.println(numTaps);
        timerTapCount = Time.now();
        if(numTaps == 3)
        {
          tapped = 1;
          Serial.println("You tapped and the other person did not tap!");
          Particle.publish("TAP_CLINK_CMU_IOT2", "tap");
          tappingLights = 1;
          // Reset numTaps
          numTaps = 0;
          timerStartTap = Time.now();
        }
      }
    }
  }
}