// This #include statement was automatically added by the Spark IDE.
#include "elapsedMillis1/elapsedMillis1.h"
#include "SparkButton/SparkButton.h"
#include "math.h"

SparkButton b = SparkButton();

elapsedMillis tElapsed1;
elapsedMillis tElapsed2;
elapsedMillis tElapsed4;

int isRaised = 0;
int isShaked = 0;
int count = 0;
int countV = 0;
bool success = false;
bool isFirst = true;
bool isFirstPlay = true;
bool drinking = false;

bool isDone = false;
int startAction = 0;

double yValue = 0;
int interval = 10000;
int motor_interval = 10000;


double lastX = 0;
double lastY = 0;
double lastZ = 0;

int LEDBack = D7;
int LEDFront = D0;
int LEDUp = D6;
int LEDDown = D5;
Servo myservo;
int pos = 0;

void setup() {
    b.begin();
    myservo.attach(A0);
    
    pinMode(LEDBack, OUTPUT);
    pinMode(LEDFront, OUTPUT);
    pinMode(LEDUp, OUTPUT);
    pinMode(LEDDown, OUTPUT);
    
    digitalWrite(LEDFront, HIGH); // red chest
    digitalWrite(LEDBack, HIGH);   // blue wings
    
    delay(1000);
    standardBird();
}

void checkIsRaised(){ // all buttons are pressed if mug on table
    if(b.allButtonsOff()) {
        isRaised = 1;
    } else {
        isRaised = 0;
    }
}

void standardBird() {
    digitalWrite(LEDUp, LOW);
    digitalWrite(LEDDown, HIGH);    // ^^ happy
    myservo.write(11);              // bird upright          
    delay(10); 

}
void mourningBird() {
    digitalWrite(LEDUp, HIGH);
    digitalWrite(LEDDown, HIGH);    // xx dead
    myservo.write(60);              // bird hanging         
    delay(10); 
}

void waitingBird() {
    digitalWrite(LEDUp, HIGH);
    digitalWrite(LEDDown, LOW);     // `´ `´ waiting
    myservo.write(11);              // bird upright
    delay(10);
}

void happyBird() {
    digitalWrite(LEDUp, LOW);
    digitalWrite(LEDDown, HIGH);
  for(pos = 11; pos < 55; pos += 1)  {  // slow backwards
    myservo.write(pos);
    delay(20);                  
  } 
  myservo.write(11);                    // fast forward
  delay(10);
}

void startCounterBlinker(){             // blinking
    if(abs(tElapsed2-(2000*countV)) == 0) {
        countV++;
    }
    if(abs(tElapsed2-(909*count)) == 0) {
        b.ledOn(count,255,255,0);
        count++;
    }
}

void setVarsBeforePlay(){
    if(isFirstPlay){
        waitingBird();
        tElapsed2 = 0;
        b.allLedsOff();
        count = 1;
        countV = 1;
        isFirstPlay = false;
        success = false;
    } 
}

void NotRaisedState(){
    if(tElapsed1 < interval){
        b.rainbow(5);
    } else {
        tElapsed1 = 0; // loop forever
    }
}

void celebrateWithLEDs(){
    if(abs(tElapsed4-(909*count)) == 0) {
        if(count%2 == 0) {
        b.allLedsOff();
        } else {
            switch(count){
            case 1:
                b.allLedsOn(100,100,0);
                break;
            case 3:
                b.allLedsOn(0,100,100);
                break;
            case 5:
                b.allLedsOn(100,0,100);
                break;
            case 7:
                b.allLedsOn(255,255,0);
                break;
            case 9:
                b.allLedsOn(255,255,255);
                break;
            }
        }
        count++;
    } 
}

void mournWithLEDs() { 
    if(abs(tElapsed4-(909*count)) == 0) {
        if(count%2 == 0) {
            b.allLedsOff();
        } else {
            b.allLedsOn(255,0,0);
        }
        count++;
    } 
}

bool checkIsShaked(){ 
    if(abs(lastY - b.readY()) > 20){
        return true; // move backwards for prost
    } /*else if (abs(lastY - b.readY()) > 100){
        drinking = true;// move forward for drinking
    }   */            
    return false;
}

void loop(){
    if(isFirst) {
        lastX = b.readX();
        lastY = b.readY();
        lastZ = b.readZ();
        isFirst = false;
    }
   
    checkIsRaised();

    if(isRaised == 0) {
        NotRaisedState();
        isFirstPlay= true;
    } else {
        setVarsBeforePlay();
        while(tElapsed2 < interval) {
            startCounterBlinker();
            if(checkIsShaked() && !success) {
                tElapsed4 = 0;
                count = 1;
                happyBird();
                while(tElapsed4 < motor_interval) {
                    celebrateWithLEDs();
                }
                success = true; 
                startAction = 0;
            }
            lastY = b.readY();
            yValue = lastY;
        }
        if(!success){
            tElapsed4 = 0;
            count = 1;
            mourningBird();
            while(tElapsed4 < interval) {
                mournWithLEDs();
                startAction = 0;
            }
        }
    }
 }