#include <ch.h>
#include <hal.h>
#include <chprintf.h>
#include <string.h>
#include "shell_commands.h"
#include "motor.h"

static int _atoi(const char *str)
{
    int result = 0;
    while (*str == ' ') str++;
    while (*str)
    {
        if (*str < '0' || *str > '9')
            break;
        
        result *= 10;
        result += *str++ - '0';
    }
    return result;
}

static void cmd_mem(BaseChannel *chp, int argc, char *argv[]) {
    size_t n, size;

    n = chHeapStatus(NULL, &size);
    chprintf(chp, "core free memory : %u bytes\r\n", chCoreStatus());
    chprintf(chp, "heap fragments   : %u\r\n", n);
    chprintf(chp, "heap free total  : %u bytes\r\n", size);
}

extern unsigned long __main_thread_stack_base__, __main_thread_stack_end__; // From linker script
static void thread_free_stack(Thread *thread, int *free_stack_now, int *free_stack_min)
{
    uint32_t current_sp = (uint32_t)thread->p_ctx.r13;
    uint32_t stack_bottom;
    if (current_sp >= (uint32_t)&__main_thread_stack_base__
        && current_sp <= (uint32_t)&__main_thread_stack_end__)
        stack_bottom = (uint32_t)&__main_thread_stack_base__;
    else
        stack_bottom = (uint32_t)(thread + 1);
    
    *free_stack_now = current_sp - stack_bottom;
    
    uint32_t *stackentry = (uint32_t*)stack_bottom;
    uint32_t empty_val = *stackentry;
    while (*stackentry == empty_val) stackentry++;
    *free_stack_min = (uint32_t)stackentry - stack_bottom;
}

static void cmd_threads(BaseChannel *chp, int argc, char *argv[]) {
    static const char *states[] = {THD_STATE_NAMES};
    Thread *tp;

    chprintf(chp, "             addr    stack prio     state time      free stack now/min\r\n");
    tp = chRegFirstThread();
    do {
        int stacknow, stackmin;
        thread_free_stack(tp, &stacknow, &stackmin);
        
        chprintf(chp, "%8s %.8lx %.8lx %4lu %9s %8lu %lu/%lu\r\n",
            tp->p_name, (uint32_t)tp, (uint32_t)tp->p_ctx.r13,
            (uint32_t)tp->p_prio,
            states[tp->p_state], (uint32_t)tp->p_time,
            stacknow, stackmin);
        tp = chRegNextThread(tp);
    } while (tp != NULL);
}

static void cmd_reboot(BaseChannel *chp, int argc, char *argv[])
{
    SCB->AIRCR = 0x05FA0000 | SCB_AIRCR_SYSRESETREQ;
}

static void cmd_angle(BaseChannel *chp, int argc, char *argv[])
{
    int angle = _atoi(argv[0]);
    
    motor_goto(angle);
    chprintf(chp, "New angle: %d\r\n", angle);
}

static void cmd_poweroff(BaseChannel *chp, int argc, char *argv[])
{
    motor_poweroff();
}

static void cmd_spin(BaseChannel *chp, int argc, char *argv[])
{
    int step = _atoi(argv[0]);
    int delay = _atoi(argv[1]);
    chprintf(chp, "Spinning %d degrees per %d ms.\r\n", step, delay);
    chprintf(chp, "Press enter to stop.\r\n");
    
    int angle = motor_getangle();
    char b;
    do {
        angle += step;
        motor_goto(angle);
        
        chThdSleepMilliseconds(delay);
        
        b = chIOGetTimeout(chp, MS2ST(10));
    } while (b != Q_RESET && b != '\r');
    
    motor_poweroff();
}

static void cmd_spinup(BaseChannel *chp, int argc, char *argv[])
{
    int speed = _atoi(argv[0]);
    chprintf(chp, "Spinning up to %d RPM.\r\n", speed);
    chprintf(chp, "Press enter to stop.\r\n");
    
    int mindelay = 60000 / (12 * speed);
    
    int angle = 0;
    int delay = 50;
    char b;
    int j = 0;
    do {
        angle = (angle + 30) % 360;
        motor_goto(angle);
        chThdSleepMilliseconds(delay);
        j += delay;
        
        if (j > 100 && delay > mindelay)
        {
            delay--;
            j = 0;
        }
        
        b = chIOGetTimeout(chp, 0);
    } while (b != Q_RESET && b != '\r');
    
    motor_poweroff();
}

static void cmd_pulse(BaseChannel *chp, int argc, char *argv[])
{
    int p1 = _atoi(argv[0]);
    int p2 = _atoi(argv[1]);
    int p3 = _atoi(argv[2]);
    int t = _atoi(argv[3]);
    
    motor_set_phases(p1, p2, p3);
    chThdSleepMilliseconds(t);
    motor_set_phases(0,0,0);
}

const ShellCommand shell_commands[] = {
  {"mem", cmd_mem},
  {"threads", cmd_threads},
  {"reboot", cmd_reboot},
  {"angle", cmd_angle},
  {"poweroff", cmd_poweroff},
  {"spin", cmd_spin},
  {"spinup", cmd_spinup},
  {"pulse", cmd_pulse},
  {NULL, NULL}
};