#include <ch.h>
#include <hal.h>
#include "sphere_shell.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <chprintf.h>
#include "sensor_task.h"
#include "ahrs_task.h"
#include "motor.h"
#include "rf_task.h"
#include "control.h"
#include "parameters.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    stack prio refs     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 %4lu %9s %8lu %lu/%lu\r\n",
            tp->p_name, (uint32_t)tp, (uint32_t)tp->p_ctx.r13,
            (uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),
            states[tp->p_state], (uint32_t)tp->p_time,
            stacknow, stackmin);
        tp = chRegNextThread(tp);
    } while (tp != NULL);
}

static void cmd_sensors(BaseSequentialStream *chp, int argc, char *argv[]) {
    chprintf(chp, "Magnetometer:  %5d %5d %5d\r\n", mag_x, mag_y, mag_z);
    chprintf(chp, "Accelerometer: %5d %5d %5d\r\n", acc_x, acc_y, acc_z);
    
    fix16_t fx, fy, fz, norm;
    get_calibrated_mag(&fx, &fy, &fz, &norm);
    chprintf(chp, "Calib. mag.:   %5d %5d %5d        (norm = %5d)\r\n",
             fix16_to_int(fx * 1000), fix16_to_int(fy * 1000), fix16_to_int(fz * 1000), fix16_to_int(norm * 1000));
    
    get_calibrated_acc(&fx, &fy, &fz, &norm);
    chprintf(chp, "Calib. acc.:   %5d %5d %5d        (norm = %5d)\r\n",
             fix16_to_int(fx * 1000), fix16_to_int(fy * 1000), fix16_to_int(fz * 1000), fix16_to_int(norm * 1000));
}


static void cmd_sensor_dump(BaseSequentialStream *chp, int argc, char *argv[]) {
    BaseChannel *ch = (BaseChannel*)chp;
    chprintf(chp, "# Timestamp    MagX,  MagY,  MagZ,  AccX,  AccY,  AccZ\r\n");
    
    char b = 0;
    do
    {
        chprintf(chp, "%12d %6d %6d %6d %6d %6d %6d\r\n",
                 chTimeNow(), mag_x, mag_y, mag_z, acc_x, acc_y, acc_z);
        b = chnGetTimeout(ch, MS2ST(10));
    } while (b != Q_RESET && b != '\r');
}

static void cmd_stats(BaseSequentialStream *chp, int argc, char *argv[]) {
    unsigned time = (chTimeNow() - sensor_start_time) / 1000;
    
    chprintf(chp, "Sensor readings:         %6d   (%3d/s)\r\n",
             sensor_readings, sensor_readings / time);
    chprintf(chp, "Magnetometer errors:     %6d   (%3d/s)\r\n",
             mag_errors, mag_errors / time);
    chprintf(chp, "Accelerometer errors:    %6d   (%3d/s)\r\n",
             acc_errors, acc_errors / time);
    
    time = (chTimeNow() - ahrs_start_time) / 1000;
    chprintf(chp, "Kalman updates:          %6d   (%3d/s)\r\n",
             ahrs_updates, ahrs_updates / time);
    
    chprintf(chp, "RF packets:              %6d\r\n", rf_packets);
    chprintf(chp, "RF flag fails:           %6d\r\n", rf_dropped_flags);
}

static void cmd_ahrs(BaseSequentialStream *chp, int argc, char *argv[]) {
    chSysLock();
    int h = fix16_to_int(ahrs_heading);
    int r = fix16_to_int(ahrs_roll);
    int p = fix16_to_int(ahrs_pitch);
    int i = fix16_to_int(ahrs_inclination);
    chSysUnlock();
    
    chprintf(chp, "Heading: %5d\r\n", h);
    chprintf(chp, "Roll: %5d\r\n", r);
    chprintf(chp, "Pitch: %5d\r\n", p);
    chprintf(chp, "Magnetic inclination: %5d\r\n", i);
}

static void cmd_ahrs_dump(BaseSequentialStream *chp, int argc, char *argv[]) {
    BaseChannel *ch = (BaseChannel*)chp;
    chprintf(chp, "# Timestamp   Heading, Roll, Pitch, Incl\r\n");
    
    char b = 0;
    do
    {
        chSysLock();
        int h = fix16_to_int(ahrs_heading);
        int r = fix16_to_int(ahrs_roll);
        int p = fix16_to_int(ahrs_pitch);
        int i = fix16_to_int(ahrs_inclination);
        chSysUnlock();
        
        chprintf(chp, "%12d %6d %6d %6d %6d\r\n",
                 chTimeNow(), h, r, p, i);
        b = chnGetTimeout(ch, MS2ST(10));
    } while (b != Q_RESET && b != '\r');
}

static void cmd_motor(BaseSequentialStream *chp, int argc, char *argv[]) {
    if (argc == 2)
    {
        manual_ctrl = true;
        set_motor_speed(fix16_div(atoi(argv[0]), 1000));
        set_servo_position(fix16_div(atoi(argv[1]), 1000));
        set_servo_power(true);
        chprintf(chp, "Switch to automatic control with 'motor'\r\n");
    }
    else
    {
        manual_ctrl = false;
        chprintf(chp, "Switch to manual control with 'motor <speed> <servo>'\r\n");
    }
    
    chprintf(chp, "Motor speed: %4d\r\n", fix16_mul(motor_speed, 1000));
    chprintf(chp, "Servo position: %4d\r\n", fix16_mul(servo_position, 1000));
}

static void cmd_rf(BaseSequentialStream *chp, int argc, char *argv[]) {
    chprintf(chp, "RF heading: %4d\r\n", rf_heading);
    chprintf(chp, "RF speed: %4d\r\n", rf_speed);
}

static void cmd_control_dump(BaseSequentialStream *chp, int argc, char *argv[]) {
    BaseChannel *ch = (BaseChannel*)chp;
    chprintf(chp, "# Timestamp   Heading, Roll, Pitch, RFHead, RFSpeed, ServoPos, MotorSpeed\r\n");
    
    char b = 0;
    do {
        chSysLock();
        int h = fix16_to_int(ahrs_heading);
        int r = fix16_to_int(ahrs_roll);
        int p = fix16_to_int(ahrs_pitch);
        chSysUnlock();
        
        chprintf(chp, "%12d %6d %6d %6d %6d %6d %6d %6d\r\n",
                 chTimeNow(), h, r, p,
                 rf_heading, rf_speed,
                 servo_position, motor_speed
                );
        b = chnGetTimeout(ch, MS2ST(10));
    } while (b != Q_RESET && b != '\r');
}

static void cmd_pid_reset(BaseSequentialStream *chp, int argc, char *argv[])
{
    pitch_ctrl.current_integral = 0;
    heading_ctrl.current_integral = 0;
}

static void mag_avg(fix16_t avgs[3])
{
    avgs[0] = avgs[1] = avgs[2] = 0;
    
    int count = 64;
    for (int i = 0; i < count; i++)
    {
        avgs[0] += mag_x * (fix16_one / count);
        avgs[1] += mag_y * (fix16_one / count);
        avgs[2] += mag_z * (fix16_one / count);
        chThdSleepMilliseconds(10);
    }
}

static void cmd_servo_mag_bias(BaseSequentialStream *chp, int argc, char *argv[])
{
    chprintf(chp, "Measuring magnetic field caused by servo\r\n");
    
    manual_ctrl = true;
    set_servo_power(true);
    set_motor_speed(0);
    set_servo_position(fix16_one);
    chThdSleepMilliseconds(500);
    
    fix16_t ref[3];
    mag_avg(ref);
    
    chprintf(chp, "# Servo_pos  X    Y    Z\r\n");
    for (int pos = fix16_one; pos >= -fix16_one; pos -= fix16_one/20)
    {
        set_servo_position(pos);
        chThdSleepMilliseconds(500);
        fix16_t meas[3];
        mag_avg(meas);
        
        chprintf(chp, "%6d %6d %6d %6d\r\n", pos,
                 fix16_to_int(meas[0] - ref[0]),
                 fix16_to_int(meas[1] - ref[1]),
                 fix16_to_int(meas[2] - ref[2]));
    }
}

static void cmd_parameters(BaseSequentialStream *chp, int argc, char *argv[])
{
    bool save = false, load = false, backup = false;
    
    if (argc == 1)
    {
        save = (strcmp(argv[0], "save") == 0);
        load = (strcmp(argv[0], "load") == 0);
        backup = (strcmp(argv[0], "backup") == 0);
    }
    
    if (save)
    {
        parameters_save();
    }
    else if (load)
    {
        if (!parameters_load())
        {
            chprintf(chp, "Load failed, invalid signature.\r\n");
        }
    }
    else if (backup)
    {
        parameters_print(chp, true);
    }
    else
    {
        chprintf(chp, "Usage: parameters [load|save|backup]\r\n");
        
        if (argc == 0)
        {
            parameters_print(chp, false);
        }
    }
}

static void cmd_get(BaseSequentialStream *chp, int argc, char *argv[])
{
    if (argc != 1)
    {
        chprintf(chp, "Usage: get <name>. Type 'parameters' for a list.\r\n");
        return;
    }
    
    int32_t value;
    if (parameter_get(argv[0], &value))
        chprintf(chp, "%s: %6d\r\n", argv[0], value);
    else
        chprintf(chp, "No such parameter: %s\r\n", argv[0]);
}

static void cmd_set(BaseSequentialStream *chp, int argc, char *argv[])
{
    if (argc != 2)
    {
        chprintf(chp, "Usage: set <name> <value>. Type 'parameters' for a list.\r\n");
        return;
    }
    
    int32_t value = atoi(argv[1]);
    if (parameter_set(argv[0], value))
        chprintf(chp, "%s: %6d\r\n", argv[0], value);
    else
        chprintf(chp, "No such parameter: %s\r\n", argv[0]);
}

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

const ShellCommand shell_commands[] = {
  {"mem", cmd_mem},
  {"threads", cmd_threads},
  {"sensors", cmd_sensors},
  {"sensor_dump", cmd_sensor_dump},
  {"stats", cmd_stats},
  {"ahrs", cmd_ahrs},
  {"ahrs_dump", cmd_ahrs_dump},
  {"motor", cmd_motor},
  {"rf", cmd_rf},
  {"control_dump", cmd_control_dump},
  {"pid_reset", cmd_pid_reset},
  {"servo_mag_bias", cmd_servo_mag_bias},
  {"parameters", cmd_parameters},
  {"get", cmd_get},
  {"set", cmd_set},
  {"reboot", cmd_reboot},
  {NULL, NULL}
};