#include <avr/interrupt.h>
#include <avr/io.h>
#define INIT_TIMER_COUNT 6
#define RESET_TIMER2 TCNT2 = INIT_TIMER_COUNT

int int_counter = 0;
volatile int second = 0;
    
#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
#define stop3 39

int Command_ok=0;

unsigned long time=0;

ISR(TIMER2_OVF_vect) 
    {
      RESET_TIMER2;
      int_counter += 1;
      if (int_counter == 700) 
      {
        second+=1;
        int_counter = 0;
      }
      if( second < 2) analogWrite( ventilador, 255);
      if( second > 2)
      {
        analogWrite( ventilador, 128);
        if( second > 4) second=0;
      }
    };
    

void setup()                    
{
  //Timer2 Settings: Timer Prescaler /64,
    TCCR2A |= ((1<<CS22) | (0<<CS21) | (0<<CS20));
    // Use normal mode
    TCCR2A |= (0<<WGM21) | (0<<WGM20);
    // Use internal clock – external clock not used in Arduino
    ASSR |= (0<<AS2);
    TIMSK2 |= (1<<TOIE2) | (0<<OCIE2A);  //Timer2 Overflow Interrupt Enable
    RESET_TIMER2;
    sei();
    
  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);
  
  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(stop3);   //stop
  delay(15);
  servo_ver(90);
  ini(); 
}

void loop()                      
{ 
    
  if (digitalRead(pir))
  {
    //Secuencia 1
    digitalWrite(ventilador,HIGH);
    servo_ver(60);
    avanza();
    giro_cero();
    mp3_on();
    on_lamp_dif();
    off_lamp();
    giro();
    delay(1000);
    on_lamp();
    delay(5000);
    off_lamp_dif();
    giro();
    delay(1000);
    on_lamp_dif();
    delay(5000);
    off_lamp();
    giro();
    delay(1000);
    on_lamp();
    delay(3000);
    servo_ver(40);
    delay(2000);
    servo_ver(90);
    retrocede();
    avanza();
    off_lamp();
    servo_ver(60);
    giro();
    delay(1000);
    on_lamp();
    delay(5000);
    off_lamp_dif();
    giro();
    delay(1000);
    parpadeo_led(9);
    retrocede();
    avanza();
    delay(1000);
    off_lamp();
    giro();
    on_lamp_dif();
    delay(4000);
    off_lamp_dif();
    
    //Secuencia 2
    giro();
    delay(1000);
    on_lamp_dif();
    delay(4000);
    off_lamp();
    giro();
    delay(1000);
    on_lamp();
    delay(3000);
    servo_ver(40);
    delay(3000);
    servo_ver(90);
    retrocede();
    avanza();
    off_lamp();
    servo_ver(60);
    giro();
    parpadeo_led(13);
    off_lamp();  
    giro();
    delay(1000);
    on_lamp();
    delay(5000);
    servo_ver(90);
    retrocede_dif();
    ini();
    delay(10000);
  }
  else 
  {
    ini();
  }
}

void off_lamp()
{
  analogWrite(lampara,0);
}

void on_lamp()
{
  analogWrite(lampara,255);
}

void parpadeo_led(int fin)
{
  for (int i = 0; i <= fin; i++) 
       { 
        delay(300);
        analogWrite(lampara,128);
        delay(300);
        analogWrite(lampara,255);
       }
}

void mp3_on()
{
  digitalWrite(mp3,HIGH);
}

void mp3_off()
{
  digitalWrite(mp3,LOW);
}

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

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(stop3);   
    analogWrite(lampara,0);
}

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

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

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_dif()
{
  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_dif()
{
  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()
{ 
    retrocede();
    mp3_off();
    analogWrite(ventilador,0);
    analogWrite(lampara,0);
}