#include "encoder.hpp"
#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>
#include <cmath>

Encoder::Encoder(int pwm_channel, int dir_gpio, int direction_polarity)
    : _channel(pwm_channel)
    , _dir_gpio(dir_gpio)
    , _polarity(direction_polarity)
    , _reg_base(nullptr)
    , _mem_fd(-1)
    , _rps(0.0f)
    , _last_period(0)
{
    _dir_gpio.setDirection("in");
    _mmapRegs();
}

Encoder::~Encoder() { _munmapRegs(); }

void Encoder::_mmapRegs()
{
    _mem_fd = open("/dev/mem", O_RDWR);
    if (_mem_fd < 0) return;

    _reg_base = (volatile uint32*)mmap(nullptr, 0x10000, PROT_READ | PROT_WRITE,
                                        MAP_SHARED, _mem_fd, ENCODER_BASE);
}

void Encoder::_munmapRegs()
{
    if (_reg_base && _reg_base != MAP_FAILED)
        munmap((void*)_reg_base, 0x10000);
    if (_mem_fd >= 0) { close(_mem_fd); _mem_fd = -1; }
}

uint32 Encoder::_readRegister(unsigned int offset) const
{
    if (!_reg_base || _reg_base == MAP_FAILED) return 0;
    return _reg_base[offset / sizeof(uint32)];
}

int Encoder::getDirection() const
{
    return _dir_gpio.getValue() * _polarity;
}

void Encoder::update()
{
    if (!_reg_base || _reg_base == MAP_FAILED) return;

    uint32 period = _readRegister(REG_OFFSET + _channel * 0x100);
    if (period > 0 && period < 0xFFFFFF)
    {
        float freq = 50000000.0f / period;       // 50MHz 基频
        _rps = (freq / 4.0f) * getDirection();    // 4倍频编码器
        _last_period = period;
    }
    else
    {
        _rps *= 0.6f;  // 无脉冲时速度衰减
    }
}