#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 "parameters.h"
#include "ahrs.h"
#include <fixstring.h>
#include "mag_calib.h"
#include "rf_task.h"
#include "powersave.h"

#ifdef SPHERE_RX
#include "speedsensor.h"
#include "control_task.h"
#include "motor.h"
#endif

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 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(BaseChannel *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);
    
    v3d acc, mag;
    sensor_get_acc(&acc);
    sensor_get_mag(&mag);
    
    chprintf(chp, "Calib. acc.: ");
    print_v3d((FILE*)chp, &acc);
    chprintf(chp, " norm ");
    print_fix16_t((FILE*)chp, v3d_norm(&acc), 5, 3);
    chprintf(chp, "\r\n");
    
    chprintf(chp, "Calib. mag.: ");
    print_v3d((FILE*)chp, &mag);
    chprintf(chp, " norm ");
    print_fix16_t((FILE*)chp, v3d_norm(&mag), 5, 3);
    chprintf(chp, "\r\n");
    
    chprintf(chp, "Current mag. calib: ");
    print_fix16_t((FILE*)chp, mag_calib.bias_x, 7, 1);
    print_fix16_t((FILE*)chp, mag_calib.bias_y, 7, 1);
    print_fix16_t((FILE*)chp, mag_calib.bias_z, 7, 1);
    print_fix16_t((FILE*)chp, mag_calib.scale_x, 7, 1);
    print_fix16_t((FILE*)chp, mag_calib.scale_y, 7, 1);
    print_fix16_t((FILE*)chp, mag_calib.scale_z, 7, 1);
    chprintf(chp, "\r\n");
    
#ifdef SPHERE_RX
    chprintf(chp, "Speed sensor: %d %d, %d steps, %d deg/sec\r\n",
             palReadPad(GPIOA, GPIOA_ENCODER_IN1),
             palReadPad(GPIOA, GPIOA_ENCODER_IN2),
             speedsensor_steps,
             fix16_to_int(fix16_mul(speedsensor_rad_per_sec, F16(57.2958)))
            );
#endif
}


static void cmd_sensor_dump(BaseChannel *chp, int argc, char *argv[]) {
    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 = chIOGetTimeout(chp, MS2ST(10));
    } while (b != Q_RESET && b != '\r');
}

static void cmd_stats(BaseChannel *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);
    
#ifdef SPHERE_RX
    chprintf(chp, "Radio RX packets:        %6d\r\n", rf_packets);
    chprintf(chp, "Radio RX errors:         %6d\r\n", rf_dropped_flags);
#endif
}


static void cmd_ahrs(BaseChannel *chp, int argc, char *argv[]) {
    qf16 r;
    ahrs_get_orientation(&r);
    
    chprintf(chp, "Quaternion orientation:");
    print_qf16((FILE*)chp, &r);
    chprintf(chp, "\r\n\r\n");
    
    chprintf(chp,     "                   east        north        up\r\n");
    
    {
        v3d a = {F16(1), 0, 0};
        qf16_rotate(&a, &r, &a);
        chprintf(chp, "Device X axis: ");
        print_v3d((FILE*)chp, &a);
        chprintf(chp, "\r\n");
    }
    
    {
        v3d a = {0, F16(1), 0};
        qf16_rotate(&a, &r, &a);
        chprintf(chp, "Device Y axis: ");
        print_v3d((FILE*)chp, &a);
        chprintf(chp, "\r\n");
    }
    
    {
        v3d a = {0, 0, F16(1)};
        qf16_rotate(&a, &r, &a);
        chprintf(chp, "Device Z axis: ");
        print_v3d((FILE*)chp, &a);
        chprintf(chp, "\r\n");
    }
    
#ifdef SPHERE_RX
    {
        qf16 q;
        control_get_rotation(&q);
        chprintf(chp, "\r\nShell rotation per 10 ms:");
        print_qf16((FILE*)chp, &q);
        chprintf(chp, "\r\n");
        
        v2d v;
        control_get_ground_velocity(&v);
        chprintf(chp, "Ground speed (m/s): ");
        print_v2d((FILE*)chp, &v);
        chprintf(chp, "\r\n");
    }
#endif
}

/*
static void cmd_ahrs_dump(BaseChannel *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');
}
*/

#ifdef SPHERE_RX
static void cmd_motor(BaseChannel *chp, int argc, char *argv[]) {
    if (argc == 2)
    {
        control_set_manual(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
    {
        control_set_manual(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_control(BaseChannel *chp, int argc, char *argv[]) {
    if (argc != 2)
    {
        chprintf(chp, "Usage: control <east> <north>\r\n");
        return;
    }
    
    v2d velocity = {
        fix16_from_str(argv[0]), fix16_from_str(argv[1])
    };
    
    control_set_target(&velocity);
}

static void cmd_odometer(BaseChannel *chp, int argc, char *argv[])
{
    chprintf(chp, "%d steps, %d meters\r\n", speedsensor_odometer,
             speedsensor_odometer / 80);
}
#endif

static void cmd_rf(BaseChannel *chp, int argc, char *argv[]) {
    v2d speed;
    rf_get_speed(&speed);
    
    chprintf(chp, "RF speed: ");
    print_v2d((FILE*)chp, &speed);
    chprintf(chp, "\r\n");
}

static void cmd_rf_dump(BaseChannel *chp, int argc, char *argv[]) {
    BaseChannel *ch = (BaseChannel*)chp;
    chprintf(chp, "# Timestamp   East   North \r\n");
    
    char b = 0;
    do {
        v2d speed;
        rf_get_speed(&speed);
        
        chprintf(chp, "%12d ", chTimeNow());
        print_v2d((FILE*)chp, &speed);
        chprintf(chp, "\r\n");
        b = chIOGetTimeout(ch, MS2ST(20));
    } while (b != Q_RESET && b != '\r');
}

static void cmd_parameters(BaseChannel *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(BaseChannel *chp, int argc, char *argv[])
{
    if (argc != 1)
    {
        chprintf(chp, "Usage: get <name>. Type 'parameters' for a list.\r\n");
        return;
    }
    
    char buf[13];
    if (parameter_get(argv[0], buf))
        chprintf(chp, "%s: %8s\r\n", argv[0], buf);
    else
        chprintf(chp, "No such parameter: %s\r\n", argv[0]);
}

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

static void cmd_halt(BaseChannel *chp, int argc, char *argv[])
{
    chprintf(chp, "Halting to test watchdog..\r\n");
    chThdSleepMilliseconds(100);
    chSysLock();
    while(1);
}

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

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

const ShellCommand shell_commands[] = {
  {"mem", cmd_mem},
  {"threads", cmd_threads},
  {"sensors", cmd_sensors},
  {"sensor_dump", cmd_sensor_dump},
  {"ahrs", cmd_ahrs},
  {"stats", cmd_stats},
  {"parameters", cmd_parameters},
  {"get", cmd_get},
  {"set", cmd_set},
  {"sleep", cmd_sleep},
  {"reboot", cmd_reboot},
  {"rf", cmd_rf},
  {"rf_dump", cmd_rf_dump},
  {"halt", cmd_halt},

#ifdef SPHERE_RX
  {"motor", cmd_motor},
  {"control", cmd_control},
  {"odometer", cmd_odometer},
#endif

  {NULL, NULL}
};