#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 <stop_token>
#include <thread>

#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, 18, 3,  // A8-A11
    23, 17, 15   // 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(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() {
    for (int i = 0; i < addressPins.size(); i++) {
        bcm2835_gpio_fsel(addressPins.at(i), BCM2835_GPIO_FSEL_INPT);
    }
    for (int i = 0; i < controlPins.size(); i++) {
        bcm2835_gpio_fsel(controlPins.at(i), BCM2835_GPIO_FSEL_INPT);
    }
    for (int i = 0; i < dataPins.size(); i++) {
        bcm2835_gpio_fsel(dataPins.at(i), BCM2835_GPIO_FSEL_INPT);
    }
}


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


bool isChipEnabled() {
    // assumes the EEPROM is only hooked up via CE. If CE is low, it will ouput data
    if (bcm2835_gpio_lev(CEb) == LOW and bcm2835_gpio_lev(RWb) == HIGH) return true;
    return false;
}

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


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);
}


void simulate(std::array<char, UINT16_MAX> data) {
    initPins();
    printf("Initalized Pins\n");

    uint16_t lastAddress = 0;
    bool outputActive = false;

    while (true) {
        uint16_t currentAddress = readAddress();
        bool readEnabled = isChipEnabled();
        if (readEnabled && (!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 << "> Address: " << gz::toBinString(currentAddress) << "[" << gz::toHexString(currentAddress) << "] - " << gz::toBinString(data.at(currentAddress)) << "[" << gz::toHexString(static_cast<uint16_t>(data.at(currentAddress)), 2) << "]";
            printData(data.at(currentAddress));
            std::cout << std::endl;
        }

        if (!readEnabled && outputActive) {
            setIODirection(BCM2835_GPIO_FSEL_INPT);
            outputActive = false;
            std::cout << "X Address: " << gz::toBinString(currentAddress) << "[" << gz::toHexString(currentAddress) << "]\n"; //" - " << gz::toBinString(data.at(address)) << std::endl;
        }

        lastAddress = currentAddress;
    }
}


void simulateEEPROM(std::stop_token token, const std::array<char, UINT16_MAX>& data) {
    printf("Begin EEPROM simulation\n");
    initPins();

    while(!token.stop_requested()) {
        waitForFallingEdge(CEb);
        uint16_t currentAddress = readAddress();
        uint32_t valueMask = byteToMask(data.at(currentAddress), dataPins);
        setIODirection(BCM2835_GPIO_FSEL_OUTP);
        bcm2835_gpio_write_mask(valueMask, ioMask);
        std::cout << "> Address: " << gz::toBinString(currentAddress) << "[" << gz::toHexString(currentAddress) << "] - " << gz::toBinString(data.at(currentAddress)) << "[" << gz::toHexString(static_cast<uint16_t>(data.at(currentAddress)), 2) << "]";
        waitForRisingEdge(CEb);
        setIODirection(BCM2835_GPIO_FSEL_INPT);
    }
    printf("Exit 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()) {
        waitForFallingEdge(clockPin);
        state.RWb = isRead();
        state.addressBus = readAddress();
        state.dataBus = readData();
        std::cout << state.toString() << 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);

    /* for (int i = 0; i < bytes.size(); i++) { */
    /*     std::cout << gz::toHexString(static_cast<uint8_t>(bytes.at(i))) << " "; */
    /*     if ((i+1) % 8 == 0) { std::cout << " "; } */
    /*     if ((i+1) % 32 == 0) { std::cout << "\n"; } */
    /* } */
    /* std::cout << std::endl; */
    std::cout << "Reset Vector " << gz::toBinString(bytes.at(0x7ffc)) << " - " << gz::toBinString(0x77fd) << std::endl;
    /* simulate(bytes); */
    std::jthread eepromT(simulateEEPROM, std::ref(bytes));
    std::atomic<CurrentState> cs;
    std::jthread clockT(printBusWithClock, PHI2, std::ref(cs));

    signal(SIGINT, signalHandler);

    while(true);
}