#include <Servo.h>
#include <SoftwareSerial.h>
#include <RDM6300.h>
 
//RFID
SoftwareSerial RFID(2, 3);
uint8_t Payload[6];
RDM6300 RDM6300(Payload);
bool debugTag = false;

//Servo
Servo motor;
int pos;
int motorPin = 6;

int openPos = 0;

int closedPos = 90;

bool debugPosition = false;
int stored_CurrentPosition = openPos;
int currentMotorAngle = openPos;
int targetMotorAngle = openPos;
int motorSpeed = 1;


//Switch
int switchPin = 4;



//Logic
bool isMoving = false;
bool canOpen = false;
String str;
unsigned long lastTagTime = 0;

void startRFID(){    
  RFID.begin(9600);
}
 
void startSerial(){
    Serial.begin(9600);
    Serial.println("Starting up");
}

void startServo(){
    motor.attach(motorPin);
    motor.write(openPos);
}

void startSwitch(){
    pinMode(switchPin, INPUT_PULLUP);
}

bool teste = false;

void setup() {
    startSerial();

    //Servo
    startServo();


    //rfid
    startRFID();

    //Switch
    //startSwitch();
}
 

bool isSwitchPressed(){
    return digitalRead(switchPin) == 1;
}

void loop() {
    checkRFIDTag();

    updateMotorPosition();
}

bool updateMotorPosition() {
    if (currentMotorAngle == targetMotorAngle) {
        isMoving = false;
        return true;
    }

    isMoving = true;


    int step = (targetMotorAngle > currentMotorAngle) ? motorSpeed : -motorSpeed;
    currentMotorAngle += step;


    currentMotorAngle = constrain(currentMotorAngle,
                                  min(currentMotorAngle, targetMotorAngle),
                                  max(currentMotorAngle, targetMotorAngle));

    motor.write(currentMotorAngle);
}



void closeLid(){
    if(currentMotorAngle == closedPos || targetMotorAngle == closedPos) return;
    
    Serial.println("Closing lid");

    moveToAngle(closedPos);
}

void openLid(){
    if(currentMotorAngle == openPos || targetMotorAngle == openPos) return;
    
    Serial.println("Opening lid");

    moveToAngle(openPos);
}

void checkRFIDTag() {
    bool tagSeen = false;

    while (RFID.available() > 0) {
        uint8_t c = RFID.read();
        tagSeen = true;
    }

    if (tagSeen) {
        closeLid();

        canOpen = true;
        lastTagTime = millis();
        if (debugTag) {
            Serial.println("Tag detected");
        }
    } else {
        if (millis() - lastTagTime > 1000) {
            canOpen = false;

            openLid();
        }
    }
}


void moveToAngle(int targetAngle){
    targetMotorAngle = targetAngle;
}