#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include <unistd.h>
#include <sys/select.h>

#include "viture.h"

#define LOG(...) fprintf(stderr, __VA_ARGS__)

static float makeFloat(uint8_t *data) 
{
	float value = 0;
	uint8_t tem[4];
	tem[0] = data[3];
	tem[1] = data[2];
	tem[2] = data[1];
	tem[3] = data[0];
	memcpy(&value, tem, 4);
	return value;
}

static void imuCallback(uint8_t *data, uint16_t len, uint32_t ts) 
{
	LOG("demo imuCallback len %d ts %d\n", len, ts);
	for (int i = 0; i < len; i++) {
	     LOG("%02x", data[i]);
	     if (i == len -1) {
	     	LOG("\n");
	     }
	}
	float eulerRoll = makeFloat(data);
	float eulerPitch = makeFloat(data + 4);
	float eulerRaw = makeFloat(data + 8);

	LOG("demo imuCallback data roll %f pitch %f raw %f\n", eulerRoll, eulerPitch, eulerRaw);
	
}

static void mcuCallback(uint16_t msgid, uint8_t *data, uint16_t len, uint32_t ts) 
{
	LOG("demo mcuCallback len %d ts %d\n", len, ts);
	for (int i = 0; i < len; i++) {
	     LOG("%02x", data[i]);
	     if (i == len -1) {
	     	LOG("\n");
	     }
	}

}

int main()
{
   init(imuCallback, mcuCallback);
   set_imu(true);


   fd_set read_fds;
    struct timeval timeout;
    char input_buffer[256];

    while (1) {
        // 清空文件描述符集合
        FD_ZERO(&read_fds);
        // 将标准输入添加到文件描述符集合中
        FD_SET(STDIN_FILENO, &read_fds);

        // 设置超时时间为NULL，表示一直等待
        timeout.tv_sec = 0;
        timeout.tv_usec = 0;
        int result;
        // 使用 select 检查文件描述符状态
        int ready = select(STDIN_FILENO + 1, &read_fds, NULL, NULL, &timeout);

        if (ready == -1) {
            perror("select");
            exit(EXIT_FAILURE);
        } else if (ready > 0) {
            // 标准输入上有可读数据
            if (FD_ISSET(STDIN_FILENO, &read_fds)) {
                // 读取用户输入
                if (fgets(input_buffer, sizeof(input_buffer), stdin) != NULL) {
                    printf("你输入的是: %s\n", input_buffer);
                    if (!strncmp("imuoff", input_buffer, strlen("imuoff"))) {
                    	result = set_imu(false);
                    	printf("set_imu off: %d\n", result);
                    }
                    if (!strncmp("imuon", input_buffer, strlen("imuon"))) {
                    	result = set_imu(true);
                    	printf("set_imu on: %d\n", result);
                    }
                    if (!strncmp("quit", input_buffer, strlen("quit"))) {
                    	printf("quit over.\n");
                    	break;
                	}
                	if (!strncmp("3d", input_buffer, strlen("3d"))) {
                		result = set_3d(true);
                		printf("set_3d on: %d\n", result);
                	}
                	if (!strncmp("2d", input_buffer, strlen("2d"))) {
                		result = set_3d(false);
                		printf("set_3d off: %d\n", result);
                	}

                	if (!strncmp("get3d", input_buffer, strlen("get3d"))) {
                	    result = get_3d_state();
                		printf("get_3D state: %d\n", result);
                	}
                	if (!strncmp("getimu", input_buffer, strlen("getimu"))) {
                		result = get_imu_state();
                		printf("getimu state: %d\n", result);
                	}
                    // 如果需要，在这里可以添加对输入的处理逻辑
                } else {
                    perror("fgets");
                    exit(EXIT_FAILURE);
                }
            }
        }

        // 在这里可以添加其他的逻辑
    }
    set_imu(false);
    deinit();
   return 0;
}
