// https://wokwi.com/projects/362524913933364225
// https://forum.arduino.cc/t/vex-motor-code-help-need/1088689

# include <Servo.h>
const byte upLimit_Pin   = 2;
const byte DownLimit_Pin = 3;
const byte StopBtn_Pin   = 4;
const byte servo_Pin     = 7;

const byte upLED_Pin     = 8;
const byte downLED_Pin   = 9;
Servo aServo;

int servoState = 1;
int lastServoState = 1;

boolean upperLimitSwitchPressed = false;
boolean lowerLimitSwitchPressed = false;

int position = 90;
  
void setup(){
  Serial.begin(115200);
  Serial.println("\nhello world.\n");

  pinMode(upLimit_Pin  , INPUT_PULLUP);
  pinMode(DownLimit_Pin, INPUT_PULLUP);
  pinMode(StopBtn_Pin,   INPUT_PULLUP);

  pinMode(upLED_Pin,   OUTPUT);
  pinMode(downLED_Pin, OUTPUT);

  aServo.attach(servo_Pin);
  aServo.write(90);  
}


void loop() {
  // does what the name says
  simulateSwitchPressing(); 
  //if (!digitalRead(upLimit_Pin)) {
  if (upperLimitSwitchPressed) {
    servoState = -1;  // gonna go DOWN now! ");
  }
  
  //if (!digitalRead(DownLimit_Pin)) {
  if (lowerLimitSwitchPressed) {  
    servoState = 1; // gonna UP now. ");
  }

  if (!digitalRead(StopBtn_Pin)) {
    servoState = 0; // gonna stop now. ");
  }

  printStateOnChange();

  // servo and limit LEDs
  static unsigned long lastTwitch;
  if (millis() - lastTwitch < 200) {
    return;
  }  
  lastTwitch = millis();
  // change servopsosition a little bit
  position -= servoState << 2;
  position = constrain(position, 0, 180);
  Serial.print("Servopisition=");
  Serial.println(position);

  if (position == 0) {
    position = 5;
  }

  if (position == 180) {
    position = 175;
  }
  
  digitalWrite(upLED_Pin, position < 25);
  digitalWrite(downLED_Pin, position > 155);
  
  aServo.write(position);
}


void printStateOnChange() {
  // check if variable "servoState" has changed 
  // its value since last call of this function
  if (lastServoState != servoState) {
    // if it has REALLY changed its value 
    Serial.println("Limit switch pressed delay(2000)");
    delay(2000);
    lastServoState = servoState; // update lastServoState

    if (servoState == -1) {
      Serial.println("going DOWN now.");    
    }

    if (servoState == 1) {
      Serial.println("going UP now!");    
    }

    if (servoState == 0) {
      Serial.println("stopped.");    
    }
  }
}


void simulateSwitchPressing() { 
  if (position < 25) {
    upperLimitSwitchPressed = true;
  }
  else {
    upperLimitSwitchPressed = false;
  }

  if (position > 175) {
    lowerLimitSwitchPressed = true;
  }
  else {
    lowerLimitSwitchPressed = false;
  }
}