//my buddy
//my tummy
int servoPin1 = D2;
Servo servo1;
int servoPos1 = 0;
//my heart
int heartPin1=A3;
int heartLight1 = 0;
//my control of his doll
int fsrPin1 = A0;
int fsrRead1 = 0;

String command1;
String command2;

//friend's buddy
//his tummy
int servoPin2 = D3;
Servo servo2;
int servoPos2 = 0;
//his heart
int heartPin2=A4;
int heartLight2 = 0;
//friend's control is not shown here yet. 
int fsrPin2 = A1; //use to demo his control of my heart
int fsrRead2 = 0;

int flag=0;
int starttime=0;
int duration=0;

int currentPos1=0;
int currentPos2=0;

void setup() {
  // attach servo pins
  servo1.attach( D2 );
  servo2.attach(D3);
  //hearts and fsr
  pinMode (heartPin1, OUTPUT);
  pinMode (heartPin2, OUTPUT);
  pinMode (fsrPin1, INPUT);
  pinMode (fsrPin2, INPUT);

  //Register our Particle to control the servo
  Particle.function("servo1", servoControl1); //my tummy
  Particle.function("servo2", servoControl2); //his tummy
  
  // Cloud variable for the current servo position
  Particle.variable(  "servoPos1" , &servoPos1 , INT );
  Particle.variable(  "servoPos2" , &servoPos2 , INT );
  
  // Cloud variable for the current heart reading
  Particle.variable(  "force", &fsrRead1, INT);//Output the fsr reading variable in console
  analogWrite(heartPin2, 255);
}


void loop() {
    //loop for fsr Readings
    fsrRead1=analogRead(fsrPin1);
    fsrRead2=analogRead(fsrPin2);
    
    //I press
    if (fsrRead1>1000) 
    {
        //his doll on my side blinks: message sent
        analogWrite(heartPin2, 0);
        delay(200);
        analogWrite(heartPin2, 255);
        delay(200);
        analogWrite(heartPin2, 0);
        delay(200);
        analogWrite(heartPin2, 255);
        delay(200);
    }
    
    //Friend press
    if (fsrRead2>1000) 
    {
        flag=1; //start to glow
        starttime=millis();//record start time of glow
    }
    
    if (flag==1) // in glowing process
    {
        duration=millis()-starttime;
        if (duration>300000) //light should fade out now
        {
            analogWrite(heartPin1, 0);
            flag=0;
        }
        else 
        {
            heartLight1=map(duration, 0, 300000, 255, 0); //light faints out
            analogWrite(heartPin1, heartLight1);
        }
    }
}

//control my tummy
int servoControl1(String command1)
{
    // Convert
   int exercise = command1.toInt();
   int newexercise = constrain( exercise, 0 , 10000); // threshold = 10000
   servoPos1 = map(exercise,0,10000,108,9); // CHANGE 0 AND 180 TO APPROPRIATE ANGLES AFTER TESTING
    
    if (servoPos1>currentPos1)
    {
        for (int i=currentPos1; i<servoPos1; i=i+5)
        {
            servo1.write( i );
            delay(50);
        }
    }
    else
    {
        for (int i=currentPos1; i>servoPos1; i=i-5)
        {
            servo1.write( i );
            delay(50);
        }
    }
    
    currentPos1=servoPos1;
   // Set the servo
   //servo1.write( servoPos1 );
   // done
   
   return 1;
}

//control his tummy
int servoControl2(String command2)
{
    // Convert
   int exercise = command2.toInt();
   int newexercise = constrain( exercise, 0 , 10000); // threshold = 10000
   servoPos2 = map(exercise,0,10000,95,5); // CHANGE 0 AND 180 TO APPROPRIATE ANGLES AFTER TESTING
    
    if (servoPos2>currentPos2)
    {
        for (int i=currentPos2; i<servoPos2; i=i+5)
        {
            servo2.write( i );
            delay(50);
        }
    }
    else
    {
        for (int i=currentPos2; i>servoPos2; i=i-5)
        {
            servo2.write( i );
            delay(50);
        }
    }
    
    currentPos2=servoPos2;
   // Set the servo
   //servo2.write( servoPos2 );
   // done
   return 1;
}