// Encoded in UTF-8
#include "udp2lcm.h"
#include <sys/signal.h>

pthread_t udpRecv, udpSend;
int8_t heartBeat[9] = {0};
pthread_mutex_t heartBeatMutex;
struct sockaddr_in clientAddr;
char clientIP[20] = "";
lcm_t *lcm;

pid_t httpServerPid = -1;

void sigIntHandler(int sig) {
    if (httpServerPid != -1) {
        kill(httpServerPid, SIGINT);
    }
    printf("Exiting...\n");
    exit(sig);
}

int main() {
    // 当遇到Ctrl-C时，递归地杀死所有子进程
    signal(SIGINT, sigIntHandler);
    if (!LCMInit()) {
        fprintf(stderr, "Error: Failed to initialize LCM\n");
        return -1;
    }
    // 初始化线程锁
    pthread_mutex_init(&heartBeatMutex, NULL);
    // 开启udp接收线程
    pthread_create(&udpRecv, NULL, udpRecvHandler, NULL);
    pose_t_subscribe(lcm, "CURRENTPOSE", poseHandler, NULL);
    if ((httpServerPid = fork()) == 0) {
        // 拉起服务器进程，手机端需要申请的文件/data/test/defaultMap.txt.txt
        // ATTENTION: 这里的路径需要根据实际存储地图的路径来修改
        chdir("/data/test");
        /*
         * execlp使用系统调用exec()执行一个程序
         * 第一个参数是文件名，将在PATH中查找，最后一个参数必须是NULL
         * 其他的是传递过去的参数，按一般约定，第一个参数是程序名，不被使用
         * 因而这里出现了两个python
         */
        execlp("python", "python", "-m", "http.server", NULL);
    }
    while (true) {
        // pause();
        lcm_handle(lcm);
    }
    return 0;
}

bool LCMInit() {
    // 多播地址范围：224.0.0.0~239.255.255.255
    if ((lcm = lcm_create(NULL)) == NULL) {
        fprintf(stderr, "Error: Failed to create Path Ctrl LCM\n");
        return false;
    }
    return true;
}

void parseCmd(const char *buffer, int bytesReceived) {
    if (buffer == NULL) {
        fprintf(stderr, "Error: Invalid message\n");
        return;
    }

    path_ctrl_t path;
    robot_control_t robotCtrlData;
    pose_t curPos;
    if (buffer[0] == 0) {
        // 手机端用来与udp2lcm服务器建立连接的初始化消息
        // 也是手机端的心跳
        // 下达30号命令，开始建图
        robotCtrlInit(&robotCtrlData, 0, 30, 0, 1, 1, 0, 0);
        robotCtrlData.dparams[0] = 0.05;
        robotCtrlData.niparams = 1;
        robotCtrlData.iparams[0] = 1;
        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
        freeRobotCtrl(&robotCtrlData);
    } else if (buffer[0] == 1) {
        // 建图过程中，手机端遥控小车进行移动，命令下发至轮控模块
        path.cmd = buffer[1];
        path.speed = buffer[2];
        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
    } else if (buffer[0] == 2) {
        /*
         * 结束建图，主要有以下几个任务：
         * - 向轮控模块发送停止命令
         * - 通知算法模块结束建图
         * - 拉起http服务器进程（改由main拉起，省的每次拉起浪费资源）
         * - 向手机端通知建图完成
         * 开始通过lcm接受算法给出的实时坐标，存储至heartBeat数组中（对应格式）
         */
        path.cmd = 0;
        path.speed = 0;
        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);

        printf("Save Map!\n");
        robotCtrlInit(&robotCtrlData, 0, 32, 0, 0, 0, 0, 0);
        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
        freeRobotCtrl(&robotCtrlData);

        sleep(2);

        robotCtrlInit(&robotCtrlData, 0, 10, 0, 7, 0, 0, 0);
        pthread_mutex_lock(&heartBeatMutex);
        int16_t x, y, sita;
        x = swapEndian(*(int16_t *)&heartBeat[3]);
        y = swapEndian(*(int16_t *)&heartBeat[5]);
        sita = swapEndian(*(int16_t *)&heartBeat[7]);
        printf("NOW point: x: %d, y: %d, sita: %d\n", x, y, sita);
        robotCtrlData.dparams[4] = (double)x / 20;
        robotCtrlData.dparams[5] = (double)y / 20;
        robotCtrlData.dparams[6] = (double)sita * M_PI / 180;
        pthread_mutex_unlock(&heartBeatMutex);
        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
        freeRobotCtrl(&robotCtrlData);
    } else if (buffer[0] == 3) {
        // 手机发来的终点坐标，大端党，获取之后转发给算法模块
        int x, y, sita;
        x = swapEndian(*(int16_t *)&buffer[3]);
        y = swapEndian(*(int16_t *)&buffer[5]);
        sita = 0;
        robotCtrlInit(&robotCtrlData, 0, 20, 0, 3, 0, 0, 0);
        robotCtrlData.dparams[0] = (double)x / 20;
        robotCtrlData.dparams[1] = (double)y / 20;
        robotCtrlData.dparams[2] = sita;
        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
        freeRobotCtrl(&robotCtrlData);
    } else if (buffer[0] == 4) {
        // 取消导航，删除上一个目标点
        robotCtrlInit(&robotCtrlData, 0, 23, 0, 0, 0, 0, 0);
        // TODO 第一次停止指令
        printf("Cancel Navigation!\n");
        path.cmd = 4;
        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
        /////////////////////////////
        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
        // TODO 再次下达停止指令
        sleep(2);
        path.cmd = 5;
        printf("Stop but can recieve cmd\n");
        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
        /////////////////////////
    } else if (buffer[0] == 5) {
        /*
        加载地图，需要注意的是所加载的地图是由电脑控制端下发的
        该部分由建图部分修改
        */
        path.cmd = 0;
        path.speed = 0;
        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);

        printf("Load Map!\n");
        // robotCtrlInit(&robotCtrlData, 0, 32, 0, 0, 0, 0, 0);
        // robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
        // freeRobotCtrl(&robotCtrlData);

        // sleep(2);

        robotCtrlInit(&robotCtrlData, 0, 10, 0, 7, 0, 0, 0);
        pthread_mutex_lock(&heartBeatMutex);
        int16_t x, y, sita;
        x = swapEndian(*(int16_t *)&heartBeat[3]);
        y = swapEndian(*(int16_t *)&heartBeat[5]);
        sita = swapEndian(*(int16_t *)&heartBeat[7]);
        printf("NOW point: x: %d, y: %d, sita: %d\n", x, y, sita);
        robotCtrlData.dparams[4] = 0;
        robotCtrlData.dparams[5] = 0;
        robotCtrlData.dparams[6] = 0;
        pthread_mutex_unlock(&heartBeatMutex);
        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
        freeRobotCtrl(&robotCtrlData);
    } else {
        fprintf(stderr, "Error: Invalid command: ");
        for (int i = 0; i < 9; i++) {
            fprintf(stderr, "%02hhx ", buffer[i]);
        }
        fprintf(stderr, "\n");
    }
}

void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel,
                 const pose_t *msg, void *userdata) {
    int16_t x, y, sita;
    x = (int16_t)((double)msg->pos[0] * 20);
    y = (int16_t)((double)msg->pos[1] * 20);
    sita = (int16_t)((double)msg->pos[2] * 180 / M_PI);
    while (sita > 180)
        sita -= 360;
    while (sita < -180)
        sita += 360;
    pthread_mutex_lock(&heartBeatMutex);
    heartBeat[0] = 0x03;
    heartBeat[1] = heartBeat[2] = 0;
    heartBeat[3] = x >> 8;
    heartBeat[4] = x & 0xff;
    heartBeat[5] = y >> 8;
    heartBeat[6] = y & 0xff;
    heartBeat[7] = sita >> 8;
    heartBeat[8] = sita;
    pthread_mutex_unlock(&heartBeatMutex);
}

int16_t swapEndian(int16_t val) { return (val << 8) | ((val >> 8) & 0xFF); }