// PINKCONNECTION2 Client Program for Helen Henny/Mitzi Mozzarella Servo

#include <Servo.h>

Servo servoChannel[11]; // mouth earL earR eyelidL eyelidR eyeL eyeR headL headR headUp armUpL armTwistL elbowL armUpR armTwistR elbowR bodyTwistR bodyTwistL bodyLean
byte onDegrees[19] =     { 45,   0,   0,   0,      0,      0,   0,   135,  45,   135,   90,    90,       90,    90,    90,       90,    0,         0,         0        };
byte offDegrees[19] =    { 0,    0,   0,   0,      0,      0,   0,   90,   90,   90,    180,   180,      180,   0,     0,        0 ,    0,         0,         0        };
byte mapping[19] =       { 0,    0,   0,   0,      0,      0,   0,   2,    2,    1,     3,     4,        5,     6,     7,        8,     9,         9,         10       };

int byte1 = 0;
int byte2 = 0;
int byte3 = 0;
int byte4 = 0;
int byte5 = 0;
int byte6 = 0;
int byte7 = 0;
int byte8 = 0;

void setup()
{
  servoChannel[0].attach(2);
  servoChannel[1].attach(3);
  servoChannel[2].attach(4);
  servoChannel[3].attach(5);
  servoChannel[4].attach(6);
  servoChannel[5].attach(7);
  servoChannel[6].attach(8);
  servoChannel[7].attach(9);
  servoChannel[8].attach(10);
  for (int i = 0; i < 19; i++) servoChannel[mapping[i]].write(offDegrees[i]);
  Serial.begin(9600);
  Serial.write("PC2,32,Mitzi/Helen,END");
}

void loop() {
  if (Serial.available() > 7)
  {
    byte1 = Serial.read();
    byte2 = Serial.read();
    byte3 = Serial.read();
    byte4 = Serial.read();
    byte5 = Serial.read();
    byte6 = Serial.read();
    byte7 = Serial.read();
    byte8 = Serial.read();

    if ((byte1 & 64) && (byte2 & 64) && (byte3 & 64) && (byte4 & 64) && (byte5 & 64))
    {
      if (byte1 & 1) servoChannel[mapping[0]].write(onDegrees[0]);
      else servoChannel[mapping[0]].write(offDegrees[0]);

      //if (byte1 & 2) servoChannel[mapping[1]].write(onDegrees[1]);
      //else servoChannel[mapping[1]].write(offDegrees[1]);

      //if (byte1 & 4) servoChannel[mapping[2]].write(onDegrees[2]);
      //else servoChannel[mapping[2]].write(offDegrees[2]);

      //if (byte1 & 8) servoChannel[mapping[3]].write(onDegrees[3]);
      //else servoChannel[mapping[3]].write(offDegrees[3]);


      //if (byte2 & 1) servoChannel[mapping[4]].write(onDegrees[4]);
      //else servoChannel[mapping[4]].write(offDegrees[4]);

      //if (byte2 & 2) servoChannel[mapping[5]].write(onDegrees[5]);
      //else servoChannel[mapping[5]].write(offDegrees[5]);

      //if (byte2 & 4) servoChannel[mapping[6]].write(onDegrees[6]);
      //else servoChannel[mapping[6]].write(offDegrees[6]);

      if (byte2 & 8) servoChannel[mapping[7]].write(onDegrees[7]);
      else servoChannel[mapping[7]].write(offDegrees[7]);


      if (byte3 & 1) servoChannel[mapping[8]].write(onDegrees[8]);
      //else servoChannel[mapping[8]].write(offDegrees[8]);
      
      if (byte3 & 2) servoChannel[mapping[9]].write(onDegrees[9]);
      else servoChannel[mapping[9]].write(offDegrees[9]);

      if (byte3 & 4) servoChannel[mapping[10]].write(onDegrees[10]);
      else servoChannel[mapping[10]].write(offDegrees[10]);

      if (byte3 & 8) servoChannel[mapping[11]].write(onDegrees[11]);
      else servoChannel[mapping[11]].write(offDegrees[11]);


      if (byte4 & 1) servoChannel[mapping[12]].write(onDegrees[12]);
      else servoChannel[mapping[12]].write(offDegrees[12]);

      if (byte4 & 2) servoChannel[mapping[13]].write(onDegrees[13]);
      else servoChannel[mapping[13]].write(offDegrees[13]);

      if (byte4 & 4) servoChannel[mapping[14]].write(onDegrees[14]);
      else servoChannel[mapping[14]].write(offDegrees[14]);

      if (byte4 & 8) servoChannel[mapping[15]].write(onDegrees[15]);
      else servoChannel[mapping[15]].write(offDegrees[15]);


      //if (byte5 & 1) servoChannel[mapping[16]].write(onDegrees[16]);
      //else servoChannel[mapping[16]].write(offDegrees[16]);

      //if (byte5 & 2) servoChannel[mapping[17]].write(onDegrees[17]);
      //else servoChannel[mapping[17]].write(offDegrees[17]);

      //if (byte5 & 4) servoChannel[mapping[18]].write(onDegrees[18]);
      //else servoChannel[mapping[18]].write(offDegrees[18]);

      //if (byte5 & 8) servoChannel[mapping[0]].write(onDegrees[0]);
      //else servoChannel[mapping[0]].write(offDegrees[0]);
    }
  }
}