/*
 * Copyright (c) 2019, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2019-08-26     sogwms       The first version
 */

#include <command.h>

#define DBG_SECTION_NAME  "command"
#define DBG_LEVEL         DBG_LOG
#include <rtdbg.h>

// Thread
#define THREAD_PRIORITY             ((RT_THREAD_PRIORITY_MAX / 3) + 1)
#define THREAD_STACK_SIZE           512
#define THREAD_TIMESLICE            10
static rt_thread_t cmd_tid = RT_NULL;

//
struct command_info
{
    rt_uint16_t         cmd;
};

typedef struct command_info *command_info_t;

// Message Queue
struct command_msg
{
    struct command_info info;
    void                *param;
    rt_uint16_t         size;
};
#define MAX_MSGS                    32
static rt_mq_t cmd_mq = RT_NULL;

// Target
chassis_t g_chas;

static rt_err_t command_handle_get(struct command_msg *msg)
{
    switch (msg->info.cmd)
    {
    case COMMAND_GET_WHEEL0_PID:
    case COMMAND_GET_WHEEL1_PID:
    case COMMAND_GET_WHEEL2_PID:
    case COMMAND_GET_WHEEL3_PID:
        if (msg->size == sizeof(struct cmd_pid))
        {
            struct cmd_pid *ppid = msg->param;
            struct controller_param parameter;

            controller_get_param(g_chas->c_wheels[msg->info.cmd - COMMAND_GET_WHEEL0_PID]->w_controller, &parameter);
            ppid->kp = parameter.data.pid.kp;
            ppid->ki = parameter.data.pid.ki;
            ppid->kd = parameter.data.pid.kd;
        }
        else
        {
            return RT_ERROR;
        }
        break;
    case COMMAND_GET_WHEELS_PID:
        if (msg->size >= 2 * sizeof(struct cmd_pid))
        {
            struct cmd_pid *ppid = msg->param;
            struct controller_param parameter;

            for (int i = 0; (i < g_chas->c_kinematics->total_wheels) && (i < (msg->size / sizeof(struct cmd_pid))); i++)
            {
                controller_get_param(g_chas->c_wheels[i]->w_controller, &parameter);
                ppid[i].id = COMMAND_GET_WHEEL0_PID + i;
                ppid[i].kp = parameter.data.pid.kp;
                ppid[i].ki = parameter.data.pid.ki;
                ppid[i].kd = parameter.data.pid.kd;
            }
        }
        else
        {
            return RT_ERROR;
        }
        break;
    
    default: return RT_ERROR;
    }

    return RT_EOK;
}

rt_err_t command_handle(rt_uint16_t cmd, void *param, rt_uint16_t size)
{
    struct command_msg msg = {
        .param = param,
        .size = size,
        .info = {
            .cmd = cmd,
        }
    };

    if (cmd > COMMAND_GET_START && cmd < COMMAND_GET_END)
    {
        return command_handle_get(&msg);
    }

    return rt_mq_send(cmd_mq, &msg, sizeof(struct command_msg));
}

rt_err_t command_send(command_sender_t sender, rt_uint16_t cmd, void *param, rt_uint16_t size)
{
    if (sender->send != RT_NULL)
    {
        return sender->send(cmd, param, size);
    }

    return RT_ERROR;
}

static void command_thread_entry(void *param)
{
    struct command_msg msg;

    while (1)
    {
        rt_mq_recv(cmd_mq, &msg, sizeof(struct command_msg), RT_WAITING_FOREVER);

        if (g_chas == RT_NULL)
        {
            LOG_D("The target is NULL");
            continue;
        }

        switch (msg.info.cmd)
        {
        case COMMAND_SET_WHEELS_PID:
            if (msg.size >= 2 * sizeof(struct cmd_pid))
            {
                struct cmd_pid *ppid = msg.param;
                struct controller_param parameter;
                for (int i = 0; i < g_chas->c_kinematics->total_wheels; i++)
                {
                    parameter.data.pid.kp = ppid[i].kp;
                    parameter.data.pid.ki = ppid[i].ki;
                    parameter.data.pid.kd = ppid[i].kd;
                    controller_set_param(g_chas->c_wheels[ppid[i].id]->w_controller, &parameter);
                }
            }
            break;
        case COMMAND_SET_WHEEL0_PID:
        case COMMAND_SET_WHEEL1_PID:
        case COMMAND_SET_WHEEL2_PID:
        case COMMAND_SET_WHEEL3_PID:
            if (msg.size == sizeof(struct cmd_pid))
            {
                if (g_chas->c_kinematics->total_wheels > msg.info.cmd - COMMAND_SET_WHEEL0_PID)
                {
                    struct cmd_pid *ppid = msg.param;
                    struct controller_param parameter = {
                        .data.pid.kp = ppid->kp,
                        .data.pid.ki = ppid->ki,
                        .data.pid.kd = ppid->kd
                    };
                    controller_set_param(g_chas->c_wheels[msg.info.cmd - COMMAND_SET_WHEEL0_PID]->w_controller, &parameter);
                }
            }
            break;
        case COMMAND_SET_CHASSIS_STOP:
            chassis_move(g_chas, 0.0f);
            break;
        case COMMAND_SET_CHASSIS_FORWARD:
            chassis_straight(g_chas, CHASSIS_VELOCITY_LINEAR_MAXIMUM / 2.0f);
            break;
        case COMMAND_SET_CHASSIS_BACKWARD:
            chassis_straight(g_chas, -CHASSIS_VELOCITY_LINEAR_MAXIMUM / 2.0f);
            break;
        case COMMAND_SET_CHASSIS_ROTATE_LEFT:
            chassis_rotate(g_chas, CHASSIS_VELOCITY_ANGULAR_MAXIMUM / 2.0f);
            break;
        case COMMAND_SET_CHASSIS_ROTATE_RIGHT:
            chassis_rotate(g_chas, -CHASSIS_VELOCITY_ANGULAR_MAXIMUM / 2.0f);
            break;
        case COMMAND_SET_CHASSIS_MOVE_LEFT:
            chassis_move(g_chas, CHASSIS_VELOCITY_LINEAR_MAXIMUM / 2.0f);
            break;
        case COMMAND_SET_CHASSIS_MOVE_RIGHT:
            chassis_move(g_chas, -CHASSIS_VELOCITY_LINEAR_MAXIMUM / 2.0f);
            break;
        case COMMAND_SET_CHASSIS_FORWARD_WITH_PARAM:
        case COMMAND_SET_CHASSIS_BACKWARD_WITH_PARAM:
        case COMMAND_SET_CHASSIS_ROTATE_LEFT_WITH_PARAM:
        case COMMAND_SET_CHASSIS_ROTATE_RIGHT_WITH_PARAM:
        case COMMAND_SET_CHASSIS_MOVE_LEFT_WITH_PARAM:
        case COMMAND_SET_CHASSIS_MOVE_RIGHT_WITH_PARAM:
            if (msg.size == sizeof(struct cmd_velocity))
            {
                struct cmd_velocity *target_velocity = (struct cmd_velocity *)msg.param;
                rt_err_t (*p_fun)(chassis_t chas, float data);
                float tmp;
                if (msg.info.cmd == COMMAND_SET_CHASSIS_FORWARD_WITH_PARAM)
                {
                    tmp = target_velocity->data.linear_x;
                    p_fun = chassis_straight;
                }
                else if (msg.info.cmd == COMMAND_SET_CHASSIS_BACKWARD_WITH_PARAM)
                {
                    tmp = -target_velocity->data.linear_x;
                    p_fun = chassis_straight;
                }
                else if (msg.info.cmd == COMMAND_SET_CHASSIS_ROTATE_LEFT_WITH_PARAM)
                {
                    tmp = target_velocity->data.angular_z;
                    p_fun = chassis_rotate;
                }
                else if (msg.info.cmd == COMMAND_SET_CHASSIS_ROTATE_RIGHT_WITH_PARAM)
                {
                    tmp = -target_velocity->data.angular_z;
                    p_fun = chassis_rotate;
                }
                else if (msg.info.cmd == COMMAND_SET_CHASSIS_MOVE_LEFT_WITH_PARAM)
                {
                    tmp = target_velocity->data.linear_y;
                    p_fun = chassis_move;
                }
                else
                {
                    tmp = -target_velocity->data.linear_y;
                    p_fun = chassis_move;
                }
                
                p_fun(g_chas, tmp);
            }
            break;
        case COMMAND_SET_CHASSIS_VELOCITY_LINEAR_X:
        case COMMAND_SET_CHASSIS_VELOCITY_LINEAR_Y:
        case COMMAND_SET_CHASSIS_VELOCITY_ANGULAR_Z:
            if (msg.size == sizeof(struct cmd_velocity))
            {
                struct cmd_velocity *target_velocity = (struct cmd_velocity *)msg.param;
                rt_err_t (*p_fun)(chassis_t chas, float data);
                float tmp;
                if (msg.info.cmd == COMMAND_SET_CHASSIS_VELOCITY_LINEAR_X)
                {
                    tmp = target_velocity->data.linear_x;
                    p_fun = chassis_set_velocity_x;
                }
                else if (msg.info.cmd == COMMAND_SET_CHASSIS_VELOCITY_LINEAR_Y)
                {
                    tmp = target_velocity->data.linear_y;
                    p_fun = chassis_set_velocity_y;
                }
                else
                {
                    tmp = target_velocity->data.angular_z;
                    p_fun = chassis_set_velocity_z;
                }

                p_fun(g_chas, tmp);
            }
            break;
        default:
            break;
        }
    }
}

rt_err_t command_init(chassis_t chas)
{
    cmd_mq = rt_mq_create("command", sizeof(struct command_msg), MAX_MSGS, RT_IPC_FLAG_FIFO);
    if (cmd_mq == RT_NULL)
    {
        LOG_E("Failed to creat meassage queue");
        return RT_ERROR;
    }

    cmd_tid = rt_thread_create("command",
                              command_thread_entry, RT_NULL,
                              THREAD_STACK_SIZE,
                              THREAD_PRIORITY, THREAD_TIMESLICE);

    if (cmd_tid == RT_NULL)
    {
        LOG_E("Failed to creat thread");
        return RT_ERROR;
    }

    g_chas = chas;

    rt_thread_startup(cmd_tid);

    return RT_EOK;
}