/*
 * ---------------------------------------
 * ---------------------------------------
 *             MFRC522      Seeed Studio       
 *             Reader/PCD   Xiao-RP2040       
 * Signal      Pin          Pin           
 * ---------------------------------------
 * RST/Reset   RST          D1            
 * SPI SS      SDA(SS)      D2            
 * SPI MOSI    MOSI         D10 / MOSI   
 * SPI MISO    MISO         D9 / MISO   
 * SPI SCK     SCK          D8 / SCK   
 *
 * 
 */

#include <SPI.h>
#include <MFRC522.h>

#define SS_PIN D2
#define RST_PIN D1


#include <Adafruit_NeoPixel.h>

#define LED_PIN     12
#define POWER_PIN   11
#define NUMPIXELS   1

const int vibrator = D0;

Adafruit_NeoPixel pixels(NUMPIXELS, LED_PIN, NEO_GRB + NEO_KHZ800);

int id = 1;
String message = "";

// --- Motor Control State ---
unsigned long motorStartTime = 0;
unsigned long lastToggleTime = 0;
unsigned long motorDuration = 0;
unsigned long toggleInterval = 0;
bool motorRunning = false;
bool motorState = false;
int motorPin = 0;


MFRC522 rfid(SS_PIN, RST_PIN); // Instance of the class

// Init array that will store new NUID 
byte nuidPICC[4];

void setup() {
  pinMode(vibrator, OUTPUT);

  SPI.begin(); // Init SPI bus
  rfid.PCD_Init(); // Init MFRC522 

  Serial.begin(9600);
  Serial1.begin(115200);
  pinMode(POWER_PIN, OUTPUT);
  digitalWrite(POWER_PIN, HIGH);
  pixels.begin();
  pixels.setBrightness(50);

  digitalWrite(vibrator, HIGH);
  delay(1000);
  digitalWrite(vibrator, LOW);
  delay(500);
  digitalWrite(vibrator, HIGH);
  delay(1000);
  digitalWrite(vibrator, LOW);
  delay(1000);
  digitalWrite(vibrator, HIGH);
}

void loop() {
  updateMotorPulse();
  // Forward user input with FROM_ID tagging
  if (Serial.available() > 0) {
    String myInput = Serial.readStringUntil('\n');
    myInput.trim();

    myInput += (String)"#"+id+(String)";"; // Ensure the message ends correctly
    Serial1.println(myInput);

  }

  if(rfid.PICC_IsNewCardPresent()){ // if there is new card
    if(rfid.PICC_ReadCardSerial()){

      Serial.print(F("PICC type: "));
      MFRC522::PICC_Type piccType = rfid.PICC_GetType(rfid.uid.sak);
      Serial.println(rfid.PICC_GetTypeName(piccType));

      // Check is the PICC of Classic MIFARE type
      if (piccType != MFRC522::PICC_TYPE_MIFARE_MINI &&  
      piccType != MFRC522::PICC_TYPE_MIFARE_1K &&
      piccType != MFRC522::PICC_TYPE_MIFARE_4K) {
      Serial.println(F("Your tag is not of type MIFARE Classic."));
      return;
      }


      if (rfid.uid.uidByte[0] != nuidPICC[0] || 
      rfid.uid.uidByte[1] != nuidPICC[1] || 
      rfid.uid.uidByte[2] != nuidPICC[2] || 
      rfid.uid.uidByte[3] != nuidPICC[3] ) {
      Serial.println(F("A new card has been detected."));

      // Store NUID into nuidPICC array
      for (byte i = 0; i < 4; i++) {
        nuidPICC[i] = rfid.uid.uidByte[i];
      }

      String dat = decToString(rfid.uid.uidByte, rfid.uid.size);
      dat = "DISPLAY:"+dat+"@3#1";
      Serial1.println(dat);
      Serial.println(dat);
    
      Serial.println(F("The NUID tag is:"));
      Serial.print(F("In hex: "));
      printHex(rfid.uid.uidByte, rfid.uid.size);
      Serial.println();
      Serial.print(F("In dec: "));
      printDec(rfid.uid.uidByte, rfid.uid.size);
      Serial.println();
    }
    else Serial.println(F("Card read previously."));

    // Halt PICC
    rfid.PICC_HaltA();

    // Stop encryption on PCD
    rfid.PCD_StopCrypto1();

    }
  }

  // Receive network message
  if (Serial1.available() > 0) {
    String incoming = Serial1.readStringUntil(';'); // Read until semicolon
    incoming.trim();

    Serial.println("Raw: " + incoming);

    // Extract command
    int colonIndex = incoming.indexOf(':');
    int atIndex = incoming.indexOf('@');
    int hashIndex = incoming.indexOf('#');

    if (colonIndex == -1 || atIndex == -1 || hashIndex == -1) return;

    String command = incoming.substring(0, colonIndex);
    String data = incoming.substring(colonIndex + 1, atIndex);
    int toID = incoming.substring(atIndex + 1, hashIndex).toInt();
    int fromID = incoming.substring(hashIndex + 1).toInt();

    Serial.println("Command: " + command);
    Serial.println("Data: " + data);
    Serial.println("To ID: " + String(toID));
    Serial.println("From ID: " + String(fromID));

    if (toID == id || toID == 0) {
      dispatchCommand(command, data);
    } else {
      Serial.println("Relaying message...");
      Serial1.println(incoming + ";"); // reappend delimiter

      //Serial.println("Not my message...");
    }
  }
}

void dispatchCommand(String command, String data) {
  if (command == "COLOR") {
    int firstComma = data.indexOf(',');
    int secondComma = data.indexOf(',', firstComma + 1);

    if (firstComma == -1 || secondComma == -1) return;

    int r = data.substring(0, firstComma).toInt();
    int g = data.substring(firstComma + 1, secondComma).toInt();
    int b = data.substring(secondComma + 1).toInt();

    Serial.println("Setting color to R:" + String(r) + " G:" + String(g) + " B:" + String(b));
    setColor(r, g, b);
  }

  if(command == "VIBRATE"){

    int firstComma = data.indexOf(',');
    if(firstComma == -1) return;

    int duration = data.substring(0, firstComma).toInt();
    int freq = data.substring(firstComma + 1).toInt();

    startMotorPulse(vibrator, freq, duration); // Motor on pin 9, 5Hz for 3s
  }
  // Add more commands like BUZZ, MOVE, etc. here:

  else {
    Serial.println("Unknown command: " + command);
  }
}


void setColor(int r, int g, int b) {
  pixels.setPixelColor(0, pixels.Color(r, g, b));
  pixels.show();
}


void startMotorPulse(int pin, float frequency, unsigned long durationMs) {
  motorPin = pin;
  pinMode(motorPin, OUTPUT);
  
  toggleInterval = 1000.0 / (frequency * 2); // Half-period in ms
  motorDuration = durationMs;
  motorStartTime = millis();
  lastToggleTime = 0;
  motorRunning = true;
  motorState = false; // Start OFF
  digitalWrite(motorPin, HIGH);
}

// Call this repeatedly inside loop()
void updateMotorPulse() {
  if (!motorRunning) return;

  unsigned long currentTime = millis();

  // Check if it's time to toggle
  if (currentTime - lastToggleTime >= toggleInterval) {
    motorState = !motorState;
    digitalWrite(motorPin, motorState);
    lastToggleTime = currentTime;
  }

  // Check if duration expired
  if (currentTime - motorStartTime >= motorDuration) {
    digitalWrite(motorPin, HIGH); // Ensure motor is off
    motorRunning = false;
  }
}

/**
 * Helper routine to dump a byte array as hex values to Serial. 
 */
void printHex(byte *buffer, byte bufferSize) {
  for (byte i = 0; i < bufferSize; i++) {
    Serial.print(buffer[i] < 0x10 ? " 0" : " ");
    Serial.print(buffer[i], HEX);
  }
}

// TO STRING
String decToString(byte *buffer, byte bufferSizer){
  String dat = "";
  for(byte i = 0; i < bufferSizer; i++){
    dat += " "+String((char)buffer[i], DEC); 
  }

  return dat;
}

/**
 * Helper routine to dump a byte array as dec values to Serial.
 */
void printDec(byte *buffer, byte bufferSize) {
  for (byte i = 0; i < bufferSize; i++) {
    Serial.print(' ');
    Serial.print(buffer[i], DEC);
  }
}