#include "imu.hpp"
#include <fcntl.h>
#include <unistd.h>
#include <cmath>
#include <cstring>
#include <termios.h>

YawTracker::YawTracker() : _fd(-1), _yaw(0), _unbounded_yaw(0), _gyro_z(0), _gyro_bias(0), _ready(false) {}

bool YawTracker::begin(const char* device, int baud)
{
    _fd = open(device, O_RDWR | O_NOCTTY);
    if (_fd < 0) return false;

    termios opt;
    tcgetattr(_fd, &opt);
    cfsetispeed(&opt, B115200);
    cfsetospeed(&opt, B115200);
    opt.c_cflag |= (CLOCAL | CREAD);
    opt.c_cflag &= ~PARENB;
    opt.c_cflag &= ~CSTOPB;
    opt.c_cflag &= ~CSIZE;
    opt.c_cflag |= CS8;
    tcsetattr(_fd, TCSANOW, &opt);

    calibrate();
    return true;
}

void YawTracker::calibrate(int samples)
{
    float sum = 0;
    for (int i = 0; i < samples; ++i)
    {
        int ret = _parseJY62();
        if (ret > 0) sum += _gyro_z;
        usleep(5000);
    }
    _gyro_bias = sum / samples;
    _yaw = 0;
    _unbounded_yaw = 0;
    _ready = true;
}

void YawTracker::update(float dt)
{
    if (_parseJY62() <= 0) return;

    float gyro_filt = _gyro_z - _gyro_bias;
    if (std::fabs(gyro_filt) < 0.03f) gyro_filt = 0;  // 死区

    _unbounded_yaw += gyro_filt * dt;
    _yaw = _unbounded_yaw;

    while (_yaw >  180.0f) _yaw -= 360.0f;
    while (_yaw < -180.0f) _yaw += 360.0f;
}

int YawTracker::_parseJY62()
{
    uint8 buf[11];
    int total = 0;
    while (total < 11)
    {
        int n = read(_fd, buf + total, 11 - total);
        if (n <= 0) return -1;
        total += n;
    }

    while (buf[0] != 0x55 && total > 10)
    {
        memmove(buf, buf + 1, 10);
        int n = read(_fd, buf + 10, 1);
        if (n <= 0) return -1;
    }
    if (buf[0] != 0x55) return -1;

    int16 gz  = (buf[5] << 8) | buf[4];
    _gyro_z = gz * (2000.0f / 32768.0f);  // ±2000°/s
    return 1;
}