#include "serial.h"
#include <math.h>

lcm_t *lcm;

// TODO 屏蔽停止指令
bool stopFlag = false;
////////////////////////

int curStatus = -1, curSpeed = 0; // 这里的速度指百分比速度
double curOmega = 0.0;
// 临界区数据
pthread_mutex_t curPoseMutex;
clock_t lastTime;
double curPose[3] = {0, 0, 0};

int main() {
    if (!whellInit()) {
        goto err;
    }
    lcm = lcm_create(NULL);
    if (!lcm) {
        fprintf(stderr, "Failed to create LCM\n");
        goto err;
    }

    // 订阅来自手机端的轮控消息
    lastTime = clock();
    path_ctrl_t_subscribe(lcm, "wheel_ctrl", parseCmd, NULL);
    // 订阅来自算法模块的路径规划消息
    path_t_subscribe(lcm, "PATH", parsePath, NULL);
    // 来自算法模块的当前位置信息
    pthread_mutex_init(&curPoseMutex, NULL);
    pose_t_subscribe(lcm, "CURRENTPOSE", setCurPose, NULL);
    // 新线程，定时发送当前位置信息
    pthread_t thread;
    if (pthread_create(&thread, NULL, sendCurPose, NULL) != 0) {
        fprintf(stderr, "Error: Failed to create thread\n");
        goto err;
    }

    while (true) {
        lcm_handle(lcm);
    }

err:
    // close(fd);
    return 0;
}

void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel,
              const path_ctrl_t *msg, void *userdata) {
    byte status = msg->cmd, speed = msg->speed;
    // 手机端在转弯过程中只能设置状态，速度是默认的
    // 所以对于转弯，只需要判断状态是否改变
    if (curStatus == status && curSpeed == speed) {
        return;
    }

    // 状态改变，计算新的位置
    renewCurPose();

    // 下发新的指令
    switch (status) {
    case 1:
        wheelSend(0x01, speed, 0x01, speed);
        curSpeed = speed;
        curOmega = 0;
        break;
    case 2:
        wheelSend(0x02, DEFAULT_SPEED, 0x01, DEFAULT_SPEED);
        curOmega = (double)DEFAULT_SPEED * FULLSPEED / 100 / RADIUS;
        curSpeed = 0;
        break;
    case 3:
        wheelSend(0x01, DEFAULT_SPEED, 0x02, DEFAULT_SPEED);
        curOmega = (double)-DEFAULT_SPEED * FULLSPEED / 100 / RADIUS;
        curSpeed = 0;
        break;
    // TODO 屏蔽停止指令
    case 4:
        wheelSend(0x00, 0x00, 0x00, 0x00);
        curOmega = 0;
        curSpeed = 0;
        stopFlag = true;
        printf("Stop for 2 seconds\n");
        break;
    // 取消屏蔽
    case 5:
        wheelSend(0x00, 0x00, 0x00, 0x00);
        curOmega = 0;
        curSpeed = 0;
        stopFlag = false;
        printf("Stop but can recieve cmd\n");
        break;
    ////////////////////////////////////
    case 0:
    default:
        wheelSend(0x00, 0x00, 0x00, 0x00);
        curOmega = 0;
        curSpeed = 0;
        break;
    }
    // 更新状态
    curStatus = status;
}

void parsePath(const lcm_recv_buf_t *rbuf, const char *channel,
               const path_t *msg, void *userdata) {
    // TODO
    if (stopFlag) {
        return;
    }
    /////////////////
    int16_t length = msg->length;
    double v, w;
    int8_t vWheels[2];
    int8_t bWheels[2];
    if (length <= 0) {
        fprintf(stderr, "Error: Invalid path\n");
        return;
    }
    v = msg->xyr[0][0];
    w = msg->xyr[0][1];
    printf("v: %f m/s, w: %f rad/s\n", v, w);
    if (fabs(v) <= 0.01 && fabs(w) <= 0.01) {
        wheelSend(0x00, 0x00, 0x00, 0x00);
        renewCurPose();
        curSpeed = 0;
        curOmega = 0;
    } else {
        if (w <= 0.2) {
            w *= 2;
        }
        vWheels[0] = (int8_t)((double)(v - w * RADIUS) / FULLSPEED * 100);
        vWheels[1] = (int8_t)((double)(v + w * RADIUS) / FULLSPEED * 100);
        bWheels[0] = vWheels[0] > 0 ? 0x01 : 0x02;
        bWheels[1] = vWheels[1] > 0 ? 0x01 : 0x02;
        wheelSend(bWheels[0], fabs(vWheels[0]), bWheels[1], fabs(vWheels[1]));
        renewCurPose();
        curSpeed = (int8_t)((double)v / FULLSPEED * 100);
        curOmega = (int8_t)w;
    }
    printf("curSpeed: %d, curOmega: %f\n", curSpeed, curOmega);
}

void setCurPose(const lcm_recv_buf_t *rbuf, const char *channel,
                const pose_t *msg, void *userdata) {
    pthread_mutex_lock(&curPoseMutex);
    curPose[0] = msg->pos[0];
    curPose[1] = msg->pos[1];
    curPose[2] = msg->pos[2];
    lastTime = clock();
    pthread_mutex_unlock(&curPoseMutex);
}

void *sendCurPose(void *args) {
    pose_t pose;
    while (true) {
        renewCurPose();
        pthread_mutex_lock(&curPoseMutex);
        pose.pos[0] = curPose[0];
        pose.pos[1] = curPose[1];
        pose.pos[2] = curPose[2];
        pthread_mutex_unlock(&curPoseMutex);
        pose_t_publish(lcm, "POSE", &pose);
        usleep(15 * 1000);
    }
}

void renewCurPose() {
    clock_t curTime = clock();
    double dt = (double)(curTime - lastTime) / CLOCKS_PER_SEC;
    double speed = (double)curSpeed * FULLSPEED / 100;

    pthread_mutex_lock(&curPoseMutex);
    curPose[0] += speed * cos(curPose[2]) * dt;
    curPose[1] += speed * sin(curPose[2]) * dt;
    curPose[2] += curOmega * dt;
    lastTime = clock();
    pthread_mutex_unlock(&curPoseMutex);
}