// #define GENERAL_SWITCH_PIN 13 // 8
// #define DIRECTION_PIN 12 // 9
// #define UP_SWITCH_PIN 11 // 10
// #define DOWN_SWITCH_PIN 10 // 11
// #define BUTTON_UP_PIN 9 // 12
// #define BUTTON_DOWN_PIN 8 // 13
 
#define GENERAL_SWITCH_PIN 6 // 8
#define DIRECTION_PIN 5 // 9
#define UP_SWITCH_PIN 3 // 10
#define DOWN_SWITCH_PIN 2 // 11
#define BUTTON_UP_PIN 1 // 12
#define BUTTON_DOWN_PIN 0 // 13
 
#define TEST_PIN 6
 
#define TEST_PIN 6
 
int up_switch = 0;
int down_switch = 0;
 
int button_up_stat = 0;
int button_down_stat = 0;
 
int move_to_down = 0;
int move_to_up = 0;
 
int state = 0;
 
void reset_state()
{
    // Serial.println("Reseting state...");
    move_to_down = 0;
    move_to_up = 0;
    button_down_stat = 0;
    button_up_stat = 0;
    up_switch = 0;
    down_switch = 0;
   
    delay(1000);
}
 
void setup() {
  pinMode(GENERAL_SWITCH_PIN, OUTPUT);
  pinMode(DIRECTION_PIN, OUTPUT);
 
  pinMode(UP_SWITCH_PIN, INPUT);
  pinMode(DOWN_SWITCH_PIN, INPUT);
  pinMode(BUTTON_DOWN_PIN, INPUT);
  pinMode(BUTTON_UP_PIN, INPUT);
 
  // Serial.begin(9600);
 
  reset_state();
}
 
void disable_outputs()
{
  // Serial.println("Disabling outputs...");
  digitalWrite(GENERAL_SWITCH_PIN, LOW);
  delay(100);
  digitalWrite(DIRECTION_PIN, LOW);
  state = 0;
}
 
void drive_up()
{
  state = 1;
  // Serial.println("Drive up...");
  digitalWrite(GENERAL_SWITCH_PIN, HIGH);
}
 
void drive_down()
{
  state = 2;
  // Serial.println("Drive down...");
  digitalWrite(DIRECTION_PIN, HIGH);
  digitalWrite(GENERAL_SWITCH_PIN, HIGH);
}
 
void print_state()
{
  // Serial.print("UP_SWITCH: ");
  // Serial.print(up_switch);
  // Serial.print(" | DOWN_SWITCH: ");
  // Serial.print(down_switch);
  // Serial.print(" | BUS: ");
  // Serial.print(button_up_stat);
  // Serial.print(" | BDS: ");
  // Serial.print(button_down_stat);
  // Serial.print(" | MTU: ");
  // Serial.print(move_to_up);
  // Serial.print(" | MTD: ");
  // Serial.print(move_to_down);
  // Serial.print(" | BUT_UP: ");
  // Serial.print(digitalRead(BUTTON_UP_PIN));
  // Serial.print(" | BUT_DOWN: ");
  // Serial.println(digitalRead(BUTTON_DOWN_PIN));
}
 
void loop() {
  // Get switches status
  up_switch = digitalRead(UP_SWITCH_PIN);
  down_switch = digitalRead(DOWN_SWITCH_PIN);
 
  // Get the read from the buttons
  // If the button was pressed once, it records that value and for the
  // subsequent loops, the vars will keep their values until
  // one of the switches are activated
  button_up_stat = digitalRead(BUTTON_UP_PIN) || button_up_stat;
  button_down_stat = digitalRead(BUTTON_DOWN_PIN) || button_down_stat;
 
  // Determine if we should move to the down or to the up
  move_to_up = button_up_stat == HIGH && up_switch == LOW;
  move_to_down = button_down_stat == HIGH && down_switch == LOW;
 
  // If both are pressed, do nothing
  if(move_to_down == move_to_up && state != 0)
  {
    print_state();
    disable_outputs();
    if(move_to_down == HIGH)
    {
      delay(1000);
    }
    reset_state();
    return;
  } else if(move_to_up == HIGH && state != 1)
  {
    print_state();
    drive_up();
  } else if(move_to_down == HIGH && state != 2)
  {
    print_state();
    drive_down();
  } else if(move_to_up == move_to_down && state == 0)
  {
    button_up_stat = 0;
    button_down_stat = 0;
  }
}