/* main.c
 *
 *
 * this is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * this is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
 */

#include <stdio.h>
#include "board.h"
#include "net/ipv6/addr.h"
#include "net/gnrc.h"
#include "net/gnrc/netif.h"
#include "shell.h"
#include "periph/flashpage.h"
#include "thread.h"
#include "periph/uart.h"
#include "periph/gpio.h"
#include "xtimer.h"
#include "ringbuffer.h"
#include "roomba.h"
#include "song.h"
#include "mutex.h"
#include "msg.h"

#ifndef SHELL_BUFSIZE
#define SHELL_BUFSIZE       (128U)
#endif

#define DISPATCHER_PRIO        (THREAD_PRIORITY_MAIN - 1)
#define BT_PRIO                (THREAD_PRIORITY_MAIN - 1)

#define MSGQ_SIZE (4)

#define GPIO_WAKEUP  GPIO_PIN(1,11)

#define SENSOR_BUFFER_SIZE (sizeof(struct sensors_code1) + sizeof(struct sensors_code2) + \
                sizeof(struct sensors_code3))

#define MSG_SENSORS_OFF       (0x4000)
#define MSG_SENSORS_ON        (0x4100)
#define MSG_SENSORS_AVAILABLE (0x4200)
#define MSG_SENSORS_TIMEOUT   (0x4300)

const char roomba_modstring[6][6] = {
    "OFF", "DRIVE", "FULL", "CLEAN", "SPOT", "DOCK"
};

extern const uint16_t song0[], song1[], song2[], song3[], song4[], song5[];
// Types
typedef struct {
    char rx_mem[SENSOR_BUFFER_SIZE];
    ringbuffer_t rx_buf;
} uart_ctx_t;

// Globals
//
static mutex_t roomba_mutex = MUTEX_INIT;
static uart_ctx_t ctx[UART_NUMOF];
static kernel_pid_t order66_pid, sensors_pid, gatt_srv_pid;
static char order66_stack[THREAD_STACKSIZE_MAIN];
static char sensors_stack[THREAD_STACKSIZE_MAIN];
static char gatt_srv_stack[THREAD_STACKSIZE_MAIN];
static msg_t sensors_msgq[MSGQ_SIZE];
static msg_t gatt_srv_msgq[MSGQ_SIZE];
static xtimer_t sensors_msg_timer;
const uint16_t *songs[] = { song0, song1, song2, song3, song4, song5 };
const uint32_t part_len[] = { 5000000, 5000000, 3200000, 2500000, 4200000, 4200000 };

// Prototypes
void *order66(void *arg);
void *sensors_loop(void *arg);
static void roomba_cmd(uint8_t cmd, uint8_t *payload, uint8_t len);
void roomba_led(uint8_t status, uint8_t spot, uint8_t clean, uint8_t max, uint8_t power_c, uint8_t power_i);

int song_len(const uint16_t *s)
{
    int l = (s[0] & 0xFF00) >> 8;
    return l;
}

uint32_t load_song(int n)
{
    uint8_t cmd = ROOMBA_SONG;
    int l = song_len(songs[n]);
    int i;
    mutex_lock(&roomba_mutex);
    uart_t dev = UART_DEV(0);
    uart_write(dev, &cmd, 1);
    uart_write(dev, (const uint8_t *)songs[n], 2 + (l << 1)); 
    mutex_unlock(&roomba_mutex);
    xtimer_usleep(100000);
    return part_len[n];
}


// Order 66
void *order66(void *arg)
{
    uint8_t x = 0;
    uint32_t st;
    st = load_song(0);
    roomba_cmd(ROOMBA_PLAY, &x, 1);
    xtimer_usleep(st);
    st = load_song(1);
    xtimer_usleep(20000);
    roomba_cmd(ROOMBA_PLAY, &x, 1);
    xtimer_usleep(st);
    st = load_song(2);
    xtimer_usleep(20000);
    roomba_cmd(ROOMBA_PLAY, &x, 1);
    xtimer_usleep(st);
    st = load_song(3);
    xtimer_usleep(20000);
    roomba_cmd(ROOMBA_PLAY, &x, 1);
    xtimer_usleep(st);
    st = load_song(4);
    xtimer_usleep(20000);
    roomba_cmd(ROOMBA_PLAY, &x, 1);
    xtimer_usleep(st);
    st = load_song(2);
    xtimer_usleep(20000);
    roomba_cmd(ROOMBA_PLAY, &x, 1);
    xtimer_usleep(st);
    st = load_song(3);
    xtimer_usleep(20000);
    roomba_cmd(ROOMBA_PLAY, &x, 1);
    xtimer_usleep(st);
    st = load_song(5);
    xtimer_usleep(20000);
    roomba_cmd(ROOMBA_PLAY, &x, 1);
    xtimer_usleep(st);
    return NULL;
}

static int roomba_mode = ROOMBA_MODE_OFF;

static void roomba_stop(void)
{
    uint8_t cmd = ROOMBA_POWER;
    uart_t dev = UART_DEV(0);
    mutex_lock(&roomba_mutex);
    uart_write(dev, &cmd, 1);
    mutex_unlock(&roomba_mutex);
    xtimer_usleep(200000);
}

static void roomba_start(void)
{
    gpio_clear(GPIO_WAKEUP);
    xtimer_usleep(200000);
    gpio_set(GPIO_WAKEUP);
    uint8_t cmd = ROOMBA_START;
    uart_t dev = UART_DEV(0);
    mutex_lock(&roomba_mutex);
    uart_write(dev, &cmd, 1);
    xtimer_usleep(20000);
    mutex_unlock(&roomba_mutex);
}

static void roomba_safe_to_full(void)
{
    uint8_t cmd = ROOMBA_FULL;
    uart_t dev = UART_DEV(0);
    mutex_lock(&roomba_mutex);
    uart_write(dev, &cmd, 1);
    xtimer_usleep(20000);
    mutex_unlock(&roomba_mutex);
}

static void roomba_full_to_safe(void)
{
    uint8_t cmd = ROOMBA_SAFE;
    uart_t dev = UART_DEV(0);
    mutex_lock(&roomba_mutex);
    uart_write(dev, &cmd, 1);
    xtimer_usleep(20000);
    mutex_unlock(&roomba_mutex);
}

static void roomba_cmd(uint8_t cmd, uint8_t *payload, uint8_t len)
{
    uart_t dev = UART_DEV(0);
    mutex_lock(&roomba_mutex);
    uart_write(dev, &cmd, 1);
    if(len && payload)
        uart_write(dev, payload, len);
    mutex_unlock(&roomba_mutex);
}

int switch_mode(int newmode)
{
    msg_t msg;
    int oldmode = roomba_mode;
    if (roomba_mode != newmode) {
        if (roomba_mode == ROOMBA_MODE_OFF) {
            roomba_start();
            roomba_mode = ROOMBA_MODE_DRIVE;
            roomba_led(0x03, 0, 0, 0, 0x80, 0x80);
        }
        switch(newmode) {
            case ROOMBA_MODE_OFF:
                roomba_stop();
                roomba_led(0x00, 0, 0, 0, 0xF0, 0x20);
                break;
            case ROOMBA_MODE_DRIVE:
                if (roomba_mode == newmode)
                    break;
                else if (roomba_mode == ROOMBA_MODE_FULL)
                    roomba_full_to_safe();
                else {
                    roomba_stop();
                    roomba_start();
                }
                roomba_led(0x03, 0, 0, 0, 0x80, 0x80);
                break;
            case ROOMBA_MODE_FULL:
                if (roomba_mode == ROOMBA_MODE_DRIVE)
                    roomba_safe_to_full();
                else {
                    roomba_stop();
                    roomba_start();
                    roomba_safe_to_full();
                }
                roomba_led(0x01, 0, 0, 0, 0xF0, 0xFF);
                break;
            case ROOMBA_MODE_CLEAN:
                roomba_cmd(ROOMBA_CLEAN, NULL, 0);
                break;
            case ROOMBA_MODE_SPOT:
                roomba_cmd(ROOMBA_SPOT, NULL, 0);
                break;
            case ROOMBA_MODE_DOCK:
                roomba_cmd(ROOMBA_DOCK, NULL, 0);
                break;
            case 66:
                order66_pid = thread_create(order66_stack, sizeof(order66_stack),
                                DISPATCHER_PRIO, 0, order66, NULL, "ORDER66");
                newmode = roomba_mode;
                break;
        }
        printf("Mode: %s\n", roomba_modstring[newmode]); 
        roomba_mode = newmode;
        if (roomba_mode == ROOMBA_MODE_FULL) {
            msg.type = MSG_SENSORS_ON;
            msg_send(&msg, sensors_pid);
        }
        if (oldmode == ROOMBA_MODE_FULL) {
            msg.type = MSG_SENSORS_OFF;
            msg_send(&msg, sensors_pid);
        }
    }
    return 0;
}

uint8_t get_mode(void)
{
    return roomba_mode;
}

static int cmd_info(int argc, char **argv)
{
    (void)argc;
    (void)argv;
    puts("iRobot ROOMBA 500 powered by RIOT-OS");
    printf("You are running RIOT on %s.\n", RIOT_BOARD);
    printf("This board features a %s MCU.\n", RIOT_MCU);
    puts("\nUART INFO:");
    printf("Available devices:               %i\n", UART_NUMOF);
    if (STDIO_UART_DEV != UART_UNDEF) {
        printf("UART used for STDIO (the shell): UART_DEV(%i)\n\n", STDIO_UART_DEV);
    }

    return 0;
}


int cmd_mode(int argc, char **argv)
{
    int i;
    if (argc < 2) {
        printf("usage: %s <data (string)>\n", argv[0]);
        return 1;
    }
    for (i = 0; i < 6; i++) {
        if (strcmp(argv[1], roomba_modstring[i]) == 0) {
            switch_mode(i);
            return 0;
        }
    }
    if (strcmp(argv[1], "ORDER66") == 0) {
        order66_pid = thread_create(order66_stack, sizeof(order66_stack),
                DISPATCHER_PRIO, 0, order66, NULL, "ORDER66");
        return 0;
    }
    printf("Mode '%s' unknown\r\n", argv[1]);
    return 1;
}

static int cmd_play(int argc, char **argv)
{
    uint8_t x = 0, z = 0;
    if ((argc != 2) || (strlen(argv[1]) != 1))
    {
        printf("usage: %s <song number>\n", argv[0]);
        return 1;
    }
    x = argv[1][0] - '0';
    load_song(x);
    roomba_cmd(ROOMBA_PLAY, &z, 1);
    return 0;
}


#define ALLSENSORS_CODE (0)
#define ALLSENSORS_SIZE (26)
#define QUICKSENSORS_CODE (1)
#define QUICKSENSORS_SIZE (10)


static volatile uint8_t sensors_expect_rx = 0;
static volatile uint8_t sensors_data_rx = 0;
static uint8_t raw_sensors_data[ALLSENSORS_SIZE];

static void invalidate_sensors(void)
{
    msg_t msg;
    memset(&raw_sensors_data, 0, sizeof(struct roomba_sensors_data));
    msg.type = MSG_SENSORS_AVAILABLE;
    msg_send(&msg, gatt_srv_pid);
}

static int read_sensors(uint8_t mode)
{
    uint8_t cmd[2] = {ROOMBA_SENSORS, 0};
    int len = -1;
    msg_t msg;
    uart_t dev = UART_DEV(0);
    msg_t msg_timeout;
    msg_timeout.type = MSG_SENSORS_TIMEOUT;

    if (mode == ALLSENSORS_CODE)
        sensors_expect_rx = ALLSENSORS_SIZE;
    else
        sensors_expect_rx = QUICKSENSORS_SIZE;

    cmd[1] = mode;
    mutex_lock(&roomba_mutex);
    sensors_data_rx = 0;
    uart_write(dev, cmd, 1);
    uart_write(dev, cmd + 1, 1);
    xtimer_set_msg(&sensors_msg_timer, 500000, &msg_timeout, sensors_pid);
    if (msg_receive(&msg) >= 0) {
        if (msg.type == MSG_SENSORS_AVAILABLE) {
            len = sensors_expect_rx;
        } else if (msg.type == MSG_SENSORS_TIMEOUT) {
            printf("Timeout while reading sensors\n");
            invalidate_sensors();
        } else if (msg.type == MSG_SENSORS_OFF) {
            printf("Paused sensor task.\n");
            invalidate_sensors();
        } else {
            printf("Invalid message type %04x\n", msg.type);
            invalidate_sensors();
        }
        sensors_expect_rx = 0;
    }
    mutex_unlock(&roomba_mutex);       
    return len;
}


// Rx callback
static void rx_cb(void *arg, uint8_t data)
{
    uart_t dev = (uart_t)arg;
    msg_t msg;
    //printf("RX: %02x\n", data);
    if (sensors_expect_rx > sensors_data_rx)
        raw_sensors_data[sensors_data_rx++] = data;
    if (sensors_expect_rx == sensors_data_rx)
    {
        msg.type = MSG_SENSORS_AVAILABLE;
        msg_send(&msg, sensors_pid);
    }
}

static struct roomba_sensors_data roomba_sensors_data = { };

void print_sensors(void)
{
    struct roomba_sensors_data *r = &roomba_sensors_data;
    printf("=== Sensors ===\n");
    printf("Valid : %s\n", r->valid?"yes":"no");
    if (r->valid) {
        printf("Drops : C:%d L:%d R:%d\n", r->drop_caster, r->drop_left, r->drop_right);
        printf("Bumps : L: %d R: %d\n", r->bump_left, r->bump_right);
        printf("Cliffs:    FL:%d    FR:%d   \n", r->cliff_frontleft, r->cliff_frontright);
        printf("       L:%d              R:%d   \n", r->cliff_left, r->cliff_right);
        printf("OVC: %02x\n", r->ovc);
    }
    printf("==============\n\n");
}


void *sensors_loop(void *arg)
{
    int ret = 0;
    msg_t msg;
    struct roomba_sensors_data tmp;
    while (1) { 
        if (get_mode() != ROOMBA_MODE_FULL) {
            while (1) {
                if ((msg_receive(&msg) >= 0) && (msg.type == MSG_SENSORS_ON)) {
                    printf("Sensors thread: resume.\n");
                    break;
                }
            }
        }
        xtimer_usleep(250000);
        ret = read_sensors(ALLSENSORS_CODE);
        if (ret < 0) {
            xtimer_usleep(1000000);
        } else {
            /* Byte 0: Wheeldrops + Bumps */
            tmp.drop_caster = !!(raw_sensors_data[0] & (1 << SENSE_DROP_CASTER));
            tmp.drop_left = !!(raw_sensors_data[0] & (1 << SENSE_DROP_LEFT));
            tmp.drop_right = !!(raw_sensors_data[0] & (1 << SENSE_DROP_RIGHT));
            tmp.bump_left = !!(raw_sensors_data[0] & (1 << SENSE_BUMP_LEFT));
            tmp.bump_right = !!(raw_sensors_data[0] & (1 << SENSE_BUMP_RIGHT));
            /* Byte 1: Wall */
            tmp.wall = raw_sensors_data[1];
            /* Byte 2: Cliff L */
            tmp.cliff_left = raw_sensors_data[2];
            /* Byte 3: Cliff FL */
            tmp.cliff_frontleft = raw_sensors_data[3];
            /* Byte 4: Cliff FR */
            tmp.cliff_frontright = raw_sensors_data[4];
            /* Byte 5: Cliff R */
            tmp.cliff_right = raw_sensors_data[5];
            /* Byte 7: Overcurrents */
            tmp.ovc = raw_sensors_data[7];
            
            /* 'valid' flag */
            tmp.valid = 1;
        }
        if (memcmp(&roomba_sensors_data, &tmp, sizeof(struct roomba_sensors_data)) != 0) {
            msg_t msg;
            memcpy(&roomba_sensors_data, &tmp, sizeof(struct roomba_sensors_data));
            print_sensors();
            msg.type = MSG_SENSORS_AVAILABLE;
            msg_send(&msg, gatt_srv_pid);
        }
    }
}

struct roomba_sensors_data *roomba_sensors(void)
{
   return &roomba_sensors_data; 
}

void roomba_led(uint8_t status, uint8_t spot, uint8_t clean, uint8_t max, uint8_t power_c, uint8_t power_i)
{
    uint8_t led_bytes[3];
    led_bytes[0] = ((status & 0x03) << 4) | 
        ((spot & 0x01) << 3) | 
        ((clean & 0x01) << 2) |
        ((max &0x01) << 1);
    led_bytes[1] = power_c;
    led_bytes[2] = power_i;
    roomba_cmd(ROOMBA_LEDS, led_bytes, 3);
}

void *gatt_srv(void*);
static const shell_command_t shell_commands[] = {
    { "mode", "Switch mode", cmd_mode },
    { "play", "play song", cmd_play },
    { "info", "device info", cmd_info },
    { NULL, NULL, NULL }
};

int main(void)
{
    /* initialize UART */
    char line_buf[SHELL_BUFSIZE];
    uart_init(UART_DEV(0), 115200, rx_cb, (void *)0);
    /* initialize ringbuffers */
    for (unsigned i = 0; i < UART_NUMOF; i++) {
        ringbuffer_init(&(ctx[i].rx_buf), ctx[i].rx_mem, SENSOR_BUFFER_SIZE);
    }
    /* initialize GPIO WAKEUP */
    gpio_init(GPIO_WAKEUP, GPIO_OUT);
    gpio_set(GPIO_WAKEUP);

    /* Gatt Server */
    gatt_srv_pid = thread_create(gatt_srv_stack, sizeof(gatt_srv_stack),
            DISPATCHER_PRIO - 1, 0, gatt_srv, NULL, "BLE_gatt");
        
    /* Create Sensors thread */
    msg_init_queue(sensors_msgq, MSGQ_SIZE);
    sensors_pid = thread_create(sensors_stack, sizeof(sensors_stack),
                DISPATCHER_PRIO, 0, sensors_loop, NULL, "sensors");


    /* run the shell */
    shell_run(shell_commands, line_buf, SHELL_BUFSIZE);
    return 0;
}