#include <iostream>

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

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

unsigned int last_seq = 0;
size_t count = 0;

int main()
{
    neon::ahrs::imu6_decoder imu;
    imu.init(125);

    cyber::csv csv;
    csv.open("imudata.csv", "ffffff");

    for (auto i = 0; i < csv.size(); i++)
    {
        float ax = csv.get<float>(i, 0);
        float ay = csv.get<float>(i, 1);
        float az = csv.get<float>(i, 2);
        float gx = csv.get<float>(i, 3);
        float gy = csv.get<float>(i, 4);
        float gz = csv.get<float>(i, 5);
        imu.decode_imu(ax, ay, az, gx, gy, gz);

        // imu.pose.euler();
        // std::cout << std::setprecision(4) <<  //
        //     "R=" << imu.pose.roll << "  " <<  //
        //     "P=" << imu.pose.pitch << "  " << //
        //     "Y=" << imu.pose.yaw << "  " << std::endl;

        std::cout << std::setprecision(4) <<  //
            "q0=" << imu.pose.q[0] << "  " << //
            "q1=" << imu.pose.q[1] << "  " << //
            "q2=" << imu.pose.q[2] << "  " << //
            "q3=" << imu.pose.q[3] << "  " << std::endl;

        // std::cout
        //     << std::setprecision(8) << //
        //     "Ax=" << ax << "  " <<     //
        //     "Ay=" << ay << "  " <<     //
        //     "Az=" << az << "  " <<     //
        //     "Gx=" << gx << "  " <<     //
        //     "Gy=" << gy << "  " <<     //
        //     "Gz=" << gz << std::endl;
    }

    return 0;
}