// constants won't change. They're used here to set pin numbers:
const int motorpwml = 9; // PWM left of BTS7960 ... switched from pin 2 on the l298n version to get a PWM pin
const int motorpwmr = 3; // PWM right of BTS7960
const int bottomsensortrig = 4; // attach pin to Echo of HC-SR04
const int bottomsensorecho = 5; //attach pin to Trig of HC-SR04
const int topsensory = A0;
const int upbutton = 6;
const int downbutton = 7;
const int stopbutton = 2; // switched from 9 on the l298n version to get a PWM pin
const int flashlight = 8; // the 7 colour flash
const int flashlight_button = 12; // the pullup to manually set off the flashlight

// variables
int uppressed = 0;
int downpressed = 0;
int stoppressed = 0;
int topsensorval;
bool attop;
bool atbottom;
bool sentalarmon, sentalarmoff, sentbottomalert, senttopalert;
long distance; // variable for the distance measurement

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
    
  // initialize the motor control pins as an output:
  pinMode(motorpwml, OUTPUT);
  pinMode(motorpwmr, OUTPUT);  

  // make sure the motor is off on startup
  turnmotor("STOP");

  // initialize the pushbutton pin as an input:
  pinMode(upbutton, INPUT);
  pinMode(downbutton, INPUT);
  pinMode(stopbutton, INPUT);

  // setup the joystick sensor, only for Y axis
  pinMode(topsensory, INPUT);

  // setup the ultrasonic sensor for the bottom sensor
  pinMode(bottomsensortrig, OUTPUT); // Sets the trigPin as an OUTPUT
  pinMode(bottomsensorecho, INPUT); // Sets the echoPin as an INPUT

  // setup the "flash" light
  pinMode(flashlight_button, INPUT_PULLUP);
  pinMode(flashlight, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:

  int checklight = digitalRead(flashlight_button);
  if (checklight == HIGH) // not pressed
  {
    // we only want to turn off the light and the LED once per "cycle" of alerting
    if (!sentalarmoff)
    {
      digitalWrite(flashlight,LOW);
      Serial.write('7'); // 7 = help button not pressed anymore, so stop showing the bell LEDs
      sentalarmoff = true;
    }
    sentalarmon = false;
  }
  else // flashlight-help button pressed
  {
    if (!sentalarmon)
    {
      Serial.write('6'); // 6 = help button pressed, so show the bell LED  
      sentalarmon = true;
    }
    sentalarmoff = false;
    digitalWrite(flashlight,HIGH);    
  }
  
  // check if we have reached the top and stop the motor
  topsensorval = analogRead(topsensory);
  if (topsensorval > 511) // seems 511 is the best option after lots of testing and not sure why this time it's the other direction, but meh.
  {
    if (!senttopalert)
    {
      Serial.write('4'); // 4 = elevator is at the top LED
      senttopalert = true;  
    }         
    attop = true;
    atbottom = false; // in the event someone puts something in front of the bottom sensor while the elevator is parked at the top causing a soft-lock
    turnmotor("STOP");
  }

  // check if we have reached the bottom and stop the motor
  distance = checkBottomDistance();
  if (distance <= 27)
  {
    // check again to avoid mis-readings?
    distance = checkBottomDistance();
    if (distance <= 27)
    {
      if (!sentbottomalert)
      {
        Serial.write('5'); // 5 = elevator is at the bottom LED
        sentbottomalert = true;  
      }      
      atbottom = true;
      attop = false; // in the event someone lifts the top lock while the elevator is parked at the bottom causing a soft-lock
      turnmotor("STOP");      
    }
  }
  
  // read if the buttons have been pressed
  uppressed = digitalRead(upbutton);
  downpressed = digitalRead(downbutton);
  stoppressed = digitalRead(stopbutton); 
  
  if (uppressed == HIGH && !attop) // only allow up direction if we're not at the top
  {        
    Serial.write('2'); // 2 = going up arrow LED
    turnmotor("UP");    
    if (atbottom)
    {
      delay(2500); // wait 2.5 seconds before starting the loop and sensor again, otherwise it will never move.
    }
    atbottom = false; // set being at the bottom to false if we are moving up from the bottom
    sentbottomalert = false;
    senttopalert = false;
  }  
  else if (downpressed == HIGH && !atbottom) // only allow down direction if we're not at the bottom
  {    
    Serial.write('1'); // 1 = going down arrow LED
    turnmotor("DOWN");    
    if (attop)
    {
      delay(2500); // wait 2.5 seconds before starting the loop and sensor again, otherwise it will never move.
    }
    attop = false; // set being at the top to false if we are moving down from the top
    sentbottomalert = false;
    senttopalert = false;
  }
  else if (stoppressed == HIGH && !attop && !atbottom) // only allow stop if not at the top and bottom
  {
    Serial.write('3');
    turnmotor("STOP");
    sentbottomalert = false;
    senttopalert = false;   
  }

  //delay(50);
}

void turnmotor(String direction)
{
  if (direction == "UP")
  {
    turnmotor("STOP");
    delay(500);
    analogWrite(motorpwml, 254);
    analogWrite(motorpwmr, 0);
  }
  else if (direction == "DOWN")
  {
    turnmotor("STOP");
    delay(500);    
    analogWrite(motorpwml, 0);
    analogWrite(motorpwmr, 254);
  }
  else if (direction == "STOP")
  {
    analogWrite(motorpwml, 0);
    analogWrite(motorpwmr, 0);
  }
}

long checkBottomDistance()
{
  // Clears the trigPin condition
  digitalWrite(bottomsensortrig, LOW);
  delayMicroseconds(2);
  // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
  digitalWrite(bottomsensortrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(bottomsensortrig, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  long sampleduration = pulseIn(bottomsensorecho, HIGH);
  // Calculating the distance in cm
  long sampledistance = sampleduration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)
  return sampledistance;
}