#include "Servo.h"
#include <FastLED.h>

#define LED_COUNT 32
#define LED_PIN 5

CRGB leds[LED_COUNT];

int max_bright = 192;
unsigned long nextFrame = 0;
int Frame = 0;
int maxFrame;
int eyeHue = 100;
int initStripFunction = true;
Servo servo1;
Servo servo2;

void clear_strip() {
   fill_solid(&(leds[0]), LED_COUNT, CRGB(0,0,0));
}

void setup() {
servo1.attach(9);
servo2.attach(10);
servo1.write(45);

servo2.write(45);


LEDS.setBrightness(max_bright);
LEDS.addLeds<WS2811, LED_PIN, GRB>(leds, LED_COUNT);
clear_strip();

LEDS.show();

Serial.begin(9600);
Serial.println("Ready!");


}



void loop() {
char ch = Serial.read();

switch(ch) {
   case 'u':

   servo1.write(20);
   servo2.write(70);

   eyeHue = 160;
   break;
   case 'd':

   servo1.write(70);
   servo2.write(20);

   eyeHue=0;
   break;
   case 'm':

   servo1.write(45);
   servo2.write(45);

   eyeHue=100;
   break;
   case 'l':

   servo1.write(45);
   servo2.write(70);

   eyeHue= 200;
   break;
}

if(millis() >= nextFrame) {
   //Frame++;
   strip_simpleWave(0.3,15,40);
}

}

void strip_simpleWave(float rate,int cycles, int wait) {
servo1.detach();
servo2.detach();

 // stripMaxFrame = 200; // how many frames to draw
  static float pos;
  if(initStripFunction) {
    // set vars
    pos = 0.0;
    initStripFunction = false;
    nextFrame = 0;
  }
  if(millis() >= nextFrame) {
    pos = pos+rate;
    for(int i=0; i<LED_COUNT; i++) {
      // sine wave, 3 offset waves make a rainbow!
      float level = sin(i+pos) * 127 + 128;
      // set color level // this does red
      leds[i] = CHSV(eyeHue, 255, level);
    }
    LEDS.show();
    nextFrame = millis() + wait; // how long to wait for the next frame

  }
  servo1.attach(9);
  servo2.attach(10);
}