/* Sweep
 by BARRAGAN <http://barraganstudio.com>
 This example code is in the public domain.

 modified 8 Nov 2013
 by Scott Fitzgerald
 http://www.arduino.cc/en/Tutorial/Sweep
*/
#include <Arduino.h>
#include <Servo.h>

Servo spin;
Servo shoulder;
Servo forearm;
Servo take;
int message = 0;
int buffer[4] = {};
int doWork = 0;
int pos = 0;    // variable to store the servo position
void moveForward(int i) {
    shoulder.write(i); // from 10 to 60
}

void moveUp(int i) {
    i += 100;
    forearm.write(i); // from 100 to 170
}

void catchItem(int i) {
    switch (i) {
        case 1:
            take.write(0);
            break;
        case 0:
            take.write(150);
            break;
    }
}

void moveRound(int i) {
    spin.write(i);
}

void runCommand(int intMessage) {
    Serial.println(intMessage);
    //if data is available
    //int intMessage =  message.toInt();
    if (intMessage > 400)
        moveForward(intMessage % 400);
    else if (intMessage > 300)
        moveUp(intMessage % 300);
    else if (intMessage >= 200)
        catchItem(intMessage % 200);
    else
        moveRound(intMessage);
}

int fromByteArray(int bytes[]) {
    return bytes[2] << 8 | bytes[3];
}

void setup() {
    spin.attach(9);
    shoulder.attach(12);
    forearm.attach(11);
    take.attach(10);
    Serial.begin(9600); //set baud rate
}

int i = 0;

void loop() {

    while (Serial.available()) {
        buffer[i] = Serial.read();
        i++;
    }
    if (!Serial.available() && buffer[3] != 0) {
        runCommand(fromByteArray(buffer));
        buffer[1] = 0;
        buffer[0] = 0;
        buffer[2] = 0;
        buffer[3] = 0;
        i = 0;
    }
}