#include <signal.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>

/* USBSIO header */
#include <lpcusbsio.h>

static bool s_running = true;

static int i2c_detect(LPC_HANDLE handle, uint8_t port_num, uint8_t start_addr, uint8_t end_addr,
                      enum I2C_ClockRate_t rate);

static int i2c_probe_addr(LPC_HANDLE i2c_handle, uint8_t addr);

static void signal_handler(int sig);

int main(int argc, const char *argv[]) {
    int        ret;
    LPC_HANDLE dev_handle;

    fprintf(stdout, "USBSIO example - I2CDetect\n");

    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);

    /* Find all SIO devices. */
    ret = LPCUSBSIO_GetNumPorts(LPCUSBSIO_VID, MCULINKSIO_PID);
    if (ret == 0) {
        fprintf(stderr, "No MCU-Link devices found.\n");
        return -1;
    }

    /* Open device */
    dev_handle = LPCUSBSIO_Open(0); /* For now, open the first device available. */
    if (dev_handle == NULL) {
        fprintf(stderr, "Failed to open device #0\n");
        return -2;
    }

    fprintf(stdout, "%s\n", LPCUSBSIO_GetVersion(dev_handle));

    ret = i2c_detect(dev_handle, 0, 0x00, 0x7F, I2C_CLOCK_STANDARD_MODE);
    if (ret < 0) {
        fprintf(stderr, "Failed to probe devices.\n");
    }

    LPCUSBSIO_Close(dev_handle);

    return ret;
}

static int i2c_detect(LPC_HANDLE handle, uint8_t port_num, uint8_t start_addr, uint8_t end_addr,
                      enum I2C_ClockRate_t rate) {
    fprintf(stdout, "WARNING! This program can confuse your I2C bus, cause data loss and worse!\n");
    fprintf(stdout,
            "WARNING! MCU-Link firmware under a specific version has a bug which\n"
            "         causes device to be unresponsive if this application exits abnormally.\n"
            "         In case that happens, don't panic, just re-plug the MCU-Link\n"
            "         and the device will be available again.\n");

    LPC_HANDLE       i2c_handle;
    I2C_PORTCONFIG_T i2c_cfg = {
        .ClockRate = rate,
        .Options   = 0U,
    };

    i2c_handle = I2C_Open(handle, &i2c_cfg, port_num);
    if (i2c_handle == NULL) {
        fprintf(stderr, "Failed to open I2C bus #%d\n", port_num);
        return -1;
    }

    fprintf(stdout, "I will probe port #%d.\n", port_num);
    fprintf(stdout, "I will probe address range 0x%02x-0x%02x.\n", start_addr, end_addr);
    fprintf(stdout, "Continue!\n");

    fprintf(stdout, "     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f\n");
    for (uint8_t i = 0; i < 0x80; i++) {
        if(!s_running) {
            fprintf(stderr, "\nInterrupted.\n");
            break;
        }

        if (i % 16 == 0) {
            fprintf(stdout, "%02x: ", i);
        }

        if (i < start_addr || i > end_addr) {
            fprintf(stdout, "   ");
        } else {
            /* Do actual probing here. */
            if (i2c_probe_addr(i2c_handle, i) > 0) {
                fprintf(stdout, "%02x ", i);
            } else {
                fprintf(stdout, "-- ");
            }
            fflush(stdout);
        }

        if (i % 16 == 15) {
            fprintf(stdout, "\n");
        }
    }

    I2C_Close(i2c_handle);
}

static int i2c_probe_addr(LPC_HANDLE i2c_handle, uint8_t addr) {
    uint8_t         rx_buf = 0U;
    I2C_FAST_XFER_T xfer   = {
          .slaveAddr = addr,
          .txSz      = 0U,
          .txBuff    = NULL,
          .rxSz      = 1U,
          .rxBuff    = &rx_buf,
          .options   = 0U,
    };

    int ret = I2C_FastXfer(i2c_handle, &xfer);
    if (ret < 0) I2C_Reset(i2c_handle);

    return ret;
}

static void signal_handler(int sig) { s_running = false; }