// PINKCONNECTION2 Client Program for Helen Henny/Chuck E./Guest Star Cyberamic Servo

#include <Servo.h>
#include <string.h>

Servo servoChannel[8];
byte onDegrees[8] = { 90, 0, 0, 0, 0, 0, 0, 0 }; // these are incomplete, i need to tune them when i get the servos in
byte offDegrees[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };

int byte1 = 0;
int byte2 = 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);
  pinMode(A0, OUTPUT);
  pinMode(A1, OUTPUT);
  pinMode(A2, OUTPUT);
  pinMode(A3, OUTPUT);
  pinMode(A4, OUTPUT);
  pinMode(A5, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  for (int i = 0; i < 8; i++) servoChannel[i].write(offDegrees[i]);
  Serial.begin(9600);
}

void loop() {
  if (Serial.available() > 1) {
    byte1 = Serial.read();
    byte2 = Serial.read();
    if ((byte1 & 64) && (byte2 & 64))
      if (byte1 & 1) {
        servoChannel[0].write(onDegrees[0]);
        digitalWrite(A0, 1);
      } else {
        servoChannel[0].write(offDegrees[0]);
        digitalWrite(A0, 0);
      }

      if (byte1 & 2) {
        servoChannel[1].write(onDegrees[1]);
        digitalWrite(A1, 1);
      } else {
        servoChannel[1].write(offDegrees[1]);
        digitalWrite(A1, 0);
      }

      if (byte1 & 4) {
        servoChannel[2].write(onDegrees[2]);
        digitalWrite(A2, 1);
      } else {
        servoChannel[2].write(offDegrees[2]);
        digitalWrite(A2, 0);
      }

      if (byte1 & 8) {
        servoChannel[2].write(onDegrees[3]);
        digitalWrite(A3, 1);
      } else {
        servoChannel[2].write(offDegrees[3]);
        digitalWrite(A3, 0);
      }

      if (byte2 & 1) {
        servoChannel[4].write(onDegrees[4]);
        digitalWrite(A4, 1);
      } else {
        servoChannel[4].write(offDegrees[4]);
        digitalWrite(A4, 0);
      }

      if (byte2 & 2) {
        servoChannel[4].write(onDegrees[5]);
        digitalWrite(A5, 1);
      } else {
        servoChannel[4].write(offDegrees[5]);
        digitalWrite(A5, 0);
      }

      if (byte2 & 4) {
        servoChannel[6].write(onDegrees[6]);
        digitalWrite(10, 1);
      } else {
        servoChannel[6].write(offDegrees[6]);
        digitalWrite(10, 0);
      }

      if (byte2 & 8) {
        servoChannel[7].write(onDegrees[7]);
        digitalWrite(11, 1);
      } else {
        servoChannel[7].write(offDegrees[7]);
        digitalWrite(11, 0);
      }
  
  }
}