//Arduino servo-ohjaus, esimerkkiohjelma

#include <Servo.h>
//#include <LiquidCrystal.h>

#define MOUTHNORM 85
#define MOUTHSMILE 50
#define MOUTHSAD 110

#define EYESNORM 80
#define EYESUP 110
#define EYESDOWN 51

#define HEADNORM 55
#define HEADFRONT 80
#define HEADBACK 25

#define NECKNORM 50
#define NECKLEFT 85
#define NECKRIGHT 10

#define LEFTNORM 80
#define RIGHTNORM 80
#define LEFTUP 10
#define LEFTDOWN 140
#define RIGHTUP 140
#define RIGHTDOWN 10

#define BASE 1
#define SMILE 2
#define ANGRY 3
#define SURPRISE 4
#define SAD 5

#define LED 10



//Servon määrittely
Servo myMouth;
Servo myEyes;
Servo myNeck;
Servo myHead;
Servo myLeft;
Servo myRight;

//Analogiportin lukumuuttuja
int analogPin = 0;

//LCD-näytön merkkojonomuuttuja
//char lcd_data[16];

int state = 0, menu = 0, i, j, k;

int val = 0, mouth, eyes, head, neck, left, right;

unsigned int tila=BASE;


void setup() {
  
  pinMode(A0, INPUT);  //Käytetään analogiporttia A0 tulona
  myMouth.attach(5);  //Määritetään servo toimimaan portissa 9
  myEyes.attach(4);
  myNeck.attach(6);
  myHead.attach(9);
  myLeft.attach(7);
  myRight.attach(8);

  myLeft.write(80);
  myMouth.write(55);  
  myRight.write(80);
  myEyes.write(55);
  myNeck.write(55);
  myHead.write(55);

  pinMode(LED, OUTPUT);

  //Serial.begin(9600);
}

void Mouth(int first, int last, int speed)
{

  if (first > MOUTHSAD) first = MOUTHSAD;
  if (first < MOUTHSMILE) first = MOUTHSMILE;
  if (last > MOUTHSAD) last = MOUTHSAD;
  if (last < MOUTHSMILE) last = MOUTHSMILE;
  
  if (last >= first)
  {
    for (mouth = first; mouth < last; mouth++){
      myMouth.write(mouth);
      delay(speed);
    }
  }
  else
  {
    for (mouth = first; mouth > last; mouth--){
      myMouth.write(mouth);
      delay(speed);
    }
  }
}

void Eyes(int first, int last, int speed)
{

  if (first > EYESUP) first = EYESUP;
  if (first < EYESDOWN) first = EYESDOWN;
  if (last > EYESUP) last = EYESUP;
  if (last < EYESDOWN) last = EYESDOWN;
  
  if (last >= first)
  {
    for (eyes = first; eyes < last; eyes++){
      myEyes.write(eyes);
      delay(speed);
    }
  }
  else
  {
    for (eyes = first; eyes > last; eyes--){
      myEyes.write(eyes);
      delay(speed);
    }
  }
}

void Neck(int first, int last, int speed)
{

  if (first > NECKLEFT) first = NECKLEFT;
  if (first < NECKRIGHT) first = NECKRIGHT;
  if (last > NECKLEFT) last = NECKLEFT;
  if (last < NECKRIGHT) last = NECKRIGHT;
  
  if (last >= first)
  {
    for (neck = first; neck < last; neck++){
      myNeck.write(neck);
      delay(speed);
    }
  }
  else
  {
    for (neck = first; neck > last; neck--){
      myNeck.write(neck);
      delay(speed);
    }
  }
}

void Head(int first, int last, int speed)
{
  if (first > HEADFRONT) first = HEADFRONT;
  if (first < HEADBACK) first = HEADBACK;
  if (last > HEADFRONT) last = HEADFRONT;
  if (last < HEADBACK) last = HEADBACK;
  
  if (last >= first)
  {
    for (head = first; head < last; head++){
      myHead.write(head);
      delay(speed);
    }
  }
  else
  {
    for (head = first; head > last; head--){
      myHead.write(head);
      delay(speed);
    }
  }
}

void LeftHand(int first, int last, int speed)
{
  if (first > LEFTDOWN) first = LEFTDOWN;
  if (first < LEFTUP) first = LEFTUP;
  if (last < LEFTUP) last = LEFTUP;
  if (last > LEFTDOWN) last = LEFTDOWN;
  
  if (last >= first)
  {
    for (left = first; left < last; left++){
      myLeft.write(left);
      delay(speed);
    }
  }
  else
  {
    for (left = first; left > last; left--){
      myLeft.write(left);
      delay(speed);
    }
  }
}

void RightHand(int first, int last, int speed)
{
  if (first < RIGHTDOWN) first = RIGHTDOWN;
  if (first > RIGHTUP) first = RIGHTUP;
  if (last > RIGHTUP) last = RIGHTUP;
  if (last < RIGHTDOWN) last = RIGHTDOWN;
  
  if (last >= first)
  {
    for (right = first; right < last; right++){
      myRight.write(right);
      delay(speed);
    }
  }
  else
  {
    for (right = first; right > last; right--){
      myRight.write(right);
      delay(speed);
    }
  }
}



void loop() {


  //Serial.println(tila);
  
  switch(tila)
  {
      case BASE:  myMouth.write(MOUTHNORM);
                  myEyes.write(EYESNORM);
                  myLeft.write(LEFTDOWN);
                  myRight.write(RIGHTDOWN);
                  myNeck.write(NECKNORM);
                  myHead.write(HEADNORM);
                  for(i=0; i<10;i++)
                  {
                      digitalWrite(LED, HIGH);
                      delay(500);
                      digitalWrite(LED, LOW);
                      delay(500);
                  }
                  delay(2000);
                  tila = SMILE;
                  break;

      case SMILE: Mouth(MOUTHNORM, MOUTHSMILE, 5);
                  Head(HEADNORM, HEADBACK, 10);
                  LeftHand(LEFTNORM, LEFTUP, 5);
                  RightHand(RIGHTNORM, RIGHTUP, 5);
                  delay(2000);
                  tila = SAD;
                  break;

       case SAD:  Head(HEADBACK, HEADNORM, 10);
                  delay(2000);
                  Mouth(MOUTHSMILE, MOUTHNORM, 10);
                  Eyes(EYESNORM, EYESUP, 5);
                  Neck(NECKNORM, NECKLEFT, 20);
                  Neck(NECKLEFT, NECKRIGHT, 30);
                  Neck(NECKRIGHT, NECKLEFT, 30);
                  Neck(NECKLEFT, NECKNORM, 10);
                  delay(1000);
                  tila = ANGRY;
                  break;

      case ANGRY: LeftHand(LEFTUP, LEFTDOWN, 10);
                  RightHand(RIGHTUP, RIGHTDOWN, 10);
                  Mouth(MOUTHNORM, MOUTHSAD, 5);
                  Head(HEADNORM, HEADFRONT, 10);
                  Eyes(EYESUP, EYESDOWN, 5);
                  delay(2000);
                  tila = SURPRISE;
                  break;

      case SURPRISE:  Head(HEADFRONT, HEADBACK, 30);
                      delay(2000);
                      for (i=0;i<5;i++)
                      {
                        RightHand(RIGHTNORM, RIGHTUP, 5);
                        RightHand(RIGHTUP, RIGHTNORM, 5);
                      }
                      RightHand(RIGHTNORM, RIGHTDOWN, 5);
                      LeftHand(LEFTDOWN, LEFTDOWN, 5);
                      delay(2000);
                      Head(HEADBACK, HEADNORM, 10);
                      Mouth(MOUTHSAD, MOUTHNORM, 5);
                      tila = BASE;
                      break;
       
      
                  
  }
}