#include <ezButton.h>

ezButton btnThreeFloor(4);
ezButton btnTwoFloor(3);
ezButton btnOneFloor(2);

ezButton sensor1(6);
ezButton sensor2(5);

const uint8_t runLed = 12;
const uint8_t stopLed = 13;

int ToSkipCount;  // 센서 감지할 횟수

bool motorDirection; // true UP, false DOWN
bool isRun = false;  // 현재층 != 목표층일때 true

int floors[] = { 1, 2, 3 };
uint8_t currPos = 0;
uint8_t targetPos= 0;

void setup() {
  Serial.begin(115200);

  // set key debounce
  btnThreeFloor.setDebounceTime(100);
  btnTwoFloor.setDebounceTime(100);
  btnOneFloor.setDebounceTime(100);
  sensor1.setDebounceTime(100);
  sensor2.setDebounceTime(100);

}

void loop() {

  // 모터 현재층과 목표층이 같으면 런모드 false
  isRun = (floors[currPos] != floors[targetPos]) ? true : false;
  
  // 런모드이면 모터 동작LED 켬
  digitalWrite(runLed, isRun);
  digitalWrite(stopLed, !isRun);

  // 런모드 false면 모터 정지 및 정지LED켬
 if (isRun == false ) {
   stopMotor;
 
 }

  // 층수 누르는 버튼감지
 if (btnCheck()) {   
       btnProcess();
 }

   // 센서가 측정되면 toSkipCount--,  순서는 없음
  if (sensorCheck() && isRun) {
    Serial.print("CURR: ");
    Serial.print(floors[currPos]);
    Serial.print("  REMAIN: ");
    ToSkipCount = motorDirection ? ToSkipCount - 1 : ToSkipCount + 1;
    Serial.println(ToSkipCount);
 }
}

// 1층, 2층 3층등 각 층에 해당하는 버튼을 누르면 타겟목표층을 설정 및 버튼누름 반환
bool btnCheck() {
  int ret = false;

  btnThreeFloor.loop();
  btnTwoFloor.loop();
  btnOneFloor.loop();

   if (btnThreeFloor.isPressed())    {
     targetPos= 2;
     ret = true;
   }
   else if (btnTwoFloor.isPressed()) {
     targetPos= 1;
     ret = true;
   } 
   else if (btnOneFloor.isPressed()) {
     targetPos= 0;
     ret = true;
   }

   return ret;
}

// 층간 센서감지 모터방향에 따라 현재층을 + / - 
bool sensorCheck() {
  bool ret = false;

  sensor1.loop();
  sensor2.loop();

  if (sensor1.isPressed()) {
    Serial.println("Sensor1 Detect");
    currPos = (motorDirection)? currPos + 1 :currPos - 1;
    ret = true;
  } 
  else if (sensor2.isPressed()) {
    Serial.println("Sensor2 Detect");
    currPos = (motorDirection)? currPos + 1 :currPos - 1;
    ret = true;
  } 
  return ret;
}

// 현재층 목표층, 보터방향, 모터시작여부를 출력함
void btnProcess() {
  if (!isRun) {
    Serial.print("CURR: ");
    Serial.print(floors[currPos]);
    Serial.print("  TARGET: ");
    Serial.print(floors[targetPos]);
    
    if ( floors[targetPos] != floors[currPos] ) {

      ToSkipCount = floors[targetPos] - floors[currPos];

      motorDirection = (ToSkipCount > 0) ? true : false;
      Serial.print(" motor: ");
      Serial.print( (motorDirection)?"+":"" );
    } 
    else {
      ToSkipCount = 0;
      Serial.print(" motor: ");
      Serial.print("=");
    }

    Serial.print(ToSkipCount);
    
    isRun =  (ToSkipCount !=0) ? true : false;
    Serial.print(" Runmode : ");
    Serial.println(isRun);
  }
}


// 미구현 isRun에 led와 같이 구동하면 될듯..
void runMotor(bool dir) {
  if (dir == true) {
     Serial.println("올라갑니다.");
  } else if (dir == false ) {
     Serial.println("내려갑니다.");
  }
}

void stopMotor() {
  //;
}