#include <stdio.h>
#include <bcm2835.h>
#include <signal.h>
#include <array>
#include <fstream>
#include <iostream>
#include <stdexcept>
#include <gz-util/string/conversion.hpp>
#include <cassert>
#include <thread>
#include <functional>

#include "6502_opcodes.hpp"

/* ADDRESS SETUP TIME
 * depends on clock frequency, see datasheet.  max 150@2MHz
 * address is set after setup time (tADS) and is being hold for address hold time (tAH)
 * 
 * 
 */
constexpr uint64_t addressSetupTimeUS = 1;  // 300e-9 s

constexpr std::array<uint8_t, 15> addressPins { 
    27, 22, 10, 9, 11, 5, 6, 13,  // A0-A7
    4, 2, 15, 3,  // A8-A11
    18, 17, 23   // A12-A14
}; 

constexpr std::array<uint8_t, 8> dataPins { 24, 25, 8, 7, 12, 16, 20, 21 };
constexpr uint8_t RWb = 19;     // Read/Write
constexpr uint8_t CEb = 14;     // Chip Enable
constexpr uint8_t PHI2 = 26;    // clock
constexpr std::array<uint8_t, 3> controlPins { CEb, RWb, PHI2 };

template<size_t N> 
constexpr uint32_t getMask(const std::array<uint8_t, N>& pins) {
    uint32_t mask = 0;
    for (int i = 0; i < pins.size(); i++) {
        /* const uint8_t bit = pins.at(i) - 1; */
        mask |= (1 << pins.at(i));
    }
    return mask;
}
constexpr uint32_t ioMask = getMask(dataPins);
constexpr uint32_t addressMask = getMask(addressPins);

struct CurrentState {
    uint16_t addressBus = 0;
    uint8_t dataBus = 0;
    bool RWb = true;
    std::string toString() const {
        std::string s;
        if (RWb) { s = "R-"; }
        else { s = "W-"; }
        s += gz::toBinString(addressBus) + "[" + gz::toHexString(addressBus, 4) + "]-" 
            + gz::toBinString(dataBus) + "[" + gz::toHexString(static_cast<uint16_t>(dataBus), 2) + "]";
        return s;
    }
};

uint32_t byteToMask(char byte, const std::array<uint8_t, 8>& pins) {
    uint32_t mask = 0;
    for (int i = 0; i < 8; i++) {
        if ((1 << i) & byte) {  // if ith bit is set, set bit of corresponding pin
            mask |= (1 << pins.at(i));
        }
    }
    return mask;
}

void initPins() {
    // set to input and pullup
    auto setPinsInput = []<std::ranges::forward_range T>(const T& t) {
        for (auto it = t.begin(); it != t.end(); it++) {
            bcm2835_gpio_fsel(*it, BCM2835_GPIO_FSEL_INPT);
            /* bcm2835_gpio_set_pud(*it,  BCM2835_GPIO_PUD_UP); */
            bcm2835_gpio_set_pud(*it,  BCM2835_GPIO_PUD_OFF);
        }
    };
    setPinsInput(addressPins);
    setPinsInput(dataPins);
    setPinsInput(controlPins);
}


void setIODirection(uint8_t direction) {
    for (int i = 0; i < dataPins.size(); i++) {
        bcm2835_gpio_fsel(dataPins.at(i), direction);
    }
}


bool isChipEnabled() {
    return !bcm2835_gpio_lev(CEb);
}

inline bool isRead() {
    return bcm2835_gpio_lev(RWb);
}


uint16_t readAddress() {
    uint16_t address = 0;
    for (int i = 0; i < addressPins.size(); i++) {
        address |= (bcm2835_gpio_lev(addressPins.at(i)) << i);
    }
    return address;
}

uint8_t readData() {
    uint8_t address = 0;
    for (int i = 0; i < dataPins.size(); i++) {
        address |= (bcm2835_gpio_lev(dataPins.at(i)) << i);
    }
    return address;
}

constexpr std::array<const char*, 256> asciiChars {
    "␀", "␁", "␂", "␃", "␄", "␅", "␆", "␇", "␈", "␉", "␊", "␋", "␌", "␍", "␎", "␏", "␐", "␑", "␒", "␓", "␔", "␕", "␖", "␗", "␘", "␙", "␚", "␛", "␜", "␝", "␞", "␟", "␠", "!", "\"", "#", "$", "%", "&", "'", "(", ")", "*", "+", ",", "-", ".", "/", "0", "1", "2", "3", "4", "5", "6", "7", "8", "9", ":", ";", "<", "=", ">", "?", "@", "A", "B", "C", "D", "E", "F", "G", "H", "I", "J", "K", "L", "M", "N", "O", "P", "Q", "R", "S", "T", "U", "V", "W", "X", "Y", "Z", "[", "\"", "]", "^", "_", "`", "a", "b", "c", "d", "e", "f", "g", "h", "i", "j", "k", "l", "m", "n", "o", "p", "q", "r", "s", "t", "u", "v", "w", "x", "y", "z", "{", "|", "}", "~", "␡"
};
inline void printData(const char& data) {
    std::cout << '\'' << data << asciiChars.at(data) << "' "<< opCodes.at(data).getName();
}

inline void waitForFallingEdge(uint8_t pin) {
    while (bcm2835_gpio_lev(pin) == LOW);
    while (bcm2835_gpio_lev(pin) == HIGH);
}
inline void waitForRisingEdge(uint8_t pin) {
    while (bcm2835_gpio_lev(pin) == HIGH);
    while (bcm2835_gpio_lev(pin) == LOW);
}
inline void waitForEdge(uint8_t pin) {
    if (bcm2835_gpio_lev(pin) == HIGH) {
        while (bcm2835_gpio_lev(pin) == HIGH);
        return;
    }
    else {
        while (bcm2835_gpio_lev(pin) == LOW);
        return;
    }
}

/**
 * @brief Simulate an AT28C256 EEPROM
 * @details
 *  While the CEb is low, constantly read the address and put the corresponding data on the bus.
 *  While CEb is high, set data pins to input.
 */
void simulateEEPROM(std::stop_token token, std::array<char, UINT16_MAX> data) {
    initPins();
    printf("Begin EEPROM simulation\n");

    uint16_t lastAddress = 0;
    bool outputActive = false;

    while (!token.stop_requested()) {
        uint16_t currentAddress = readAddress();
        bool chipEnabled = isChipEnabled();
        if (chipEnabled && (!outputActive || (currentAddress != lastAddress))) {
            setIODirection(BCM2835_GPIO_FSEL_OUTP);
            outputActive = true;

            uint32_t valueMask = byteToMask(data.at(currentAddress), dataPins);
            bcm2835_gpio_write_mask(valueMask, ioMask);
            /* std::cout << ">-" << gz::toBinString(currentAddress) << "[" << gz::toHexString(currentAddress) << "]-" << gz::toBinString(data.at(currentAddress)) << "[" << gz::toHexString(static_cast<uint16_t>(data.at(currentAddress)), 2) << "]"; */
            /* std::cout << std::endl; */
        }

        if (!chipEnabled && outputActive) {
            setIODirection(BCM2835_GPIO_FSEL_INPT);
            outputActive = false;
        }

        lastAddress = currentAddress;
    }
    printf("End EEPROM simulation\n");
}


void printBusWithClock(std::stop_token token, uint8_t clockPin, const std::atomic<CurrentState>& currentState) {
    printf("Begin bus printing\n");
    auto state = currentState.load();
    while (!token.stop_requested()) {
        waitForRisingEdge(clockPin);
        uint8_t chipEnabled = !bcm2835_gpio_lev(CEb);
        uint8_t clock = bcm2835_gpio_lev(PHI2);
        state.RWb = isRead();
        state.addressBus = readAddress();
        state.addressBus |= (chipEnabled << 15);
        state.dataBus = readData();
        if (clock == 0) { std::cout << "L-"; }
        else { std::cout << "H-"; }
        std::cout << state.toString(); 
        if (chipEnabled) {
            std::cout << " " << opCodes.at(state.dataBus).getName();
        }
        std::cout << std::endl;
    }
    printf("End bus printing\n");
}


void readFile(const char* filepath, std::array<char, UINT16_MAX>& bytes) {
    std::ifstream file(filepath, std::ios_base::binary | std::ios_base::ate);
    if (!file.is_open()) {
        throw std::runtime_error("Error: Could not open file");
    }
    auto size = file.tellg();
    if (size > UINT16_MAX) {
        throw std::runtime_error("File is larger than UINT16_MAX");
    }
    file.seekg (0, std::ios::beg);
    file.read(bytes.data(), size);
    file.close();
}

void signalHandler(int signal) {
    setIODirection(BCM2835_GPIO_FSEL_INPT);
    bcm2835_close();
    printf("Caught signal %d, exiting.\n", signal);
    exit(0);
}

int main(int argc, const char** argv) {
    if (bcm2835_init() != 1) {
        printf("Error: Could not initalise gpio libraray\n");
        return 1;
    }

    if (argc != 2) {
        printf("Error: Expected exactly one argument (filename), got %d\n", argc);
        return 1;
    }

    /* std::cout << "IOMask=" << gz::toBinString(ioMask) << ", AddressMask=" << gz::toBinString(addressMask) << std::endl; */

    std::array<char, UINT16_MAX> bytes{};
    readFile(argv[1], bytes);

    std::cout << "Reset Vector " << gz::toBinString(bytes.at(0x7ffc)) << " - " << gz::toBinString(0x77fd) << std::endl;

    std::jthread eepromT(simulateEEPROM, bytes);
    std::atomic<CurrentState> cs;
    std::jthread clockT(printBusWithClock, PHI2, std::ref(cs));
    /* auto handler = std::bind(signalHandler, std::placeholders::_1, std::move(eepromT)); */
    signal(SIGINT, signalHandler);

    while(true) {
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
}