//  Code simulation for Fishin' Magician 3.0 Electrical Concept 
//  This code was written by Tyler, 2/22/2024 in partnership with 
//  May We Help during E-Week24 at AtriCure 
//  
//  This code is a LED simulation of all functionality present in 
//  the electrical concept. incorporating 2 limit switches, 2 LEDs
//  representing forward and backward motion of the linear arm, 3 LEDs
//  representing Ready, Cast and Reel functionality. the 2 push buttons 
//  representing the 2 limit switches
//
//  Code has 4 states, inizilation, load, cast, reel. 
//  With single push button the code cycles thorugh the states 
//  Throughout the code are comment callouts for where the actual 
//  electrical code for control can go. 
//  
//  This simulation as of 5pm 2/22/24 is not always full functional
//  without case 3 (case 3 commented out) it works most of the time,
//  Sometimes, the switch bounce will cause it to skip case 1 and go 
//  go striaght to case 2. this may be part of the problem with case 3 
//  also. Pretty sure logic is there and should always work in the 
//  real world. with case 3 it's way more problamatic and doesn't 
//  work at all. currently case 3 is commented out for simplicity sake 

const int Proximal = 2;        // Proximal Sensor (green)
const int Distal = 3;          // Distal Sensor (Blue)
const int StateButton = 4;     //  State Switch, the master button on the panel or the wheelchair 

const int DistalLED = 13;      // (yellow)    Representive of stepper motor moving toward distal sensor
const int ProximalLED = 12;    // (light green)   Representive of stepper Motor Moving toward Proximal Sensor
const int CastLED = 11;        // (purple)  LED Representive of Solenoid that will release the CAM and cast the line
const int ReadyLED = 10;       // (blue)   LED to represent ready state
const int ReelLED = 9;         // (dark green)  LED represeting Reeling 

// variables will change:
int ProximalSens = 0;  // variable for reading the pushbutton status
int DistalSens = 0;
int State = 0;
int SubState = 0;
int n = 0;

void setup() {
  // initialize the LED pin as an output:
  pinMode(ProximalLED, OUTPUT);
  pinMode(DistalLED, OUTPUT);
  pinMode(CastLED, OUTPUT);
  pinMode(ReadyLED,OUTPUT);
  pinMode(ReelLED, OUTPUT);
  // initialize the pushbutton pin as an input:
  pinMode(Proximal, INPUT);
  pinMode(Distal, INPUT);
  pinMode(StateButton, INPUT);
}

void loop() {
  switch (State) {
    case 0:
      digitalWrite(ReelLED, LOW);
      digitalWrite(DistalLED, LOW);
      digitalWrite(CastLED, LOW);
      digitalWrite(ReadyLED, LOW);
      digitalWrite(ProximalLED, LOW);
      ProximalSens = digitalRead(Proximal);
      while (ProximalSens == LOW){  
        digitalWrite(ProximalLED, HIGH);
        ProximalSens = digitalRead(Proximal);
        // code location to reset system to defult 
      }
      digitalWrite(ProximalLED, LOW);
      digitalWrite(ReadyLED, HIGH);
      State = 1; // moves to case 1
      //  system's reset
      break;
    
    case 1:
      if (digitalRead(StateButton)==HIGH){
        SubState = 1;
      } else{}
      if (SubState == 1){  // section to move linear arm and set spring, then move linear arm back to proximal location 
        digitalWrite(ReadyLED, LOW);
        DistalSens = digitalRead(Distal);
        ProximalSens = digitalRead(Proximal);
        digitalWrite(ReadyLED, LOW);
        while (DistalSens == LOW){
          digitalWrite(DistalLED, HIGH);
          DistalSens = digitalRead(Distal);
        }
        digitalWrite(DistalLED, LOW);
        while (ProximalSens == LOW){  
          digitalWrite(ProximalLED, HIGH);
          ProximalSens = digitalRead(Proximal);
        }
        digitalWrite(ProximalLED, LOW);
        digitalWrite(ReadyLED, HIGH);
        State = 2; // moves to case 2   
        // System Set, ready for Cast 
        break;
      } else{}
    
    case 2: // section to code cast control
      if (digitalRead(StateButton)==HIGH){ 
        SubState = 2;
      } else{}
      if (SubState == 2){
        digitalWrite(ReadyLED, LOW);
        digitalWrite(CastLED, HIGH);
        delay(5000);
        digitalWrite(ReadyLED, HIGH);
        digitalWrite(CastLED, LOW);
        State = 3; // moves to case 3   
        // System Cast
        break;
      } else{}

/*    case 3: // section to code reel control 
        if (digitalRead(StateButton)==HIGH){
          digitalWrite(ReelLED, HIGH); 
          // Place to Activate Reel
          n = 0;  
          // System Reel
          if (n < 10000){
            State = 0;
            digitalWrite(ReadyLED, LOW);
            break;
          }
        } else {
          n = n++;
        }  */  
 } 
}