#include "MKS42C.h"

MKS42C::MKS42C(HardwareSerial& serial, uint8_t address) {
    _serial = &serial;
    _addr = address;
    _isMoving = false;
}

uint8_t MKS42C::calculateCRC(uint8_t* data, uint8_t len) {
    // вычисление контрольной суммы
    uint16_t checksum = 0;
    for (uint8_t i = 0; i < len; i++) {
        checksum += data[i];
    }
    return (uint8_t)(checksum & 0xFF);
}

void MKS42C::sendRaw(uint8_t* data, uint8_t len) {
    // отправить в порт
    _serial->write(data, len);
    _serial->write(calculateCRC(data, len));
}

void MKS42C::clearBuffer() {
    // очистить входящий буфер
    while (_serial->available()) {
        _serial->read();
    }
}

void MKS42C::setEnable(bool state) {
    // вкл / выкл движка
    uint8_t cmd[] = {_addr, 0xF3, (uint8_t)(state ? 0x01 : 0x00)};
    sendRaw(cmd, 3);
}

void MKS42C::stop() {
    // стоп
    uint8_t cmd[] = {_addr, 0xF7};
    sendRaw(cmd, 2);
    _isMoving = false;
}

void MKS42C::setHome() {
    // задать текущую позицию, как 0 (сброс энкодера)
    uint8_t cmd[] = {_addr, 0x34};
    sendRaw(cmd, 2);
    delay(100); 
}

void MKS42C::setTorque(uint16_t ma) {
    // задать момент вращения
    uint8_t cmd[] = {_addr, 0xAF, (uint8_t)((ma >> 8) & 0xFF), (uint8_t)(ma & 0xFF)};
    sendRaw(cmd, 4);
}

void MKS42C::run(uint8_t dir, uint8_t speed) {
    // просто включить
    uint8_t val = ((dir & 0x01) << 7) | (speed & 0x7F);
    uint8_t cmd[] = {_addr, 0xF6, val};
    
    sendRaw(cmd, 3);
    _isMoving = true; 
}

void MKS42C::rotateDegrees(float degrees, uint8_t speed) {
    // включить на определённое количество гражут=сов
    long pulses = (long)((degrees / 360.0) * 16384.0);
    uint8_t dir = (pulses >= 0) ? 0x00 : 0x01;
    uint32_t absP = abs(pulses);

    uint8_t cmd[] = {
        _addr, 
        0xFD, 
        (uint8_t)((dir << 7) | (speed & 0x7F)), 
        (uint8_t)((absP >> 24) & 0xFF), 
        (uint8_t)((absP >> 16) & 0xFF), 
        (uint8_t)((absP >> 8) & 0xFF), 
        (uint8_t)(absP & 0xFF)
    };
    sendRaw(cmd, 7);
    _isMoving = true;
}

float MKS42C::readPosition() {
    // считать значение энкодера
    clearBuffer();
    uint8_t cmd[] = {_addr, 0x30}; 
    sendRaw(cmd, 2);

    unsigned long start = millis();
    while (_serial->available() < 5 && millis() - start < 50);

    if (_serial->available() >= 5) {
        uint8_t buf[5];
        _serial->readBytes(buf, 5);
        
        uint16_t rawPos = (uint16_t)((buf[2] << 8) | buf[3]);
        
        // Переводим в понятные градусы
        return (rawPos * 360.0) / 16384.0;
    }
    
    return -1.0; 
}

long MKS42C::readAbsolutePosition() {
    // считать абсолютное значение энкодера
    clearBuffer();
    uint8_t cmd[] = {_addr, 0x31};
    sendRaw(cmd, 2);

    unsigned long start = millis();
    while (_serial->available() < 6 && millis() - start < 100);

    if (_serial->available() >= 6) {
        uint8_t buf[6];
        _serial->readBytes(buf, 6);
        uint32_t rawPos = ((uint32_t)buf[2] << 24) | ((uint32_t)buf[3] << 16) | 
                          ((uint32_t)buf[4] << 8) | (uint32_t)buf[5];
        return (buf[1] == 0) ? (long)rawPos : -(long)rawPos;
    }
    return 0;
}

bool MKS42C::isBusy() {
    // проверить движется ли мотор
    if (_serial->available() >= 3) {
        uint8_t buf[3];
        if (_serial->peek() == _addr) {
            _serial->readBytes(buf, 3);
            if (buf[1] == 0x02) {
                _isMoving = false;
                return false;
            }
        }
    }
    return _isMoving;
}

bool MKS42C::isBlocked() {
    // проверить блокировку вала
    clearBuffer();
    uint8_t cmd[] = {_addr, 0x3A};
    sendRaw(cmd, 2);

    unsigned long start = millis();
    while (_serial->available() < 3 && millis() - start < 50);

    if (_serial->available() >= 3) {
        uint8_t buf[3];
        _serial->readBytes(buf, 3);
        return (buf[1] == 0x02);
    }
    return false;
}