#include<Servo.h> // MOTOR CONTROL
#define MotorPin D2
#define Solonoid D1

Servo esc;

#include <SPI.h> //OLED STUFF
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

  #define SCREEN_WIDTH 128 // OLED display width, in pixels
  #define SCREEN_HEIGHT 64 // OLED display height, in pixels

#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3D ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


static const unsigned char PROGMEM logo_bmp[] =
{ 0b00000000, 0b11000000,
  0b00000001, 0b11000000,
  0b00000001, 0b11000000,
  0b00000011, 0b11100000,
  0b11110011, 0b11100000,
  0b11111110, 0b11111000,
  0b01111110, 0b11111111,
  0b00110011, 0b10011111,
  0b00011111, 0b11111100,
  0b00001101, 0b01110000,
  0b00011011, 0b10100000,
  0b00111111, 0b11100000,
  0b00111111, 0b11110000,
  0b01111100, 0b11110000,
  0b01110000, 0b01110000,
  0b00000000, 0b00110000 }; // OLED done



  #define CLK_PIN D10      // Rotary Encoder Stuff
  #define DT_PIN D9 
  #define SW_PIN D8

  int counter = 0;
  int CLK_state;
  int CLK_laststate;
  int storedValues[2]; // Array to store the values
  int buttonPressCounter = 0; // Counter for button presses
  bool valstored = false;

  int Distance = 1;
  int Delay=1;

  bool AlreadyFired = false;

  // Start Button
  int ButtonPin = D0;
  int LightStrip = D3;

void setup() {

  esc.attach(MotorPin);
  pinMode(ButtonPin, INPUT);
  pinMode(Solonoid, OUTPUT);
  pinMode(CLK_PIN, INPUT);
  pinMode(DT_PIN, INPUT);
  pinMode(SW_PIN, INPUT_PULLUP);
  pinMode(LightStrip, OUTPUT);

  Serial.begin(9600);

  // initialize OLED display with address 0x3C for 128x64
  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (true);
  }

  delay(2000);         // wait for initializing
  oled.clearDisplay(); // clear display

  oled.setTextSize(1);          // text size
  oled.setTextColor(WHITE);     // text color
  oled.setCursor(0, 0);        // position to display
  oled.println("Starting Up"); // text to display
  oled.display();
  delay(500);         // show on OLED
  oled.clearDisplay();
  oled.setTextSize(2);

    esc.writeMicroseconds(1000);
    digitalWrite(Solonoid, LOW);
  delay(1000); // MOTOR INITAITING 

}

void loop(){

  // SETUP / GETTING UI INPUT

  CLK_state = digitalRead(CLK_PIN);

  if (digitalRead(SW_PIN) == LOW) {

    Serial.println("Value Stored!");
    oled.display();
    storedValues[buttonPressCounter] = counter; // Store the current value
    buttonPressCounter++; // Increment button press counter
    if (buttonPressCounter >= 2) { // If pressed three times
      // Print all stored values
        oled.setTextSize(1);  
        oled.clearDisplay();
        oled.setCursor(5, 10);        // position to display
        oled.print("Click Below To Fire");
        oled.setCursor(5, 30);        // position to display
        oled.print("Distance : ");
        Distance = (abs(storedValues[0]));
        oled.print(Distance);
        oled.setCursor(5, 50); 
        oled.print("Delay : ");
        Delay = (abs(storedValues[1]));
        oled.print(Delay+1);
        valstored = true;
      }
      // Reset button press counter and clear stored values
    
    delay(250); // Debouncing delay
  } 
  if (CLK_state != CLK_laststate){     
    if (digitalRead(DT_PIN) != CLK_state) { 
      counter ++;
    } else {
      counter --;
    }
  CLK_laststate = CLK_state;

    if (buttonPressCounter == 0){
      oled.clearDisplay();
      oled.setCursor(20,30);
      oled.print("Distance = ");
      oled.println(abs(counter));
      oled.display();
    } else if (buttonPressCounter == 1){
      oled.clearDisplay();
      oled.setCursor(20,30);
      oled.print("Delay = ");
      oled.println((abs(counter)+1));
      oled.display();
    }
    Serial.print("Counter: ");
    Serial.println(counter);
  }
  
  if (counter >= 9 || counter <=-9){
    counter = 0;
  }
  // END OF SETTING UP STUFF
  int ButtonPressed = analogRead(ButtonPin);
  if (ButtonPressed >= 400){
    Serial.println("Fire");
    delay(250); //Debounce 
    // FIRE THE WEOPON HERE!!!!

    Shoot(abs(storedValues[0]), abs(storedValues[1]));
  
    delay(1000);

    NVIC_SystemReset();
  }


}

void Shoot(int distance, int delayT){
  int throttle = (distance*20);
  int escOutput = map(throttle, 0, 180, 1000, 2000);
  delay(delayT*1000);
  digitalWrite(LightStrip, LOW);
  esc.writeMicroseconds(escOutput);
  delay(1000);
  digitalWrite(LightStrip, HIGH);
  delay(100);
  digitalWrite(LightStrip, LOW);
  digitalWrite(Solonoid, HIGH);
  delay(3000);
  digitalWrite(LightStrip, HIGH);
  digitalWrite(Solonoid, LOW);
  esc.writeMicroseconds(0);
}