#include <Servo.h>

Servo servo1;
Servo servo2;
int angle = 0;
const int greenPin = 13;
const int redPin = 12;
const int touchSensorPin = 2;
const int buzzerPin = A1;
int should_dance = 0;

void do_dance() {
  should_dance = 1;
}

void setup() {
  // put your setup code here, to run once:
  servo1.attach(7);
  servo2.attach(8);
  pinMode(greenPin, OUTPUT);
  pinMode(redPin, OUTPUT);
  pinMode(touchSensorPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(touchSensorPin), do_dance, HIGH);
}

void loop() {
  if (should_dance == 1) {
    int del_st = 70;
    int del_nd = 100;
    for (int i = 0; i < 65; ++i) {
      analogWrite(buzzerPin, 100);
      servo1.write(del_st * 2);
      delay(del_st);
      servo1.write(-(del_st * 2));
      analogWrite(buzzerPin, 200);
      digitalWrite(greenPin, HIGH);
      servo2.write(del_nd);
      delay(del_nd);
      analogWrite(buzzerPin, 130);
      digitalWrite(redPin, HIGH);
      digitalWrite(greenPin, LOW);
      servo2.write(-del_nd);
      delay(del_nd);
      analogWrite(buzzerPin, 50);
      digitalWrite(redPin, LOW);
      del_st--;
      del_nd--;
    }
    analogWrite(buzzerPin, 0);
    should_dance = 0;
  }
}