#include "MKS42C.h"

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

// Вспомогательная функция для CRC (стр. 13 PDF)
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) {
    // Команда 0xF3 (стр. 16 PDF)
    uint8_t cmd[] = {_addr, 0xF3, (uint8_t)(state ? 0x01 : 0x00)};
    sendRaw(cmd, 3);
}

bool MKS42C::setMaxTorque(uint16_t torque) {
    if (torque > 3000) torque = 3000;
    uint8_t cmd[] = {_addr, 0xA5, (uint8_t)(torque >> 8), (uint8_t)(torque & 0xFF)};
    sendRaw(cmd, 4);
    
    // Ждем подтверждение e0 01 e1
    unsigned long wait = millis();
    while (_serial->available() < 3 && millis() - wait < 100);
    if (_serial->available() >= 3) {
        uint8_t res[3];
        _serial->readBytes(res, 3);
        return (res[1] == 0x01);
    }
    return false;
}

void MKS42C::run(uint8_t dir, uint8_t speed) {
    // Команда 0xF6 (стр. 16 PDF)
    // dir: 0 - CW, 1 - CCW
    // speed: 0-127
    uint8_t val = ((dir & 0x01) << 7) | (speed & 0x7F);
    uint8_t cmd[] = {_addr, 0xF6, val};
    
    sendRaw(cmd, 3);
    _isMoving = true; 
    _lastMoveTime = millis();
    _lastAbsPos = readAbsolutePosition();
}

void MKS42C::run(int16_t speed) {
    uint8_t dir = (speed < 0) ? 1 : 0; // 1 - CCW, 0 - CW
    uint8_t absSpeed = abs(speed);
    
    if (absSpeed > 127) absSpeed = 127; // Максимальная скорость 7 бит

    // Формируем байт: 7-й бит это направление, 0-6 биты это скорость
    uint8_t val = (dir << 7) | (absSpeed & 0x7F);

    // Передаем пакет из 3 байт + CRC
    uint8_t cmd[] = {_addr, 0xF6, val};
    sendRaw(cmd, 3);
    
    _isMoving = true;
}

void MKS42C::stop() {
    // Команда 0xF7 (стр. 16 PDF)
    uint8_t cmd[] = {_addr, 0xF7};
    sendRaw(cmd, 2);
    _isMoving = false;
}

void MKS42C::rotateDegrees(float degrees, uint8_t speed) {
    // Команда 0xFD (стр. 17 PDF)
    // Используем проверенный коэффициент 3200 имп/оборот
    long pulses = (long)((degrees / 360.0) * 3200.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;
    _lastMoveTime = millis();
    _lastAbsPos = readAbsolutePosition();
}

// --- ЧТЕНИЕ ДАННЫХ (8-байтовый пакет из V1.1.2) ---

float MKS42C::readPosition() {
    // 1. Убираем clearBuffer() отсюда — он может "съедать" начало ответа
    
    // 2. Шлем запрос
    uint8_t cmd[] = {_addr, 0x30};
    sendRaw(cmd, 2);
    delay(5);
    // 3. Ждем появления данных (увеличим до 200мс)
    unsigned long start = millis();
    while (_serial->available() < 8 && millis() - start < 200) {
        delay(1);
    }

    if (_serial->available() < 8) {
        // Если данных нет, попробуем еще раз послать запрос (иногда первый теряется)
        sendRaw(cmd, 2);
        start = millis();
        while (_serial->available() < 8 && millis() - start < 200) delay(1);
    }

    if (_serial->available() >= 8) {
        // 4. Ищем заголовок 0xE0
        while (_serial->available() > 0 && _serial->peek() != _addr) {
            _serial->read(); // Выкидываем мусор
        }

        if (_serial->available() >= 8) {
            uint8_t buf[8];
            _serial->readBytes(buf, 8);
            
            // Считаем CRC
            uint16_t sum = 0;
            for (int i = 0; i < 7; i++) sum += buf[i];
            
            if ((uint8_t)(sum & 0xFF) == buf[7]) {
                uint16_t value = ((uint16_t)buf[5] << 8) | buf[6];
                return (value * 360.0) / 65536.0;
            } else {
                Serial.print("CRC Err: "); Serial.println((uint8_t)(sum & 0xFF), HEX);
            }
        }
    } else {
        Serial.print("Buf low: "); Serial.println(_serial->available());
    }
    return -1.0;
}

long MKS42C::readAbsolutePosition() {
    static long lastValidPos = 0; // Храним последнее успешное значение
    
    uint8_t cmd[] = {_addr, 0x30};
    sendRaw(cmd, 2);

    delay(5);

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

    if (_serial->available() >= 8) {
        while (_serial->available() > 8 && _serial->peek() != _addr) _serial->read();

        uint8_t buf[8];
        _serial->readBytes(buf, 8);
        
        uint16_t sum = 0;
        for (int i = 0; i < 7; i++) sum += buf[i];
        
        if ((uint8_t)(sum & 0xFF) == buf[7]) {
            int32_t carry = ((int32_t)(int8_t)buf[1] << 24) | 
                            ((uint32_t)buf[2] << 16) | 
                            ((uint32_t)buf[3] << 8)  | 
                            (uint32_t)buf[4];
            uint16_t value = ((uint16_t)buf[5] << 8) | buf[6];
            lastValidPos = (long)((long long)carry * 65536LL + value);
        }
    }
    return lastValidPos; // Возвращаем последнее нормальное, если текущее битое
}

bool MKS42C::isBusy() {
    while (_serial->available() >= 3) {
        if (_serial->peek() != _addr) { _serial->read(); continue; }
        uint8_t b[3];
        _serial->readBytes(b, 3);
        if (b[1] == 0x02) { 
            _isMoving = false; 
            return false; 
        }
    }

    if (_isMoving) {
        long currentPos = readAbsolutePosition();
        
        // Если позиция изменилась — мотор точно едет
        if (currentPos != _lastAbsPos) {
            _lastAbsPos = currentPos;
            _lastMoveTime = millis();
            return true;
        } 
        
        if (millis() - _lastMoveTime > 150) {
            _isMoving = false;
        }
    }

    return _isMoving;
}