#include <Wire.h> 
#include <SoftwareServo.h>

SoftwareServo servo1; //
SoftwareServo servo2; //
SoftwareServo servo3; //Carril

//Pines L298N
#define input2 13
#define input1 12
#define enable 11

//Finales de carrera
#define s1 17  //delante
#define s2 16  //detras
#define s3 15
#define s4 14

//Dispositivos
#define mp3 8
#define ventilador 10
#define lampara 9
#define pir 6

int Command_ok=0;

unsigned long time=0;

void setup()                    
{
  pinMode(mp3, OUTPUT);
  pinMode(ventilador, OUTPUT);
  pinMode(input1, OUTPUT);
  pinMode(input2, OUTPUT);
  
  pinMode(pir, INPUT);
  pinMode(s1, INPUT);
  pinMode(s2, INPUT);
  pinMode(s3, INPUT);
  pinMode(s4, INPUT);
  
  stop_motor();
  servo1.attach(3);
  servo2.attach(5);
  servo3.attach(4);
  servo1.setMinimumPulse(700);
  servo1.setMaximumPulse(2000);
  servo2.setMinimumPulse(700);
  servo2.setMaximumPulse(2000);
  servo3.setMinimumPulse(700);
  servo3.setMaximumPulse(2000);
  
  analogWrite(lampara, 0);
  digitalWrite(s1, HIGH);
  digitalWrite(s2, HIGH);
  digitalWrite(s3, HIGH);
  digitalWrite(s4, HIGH);
  digitalWrite(mp3,LOW);
  digitalWrite(ventilador,LOW);
  

  servo1.write(33);   //stop
  servo2.write(90);   //reposo
  servo3.write(39);   //stop
  delay(15);
  ini(); 
}

void loop()                      
{ 
    
  if (digitalRead(pir))
  {
    //Secuencia 1
    digitalWrite(ventilador,HIGH);
    servo3.write(0); 
    servo2.write(60); 
    //servo1.write(0); 
    while (digitalRead(s1))
    {
    SoftwareServo::refresh();
    }
    servo3.write(39); 
    giro();
    digitalWrite(mp3,HIGH);
    on_lamp();
    analogWrite(lampara,0);
    giro();
    delay(1000);
    analogWrite(lampara,255);
    delay(5000);
    off_lamp();
    giro();
    delay(1000);
    on_lamp();
    delay(5000);
    analogWrite(lampara,0);
    giro();
    delay(1000);
    analogWrite(lampara,255);
    delay(3000);
    servo_ver(40);
    delay(2000);
    servo_ver(90);
    retrocede();
    avanza();
    analogWrite(lampara,0);
    servo_ver(60);
    giro();
    delay(1000);
    analogWrite(lampara,255);
    delay(5000);
    off_lamp();
    giro();
    delay(1000);
    
    for (int i = 0; i <= 9; i++) 
       { 
        delay(300);
        analogWrite(lampara,128);
        delay(300);
        analogWrite(lampara,255);
       }
    retrocede();
    avanza();
    delay(1000);
    analogWrite(lampara,0);
    giro();
    on_lamp();
    delay(4000);
    off_lamp();
    
    //Secuencia 2
    giro();
    delay(1000);
    on_lamp();
    delay(4000);
    analogWrite(lampara,0);
    giro();
    delay(1000);
    analogWrite(lampara,255);
    delay(3000);
    servo_ver(40);
    delay(3000);
    servo_ver(90);
    retrocede();
    avanza();
    analogWrite(lampara,0);
    servo_ver(60);
    giro();
    for (int i = 0; i <= 13; i++) 
       { 
        delay(300);
        analogWrite(lampara,128);
        delay(300);
        analogWrite(lampara,255);
       }
    analogWrite(lampara,0);  
    giro();
    delay(1000);
    analogWrite(lampara,255);
    delay(5000);
    retrocede_dif();
    ini();
    delay(10000);
  }
  else 
  {
    servo3.write(180); 
    servo2.write(90); 
    while (digitalRead(s2))
    {
    SoftwareServo::refresh();
    }
    servo3.write(39);  
    digitalWrite(mp3,LOW);
    analogWrite(ventilador,0);
    analogWrite(lampara,0);
  }
}


void stop_motor()
{
  digitalWrite(input1,  LOW);
  digitalWrite(input2,  LOW);
  analogWrite(enable, 0);
}

void retrocede() 
{ 
    servo3.write(180); 
    while (digitalRead(s2))
    {
    SoftwareServo::refresh();
    }
    servo3.write(39);  
}

void retrocede_dif() 
{ 
  servo3.write(180); 
    int e=255;
    while (digitalRead(s2))
    {
    if (e>=8)
    {
      e=e-8;
      analogWrite(lampara,e);
    }
    SoftwareServo::refresh();
    delay(50);
    }
    servo3.write(39);   
    analogWrite(lampara,0);
}

void avanza() 
{ 
    servo3.write(0); 
    while (digitalRead(s1))
    {
    SoftwareServo::refresh();
    }
    servo3.write(39); 
} 

void giro()
{
  unsigned long time_servo=0;
  time_servo=millis();
  servo1.write(20); 
  while ((millis()-time_servo)<=300)
    {
     SoftwareServo::refresh();
    }
    
  while (digitalRead(s4))
  {
    servo1.write(20); 
    SoftwareServo::refresh();
  }
  servo1.write(33);
}

void servo_ver(int angulo)
{
  unsigned long time_servo2=0;
  time_servo2=millis();
  servo2.write(angulo); 
  while ((millis()-time_servo2)<=1000)
    {
     SoftwareServo::refresh();
    }
}

void on_lamp()
{
  for (int i = 5; i <= 255; i=i+5) 
       { 
         analogWrite(lampara,i);
         if (i<50) delay(200);
         else if (i<140) delay(100);
         else if (i<180) delay(50);
         else if (i<200) delay(25);
         else delay(10);
       }
}

void off_lamp()
{
  for (int i = 255; i >= 0; i=i-5) 
       { 
         analogWrite(lampara,i);
         if (i<50) delay(200);
         else if (i<140) delay(100);
         else if (i<180) delay(50);
         else if (i<200) delay(25);
         else delay(10);
       }
}

void ini()
{
    servo3.write(180); 
    servo2.write(90); 
    while (digitalRead(s2))
    {
    SoftwareServo::refresh();
    }
    servo3.write(39);  
    digitalWrite(mp3,LOW);
    analogWrite(ventilador,0);
    analogWrite(lampara,0);
}