// Khử nhiễu Encoder.
#include "EncoderTool.h"            // Thư viện đọc encoder  
using namespace EncoderTool;
#include <Bounce2.h>
#include <EEPROMex.h>
#include "Arduino.h"

Encoder encoder; // Định nghĩa tên hàm thư viện encoder

// ĐỊNH NGHĨA NGÕ VÀO
#define PIN_A 2
#define PIN_B 3
#define BT_UP 4
#define BT_OFF 5
#define BT_DOWN 6
#define BT_AUTO_MANUAL 7
#define BT_SET 8
#define BT_HAND 9
#define CB 10
#define POWER 20

// ĐỊNH NGHĨA NGÕ RA
#define M_UP 11
#define M_DOWN 12
#define LED_UP 13
#define LED_DOWN A5
#define LED_SET A4
#define LED_AUTO_ON A1
#define LED_AUTO_OFF A0
#define LED_MANUAL_ON A3
#define LED_MANUAL_OFF A2
//#define BRAKE 0

void(*door_state)(); // Trạng thái cửa.
void(*motor_state)();

int noise=600;

static int pos;
int upPos;
int downPos;

int dataRealPos = 10;
int dataUpPos = 30;
int dataDownPos = 60;

byte ledState;

unsigned long timerLed = 0;
unsigned long timerDelay = 0;
unsigned long timerDelay2 = 0;

void setup() {
//Khai báo chân cho encoder
  encoder.begin(3, 2);
  delay(1000);
  
//Khai báo ngõ vào
  pinMode(PIN_A, INPUT_PULLUP);
  pinMode(PIN_B, INPUT_PULLUP);
  pinMode(BT_UP, INPUT_PULLUP);
  pinMode(BT_DOWN, INPUT_PULLUP);
  pinMode(BT_OFF, INPUT_PULLUP);
  pinMode(BT_AUTO_MANUAL, INPUT_PULLUP);
  pinMode(BT_SET, INPUT_PULLUP);
  pinMode(BT_HAND, INPUT_PULLUP);
  pinMode(CB, INPUT_PULLUP);
  pinMode(POWER, INPUT_PULLUP);

//Khai bao ngo ra.
  pinMode(M_UP, OUTPUT);
  pinMode(M_DOWN, OUTPUT);
  pinMode(LED_UP, OUTPUT);
  pinMode(LED_DOWN, OUTPUT);
  pinMode(LED_SET, OUTPUT);
  pinMode(LED_AUTO_ON, OUTPUT);
  pinMode(LED_AUTO_OFF, OUTPUT);
  pinMode(LED_MANUAL_ON, OUTPUT);
  pinMode(LED_MANUAL_OFF, OUTPUT);
  //pinMode(BRAKE, OUTPUT);

//Trạng thái ban đầu.
  door_state=&door_manual;
  motor_state=&motor_stop;

//Khai báo bộ nhớ EEPROM lưu khi mất điện.
  EEPROM.setMemPool(0, 4096); //vị tri dau tien và dung luong cua eeprom
  EEPROM.setMaxAllowedWrites(4096); // số byte cho phép lưu
  delay(1000);

//Lấy vị trí khi khởi động.
  pos=EEPROM.readInt(dataRealPos);
  encoder.setValue(pos); 
  upPos = EEPROM.readInt(dataUpPos);
  downPos = EEPROM.readInt(dataDownPos);
  
  Serial.begin(9600);
  Serial.println("DOOR MANUAL");
  Serial.println(encoder.getValue());
  Serial.println(upPos);
  Serial.println(downPos);
  
//Ngắt khi mất điện
  attachInterrupt(digitalPinToInterrupt(POWER), lostPower, FALLING);  
}

void loop() {
//Chương trình chính.
  door_state();
  motor_state();
  //readEncoder();  
}

//CT khử nhiễu nút Set.
boolean Debounce(int button){
  static int reading = 0;   
  if (digitalRead(button) == LOW)   //  button reads LOW when Pressed
  {
    reading++;              // Increment Debounce variable
  } else {
    reading = 0;            // if it bounces reset
  }
  if (reading  == noise)    //number of consecutive positive reads to be accepted as a legitimate press
  {                                                // Adjust as needed
    return 0;               // Confirmed button Press
  } else {
    return 1;               // button not pressed
  }  
}

//CT khử nhiễu nút Auto/Manual.
boolean Debounce2(int button){  
  static int reading2 = 0;    
  if (digitalRead(button) == LOW)   //  button reads LOW when Pressed
  {
    reading2++;              // Increment Debounce variable
  } else {
    reading2 = 0;            // if it bounces reset
  }
  if (reading2  == noise)    //number of consecutive positive reads to be accepted as a legitimate press
  {                                                // Adjust as needed
    return 0;               // Confirmed button Press
  } else {
    return 1;               // button not pressed
  }  
}

//CT khử nhiễu nút UP.
boolean Debounce3(int button){  
  static int reading3 = 0;    
  if (digitalRead(button) == LOW)   //  button reads LOW when Pressed
  {
    reading3++;              // Increment Debounce variable
  } else {
    reading3 = 0;            // if it bounces reset
  }
  if (reading3  == noise)    //number of consecutive positive reads to be accepted as a legitimate press
  {                                                // Adjust as needed
    return 0;               // Confirmed button Press
  } else {
    return 1;               // button not pressed
  }  
}

//CT khử nhiễu nút DOWN.
boolean Debounce4(int button){  
  static int reading4 = 0;    
  if (digitalRead(button) == LOW)   //  button reads LOW when Pressed
  {
    reading4++;              // Increment Debounce variable
  } else {
    reading4 = 0;            // if it bounces reset
  }
  if (reading4  == noise)    //number of consecutive positive reads to be accepted as a legitimate press
  {                                                // Adjust as needed
    return 0;               // Confirmed button Press
  } else {
    return 1;               // button not pressed
  }  
}

//CT khử nhiễu nút HAND

boolean Debounce6(int button){  
  static int reading6 = 0;    
  if (digitalRead(button) == LOW)   //  button reads LOW when Pressed
  {
    reading6++;              // Increment Debounce variable
  } else {
    reading6 = 0;            // if it bounces reset
  }
  if (reading6  == noise)    //number of consecutive positive reads to be accepted as a legitimate press
  {                                                // Adjust as needed
    return 0;               // Confirmed button Press
  } else {
    return 1;               // button not pressed
  }  
}

//CT khử nhiễu nút OFF.
boolean Debounce5(int button){  
  static int reading5 = 0;    
  if (digitalRead(button) == LOW)   //  button reads LOW when Pressed
  {
    reading5++;              // Increment Debounce variable
  } else {
    reading5 = 0;            // if it bounces reset
  }
  if (reading5  == noise)    //number of consecutive positive reads to be accepted as a legitimate press
  {                                                // Adjust as needed
    return 0;               // Confirmed button Press
  } else {
    return 1;               // button not pressed
  }  
}

//CT trạng thái bảng hiển thị cửa.
void door_manual(){
  digitalWrite(LED_AUTO_ON, LOW);   
  digitalWrite(LED_AUTO_OFF, HIGH);
  digitalWrite(LED_MANUAL_ON, HIGH);   
  digitalWrite(LED_MANUAL_OFF, LOW);  
 
  if((Debounce3(BT_UP)==LOW)||(Debounce6(BT_HAND)==LOW)){
    digitalWrite(LED_UP, LOW);
    if(pos<upPos){
      if((digitalRead(BT_UP)==LOW)||(digitalRead(BT_HAND)==LOW)){
        motor_state=&motor_upPos;
        Serial.println("Cửa lên");
      }
    }
  }else if(Debounce4(BT_DOWN)==LOW){
    digitalWrite(LED_DOWN, LOW);
    if(downPos<pos){
      if(digitalRead(BT_DOWN)==LOW){    
        motor_state=&motor_downPos;
        Serial.println("Cửa xuống");
      }
    }
  
  }else if(Debounce5(BT_OFF)==LOW){
    motor_state=&motor_stop;
    Serial.println("Cửa Dừng");
    Serial.println(pos);
  }else if(Debounce(BT_SET)==LOW){
    digitalWrite(LED_SET, LOW);    
    door_state =&door_setUp;
    Serial.println("Set UP Pos");        
  }else if(Debounce2(BT_AUTO_MANUAL)==LOW){
    door_state =&door_auto; 
    Serial.println("DOOR AUTO");
    motor_state=&motor_waitUp;
    Serial.println("Cửa chờ chạy lên");
    timerDelay=millis();    
  }else{    
    digitalWrite(LED_UP, HIGH);
    digitalWrite(LED_DOWN, HIGH);
    digitalWrite(LED_SET, HIGH);
  }
}

void door_auto(){
  digitalWrite(LED_AUTO_ON, HIGH);   
  digitalWrite(LED_AUTO_OFF, LOW);
  digitalWrite(LED_MANUAL_ON, LOW);   
  digitalWrite(LED_MANUAL_OFF, HIGH); 
  
  if((Debounce3(BT_UP)==LOW)||(Debounce6(BT_HAND)==LOW)||(digitalRead(CB)==LOW)){
    digitalWrite(LED_UP, LOW);
    if(pos<upPos){
      if(motor_state==&motor_runDownPos){
          if((digitalRead(BT_UP)==LOW)||(digitalRead(BT_HAND)==LOW)||(digitalRead(CB)==LOW)){
            timerDelay2=millis();
            motor_state=&motor_waitUp;
            Serial.println("Cửa chờ chạy lên");          
          }
      }else if (motor_state==&motor_stop){
        if((digitalRead(BT_UP)==LOW)||(digitalRead(BT_HAND)==LOW)||(digitalRead(CB)==LOW)){
          motor_state=&motor_runUpPos;
          Serial.println("Cửa chạy lên");
        }
      }
    }   
  }else if(Debounce4(BT_DOWN)==LOW){
    digitalWrite(LED_DOWN, LOW);
    if(downPos<pos){
      if(digitalRead(BT_DOWN)==LOW){
        motor_state=&motor_runDownPos;
        Serial.println("Cửa chạy xuống");
      }
    }
  }else if(Debounce5(BT_OFF)==LOW){
    motor_state=&motor_stop;
    Serial.println("Cửa Dừng");
  }else if(Debounce2(BT_AUTO_MANUAL)==LOW){
    door_state =&door_manual;   
    Serial.println("DOOR MANUAL");   
  }else{    
    digitalWrite(LED_UP, HIGH);
    digitalWrite(LED_DOWN, HIGH);
    digitalWrite(LED_SET, HIGH);
  }
}

void door_setUp(){
  digitalWrite(LED_AUTO_ON, ledState);   
  digitalWrite(LED_AUTO_OFF, ledState);
  digitalWrite(LED_MANUAL_ON, HIGH);   
  digitalWrite(LED_MANUAL_OFF, HIGH);

 //Chớp tắt Led
  if (millis() - timerLed > 500){
      timerLed=millis();
    if (ledState == LOW) {
      ledState = HIGH;
    } else {
      ledState = LOW;
    }
  }
 
  if(Debounce3(BT_UP)==LOW){
    digitalWrite(LED_UP, LOW);
    motor_state=&motor_setUp;
    Serial.println("Lên");    
  }else if(Debounce4(BT_DOWN)==LOW){
    digitalWrite(LED_DOWN, LOW);
    motor_state=&motor_setDown;
    Serial.println("Xuống");
  }else if(Debounce5(BT_OFF)==LOW){    
    door_state =&door_setDown;
    Serial.println("Set Down Pos");
    Serial.println(pos);
  }else if(Debounce(BT_SET)==LOW){
    digitalWrite(LED_SET, LOW);
    setUpPos();
    door_state =&door_setDown;    
    Serial.println("Set Down Pos"); 
  }else if(Debounce2(BT_AUTO_MANUAL)==LOW){
    door_state =&door_manual;   
    Serial.println("DOOR MANUAL");
  }else{
    digitalWrite(LED_UP, HIGH);
    digitalWrite(LED_DOWN, HIGH);
    digitalWrite(LED_SET, HIGH);
  }
}

void door_setDown(){
  digitalWrite(LED_AUTO_ON, HIGH);   
  digitalWrite(LED_AUTO_OFF, HIGH);
  digitalWrite(LED_MANUAL_ON, ledState);   
  digitalWrite(LED_MANUAL_OFF, ledState);


 //Chớp tắt Led set point
  if (millis() - timerLed > 500){
      timerLed=millis();
    if (ledState == LOW) {
      ledState = HIGH;
    } else {
      ledState = LOW;
    }
  }

  if(Debounce3(BT_UP)==LOW){
    digitalWrite(LED_UP, LOW);
    motor_state=&motor_setUp;
    Serial.println("Lên");
  }else if(Debounce4(BT_DOWN)==LOW){
    digitalWrite(LED_DOWN, LOW);
    motor_state=&motor_setDown;
    Serial.println("Xuống");
  }else if(Debounce5(BT_OFF)==LOW){    
    door_state =&door_setUp;
    Serial.println("Set Up Pos");
    Serial.println(pos);
  }else if(Debounce(BT_SET)==LOW){
    digitalWrite(LED_SET, LOW);
    setDownPos();     
    door_state =&door_manual;    
    Serial.println("DOOR MANUAL");
  }else if(Debounce2(BT_AUTO_MANUAL)==LOW){
    door_state =&door_manual;
    Serial.println("DOOR MANUAL");
  }else{
    digitalWrite(LED_UP, HIGH);
    digitalWrite(LED_DOWN, HIGH);
    digitalWrite(LED_SET, HIGH);
  }
}

void door_error(){
  digitalWrite(LED_AUTO_ON, HIGH);   
  digitalWrite(LED_AUTO_OFF, ledState);
  digitalWrite(LED_MANUAL_ON, HIGH);   
  digitalWrite(LED_MANUAL_OFF, ledState);
  
    if (millis() - timerLed > 500){
      timerLed=millis();
    if (ledState == LOW) {
      ledState = HIGH;
    } else {
      ledState = LOW;
    }
  }

  
  }

void door_power(){
  digitalWrite(LED_AUTO_ON, LOW);   
  digitalWrite(LED_AUTO_OFF, LOW);
  digitalWrite(LED_MANUAL_ON, LOW);   
  digitalWrite(LED_MANUAL_OFF, LOW);
  digitalWrite(LED_SET, HIGH);
  digitalWrite(LED_UP, HIGH);
  digitalWrite(LED_DOWN, HIGH);
}

//CT trạng thái motor
void motor_up(){
  digitalWrite(M_UP, LOW);
  digitalWrite(M_DOWN, HIGH);
  readEncoder();
}
void motor_down(){
  digitalWrite(M_UP, HIGH);
  digitalWrite(M_DOWN, LOW);
  readEncoder();
}
void motor_stop(){
  digitalWrite(M_UP, HIGH);
  digitalWrite(M_DOWN, HIGH);
  readEncoder();
}

void motor_setUp(){
  motor_up();
  if(digitalRead(BT_UP)==HIGH){
    motor_state=&motor_stop;
    Serial.println("Cửa Dừng");
    Serial.println(pos);
  }
}

void motor_setDown(){
  motor_down(); 
  if(digitalRead(BT_DOWN)==HIGH){
    motor_state=&motor_stop;
    Serial.println("Cửa Dừng");
    Serial.println(pos);
  }
}

void motor_upPos(){
  motor_up(); 
  if((pos>upPos)){
    motor_state =&motor_stop;
    Serial.println("Cửa Dừng");
  }
}

void motor_downPos(){
  motor_down();
  if((pos<downPos)){
    motor_state =&motor_stop;
    Serial.println("Cửa Dừng");
  }
}

void motor_waitUp(){
  motor_stop();  
  if (millis() - timerDelay2 > 300){
    timerDelay2=millis();
    motor_state =&motor_runUpPos;
    Serial.println("Cửa lên");
  }
}
void motor_runUpPos(){
  motor_up();
  if(pos>upPos){
    motor_state =&motor_waitDown;
    Serial.println("Cửa chờ xuống");
    timerDelay=millis();   
  }
}

void motor_waitDown(){
  motor_stop();
  if (millis() - timerDelay > 6000){
    timerDelay=millis();
    motor_state =&motor_runDownPos;
    Serial.println("Cửa xuống");
  }
}

void motor_runDownPos(){
  motor_down();
  if((pos<downPos)){
    motor_state =&motor_stop;
    Serial.println("Cửa Dừng");
  }  
}

void readEncoder() { 
  if (encoder.valueChanged()){
    pos=encoder.getValue();        
  }
}

void setUpPos(){
  EEPROM.writeInt(dataUpPos,pos);
  upPos = EEPROM.readInt(dataUpPos);
  Serial.print("Pos Up : ");
  Serial.println(upPos);  
}

void setDownPos(){
  EEPROM.writeInt(dataDownPos,pos);
  downPos = EEPROM.readInt(dataDownPos);
  Serial.print("Pos Down : ");
  Serial.println(downPos); 
}

void lostPower(){
  door_state=&door_power;
  motor_state=&motor_stop;
  EEPROM.writeInt(dataRealPos,pos);
  Serial.println("Mất điện");
  Serial.print("Vị trí lưu : ");
  Serial.println(pos);
}