#include <Arduino.h>
#include <ESP32Servo.h>

char flag[] = "IAMTHEFLAGBEAREROF2023JCTF";

// A
void Tesla()
{
    systemRun(45, 0);
}

// B
void Faraday()
{
    systemRun(90, 0);
}

// C
void Ohm()
{
    systemRun(135, 0);
}

// D
void Bell()
{
    systemRun(180, 0);
}

// E
void Morse()
{
    systemRun(0, 135);
}

// F
void Marconi()
{
    systemRun(0, 90);
}

// G
void Maxwell()
{
    systemRun(0, 45);
}

// H
void Curie()
{
    systemRun(90, -45);
}

// I
void Einstein()
{
    systemRun(180, -45);
}

// J
void Galileo()
{
    systemRun(180, 90);
}

// K
void Newton()
{
    systemRun(45, 180);
}

// L
void Copernicus()
{
    systemRun(45, 135);
}

// M
void Kepler()
{
    systemRun(45, 90);
}

// N
void Archimedes()
{
    systemRun(45, 45);
}

// O
void Pascal()
{
    systemRun(90, -135);
}

// P
void Euler()
{
    systemRun(90, 180);
}

// Q
void Ramanujan()
{
    systemRun(90, 135);
}

// R
void Gauss()
{
    systemRun(90, 90);
}

// S
void Hawking()
{
    systemRun(90, 45);
}

// T
void Heisenberg()
{
    systemRun(135, 180);
}

// U
void Bohr()
{
    systemRun(135, 135);
}

// V
void Planck()
{
    systemRun(135, 45);
}

// W
void Hubble()
{
    systemRun(-135, 90);
}

// X
void Fermi()
{
    systemRun(-135, 45);
}

// Y
void Volta()
{
    systemRun(135, 90);
}

// Z
void Turing()
{
    systemRun(-45, 90);
}

// 0
void Wheatstone()
{
    Newton();
}

// 1
void Franklin()
{
    Tesla();
}

// 2
void Kelvin()
{
    Faraday();
}

// 3
void Feynman()
{
    Ohm();
}

// 4
void Schrodinger()
{
    Bell();
}

// 5
void Strang()
{
    Morse();
}

// 6
void Rutherford()
{
    Marconi();
}

// 7
void Joule()
{
    Maxwell();
}

// 8
void Fourier()
{
    Curie();
}

// 9
void Nyquist()
{
    Einstein();
}

// WRONG
void Edison()
{
    for(int i = 0; i < 3; i++)
    {
        Bohr();
        delay(1000);
        Archimedes();
        delay(1000);
    }
    Serial.println("WRONG");
}

// ANNUL
void Moore()
{
    systemRun(135, 45);
    Serial.println("ANNUL");
}

// NUMERALS
void Navier()
{
    systemRun(180, 135);
    Serial.println("NUMERALS");
}

// ALPHABET
void Stokes()
{
    Galileo();
    Serial.println("ALPHABET");
}

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

#define IGNORE1 0
#define IGNORE2 180
#define IGNORE3 0
#define IGNORE4 180

void systemRun(int leftHand, int rightHand)
{
    if(leftHand >= 0 && leftHand <= 90)
    {
        servo1.write(leftHand + 90);
        servo2.write(IGNORE2);
    }
    else if(leftHand > 90 && leftHand <= 180)
    {
        servo1.write(IGNORE1);
        servo2.write(leftHand-90);
    }
    else if(leftHand >= -90 && leftHand < 0)
    {
        leftHand = map(leftHand, 0, -90, 90, 0);
        servo1.write(leftHand);
        servo2.write(IGNORE2);
    }
    else if(leftHand < -90 && leftHand >= -180)
    {
        leftHand = map(leftHand, -90, -180, 180, 90);
        servo1.write(IGNORE1);
        servo2.write(leftHand);
    }
    else
    {
        servo1.write(IGNORE1);
        servo2.write(IGNORE2);
    }

    if(rightHand >= 0 && rightHand <= 90)
    {
        servo3.write(IGNORE3);
        rightHand = 90 - rightHand;
        servo4.write(rightHand);
    }
    else if(rightHand > 90 && rightHand <= 180)
    {
        rightHand = map(rightHand, 90, 180, 180, 90);
        servo3.write(rightHand);
        servo4.write(IGNORE4);
    }
    else if(rightHand >= -90 && rightHand < 0)
    {
        servo3.write(IGNORE3);
        servo4.write(90 + abs(rightHand));
    }
    else if(rightHand < -90 && rightHand >= -180)
    {
        rightHand = map(rightHand, -90, -180, 0, 90);
        servo3.write(rightHand);
        servo4.write(IGNORE4);
    }
    else
    {
        servo3.write(IGNORE3);
        servo4.write(IGNORE4);
    }
    delay(2000);

    ignore();
    Serial.println("RESETTING...");

    delay(2000);
}

void ignore()
{
    servo1.write(IGNORE1);
    servo2.write(IGNORE2);
    servo3.write(IGNORE3);
    servo4.write(IGNORE4);
}

void setup()
{
    Serial.begin(115200);
    Serial.println("WELCOME.");
    
    servo1.attach(5);
    servo2.attach(18);
    servo3.attach(19);
    servo4.attach(21);

    ignore();
}

int degree = 0;
int input = 0;
unsigned long lastTime = 0;

void loop()
{
    Einstein();
    Tesla();
    Kepler();
    Heisenberg();
    Curie();
    Morse();
    Marconi();
    Copernicus();
    Tesla();
    Maxwell();
    Faraday();
    Morse();
    Tesla();
    Gauss();
    Morse();
    Gauss();
    Pascal();
    Marconi();
    Heisenberg();
    Curie();
    Morse();
    Navier();
    Kelvin();
    Wheatstone();
    Kelvin();
    Feynman();
    Stokes();
    Galileo();
    Ohm();
    Heisenberg();
    Marconi();
}