#include <iostream>

#include "cyber/cyber.hpp"
#include "neon/ahrs/ahrs.hpp"
#include "visualslam.hpp"

typedef struct _package
{
    unsigned int header;
    unsigned int seq;
    float imudata[6];
} package;

unsigned int last_seq = 0;

visualslam vs;

void init()
{
    vs.pose_displayer_create("imu_test");
}

int main()
{
    vs.set_init(init);

    neon::ahrs::imu6_decoder imu;
    imu.init(125);
    cyber::timeit it;

    cyber::serial s;
    s.set_baudrate(115200);
    s.open("/dev/serial/by-id/usb-jixin.pro_CMSIS-DAP_LU_LU_2022_8888-if00");
    if (!s.is_open())
    {
        std::cout << "Open Serial Failed!" << std::endl;
        return 1;
    }

    while (1)
    {
        package pkg;
        s.read(&pkg, sizeof(pkg), 1000);
        if (pkg.header != 0xA1B2C3D4)
        {
            s.flush();
            continue;
        }

        if (last_seq == 0)
        {
            last_seq = pkg.seq;
        }
        else
        {
            if (pkg.seq - last_seq != 1)
            {
                std::cout << "SEQ" << std::endl;
                // break;
            }
            last_seq = pkg.seq;
        }

        float ax = pkg.imudata[0];
        float ay = pkg.imudata[1];
        float az = pkg.imudata[2];
        float gx = pkg.imudata[3];
        float gy = pkg.imudata[4];
        float gz = pkg.imudata[5];
        imu.decode_imu(ax, ay, az, gx, gy, gz);

        if (vs.ok())
        {
            vs.pose_displayer_set_left_hand(imu.pose.q[0], imu.pose.q[1], imu.pose.q[2], imu.pose.q[3]);
        }

        it.tick();

        // std::cout
        //     << std::setprecision(4) <<         //
        //     "S=" << pkg.seq << "  " <<         //
        //     "Ax=" << pkg.imudata[0] << "  " << //
        //     "Ay=" << pkg.imudata[1] << "  " << //
        //     "Az=" << pkg.imudata[2] << "  " << //
        //     "Gx=" << pkg.imudata[3] << "  " << //
        //     "Gy=" << pkg.imudata[4] << "  " << //
        //     "Gz=" << pkg.imudata[5] << std::endl;
    }

    s.close();

    return 0;
}