/*
Adafruit Arduino - Lesson 3. RGB LED
*/
// 
#include <Servo.h> 

Servo servo;  //  servo

int pos = 0;           // laser servo position variable
int redPin = 6;
int greenPin = 5;
int bluePin = 3;

int incomingByte;
int wait = 10; //10ms

//sets LED pins as outputs, assigns pin to servo, and opens Serial port at same baud rate as processing
void setup()
{
  pinMode(redPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  pinMode(bluePin, OUTPUT);  
  
  servo.attach(8);  // laser servo PWM control - Arduino pin 9
  
  Serial.begin(19200);
}

//reads in colour data from processing serial port and returns it as an array (colour = ['C', r_value, g_value, b_value])
int* getColour() {
  int* colour;
  int i = 0;
  
  //for some reason it only works if we put a dud value between the C and 
  // the R value
  while (i < 4)
  {
    if (Serial.available() > 0) {
        colour[i] = Serial.read();
        i++;
    }
  }
  return colour;
}

//writes pwm colours to rgb led pins
void outputColour(int red, int green, int blue)
{
  analogWrite(redPin, red);
  analogWrite(greenPin, green);
  analogWrite(bluePin, blue);  
}

//reads in colour data from processing serial port and returns it as an array (colour = ['C', r_value, g_value, b_value])
int* getServoPos() {
  int* spos;
  int i = 0;
  
  //for some reason it only works if we put a dud value between the C and 
  // the R value
  while (i < 2)
  {
    if (Serial.available() > 0) {
        spos[i] = Serial.read();
        i++;
    }
  }
  return spos;
}

//reads in byte from Serial and if it begins with C assigns the next 3 numbers into an [r,g,b] colour array
void loop()
{
  static int angle = 0;
  
  if (Serial.available() > 0) {
    // get incoming byte:
    incomingByte = Serial.read();
    
       if (incomingByte == 'C') {
          int* ledColour;
        ledColour =  getColour();

        //1 2 3 not 0 1 2 due to the dud value
        outputColour(ledColour[1],ledColour[2],ledColour[3]);
      } 
      
      if (incomingByte == 'S') {
          int* servoPos;
        servoPos =  getServoPos();
        
        angle = servoPos[1];
       // Serial.println(angle);
        //1 2 3 not 0 1 2 due to the dud value
        servo.write(angle);
        angle = 0;
      }
  } 
}