#include <ch.h>
#include <hal.h>
#include <chprintf.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include "shell_commands.h"
#include "mma7660_acc.h"
#include "stm32l_rtc.h"
#include "stm32l_adc.h"
#include "sensor_task.h"
#include "stats.h"

static void cmd_mem(BaseSequentialStream *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(BaseSequentialStream *chp, int argc, char *argv[]) {
    static const char *states[] = {THD_STATE_NAMES};
    Thread *tp;

    chprintf(chp, "             addr      LR    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 %.8lx %4lu %9s %8lu %lu/%lu\r\n",
            tp->p_name, (uint32_t)tp, (uint32_t)tp->p_ctx.r13->lr, (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(BaseSequentialStream *chp, int argc, char *argv[])
{
    SCB->AIRCR = 0x05FA0000 | SCB_AIRCR_SYSRESETREQ;
}

static void cmd_leds(BaseSequentialStream *chp, int argc, char *argv[])
{
    for (int j = 0; j < atoi(argv[0]); j++)
    {
        for (int i = 0; i < 3; i++)
        {
            palClearPad(GPIOA, GPIOA_LEDS);
            chThdSleepMilliseconds(10);
            palSetPad(GPIOA, GPIOA_LEDS);
            chThdSleepMilliseconds(40);
        }
        
        chThdSleepMilliseconds(500);
    }
}

static void cmd_acc(BaseSequentialStream *chp, int argc, char *argv[])
{
    if (!mma7660_poweron(MMA7660_SR_120))
        chprintf(chp, "MMA7660 poweron failed\n");
    
    chThdSleepMilliseconds(30);
    
    int x, y, z;
    if (!mma7660_read(&x, &y, &z))
        chprintf(chp, "MMA7660 read failed\n");
    
    chprintf(chp, "X=%5d, Y=%5d, Z=%5d\n", x, y, z);
    
    mma7660_poweroff();
}

static void cmd_rtc(BaseSequentialStream *chp, int argc, char *argv[])
{
    if (argc == 2)
    {
        time_t time = {};
        time.year = atoi(argv[0] + 0);
        time.month = atoi(argv[0] + 5);
        time.day = atoi(argv[0] + 8);
        time.hour = atoi(argv[1] + 0);
        time.minute = atoi(argv[1] + 3);
        time.second = atoi(argv[1] + 6);
        set_rtc(&time);
    }
    else
    {
        chprintf(chp, "Use   rtc YYYY-MM-DD HH:MM:SS   to set time.\n");
        chprintf(chp, "Note: No daylight saving time.\n\n");
    }
    
    {
        time_t time;
        if (!get_rtc(&time))
        {
            chprintf(chp, "RTC is not set.\n");
        }
        else
        {
            static const char* days[] = {"Mon", "Tue", "Wed", "Thu", "Fri", "Sat", "Sun"};
            chprintf(chp, "%s %04d-%02d-%02d %02d:%02d:%02d\n",
                     days[time.day_of_week - 1],
                     time.year, time.month, time.day,
                     time.hour, time.minute, time.second);
            
            chprintf(chp, "Sun is %s.\n", is_sun_up(&time) ? "up" : "down");
        }
    }
}

static void cmd_sleep(BaseSequentialStream *chp, int argc, char *argv[])
{
    if (argc != 1)
    {
        chprintf(chp, "Usage: sleep <milliseconds>\n");
        return;
    }
    
    rtc_sleep(atoi(argv[0]));
}

static void cmd_adc(BaseSequentialStream *chp, int argc, char *argv[])
{
    adc_on();
    chprintf(chp, "Battery voltage: %d mV\n", adc_vbat());
    chprintf(chp, "Temperature: %d mC\n", adc_temperature());
    adc_off();
}

static void cmd_sensor(BaseSequentialStream *chp, int argc, char *argv[])
{
    if (argc == 0)
    {
        chprintf(chp, "Usage: sensor {start|stop|read}\n");
        return;
    }
    
    if (strcmp(argv[0], "start") == 0)
    {
        sensors_start();
    }
    else if (strcmp(argv[0], "stop") == 0)
    {
        sensors_stop();
    }
    else
    {
        chprintf(chp, "Movement estimate:    %5d %%\n", sensors_movement());
        chprintf(chp, "Movement raw:         %5d mg\n", sensors_raw_movement());
        chprintf(chp, "Temperature estimate: %5d %%\n", sensors_temperature());
        chprintf(chp, "Temperature raw:      %5d mC\n", sensors_raw_temperature());
    }
}

static void cmd_temperature(BaseSequentialStream *chp, int argc, char *argv[])
{
    int samplecount = 60;
    if (argc >= 1)
    {
        samplecount = atoi(argv[0]);
    }
    
    int sampleinterval = 1000;
    if (argc >= 2)
    {
        sampleinterval = atoi(argv[1]);
    }
    
    static int16_t samples[128];
    if (samplecount > 128) samplecount = 128;
    
    chprintf(chp, "Sample count = %d, sample interval = %d ms\n", samplecount, sampleinterval);
    
    adc_on();
    for (int i = 0; i < samplecount; i++)
    {
        samples[i] = adc_temperature();
        chThdSleepMilliseconds(sampleinterval);
        chprintf(chp, "%d / %d\n", i, samplecount);
    }
    adc_off();
    
    for (int i = 0; i < samplecount; i++)
    {
        chprintf(chp, "%d\n", samples[i]);
    }
}

static void cmd_stats(BaseSequentialStream *chp, int argc, char *argv[])
{
    if (argc == 1 && strcmp(argv[0], "save") == 0)
    {
        stats_save();
    }
    else if (argc == 1 && strcmp(argv[0], "reset") == 0)
    {
        stats_reset();
        stats_save();
    }
    else if (argc == 1 && strcmp(argv[0], "show") == 0)
    {
        stats_print(chp);
    }
    else
    {
        chprintf(chp, "Usage: stats <save|reset|show>\n");
    }
}

const ShellCommand shell_commands[] = {
  {"mem", cmd_mem},
  {"threads", cmd_threads},
  {"reboot", cmd_reboot},
  {"leds", cmd_leds},
  {"acc", cmd_acc},
  {"rtc", cmd_rtc},
  {"sleep", cmd_sleep},
  {"adc", cmd_adc},
  {"sensor", cmd_sensor},
  {"temperature", cmd_temperature},
  {"stats", cmd_stats},
  {NULL, NULL}
};