#include <Servo.h>
#define OCTAVE_OFFSET 0

//MUSICA COFFIN DANCE
#define NOTE_B0  31
#define NOTE_C1  33
#define NOTE_CS1 35
#define NOTE_D1  37
#define NOTE_DS1 39
#define NOTE_E1  41
#define NOTE_F1  44
#define NOTE_FS1 46
#define NOTE_G1  49
#define NOTE_GS1 52
#define NOTE_A1  55
#define NOTE_AS1 58
#define NOTE_B1  62
#define NOTE_C2  65
#define NOTE_CS2 69
#define NOTE_D2  73
#define NOTE_DS2 78
#define NOTE_E2  82
#define NOTE_F2  87
#define NOTE_FS2 93
#define NOTE_G2  98
#define NOTE_GS2 104
#define NOTE_A2  110
#define NOTE_AS2 117
#define NOTE_B2  123
#define NOTE_C3  131
#define NOTE_CS3 139
#define NOTE_D3  147
#define NOTE_DS3 156
#define NOTE_E3  165
#define NOTE_F3  175
#define NOTE_FS3 185
#define NOTE_G3  196
#define NOTE_GS3 208
#define NOTE_A3  220
#define NOTE_AS3 233
#define NOTE_B3  247
#define NOTE_C4  262
#define NOTE_CS4 277
#define NOTE_D4  294
#define NOTE_DS4 311
#define NOTE_E4  330
#define NOTE_F4  349
#define NOTE_FS4 370
#define NOTE_G4  392
#define NOTE_GS4 415
#define NOTE_A4  440
#define NOTE_AS4 466
#define NOTE_B4  494
#define NOTE_C5  523
#define NOTE_CS5 554
#define NOTE_D5  587
#define NOTE_DS5 622
#define NOTE_E5  659
#define NOTE_F5  698
#define NOTE_FS5 740
#define NOTE_G5  784
#define NOTE_GS5 831
#define NOTE_A5  880
#define NOTE_AS5 932
#define NOTE_B5  988
#define NOTE_C6  1047
#define NOTE_CS6 1109
#define NOTE_D6  1175
#define NOTE_DS6 1245
#define NOTE_E6  1319
#define NOTE_F6  1397
#define NOTE_FS6 1480
#define NOTE_G6  1568
#define NOTE_GS6 1661
#define NOTE_A6  1760
#define NOTE_AS6 1865
#define NOTE_B6  1976
#define NOTE_C7  2093
#define NOTE_CS7 2217
#define NOTE_D7  2349
#define NOTE_DS7 2489
#define NOTE_E7  2637
#define NOTE_F7  2794
#define NOTE_FS7 2960
#define NOTE_G7  3136
#define NOTE_GS7 3322
#define NOTE_A7  3520
#define NOTE_AS7 3729
#define NOTE_B7  3951
#define NOTE_C8  4186
#define NOTE_CS8 4435
#define NOTE_D8  4699
#define NOTE_DS8 4978"

int melody[] = {
                 NOTE_A4, 0, NOTE_A4, NOTE_A4,
                 NOTE_C5, 0, NOTE_AS4, NOTE_A4,
                 NOTE_G4,0, NOTE_G4, NOTE_AS5,
                 NOTE_A5, NOTE_AS5, NOTE_A5, NOTE_AS5,
                 NOTE_G4,0, NOTE_G4, NOTE_AS5,
                 NOTE_A5, NOTE_AS5, NOTE_A5, NOTE_AS5,
                 NOTE_AS4, NOTE_AS4, NOTE_AS4, NOTE_AS4,
                 NOTE_AS4, NOTE_AS4, NOTE_AS4, NOTE_AS4,
                 NOTE_D5, NOTE_D5, NOTE_D5, NOTE_D5,
                 NOTE_C5, NOTE_C5, NOTE_C5, NOTE_C5,
                 NOTE_F5, NOTE_F5, NOTE_F5, NOTE_F5,
                 NOTE_G5, NOTE_G5, NOTE_G5, NOTE_G5,
                 NOTE_G5, NOTE_G5, NOTE_G5, NOTE_G5,
                 NOTE_G5, NOTE_G5, NOTE_G5, NOTE_G5,
                 NOTE_C5, NOTE_AS4, NOTE_A4, NOTE_F4,
                 NOTE_G4, 0, NOTE_G4, NOTE_D5,
                 NOTE_C5, 0, NOTE_AS4, 0,
                 NOTE_A4, 0, NOTE_A4, NOTE_A4,
                 NOTE_C5, 0, NOTE_AS4, NOTE_A4,
                 NOTE_G4,0, NOTE_G4, NOTE_AS5,
                 NOTE_A5, NOTE_AS5, NOTE_A5, NOTE_AS5,
                 NOTE_G4,0, NOTE_G4, NOTE_AS5,
                 NOTE_A5, NOTE_AS5, NOTE_A5, NOTE_AS5,
                 NOTE_G4, 0, NOTE_G4, NOTE_D5,
                 NOTE_C5, 0, NOTE_AS4, 0,
                 NOTE_A4, 0, NOTE_A4, NOTE_A4,
                 NOTE_C5, 0, NOTE_AS4, NOTE_A4,
                 NOTE_G4,0, NOTE_G4, NOTE_AS5,
                 NOTE_A5, NOTE_AS5, NOTE_A5, NOTE_AS5,
                 NOTE_G4,0, NOTE_G4, NOTE_AS5,
                 NOTE_A5, NOTE_AS5, NOTE_A5, NOTE_AS5
               };

int noteDurations[] = {
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                        4,4,4,4,
                      };

Servo myservo2;
Servo myservo3;
Servo myservo1;

int piezoPin = A3;
int state = 0;

//CODI PROVA 1
int sPins[] = {10, 1, 2, 3};
int valPROVA1 = 0; //grausº servo1
int ledPin1 = 5;
int wrongLedPin = 4;

int pos[] = {0, 0, 0, 0};
int goalPos[] = {1, 0, 1, 1};

//CODI PROVA 3
int ledPin3 = 12;
int forcePin = 0;
int forceReading;
int valPROVA3 = 0; //grausº servo3
int NUMERO = 90; //Valor de pressió sensor tàctil

//Pel potenciometre (Prova 3)
int potenciometre = A2;
int inputval = 0;
int numMIN = 0; //Valor minim del potenciometre perque s'encengui el LED
int numMAX = 700; //Valor maxim del potenciometre perque s'encengui el LED

//CODI PROVA 2
int valPROVA2 = 0; //grausº servo2
int ledPin2 = 11;
int fot = A0;
int x=0;
int llum = 20;

const int echoPin = 6; //Ultrasonic sensor
const int trigPin = 7; //Ultrasonic sensor

int inches = 0;
boolean ledState = false;
int cm = 0;

long readUltrasonicDistance(int trigPin, int echoPin)
{
  pinMode(trigPin, OUTPUT); // Clear the trigger
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigger pin to HIGH state for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  pinMode(echoPin, INPUT);
  // Reads the echo pin, and returns the sound wave travel time in microseconds
  return pulseIn(echoPin, HIGH);
}

//Prova 1 botons
boolean array_cmp(int *a, int *b, int len_a, int len_b) {
  int n;
  if (len_a != len_b) return false;
  for (n = 0; n < len_a; n++) if (a[n] != b[n]) return false;
  return true;
}

void setup()
{

  //CODI PROVA 1
  myservo1.attach(13);
  myservo1.write (-1);

  for (int i = 0; i < 4; i++) {
    pinMode(sPins[i], INPUT);
  }
  pinMode(ledPin1, OUTPUT);
  pinMode(wrongLedPin, OUTPUT);
  myservo1.write(0);

  //CODI PROVA 2
  pinMode(ledPin2, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  myservo2.attach(9); 
  Serial.begin(9600);
  // myservo2.write(0);

  //CODI PROVA 3
  pinMode(ledPin3, OUTPUT);
  myservo3.attach(8);
  Serial.begin(9600);
  myservo3.write(0);

}

void loop()
{
  //CODI PROVA 1
  // Checking and setting switch positions
  // Check if combo is correct
  if (state == 0) {
    for (int i = 0; i < 4; i++) {
      int estatbutons = digitalRead(sPins[i]);
      Serial.println(estatbutons);
      
      if (estatbutons == HIGH) {
        pos[i] = 1;
      } else if (estatbutons == LOW) {
        pos[i] = 0;
      }
    }

    if (array_cmp(pos, goalPos, 4, 4)) {
      digitalWrite(ledPin1, HIGH);
      digitalWrite(wrongLedPin, LOW);
      myservo1.write(90);
      state = 1; //Passa a la seguent prova
    } else {
      digitalWrite(wrongLedPin, HIGH);
    }
    delay(15);
  } else if (state == 1) { //"IF" PROVA 2
    //CODI PROVA 2
    x = analogRead(fot);
    Serial.println("The light you're emitting is ");
    Serial.print(x);
    delay (300);
    // measure the ping time in cm
    cm = 0.01723 * readUltrasonicDistance(trigPin, echoPin);
    // convert to inches by dividing by 2.54
    inches = (cm / 2.54);
    //Serial.print(inches);
    //Serial.print("in, ");
    Serial.print("You're at ");
    Serial.println(cm);
    Serial.print("cm");
    if ((x > llum) && (inches > 10 && inches < 25))
    {
      digitalWrite(ledPin2, HIGH);
      state = 2;
      myservo2.write(180);
      delay(50);
    }
  }
  //"IF" PROVA 3
  else if (state == 2) {

    //CODI PROVA 3
    forceReading = analogRead(forcePin);
    Serial.println("The force you're doing is ");
    Serial.print(forceReading);
    delay(500);

    //Pel potenciometre (Prova 3)
    //Serial.println(inputval);
    inputval = analogRead(potenciometre);
    Serial.println("The intensity is ");
    Serial.print(forceReading);
    if ((forceReading >= NUMERO) && (inputval > numMIN && inputval < numMAX)) 
    {
      digitalWrite(ledPin3, HIGH);
      valPROVA3 = 180;
      delay(50);
      Serial.println(state);
       myservo3.write(180);
      delay(50);

      if (digitalRead(ledPin1) && digitalRead(ledPin2) && digitalRead(ledPin3)) {
        delay(2000);
        for (int thisNote = 0; thisNote < 112; thisNote++) {
          int noteDuration = 750 / noteDurations[thisNote];
          tone(piezoPin, melody[thisNote], noteDuration);
          int pauseBetweenNotes = noteDuration * 1.30;
          delay(pauseBetweenNotes);

          noTone(piezoPin);
        }
      }
    }
  }

}
