Files/ComputerAssignments/CA1/code/include/CPS4042/Sketchs/CombinedController.h

#ifndef COMBINED_CONTROLLER_H
#define COMBINED_CONTROLLER_H

#include <CPS4042/Sketchs/AbstractSketch.h>
#include <CPS4042/Hardwares/Boards/Esp8266.h>
#include <CPS4042/Hardwares/Comm/HardDisk.h>
#include <CPS4042/Hardwares/Comm/I2CMux.h>
#include <iostream>
#include <chrono>
#include <thread>

constexpr const char* COLOR_BLUE   = "\033[34m";
constexpr const char* COLOR_YELLOW = "\033[33m";
constexpr const char* COLOR_CYAN   = "\033[36m";
constexpr const char* COLOR_RESET  = "\033[0m";

class CombinedController : public AbstractSketch<Boards::Esp8266> {
public:
    CombinedController(Boards::Esp8266* esp, Comm::HardDisk* hd, Comm::I2CMux* mux)
        : AbstractSketch<Boards::Esp8266>(esp), m_esp(esp), m_hd(hd), m_mux(mux) {}

    std::int32_t setup(Boards::Esp8266::Gpio& gpio) override {
        gpio.vdd1.write(Byte(0xFF));
        gpio.gnd1.write(Byte(0x00));
        std::cout << COLOR_BLUE << "[Controller] Setup completed. Hardware protocols active.\n" << COLOR_RESET;
        return 0;
    }

    std::int32_t loop(Boards::Esp8266::Gpio&) override {
        static int cycle = 0;
        if (cycle % 3 == 0) {
            // ---------- REAL USART1 REQUEST TO HARD DISK ----------
            uint8_t addr = static_cast<uint8_t>((cycle / 3) % 256);
            std::cout << COLOR_YELLOW << "\n┌─ USART1 HardDisk Request ──────────────────────┐\n";
            std::cout << "│ [Controller] Sending address 0x" << std::hex << (int)addr << std::dec << " via USART1 TX pin\n";
            
            // Send address byte over USART1 (TX -> Link -> RX)
            m_esp->usart().write(static_cast<Byte>(addr));
            
            // Wait for response from HardDisk via USART1
            int waitMs = 0;
            while (m_esp->usart().bufferSize() == 0 && waitMs < 100) {
                std::this_thread::sleep_for(std::chrono::milliseconds(5));
                waitMs += 5;
            }

            if (m_esp->usart().bufferSize() > 0) {
                Byte response = m_esp->usart().read();
                uint8_t val = static_cast<uint8_t>(response);
                std::cout << "│ [Controller] Received response via USART1 RX pin: 0x" << std::hex << (int)val << std::dec << "\n";
                std::cout << "│ HardDisk[0x" << std::hex << (int)addr << "] = 0x" << (int)val << std::dec << "\n";
            } else {
                std::cout << "│ [Controller] Timeout waiting for HardDisk response\n";
            }
            std::cout << "└───────────────────────────────────────────────┘\n" << COLOR_RESET;
        }
        else if (cycle % 3 == 1) {
            // ---------- REAL USART2 + I2C READ FROM VL530X (channel 0) ----------
            std::cout << COLOR_CYAN << "\n┌─ I2C MUX Channel 0 (VL530X) ───────────────────┐\n";
            std::cout << "│ [Controller] Requesting channel 0 via USART2 TX2 pin\n";

            // Send channel 0 byte via USART2 (TX2 -> Link -> RX) to MUX
            m_esp->usart2().write(Byte(0x00));

            // Wait for 3-byte response via USART2
            int waitMs = 0;
            while (m_esp->usart2().bufferSize() < 3 && waitMs < 100) {
                std::this_thread::sleep_for(std::chrono::milliseconds(5));
                waitMs += 5;
            }

            if (m_esp->usart2().bufferSize() >= 3) {
                uint8_t high = static_cast<uint8_t>(m_esp->usart2().read());
                uint8_t low  = static_cast<uint8_t>(m_esp->usart2().read());
                uint8_t chk  = static_cast<uint8_t>(m_esp->usart2().read());
                uint16_t dist = (high << 8) | low;
                uint8_t expectedChk = (high > low) ? (high - low) : (low - high);
                bool valid = (chk == expectedChk);

                std::cout << "│ [Controller] Received MUX ch0 bytes via USART2: 0x" 
                          << std::hex << (int)high << " 0x" << (int)low << " 0x" << (int)chk << std::dec << "\n";
                std::cout << "│ MUX ch0 (VL530X) distance: " << dist << " mm (Checksum: " 
                          << (valid ? "OK" : "FAILED") << ")\n";
            } else {
                std::cout << "│ [Controller] Timeout waiting for MUX ch0 response\n";
            }
            std::cout << "└────────────────────────────────────────────────┘\n" << COLOR_RESET;
        }
        else {
            // ---------- REAL USART2 + I2C READ FROM SimpleSensor (channel 1) ----------
            std::cout << COLOR_CYAN << "\n┌─ I2C MUX Channel 1 (SimpleSensor) ─────────────┐\n";
            std::cout << "│ [Controller] Requesting channel 1 via USART2 TX2 pin\n";

            // Send channel 1 byte via USART2 to MUX
            m_esp->usart2().write(Byte(0x01));

            // Wait for 3-byte response via USART2
            int waitMs = 0;
            while (m_esp->usart2().bufferSize() < 3 && waitMs < 100) {
                std::this_thread::sleep_for(std::chrono::milliseconds(5));
                waitMs += 5;
            }

            if (m_esp->usart2().bufferSize() >= 3) {
                uint8_t high = static_cast<uint8_t>(m_esp->usart2().read());
                uint8_t low  = static_cast<uint8_t>(m_esp->usart2().read());
                uint8_t chk  = static_cast<uint8_t>(m_esp->usart2().read());
                uint16_t dist = (high << 8) | low;
                uint8_t expectedChk = (high > low) ? (high - low) : (low - high);
                bool valid = (chk == expectedChk);

                std::cout << "│ [Controller] Received MUX ch1 bytes via USART2: 0x" 
                          << std::hex << (int)high << " 0x" << (int)low << " 0x" << (int)chk << std::dec << "\n";
                std::cout << "│ MUX ch1 (SimpleSensor) distance: " << dist << " mm (Checksum: " 
                          << (valid ? "OK" : "FAILED") << ")\n";
            } else {
                std::cout << "│ [Controller] Timeout waiting for MUX ch1 response\n";
            }
            std::cout << "└────────────────────────────────────────────────┘\n" << COLOR_RESET;
        }
        ++cycle;
        std::this_thread::sleep_for(std::chrono::milliseconds(600));
        return 0;
    }

private:
    Boards::Esp8266* m_esp;
    Comm::HardDisk*  m_hd;
    Comm::I2CMux*    m_mux;
};

#endif