//#define LOG_LOCAL_LEVEL ESP_LOG_DEBUG

#include <stddef.h>
#include <stdint.h>
#include <sys/queue.h>
#include <malloc.h>
#include <stdbool.h>
#include <esp_log.h>
#include <linenoise/linenoise.h>
#include <esp_system.h>
#include "telnet_parser.h"
#include "console_server.h"

static const char *TAG = "telnet";

// The console is all single-threaded, using a circular buffer, so we can get away
// with a global state here

enum telnet_code {
    CODE_SE = 240,
    CODE_NOP = 241,
    CODE_DATA_MARK = 242,
    CODE_BRK = 243, // break signal - kick all clients (including yourself)
    CODE_IP = 244, // interrupt process (reboots ESP)
    CODE_AO = 245, // abort output
    CODE_AYT = 246, // are you there? (sends an ASCII text reply)
    CODE_ERASE_CHAR = 247, // these are obsolete
    CODE_ERASE_LINE = 248,
    CODE_GO_AHEAD = 249, // not used, always negotiate to SGA
    CODE_SB = 250,
    CODE_WILL = 251,
    CODE_WONT = 252,
    CODE_DO = 253,
    CODE_DONT = 254,
    CODE_IAC = 255
};

enum parser_state {
    // next character is expected to be text or IAC
    PS_TEXT = 0,
    // received IAC, next character should be a verb
    PS_IAC,
    PS_WILL,
    PS_WONT,
    PS_DO,
    PS_DONT,
    PS_SUBNEG,
    PS_SUBNEG_BODY,
    PS_SUBNEG_BODY_IAC
};

struct awaiting_reply_item;

struct awaiting_reply_item {
    enum telnet_code code;
    enum telnet_option option;
    SLIST_ENTRY(awaiting_reply_item) next;
};

SLIST_HEAD(awaiting_reply_list, awaiting_reply_item);

#define SUBNEG_BUF_LEN 16
static struct telnet_state {
    enum parser_state pstate;
    enum telnet_option subneg_option;
    struct awaiting_reply_list awaiting_reply;
    uint8_t subneg_buf[SUBNEG_BUF_LEN];
    uint8_t subneg_buf_i;
    bool mode_rx_binary;
    bool mode_tx_binary;
} s_ts = { /* all zeros */ };

static void _expect_reply(enum telnet_code code, enum telnet_option option)
{
    ESP_LOGV(TAG, "Awaiting reply to %d:0x%02x", code, option);
    struct awaiting_reply_item *item = calloc(sizeof(struct awaiting_reply_item), 1);
    item->code = code;
    item->option = option;
    SLIST_INSERT_HEAD(&s_ts.awaiting_reply, item, next);
}

static void send_will(console_ctx_t *cctx, enum telnet_option opt, bool is_response)
{
    ESP_LOGD(TAG, "Send WILL 0x%02x", opt);
    if (!is_response) _expect_reply(CODE_WILL, opt);
    const uint8_t buf[3] = {CODE_IAC, CODE_WILL, opt};
    console_write_ctx(cctx, (const char *) buf, 3);
}

static void send_wont(console_ctx_t *cctx, enum telnet_option opt, bool is_response)
{
    ESP_LOGD(TAG, "Send WON'T 0x%02x", opt);
    if (!is_response) _expect_reply(CODE_WONT, opt);
    const uint8_t buf[3] = {CODE_IAC, CODE_WONT, opt};
    console_write_ctx(cctx, (const char *) buf, 3);
}

static void send_do(console_ctx_t *cctx, enum telnet_option opt, bool is_response)
{
    ESP_LOGD(TAG, "Send DO 0x%02x", opt);
    if (!is_response) _expect_reply(CODE_DO, opt);
    const uint8_t buf[3] = {CODE_IAC, CODE_DO, opt};
    console_write_ctx(cctx, (const char *) buf, 3);
}

static void send_dont(console_ctx_t *cctx, enum telnet_option opt, bool is_response)
{
    ESP_LOGD(TAG, "Send DON'T 0x%02x", opt);
    if (!is_response) _expect_reply(CODE_DONT, opt);
    const uint8_t buf[3] = {CODE_IAC, CODE_DONT, opt};
    console_write_ctx(cctx, (const char *) buf, 3);
}

static void send_subnegotiate(console_ctx_t *cctx, enum telnet_option opt, const uint8_t *data, size_t len, bool is_response)
{
    if (!is_response) _expect_reply(CODE_SB, opt);
    ESP_LOGD(TAG, "Send SUBNEG 0x%02x", opt);

    const uint8_t hdr[] = {CODE_IAC, CODE_SB, opt};
    console_write_ctx(cctx, (const char *) hdr, 3);

    // a heroic attempt at doing the double-IAC encoding without malloc
    const uint8_t *last_chunk = data;
    const uint8_t a_iac = CODE_IAC;
    for (int i = 0; i < len; i++) {
        if (data[i] == CODE_IAC) {
            if (last_chunk != &data[i]) {
                console_write_ctx(cctx, (const char *) last_chunk, &data[i] - last_chunk);
                console_write_ctx(cctx, (const char *) &a_iac, 1);
                last_chunk = &data[i+1];
            }
        }
    }

    if (last_chunk != &data[len]) {
        console_write_ctx(cctx, (const char *) last_chunk, &data[len] - last_chunk);
    }

    const uint8_t foot[] = {CODE_IAC, CODE_SE};
    console_write_ctx(cctx, (const char *) foot, 2);
}

void telnet_send_subnegotiate(console_ctx_t *cctx, enum telnet_option opt, const uint8_t *data, size_t len)
{
    send_subnegotiate(cctx, opt, data, len, false);
}

void telnet_send_will(console_ctx_t *cctx, enum telnet_option opt)
{
    send_will(cctx, opt, false);
}

void telnet_send_wont(console_ctx_t *cctx, enum telnet_option opt)
{
    send_wont(cctx, opt, false);
}

void telnet_send_do(console_ctx_t *cctx, enum telnet_option opt)
{
    send_do(cctx, opt, false);
}

void telnet_send_dont(console_ctx_t *cctx, enum telnet_option opt)
{
    send_dont(cctx, opt, false);
}

/**
 * Peer agreed to DO something,
 * or announces it WILL do something
 */
static void handle_will(console_ctx_t *cctx, enum telnet_option opt)
{
    struct awaiting_reply_item *it;
    SLIST_FOREACH(it, &s_ts.awaiting_reply, next) {
        if (it->option == opt) {
            if (it->code == CODE_DO) {
                ESP_LOGD(TAG, "Peer agreed to DO 0x%02x", opt);
            } else if (it->code == CODE_DONT) {
                ESP_LOGW(TAG, "Peer refused to NOT DO 0x%02x", opt);
                // TODO deal with it
            } else {
                continue; // leave this entry alone, it's not related
            }
            SLIST_REMOVE(&s_ts.awaiting_reply, it, awaiting_reply_item, next);
            free(it);
            return;
        }
    }

    // unexpected WILL - peer offers to enable an option
    switch (opt) {
        case OPT_BINARY:
            send_do(cctx, opt, true);
            s_ts.mode_rx_binary = true;
            break;
        case OPT_ECHO:
            send_dont(cctx, opt, true);
            // that's a bad idea, don't do it
            break;
        case OPT_SUPPRESS_GO_AHEAD:
            send_do(cctx, opt, true);
            break;

        default:
            ESP_LOGD(TAG, "unknown option for WILL: 0x%02x", opt);
            // please don't, we don't understand what it is
            send_dont(cctx, opt, true);
    }
}

/**
 * Peer refused to DO something,
 * or announces it WON'T do something
 */
static void handle_wont(console_ctx_t *cctx, enum telnet_option opt)
{
    struct awaiting_reply_item *it;
    SLIST_FOREACH(it, &s_ts.awaiting_reply, next) {
        if (it->option == opt) {
            if (it->code == CODE_DO) {
                ESP_LOGW(TAG, "Peer refused to DO 0x%02x", opt);
                // TODO deal with it
            } else if (it->code == CODE_DONT) {
                ESP_LOGD(TAG, "Peer agreed to NOT DO 0x%02x", opt);
            } else {
                continue; // leave this entry alone, it's not related
            }
            SLIST_REMOVE(&s_ts.awaiting_reply, it, awaiting_reply_item, next);
            free(it);
            return;
        }
    }

    // unexpected WON'T - peer offers to disable an option
    switch (opt) {
        case OPT_BINARY:
            send_dont(cctx, opt, true);
            s_ts.mode_rx_binary = false;
            break;
        case OPT_ECHO:
            send_dont(cctx, opt, true);
            // OK, we don't care
            break;
        case OPT_SUPPRESS_GO_AHEAD:
            // We want SGA, GA is some legacy nonsense nobody uses
            send_do(cctx, opt, true);
            break;

        // TODO handle supported options
        default:
            ESP_LOGD(TAG, "unknown option for WON'T: 0x%02x", opt);
            // yeah sure, don't do it, whatever
            send_dont(cctx, opt, true);
    }
}

/**
 * Peer requests we DO something,
 * or agrees to something we announced with WILL
 */
static void handle_do(console_ctx_t *cctx, enum telnet_option opt)
{
    struct awaiting_reply_item *it;
    SLIST_FOREACH(it, &s_ts.awaiting_reply, next) {
        if (it->option == opt) {
            if (it->code == CODE_WONT) {
                ESP_LOGW(TAG, "Peer rejected that we WON'T 0x%02x", opt);
                // TODO deal with it
            } else if (it->code == CODE_WILL) {
                ESP_LOGD(TAG, "Peer accepted that we WILL 0x%02x", opt);
            } else {
                continue; // leave this entry alone, it's not related
            }
            SLIST_REMOVE(&s_ts.awaiting_reply, it, awaiting_reply_item, next);
            free(it);
            return;
        }
    }

    // unexpected DO - peer requests we do something
    switch (opt) {
        case OPT_BINARY:
            send_will(cctx, opt, true);
            s_ts.mode_tx_binary = true;
            break;
        case OPT_ECHO:
            // FIXME
            send_wont(cctx, opt, true);
//            send_will(cctx, opt, true);
//            linenoiseSetEchoMode(1);
            break;
        case OPT_SUPPRESS_GO_AHEAD:
            send_will(cctx, opt, true);
            break;
        case OPT_STATUS:
            send_will(cctx, opt, true);
            break;
        case OPT_TIMING_MARK:
            send_will(cctx, opt, true);
            break;

        // TODO handle supported options
        default:
            ESP_LOGD(TAG, "unknown option for DO: 0x%02x", opt);
            // we don't know how to do that, so we won't
            send_wont(cctx, opt, true);
    }
}

/**
 * Peer requests we DON'T do something,
 * or agrees we shouldn't do something we announced for with WON'T
 */
static void handle_dont(console_ctx_t *cctx, enum telnet_option opt)
{
    struct awaiting_reply_item *it;
    SLIST_FOREACH(it, &s_ts.awaiting_reply, next) {
        if (it->option == opt) {
            if (it->code == CODE_WILL) {
                ESP_LOGW(TAG, "Peer rejected that we WILL 0x%02x", opt);
                // TODO deal with it

                switch (opt) {
                    case OPT_ECHO:
                        // ok, let's not echo then
                        // FIXME
//                        linenoiseSetEchoMode(0);
                        break;
                    default:
                        ESP_LOGW(TAG, "Rejection unhandled");
                }
            } else if (it->code == CODE_WONT) {
                ESP_LOGD(TAG, "Peer accepted that we WON'T 0x%02x", opt);
            } else {
                continue; // leave this entry alone, it's not related
            }
            SLIST_REMOVE(&s_ts.awaiting_reply, it, awaiting_reply_item, next);
            free(it);
            return;
        }
    }

    // unexpected DON'T - peer requests we don't do something
    switch (opt) {
        case OPT_BINARY:
            send_wont(cctx, opt, true);
            s_ts.mode_tx_binary = false;
            break;
        case OPT_ECHO:
            send_wont(cctx, opt, true);
            // FIXME
//            linenoiseSetEchoMode(0);
            break;
        case OPT_SUPPRESS_GO_AHEAD:
            // no, we absolutely won't use GA
            send_will(cctx, opt, true);
            break;

            // TODO handle supported options
        default:
            ESP_LOGD(TAG, "unknown option for DON'T: 0x%02x", opt);
            // false is probably the default, so we won't do that, ok...
            send_wont(cctx, opt, true);
    }
}

/**
 * subnegotiation command (ts.subneg_option) was received and is now stored in
 * ts.subneg_buf[0 .. ts.subneg_buf_i]. Handle it accordingly
 */
static void handle_subnegotiate(void)
{
    struct awaiting_reply_item *it;
    SLIST_FOREACH(it, &s_ts.awaiting_reply, next) {
        if (it->code == CODE_SB && it->option == s_ts.subneg_option) {
            // this is an expected reply, delete the await token
            SLIST_REMOVE(&s_ts.awaiting_reply, it, awaiting_reply_item, next);
            ESP_LOGD(TAG, "got reply to SB 0x%02x", s_ts.subneg_option);

            // TODO now we would handle the received data
            free(it);
            return;
        }
    }

    // it was not a reply to our request, i.e. the client is asking for something

    ESP_LOGW(TAG, "recv unsupported subneg request 0x%02x", s_ts.subneg_option);

    // TODO handle some requests..
}

size_t telnet_middleware_read(console_ctx_t *cctx, uint8_t *buffer, size_t len)
{
    ESP_LOGV(TAG, "-- Telnet handle %d chars --", len);
    ESP_LOG_BUFFER_HEX_LEVEL(TAG, buffer, (uint16_t) len, ESP_LOG_VERBOSE);

    uint8_t *wp = buffer;
    for (size_t i = 0; i < len; i++) {
        uint8_t c = buffer[i];

        if (c >= 32 && c < 127) {
            ESP_LOGV(TAG, "char %d (%c)", c, c);
        } else {
            ESP_LOGV(TAG, "char %d", c);
        }

        switch (s_ts.pstate) {
            case PS_TEXT:
                if (c == CODE_IAC) {
                    ESP_LOGV(TAG, "TEXT->IAC");
                    s_ts.pstate = PS_IAC;
                } else {
                    ESP_LOGV(TAG, "TEXT(%c)", c);
                    *wp++ = c;
                }
                break;
            case PS_IAC:
                switch (c) {
                    case CODE_IAC:
                        // double IAC means literal IAC (but this shouldn't normally happen)
                        *wp++ = c;
                        s_ts.pstate = PS_TEXT;
                        ESP_LOGV(TAG, "Double IAC in TEXT");
                        break;
                    case CODE_WILL:
                        ESP_LOGV(TAG, "IAC->WILL");
                        s_ts.pstate = PS_WILL;
                        break;
                    case CODE_WONT:
                        ESP_LOGV(TAG, "IAC->WONT");
                        s_ts.pstate = PS_WONT;
                        break;
                    case CODE_DO:
                        ESP_LOGV(TAG, "IAC->DO");
                        s_ts.pstate = PS_DO;
                        break;
                    case CODE_DONT:
                        ESP_LOGV(TAG, "IAC->DONT");
                        s_ts.pstate = PS_DONT;
                        break;
                    case CODE_SB:
                        ESP_LOGV(TAG, "IAC->SUBNEG");
                        s_ts.pstate = PS_SUBNEG;
                        break;
                    case CODE_AYT:
                        ESP_LOGV(TAG, "IAC->AYT");
                        s_ts.pstate = PS_TEXT;
                        const char *reply = "\r\nI am here.\r\n";
                        console_write_ctx(cctx, reply, strlen(reply));
                        break;
                    case CODE_IP:
                        ESP_LOGV(TAG, "IAC->IP, reboot");
                        esp_restart();
                        break;
                    case CODE_BRK:
                        ESP_LOGV(TAG, "IAC->BRK, kick clients");
                        telnetsrv_kick_all();
                        break;
                    default:
                        ESP_LOGW(TAG, "Unexpected code IAC+%d, discard!", c);
                        // Unecpected character, discard and go to base state
                        s_ts.pstate = PS_TEXT;
                }
                break;
            case PS_WILL:
                handle_will(cctx, c);
                s_ts.pstate = PS_TEXT;
                ESP_LOGV(TAG, "WILL->TEXT");
                break;
            case PS_WONT:
                handle_wont(cctx, c);
                s_ts.pstate = PS_TEXT;
                ESP_LOGV(TAG, "WONT->TEXT");
                break;
            case PS_DO:
                handle_do(cctx, c);
                s_ts.pstate = PS_TEXT;
                ESP_LOGV(TAG, "DO->TEXT");
                break;
            case PS_DONT:
                handle_dont(cctx, c);
                s_ts.pstate = PS_TEXT;
                ESP_LOGV(TAG, "DONT->TEXT");
                break;
            case PS_SUBNEG:
                s_ts.subneg_option = c;
                s_ts.pstate = PS_SUBNEG_BODY;
                s_ts.subneg_buf_i = 0;
                ESP_LOGV(TAG, "SUBNEG->SUBNEG_BODY, opt %d", c);
                break;
            case PS_SUBNEG_BODY:
                if (c == CODE_IAC) {
                    ESP_LOGV(TAG, "IAC in subneg body");
                    s_ts.pstate = PS_SUBNEG_BODY_IAC;
                } else {
                    if (s_ts.subneg_buf_i < SUBNEG_BUF_LEN) {
                        ESP_LOGV(TAG, "subneg += %d", c);
                        s_ts.subneg_buf[s_ts.subneg_buf_i++] = c;
                    } else {
                        ESP_LOGV(TAG, "subneg too long!");
                    }
                }
                break;
            case PS_SUBNEG_BODY_IAC:
                switch (c) {
                    case CODE_IAC:
                        ESP_LOGV(TAG, "Literal IAC in subneg");
                        // double IAC means literal IAC code
                        s_ts.subneg_buf[s_ts.subneg_buf_i++] = CODE_IAC;
                        s_ts.pstate = PS_SUBNEG_BODY;
                        break;
                    case CODE_SE:
                        ESP_LOGV(TAG, "Subneg END");
                        // end of subneg block
                        if (s_ts.subneg_buf_i == SUBNEG_BUF_LEN) {
                            ESP_LOGW(TAG, "Subneg buf OV, discard command SB %d!", s_ts.subneg_option);
                        } else {
                            ESP_LOGV(TAG, "Subneg OK! Handling...");
                            handle_subnegotiate();
                        }
                        s_ts.pstate = PS_TEXT;
                        break;
                    default:
                        ESP_LOGV(TAG, "Illegal char in subneg IAC+%d, discard", c);
                        // illegal character, act like the subneg block was closed, but discard the content
                        s_ts.pstate = PS_TEXT;
                }
                break;
        }
    }

    size_t remains = wp - buffer;

    ESP_LOGV(TAG, "Remain %d cleartext chars", remains);
    ESP_LOGV(TAG, "> %*.s", remains, buffer);
    return remains;
}