#include <stdint.h>
#include <kernel/dpl/DebugP.h>
#include <kernel/dpl/ClockP.h>
#include <kernel/dpl/HwiP.h>
#include <drivers/gpio.h>
#include <drivers/hw_include/cslr_gpio.h>

#include "ti_drivers_open_close.h"
#include "ti_board_open_close.h"
#include "ti_drivers_config.h"

/* Use SysConfig macros if available, otherwise fallback to constants */
#ifndef CONFIG_GPIO119_BASE_ADDR
#error "CONFIG_GPIO119_BASE_ADDR must be generated by SysConfig"
#endif

uint32_t gGpioBaseAddr = CONFIG_GPIO119_BASE_ADDR;
uint32_t gGpioPinNum   = CONFIG_GPIO119_PIN;
uint32_t gGpioPinIntrNum = CSLR_R5FSS0_CORE0_INTR_GPIO_INTRXBAR_OUT_14; //142

/* XBAR output mapping chosen in SysConfig:
   You routed GPIO_MUX_119 -> GPIO_INT_XBAR_VIM_MODULE0_0 so this is the
   matching R5FSS0 core interrupt macro (example: OUT_14). */
#define ENC_GPIO_INTR_NUM  (CSLR_R5FSS0_CORE0_INTR_GPIO_INTRXBAR_OUT_14)

static HwiP_Object gGpioHwiObject;

/* Encoder pins: change only if different hardware mapping */
#define ENC_A_GPIO_BASE_ADDR   (CSL_GPIO0_U_BASE)
#define ENC_A_PIN              (119U)
#define ENC_B_GPIO_BASE_ADDR   (CSL_GPIO0_U_BASE)
#define ENC_B_PIN              (120U)

#define ENCODER_COUNTS_PER_REV (100U)

static volatile int32_t  g_encoderPos    = 0;
static volatile int32_t  g_lastDirection = 0;
static volatile uint32_t g_stepCount     = 0;

/* ISR that updates encoder state */
void ENC_A_isr(void *args)
{
    (void)args;
    uint32_t b = GPIO_pinRead(ENC_B_GPIO_BASE_ADDR, ENC_B_PIN);

    if (b == GPIO_PIN_LOW) {
        g_encoderPos++;
        g_lastDirection = +1;
    } else {
        g_encoderPos--;
        g_lastDirection = -1;
    }

    g_stepCount++;

    /* Clear per-pin intr status if your driver requires it (safe to call) */
    /* GPIO_clearPinIntrStatus(ENC_A_GPIO_BASE_ADDR, ENC_A_PIN); */
}

/* Hwi wrapper */
static void GPIO_pinIsrFxn(void *args)
{
    DebugP_log("Interrupt Called.\r\n");

    ENC_A_isr(args);
}


void encoder_gpio_interrupt_init(void)
{
    HwiP_Params hwiPrms;
    int32_t retVal;
    uint32_t pinNum = ENC_A_PIN;  /* 119 */
    uint32_t bankNum;           
    bankNum = GPIO_GET_BANK_INDEX(pinNum);

    DebugP_log("Init: base=0x%08x pin=%u bank=%u\r\n",
               (unsigned)ENC_A_GPIO_BASE_ADDR, (unsigned)pinNum, (unsigned)bankNum);

    /* make pin an input (harmless if already done) */
    GPIO_setDirMode(ENC_A_GPIO_BASE_ADDR, pinNum, GPIO_DIRECTION_INPUT);

    /* Set both-edges so we don't miss polarity mis-match while debugging */
    GPIO_setTrigType(ENC_A_GPIO_BASE_ADDR, pinNum, GPIO_TRIG_TYPE_RISE_EDGE);

    /* Clear any pending pin status if API exists */
#ifdef GPIO_clearPinIntrStatus
    GPIO_clearPinIntrStatus(ENC_A_GPIO_BASE_ADDR, pinNum);
    DebugP_log("Cleared per-pin intr status.\r\n");
#endif

    /* Enable per-pin intr if driver provides it */
#ifdef GPIO_pinIntrEnable
    GPIO_pinIntrEnable(ENC_A_GPIO_BASE_ADDR, pinNum);
    DebugP_log("Called GPIO_pinIntrEnable()\r\n");
#elif defined(GPIO_pinIntEnable)
    GPIO_pinIntEnable(ENC_A_GPIO_BASE_ADDR, pinNum);
    DebugP_log("Called GPIO_pinIntEnable()\r\n");
#else
    DebugP_log("No per-pin enable API found; relying on bank enable.\r\n");
#endif

    /* Ensure bank interrupt is enabled */
    GPIO_bankIntrEnable(ENC_A_GPIO_BASE_ADDR, bankNum);
    DebugP_log("Enabled bank interrupt.\r\n");

    /* Hwi config (use the XBAR->VIM macro you selected in SysConfig) */
    HwiP_Params_init(&hwiPrms);
    hwiPrms.intNum   = ENC_GPIO_INTR_NUM;        /* must match SysConfig XBAR -> VIM */
    hwiPrms.callback = &GPIO_pinIsrFxn;
    hwiPrms.args     = NULL;
    hwiPrms.isPulse  = FALSE;

    retVal = HwiP_construct(&gGpioHwiObject, &hwiPrms);
    DebugP_log("HwiP_construct returned %ld (SystemP_SUCCESS=%ld)\r\n",
               (long)retVal, (long)SystemP_SUCCESS);
    if (retVal == SystemP_SUCCESS) {
        DebugP_log("HwiP constructed OK: intNum=%u\r\n", (unsigned)ENC_GPIO_INTR_NUM);
    } else {
        DebugP_log("HwiP construct FAILED: code=%ld\r\n", (long)retVal);
    }
}

/* Main logging loop */
void encoder_demo_main(void *args)
{
    (void)args;
    DebugP_log("Encoder demo starting...\r\n");

    uint64_t lastTimeUsec = ClockP_getTimeUsec();
    int32_t  lastPos      = g_encoderPos;
    int32_t count = 0;

    while (1) {
        ClockP_usleep(1000000U);

        uint64_t nowUsec = ClockP_getTimeUsec();
        uint64_t dtUsec  = nowUsec - lastTimeUsec;

        int32_t pos  = g_encoderPos;
        int32_t dPos = pos - lastPos;
        count++;

        float countsPerSec = 0.0f;
        float rpm          = 0.0f;

        if (dtUsec > 0 && dPos != 0) {
            countsPerSec = (float)dPos * (1000000.0f / (float)dtUsec);
            rpm = (countsPerSec / (float)ENCODER_COUNTS_PER_REV) * 60.0f;
        }

        uint32_t a = GPIO_pinRead(ENC_B_GPIO_BASE_ADDR, ENC_B_PIN);
        uint32_t b = GPIO_pinRead(ENC_B_GPIO_BASE_ADDR, ENC_B_PIN);
        int32_t v_a = 0;
        int32_t v_b = 0;

        if (a == GPIO_PIN_LOW)
        {
            v_a = -1;
        }
        else {
            v_a = 1;
        }

        if (b == GPIO_PIN_LOW)
        {
            v_b = -1;
        }
        else {
            v_b = 1;
        }

        DebugP_log("Enc: pos=%d, dir=%d, dPos=%d, cps=%.2f, rpm=%.2f, steps=%u, voltage_A=%d, voltage_B=%d, pass=%d\r \n",
                   (int)pos, (int)g_lastDirection, (int)dPos, countsPerSec, rpm, (unsigned)g_stepCount, (int)v_a, (int)v_b, (int)count);

        lastTimeUsec = nowUsec;
        lastPos      = pos;
    }
}