#include <Arduino.h>
#include <WiFi.h>

#include "utils.h"
#include "main.h"
#include "bafang.h"
#include "commands.h"

namespace Commands {

static CommandEntry *s_pCurrentCommand = NULL;

int parseLine(char *line)
{
    const int MAX_ARGV = 16;
    char *lineStart = line;
    char *argv[MAX_ARGV];
    int argc = 0;
    bool found = false;
    bool end = false;
    bool inString = false;

    while(1)
    {
		if(*line == '"' && (line == lineStart || *(line - 1) != '\\'))
		{
			inString ^= true;
			if(!inString)
			{
                found = true;
				end = true;
				lineStart++;
			}
		}

        if(!inString && *line == ' ')
            end = true;

        if(end || !*line)
        {
            if(found && argc < MAX_ARGV)
                argv[argc++] = lineStart;

            found = false;
            end = false;

            if(!*line)
                break;

            *line = 0;
            lineStart = ++line;
            continue;
        }

        line++;
        found = true;
    }

    if(!argc)
        return -1;

    for(int i = 0; g_Commands[i].cmd; i++)
    {
        if(strcmp(g_Commands[i].cmd, argv[0]) == 0)
        {
            s_pCurrentCommand = &g_Commands[i];
            int ret = g_Commands[i].fun(argc, argv);
            s_pCurrentCommand = NULL;
            return ret;
        }
    }

    return -1;
}

void printUsage()
{
    if(!s_pCurrentCommand)
        return;
    Main::channel()->print("Usage: ");
    Main::channel()->println(s_pCurrentCommand->help);
}

int CMD_help(int argc, char **argv)
{
    Main::channel()->println("Available Commands\n------------------");

    int i = 0;
    while(g_Commands[i].cmd)
    {
        Main::channel()->printf("%17s %s\n", g_Commands[i].cmd, g_Commands[i].help);
        i++;
    }
    Main::channel()->println();

    return 0;
}

int CMD_debug(int argc, char **argv)
{
    if(argc == 1) {
        Serial.printf("Channel %d -> Debug: %d\n", Main::g_CurrentChannel,
            Main::g_Debug[Main::g_CurrentChannel]);
        return 0;
    }

    if(argc != 2) {
        printUsage();
        return 1;
    }

    Main::g_Debug[Main::g_CurrentChannel] = strtoul(argv[1], NULL, 10);
    return 0;
}

int CMD_wifi(int argc, char **argv)
{
    if(argc == 1)
    {
        Main::channel()->printf("connected: %d\n", WiFi.status());
        Main::channel()->println(WiFi.localIP());
        if(WiFi.status() == WL_CONNECTED)
            WiFi.printDiag(*Main::channel());
        return 0;
    }

    if(argc != 2) {
        printUsage();
        return 1;
    }

    if(strtoul(argv[1], NULL, 10))
        Main::toggleWiFi(1);
    else
        Main::toggleWiFi(0);

    return 0;
}

int CMD_bluetooth(int argc, char **argv)
{
    if(argc == 1)
    {
        Main::channel()->printf("enabled: %d\n", btStarted());
        return 0;
    }

    if(argc != 2) {
        printUsage();
        return 1;
    }

    if(strtoul(argv[1], NULL, 10))
        Main::toggleBluetooth(1);
    else
        Main::toggleBluetooth(0);

    return 0;
}

int CMD_proxy(int argc, char **argv)
{
    if(argc != 2) {
        printUsage();
        return 1;
    }

    bool proxy = false;
    if(strtoul(argv[1], NULL, 10))
        proxy = true;

    Bafang::g_Proxy = proxy;

    return 0;
}

int CMD_motor(int argc, char **argv)
{
    if(argc < 2 || argc > 3) {
        printUsage();
        return 1;
    }

    uint8_t bytes[255];
    int len = hex2bytes(argv[1], bytes, sizeof(bytes));

    uint16_t waitMs = 500;
    if(argc >= 3)
        waitMs = strtoul(argv[2], NULL, 10);

    Bafang::QueueMotor(bytes, len, Bafang::PrintCallback, (void *)Main::g_CurrentChannel, waitMs);

    return 0;
}

int CMD_profile(int argc, char **argv)
{
    if(argc != 2) {
        printUsage();
        return 1;
    }

    int profile = strtoul(argv[1], NULL, 10);
    Main::motorProfile(profile);

    return 0;
}

CommandEntry g_Commands[] =
{
    {"help",        CMD_help,       " : Display list of commands"},
    {"debug",       CMD_debug,      " : debug [0-9999]"},
    {"wifi",        CMD_wifi,       " : wifi [0-1]"},
    {"bluetooth",   CMD_bluetooth,  " : bluetooth [0-1]"},
    {"proxy",       CMD_proxy,      " : proxy <0|1>"},
    {"motor",       CMD_motor,      " : motor <hex> [waitMs]"},
    {"profile",     CMD_profile,    " : profile <0-1>"},
    { 0, 0, 0 }
};

}