#include "Servo.h"
#include "spc.h"


int led = 3;         // the PWM pin the LED is attached to

int HeadX_;
int HeadY_;
int Feet_;
int Wings_;

Servo s0, s1, s2, s3, s4;
SPC spc0, spc1, spc2, spc3, spc4, spc5;

namespace globals {
  void servoSetup(){
    s0.attach(11); s0.write(90);
    s1.attach(10); s1.write(90);
    s2.attach( 6); s2.write(90);
    s3.attach( 5); s3.write(90);
    s4.attach( 9); s4.write(90);
    s5.attach( 9); s5.write(90);
  }
}

int Beak() { // (X-Position); move head left/right axis 
  static unsigned long previousMillis = millis();
  static unsigned int servoPos;
  if (millis() - previousMillis >= 100UL) {
    previousMillis = millis();
    servoPos = spc5.getServoPos();
    if (spc5.isMoving()) {servoPos = spc5.getServoPos();} 
  }
  return servoPos;
}


int moveHeadX() { // (X-Position); move head left/right axis 
  static unsigned long previousMillis = millis();
  static unsigned int servoPos;
  if (millis() - previousMillis >= 100UL) {
    previousMillis = millis();
    servoPos = spc4.getServoPos();
    if (spc4.isMoving()) {servoPos = spc4.getServoPos();} 
  }
  return servoPos;
}

int moveHeadY() { // (X-Position); move head left/right axis 
  static unsigned long previousMillis = millis();
  static unsigned int servoPos;
  if (millis() - previousMillis >= 100UL) {
    previousMillis = millis();
    servoPos = spc5.getServoPos();
    if (spc5.isMoving()) {servoPos = spc5.getServoPos();} 
  }
  return servoPos;
}


int moveFeet() {
  static unsigned long previousMillis = millis();
  static unsigned int servoPos;
  if (millis() - previousMillis >= 100UL) {
    previousMillis = millis();
    servoPos = spc0.getServoPos();
    if (spc0.isMoving()) {servoPos = spc0.getServoPos();} 
  }
  return servoPos;
}
  
int moveWings() {
  static unsigned long previousMillis = millis();
  static unsigned int servoPos;
  if (millis() - previousMillis >= 100UL) {
    previousMillis = millis();
    servoPos = spc1.getServoPos();
    if (spc1.isMoving()) {servoPos = spc1.getServoPos();} 
  }
  return servoPos;
}


namespace Raven {
  void Eyes() {   
   static unsigned long pulseMillis = millis();
   static unsigned int lampValue;
   if (millis() - pulseMillis > random(30UL,1000UL)) {
   pulseMillis = millis();
   /*
   if (random(10) % 2 == 0) {lampValue = 255;} else {
    //lampValue = 128+127*cos(2*PI/(random(30UL,2000UL)*2)*(50-pulseMillis));
    lampValue = 128+127*cos(2*PI/(random(30UL,2000UL)*2)*millis());
   }
   */
   lampValue = 128+127*cos(2*PI/(random(30UL,2000UL)*2)*(50-pulseMillis));
   // lampValue = 128+127*cos(2*PI/(5000UL*2)*millis());
   analogWrite(led, lampValue);
   }
  };
  
  void HeadX() {
    if (spc4.isMoving()) {
      HeadX_ = moveHeadX(); 
      s4.write(HeadX_); 
    } else { 
      static unsigned long HeadXMillis = millis();
      if (millis() - HeadXMillis >= (random(3UL,20UL) * random(800UL,1500UL))) {
        HeadXMillis = millis();
        spc4.gotoServoPos(random(0, 180),random(100,200)); 
      } 
    }
  }

  void HeadY() {
    if (spc5.isMoving()) {
      HeadY_ = moveHeadY(); 
      s2.write(HeadY_); 
    } else { 
      static unsigned long HeadYMillis = millis();
      if (millis() - HeadYMillis >= (random(3UL,20UL) * random(800UL,1500UL))) {
        HeadYMillis = millis();
        spc5.gotoServoPos(random(0, 180),random(100,200)); 
      } 
    }
  }

  void Feet() {
    if (spc0.isMoving()) {
      Feet_ = moveFeet(); 
      s1.write(Feet_); 
      
      //Serial.println(Feet_);
    } else { 
      static unsigned long feetMillis = millis();
      if (millis() - feetMillis >= (random(3UL,20UL) * random(800UL,1500UL))) {
        feetMillis = millis();
        spc0.gotoServoPos(random(0, 180),random(100,200)); 
      } 
    }
  }

  void Wings() {
    if (spc1.isMoving()) {
      Wings_ = moveWings();
      s0.write(Wings_);
      s3.write(map(Wings_,180,0,0,180));
      //Serial.println(Wings_);
    } else {
      static unsigned long wingsMillis = millis();
      if (millis() - wingsMillis >= (random(3UL,20UL) * random(800UL,1500UL))) {
        wingsMillis = millis();
        spc1.gotoServoPos(random(0, 180),random(100,200)); 
      } 
    }
  }
} //raven

void setup() {
  Serial.begin(115200);
  pinMode(led, OUTPUT);
   
  randomSeed(analogRead(A0));
  delay(100);

  globals::servoSetup();

}

void loop() {
  Raven::  Eyes();
  Raven:: HeadX();
  Raven:: HeadY();
  Raven::  Feet();
  Raven:: Wings();
}