/*
 * canbus_uds.c
 * UDS (ISO 14229) diagnostic services implementation.
 *
 * Each function builds a UDS payload, sends via ISO-TP,
 * parses the response (positive = SID+0x40, negative = 0x7F+SID+NRC).
 * Handles NRC 0x78 (ResponsePending) with extended timeout.
 */
#include "sdkconfig.h"

#if defined(CONFIG_MODULE_CANBUS) && defined(CONFIG_CANBUS_UDS)

#include <string.h>
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "canbus_uds.h"
#include "canbus_isotp.h"
#include "canbus_driver.h"
#include "utils.h"

#define TAG "CAN_UDS"

/* Max retries for ResponsePending (NRC 0x78) */
#define MAX_PENDING_RETRIES  10
#define PENDING_TIMEOUT_MS   5000

/* ============================================================
 * Internal: UDS request with ResponsePending handling
 * ============================================================ */

static int uds_transact(uds_ctx_t *ctx,
                        const uint8_t *req, size_t req_len,
                        uint8_t *resp, size_t resp_cap, size_t *resp_len)
{
    int timeout = ctx->timeout_ms > 0 ? ctx->timeout_ms : 2000;

    for (int retry = 0; retry <= MAX_PENDING_RETRIES; retry++) {
        int t = (retry == 0) ? timeout : PENDING_TIMEOUT_MS;

        isotp_status_t st = isotp_request(
            ctx->tx_id, ctx->rx_id,
            req, req_len,
            resp, resp_cap, resp_len,
            t
        );

        if (st == ISOTP_TIMEOUT) {
            if (retry > 0) {
                ESP_LOGW(TAG, "ResponsePending timeout after %d retries", retry);
            }
            return -1;
        }
        if (st != ISOTP_OK) return -1;

        /* Check for Negative Response */
        if (*resp_len >= 3 && resp[0] == 0x7F) {
            uint8_t nrc = resp[2];

            /* ResponsePending — ECU needs more time */
            if (nrc == UDS_NRC_RESPONSE_PENDING) {
                ESP_LOGI(TAG, "ResponsePending from 0x%03lX (retry %d/%d)",
                         (unsigned long)ctx->rx_id, retry + 1, MAX_PENDING_RETRIES);
                /* Re-listen for the real response (no re-send needed) */
                *resp_len = 0;
                isotp_status_t st2 = isotp_recv(
                    ctx->rx_id, resp, resp_cap, resp_len, PENDING_TIMEOUT_MS
                );
                if (st2 == ISOTP_TIMEOUT) continue;  /* Try again */
                if (st2 != ISOTP_OK) return -1;

                /* Check if we got another NRC 0x78 or the real response */
                if (*resp_len >= 3 && resp[0] == 0x7F && resp[2] == UDS_NRC_RESPONSE_PENDING) {
                    continue;  /* Still pending */
                }
                /* Got the real response, fall through to return */
            }

            /* Other negative responses */
            if (resp[0] == 0x7F && resp[2] != UDS_NRC_RESPONSE_PENDING) {
                ESP_LOGW(TAG, "NRC 0x%02X (%s) for SID 0x%02X from 0x%03lX",
                         resp[2], uds_nrc_name(resp[2]), resp[1],
                         (unsigned long)ctx->rx_id);
                return -1;
            }
        }

        /* Positive response or parsed NRC */
        return (int)*resp_len;
    }

    return -1;
}

/* ============================================================
 * Public API
 * ============================================================ */

int uds_diagnostic_session(uds_ctx_t *ctx, uint8_t session_type)
{
    uint8_t req[2] = { UDS_DIAG_SESSION_CTRL, session_type };
    uint8_t resp[64];
    size_t resp_len = 0;

    int ret = uds_transact(ctx, req, 2, resp, sizeof(resp), &resp_len);
    if (ret < 0) return -1;

    /* Positive response: 0x50 + session type */
    if (resp_len >= 2 && resp[0] == (UDS_DIAG_SESSION_CTRL + 0x40)) {
        ctx->session = session_type;
        return 0;
    }
    return -1;
}

int uds_tester_present(uds_ctx_t *ctx)
{
    uint8_t req[2] = { UDS_TESTER_PRESENT, 0x00 };  /* subFunction = 0 */
    uint8_t resp[16];
    size_t resp_len = 0;

    int ret = uds_transact(ctx, req, 2, resp, sizeof(resp), &resp_len);
    if (ret < 0) return -1;

    if (resp_len >= 2 && resp[0] == (UDS_TESTER_PRESENT + 0x40)) {
        return 0;
    }
    return -1;
}

int uds_read_data_by_id(uds_ctx_t *ctx, uint16_t did,
                        uint8_t *out, size_t cap)
{
    uint8_t req[3] = {
        UDS_READ_DATA_BY_ID,
        (uint8_t)(did >> 8),
        (uint8_t)(did & 0xFF),
    };
    uint8_t resp[512];
    size_t resp_len = 0;

    int ret = uds_transact(ctx, req, 3, resp, sizeof(resp), &resp_len);
    if (ret < 0) return -1;

    /* Positive: 0x62 + DID (2 bytes) + data */
    if (resp_len >= 3 && resp[0] == (UDS_READ_DATA_BY_ID + 0x40)) {
        size_t data_len = resp_len - 3;
        if (data_len > cap) data_len = cap;
        memcpy(out, &resp[3], data_len);
        return (int)data_len;
    }
    return -1;
}

int uds_security_access_seed(uds_ctx_t *ctx, uint8_t level,
                             uint8_t *seed, size_t *seed_len)
{
    /* Seed request: odd subFunction (level = 0x01, 0x03, ...) */
    uint8_t req[2] = { UDS_SECURITY_ACCESS, level };
    uint8_t resp[64];
    size_t resp_len = 0;

    int ret = uds_transact(ctx, req, 2, resp, sizeof(resp), &resp_len);
    if (ret < 0) return -1;

    /* Positive: 0x67 + level + seed bytes */
    if (resp_len >= 2 && resp[0] == (UDS_SECURITY_ACCESS + 0x40)) {
        size_t slen = resp_len - 2;
        if (seed && seed_len) {
            memcpy(seed, &resp[2], slen);
            *seed_len = slen;
        }
        return 0;
    }
    return -1;
}

int uds_security_access_key(uds_ctx_t *ctx, uint8_t level,
                            const uint8_t *key, size_t key_len)
{
    /* Key send: even subFunction (level+1 = 0x02, 0x04, ...) */
    uint8_t req[34] = { UDS_SECURITY_ACCESS, (uint8_t)(level + 1) };
    if (key_len > 32) return -1;
    memcpy(&req[2], key, key_len);

    uint8_t resp[16];
    size_t resp_len = 0;

    int ret = uds_transact(ctx, req, 2 + key_len, resp, sizeof(resp), &resp_len);
    if (ret < 0) return -1;

    if (resp_len >= 2 && resp[0] == (UDS_SECURITY_ACCESS + 0x40)) {
        ctx->security_unlocked = true;
        return 0;
    }
    return -1;
}

int uds_read_memory(uds_ctx_t *ctx, uint32_t addr, uint16_t size,
                    uint8_t *out)
{
    /* addressAndLengthFormatIdentifier: 0x24 = 2 bytes size, 4 bytes addr */
    uint8_t req[7] = {
        UDS_READ_MEM_BY_ADDR,
        0x24,                              /* format: 2+4 */
        (uint8_t)(addr >> 24),
        (uint8_t)(addr >> 16),
        (uint8_t)(addr >> 8),
        (uint8_t)(addr),
        (uint8_t)(size >> 8),
    };
    /* Append size low byte */
    uint8_t req_full[8];
    memcpy(req_full, req, 7);
    req_full[7] = (uint8_t)(size & 0xFF);

    /* Wait — need proper format. Let's redo:
     * SID(1) + addressAndLengthFormatId(1) + memAddr(4) + memSize(2) = 8 bytes */
    uint8_t request[8] = {
        UDS_READ_MEM_BY_ADDR,
        0x24,
        (uint8_t)(addr >> 24),
        (uint8_t)(addr >> 16),
        (uint8_t)(addr >> 8),
        (uint8_t)(addr),
        (uint8_t)(size >> 8),
        (uint8_t)(size),
    };

    uint8_t resp[512];
    size_t resp_len = 0;

    int ret = uds_transact(ctx, request, 8, resp, sizeof(resp), &resp_len);
    if (ret < 0) return -1;

    /* Positive: 0x63 + data */
    if (resp_len >= 1 && resp[0] == (UDS_READ_MEM_BY_ADDR + 0x40)) {
        size_t data_len = resp_len - 1;
        if (out) memcpy(out, &resp[1], data_len);
        return (int)data_len;
    }
    return -1;
}

int uds_raw_request(uds_ctx_t *ctx,
                    const uint8_t *req, size_t req_len,
                    uint8_t *resp, size_t resp_cap, size_t *resp_len)
{
    return uds_transact(ctx, req, req_len, resp, resp_cap, resp_len);
}

/* ============================================================
 * ECU Discovery
 * ============================================================ */

int uds_scan_ecus(uint32_t *found_ids, int max_ecus)
{
    int found = 0;

    /* Standard UDS range: 0x7E0-0x7E7 (physical addressing) */
    for (uint32_t tx = 0x7E0; tx <= 0x7E7 && found < max_ecus; tx++) {
        uint32_t rx = tx + 0x08;  /* Response IDs: 0x7E8-0x7EF */

        uds_ctx_t ctx = {
            .tx_id = tx,
            .rx_id = rx,
            .timeout_ms = 200,  /* Short timeout for scan */
            .session = UDS_SESSION_DEFAULT,
            .security_unlocked = false,
        };

        if (uds_tester_present(&ctx) == 0) {
            ESP_LOGI(TAG, "ECU found: TX=0x%03lX RX=0x%03lX",
                     (unsigned long)tx, (unsigned long)rx);
            found_ids[found++] = tx;
        }
    }

    /* Extended range: 0x700-0x7DF */
    for (uint32_t tx = 0x700; tx <= 0x7DF && found < max_ecus; tx++) {
        /* Skip standard range (already scanned) */
        if (tx >= 0x7E0) break;

        uint32_t rx = tx + 0x08;

        uds_ctx_t ctx = {
            .tx_id = tx,
            .rx_id = rx,
            .timeout_ms = 100,
            .session = UDS_SESSION_DEFAULT,
            .security_unlocked = false,
        };

        if (uds_tester_present(&ctx) == 0) {
            ESP_LOGI(TAG, "ECU found: TX=0x%03lX RX=0x%03lX",
                     (unsigned long)tx, (unsigned long)rx);
            found_ids[found++] = tx;
        }

        /* Yield every 16 IDs to avoid watchdog */
        if ((tx & 0x0F) == 0x0F) {
            vTaskDelay(pdMS_TO_TICKS(1));
        }
    }

    return found;
}

/* ============================================================
 * NRC Name Lookup
 * ============================================================ */

const char *uds_nrc_name(uint8_t nrc)
{
    switch (nrc) {
        case 0x10: return "generalReject";
        case 0x11: return "serviceNotSupported";
        case 0x12: return "subFunctionNotSupported";
        case 0x13: return "incorrectMessageLength";
        case 0x14: return "responseTooLong";
        case 0x21: return "busyRepeatRequest";
        case 0x22: return "conditionsNotCorrect";
        case 0x24: return "requestSequenceError";
        case 0x25: return "noResponseFromSubnet";
        case 0x26: return "failurePreventsExecution";
        case 0x31: return "requestOutOfRange";
        case 0x33: return "securityAccessDenied";
        case 0x35: return "invalidKey";
        case 0x36: return "exceededNumberOfAttempts";
        case 0x37: return "requiredTimeDelayNotExpired";
        case 0x70: return "uploadDownloadNotAccepted";
        case 0x71: return "transferDataSuspended";
        case 0x72: return "generalProgrammingFailure";
        case 0x73: return "wrongBlockSequenceCounter";
        case 0x78: return "responsePending";
        case 0x7E: return "subFunctionNotSupportedInActiveSession";
        case 0x7F: return "serviceNotSupportedInActiveSession";
        default:   return "unknown";
    }
}

#endif /* CONFIG_MODULE_CANBUS && CONFIG_CANBUS_UDS */