#include <Servo_Hardware_PWM.h> //comment out for arduino nano

const int buttonPin = 2;     // the number of the pushbutton pin
const int ledPin =  9;      // the number of the LED pin
const int ledPin2 = 8;
const int ledPin3 = 7;
// variables will change:
int buttonState = 0;         // variable for reading the pushbutton status
int count_value = 1;
int prestate =0;
int i = 0;

const int buttonPinF = 5;      // Flywheel Rev microswitch pin number
const int buttonPinS = 4;      // Solenoid microswitch pin number
int buttonStateF = 0;             // Variable for reading the Flywheel Rev microswitch status
int buttonStateS = 0;             // Variable for reading the Solenoid microswitch status
int solenoidPin = 3;              // Solenoid MOSFET Gate pin number

//#include <Servo.h>  //include in sketch for arduino nano

Servo throttle;   //servo object to control the ESC
int potValue; //value from the analog pin POT

//int pos = 0;    //from old code controlling esc
int ESCpin = 10;                            // ESC signal pin

void setup() {
  Serial.begin(9600);
  
  // initialize the LED pin as an output:
  pinMode(ledPin, OUTPUT);
  pinMode(ledPin2, OUTPUT);
  pinMode(ledPin3, OUTPUT);
  // initialize the pushbutton pin as an input:
  pinMode(buttonPin, INPUT);
  
  pinMode(buttonPinF, INPUT); 	       // Initialize the Flywheel microswitch pin as an input //when testing on wokwi change to input_pullup all caps
  pinMode(buttonPinS, INPUT); 	       // Initialize the Solenoid microswitch pin as an input
  throttle.attach(ESCpin,1000,2000);     //(pin, min pulse width, max pulse width in microseconds)
  pinMode(solenoidPin, OUTPUT);         // Sets Solenoid MOSFET Gate pin as an output
  pinMode(ESCpin, OUTPUT);  
 // ESC Arming Sequence
   //for (pos = 90; pos <= 91; pos += 1) { //part of the old code for controlling esc
      throttle.write(0);             
      delay(3700);                                         // Wait for ESC to arm / Exit safety mode

// Increase this 3700 value depending on how long it takes for your ESC to arm
   }
  
 

void loop() {
  potValue = analogRead(A0);  // reads the value of the potentiometer 9value between 0 and 1023)
  potValue = map(potValue, 0, 1023, 0, 180);  //scale it to use it with the servo library (value between 0 and 180)
  
  // read the state of the pushbutton value:
  buttonState = digitalRead(buttonPin);
  buttonStateF = digitalRead(buttonPinF);
  buttonStateS = digitalRead(buttonPinS);
  // check if the pushbutton is pressed. If it is, then the buttonState is HIGH:
  if (buttonState == HIGH && prestate == 0) {
    count_value ++;
    Serial.println(count_value);
    // turn LED on
    digitalWrite(ledPin, HIGH);
    delay(100);
    // turn LED off
    digitalWrite(ledPin, LOW);

    prestate = 1;
  } else if(buttonState == LOW) {
    prestate = 0;
  }

  if (count_value >= 3){
      count_value = 0;
  }
  
  switch (count_value) {
    case 1: //single fire mode
      count_value = 1;
      digitalWrite(ledPin, HIGH);
      digitalWrite(ledPin2, LOW);
      digitalWrite(ledPin3, LOW);
      Serial.println(count_value);

    if (buttonStateF == HIGH) {  // Check microswitch pressed, if so Flywheel buttonState is HIGH
     // digitalWrite(ESCpin, HIGH); //Testing Button on WOKWI
    throttle.write(potValue);         //changed 92 to potValue        // <(92) = Motor off / (92) = Idle speed  
  } else {
    throttle.write(0);              //changed 97 to 0   // Motor on (92) = Idle speed / ~(115) = Max speed
     // digitalWrite(ESCpin, LOW); //testing button on WOKWI
  }

buttonStateS = digitalRead(buttonPinS);    // Read state of Solenoid microswitch value

  if (buttonStateF == HIGH && buttonStateS == HIGH) {
  digitalWrite(solenoidPin, HIGH);           // Switch Solenoid ON
  delay(90);                                                // ON duration
  digitalWrite(solenoidPin, LOW);           // Switch Solenoid OFF
  delay(100);                                             // OFF duration

while (digitalRead (buttonPinS) == HIGH && digitalRead (buttonPinF) == HIGH)
  {//do nothing
  }

   } else {
      digitalWrite(solenoidPin, LOW);      // Switch Solenoid OFF
  }

      break;
    
    case 2: //burst fire mode
      count_value = 2;
      digitalWrite(ledPin2, HIGH);
      digitalWrite(ledPin, LOW);
      digitalWrite(ledPin3, LOW);
      Serial.println(count_value);

if (buttonStateF == HIGH) {  // Check microswitch pressed, if so Flywheel buttonState is HIGH
      //digitalWrite(ESCpin, HIGH); //Testing Button on WOKWI
    throttle.write(potValue);   //changed 92 to potValue                // <(92) = Motor off / (92) = Idle speed
  } else {
    throttle.write(0);    //changed 97 to 0               // Motor on (92) = Idle speed / ~(115) = Max speed
     // digitalWrite(ESCpin, LOW); //testing button on WOKWI
  }

buttonStateS = digitalRead(buttonPinS);    // Read state of Solenoid microswitch value

  if (buttonStateF == HIGH && buttonStateS == HIGH) {
    for (int i=0; i<3; i++){
      digitalWrite(solenoidPin, HIGH);           // Switch Solenoid ON
      delay(90);                                                // ON duration
      digitalWrite(solenoidPin, LOW);           // Switch Solenoid OFF
      delay(100);  
      }                                           // OFF duration
  
  while (digitalRead (buttonPinS) == HIGH && digitalRead (buttonPinF) == HIGH)
  {//do nothing
    }
  }
  else {
      digitalWrite(solenoidPin, LOW);      // Switch Solenoid OFF
      }
  

  break;

    default: //full auto mode
      count_value = 0;
      digitalWrite(ledPin3, HIGH);
      digitalWrite(ledPin2, LOW);
      digitalWrite(ledPin, LOW);
      Serial.println(count_value);
      digitalRead(buttonPin);
     
    Serial.println(count_value);

    if (buttonStateF == HIGH) {  // Check microswitch pressed, if so Flywheel buttonState is HIGH
      //digitalWrite(ESCpin, HIGH); //Testing Button on WOKWI
    throttle.write(potValue);   //changed 92 to potValue                // <(92) = Motor off / (92) = Idle speed
  } else {
    throttle.write(0);    //changed 97 to 0               // Motor on (92) = Idle speed / ~(115) = Max speed
     // digitalWrite(ESCpin, LOW); //testing button on WOKWI
  }

buttonStateS = digitalRead(buttonPinS);    // Read state of Solenoid microswitch value

  if (buttonStateF == HIGH && buttonStateS == HIGH) {
  digitalWrite(solenoidPin, HIGH);           // Switch Solenoid ON
  delay(90);                                                // ON duration
  digitalWrite(solenoidPin, LOW);           // Switch Solenoid OFF
  delay(100);                                             // OFF duration
   } else {
      digitalWrite(solenoidPin, LOW);      // Switch Solenoid OFF
  }
      break;
  }
 }