st-4-esp32/src/ST4Serial.cpp

// SPDX-License-Identifier: LGPL-3.0-or-later
// Serial protocol: backward compatible with original ArduinoCode.ino
// Original commands: CONNECT#, DISCONNECT#, RA+#, RA-#, RA0#, DEC+#, DEC-#, DEC0#
// Extended commands: PULSE RA+ 500#, POS?#, SYNC 12.345 45.678#, STATUS?#, VERSION?#
#include "ST4Serial.h"

ST4Serial::ST4Serial()
    : controller_(nullptr), serial_(nullptr), extendedMode_(true),
      bufferOverflow_(false) {}

void ST4Serial::begin(ST4Controller& controller, HardwareSerial& serial,
                      bool extendedMode) {
    controller_ = &controller;
    serial_ = &serial;
    extendedMode_ = extendedMode;
    buffer_.reserve(64);
    serial_->begin(ST4Constants::DEFAULT_BAUD_RATE, SERIAL_8N1);
    serial_->println("INITIALIZED#");
}

String ST4Serial::directionStr(ST4Direction dir) const {
    switch (dir) {
        case ST4Direction::PLUS:  return "+";
        case ST4Direction::MINUS: return "-";
        default: return "0";
    }
}

void ST4Serial::processCommand(const String& cmd) {
    bool valid = true;

    if (cmd == "CONNECT") {
        controller_->connect();
    } else if (cmd == "DISCONNECT") {
        controller_->disconnect();
    } else if (cmd == "RA0") {
        controller_->stopAxis(ST4AxisId::RA);
    } else if (cmd == "RA+") {
        controller_->move(ST4AxisId::RA, ST4Direction::PLUS);
    } else if (cmd == "RA-") {
        controller_->move(ST4AxisId::RA, ST4Direction::MINUS);
    } else if (cmd == "DEC0") {
        controller_->stopAxis(ST4AxisId::DECLINATION);
    } else if (cmd == "DEC+") {
        controller_->move(ST4AxisId::DECLINATION, ST4Direction::PLUS);
    } else if (cmd == "DEC-") {
        controller_->move(ST4AxisId::DECLINATION, ST4Direction::MINUS);
    } else if (extendedMode_) {
        processExtendedCommand(cmd);
        return;
    } else {
        valid = false;
    }

    if (valid) {
        serial_->println("OK#");
    }
}

void ST4Serial::processExtendedCommand(const String& cmd) {
    if (cmd.startsWith("PULSE ")) {
        // Format: PULSE RA+ 500  or  PULSE DEC- 1000
        String rest = cmd.substring(6);
        rest.trim();

        ST4AxisId axis;
        if (rest.startsWith("RA")) {
            axis = ST4AxisId::RA;
            rest = rest.substring(2);
        } else if (rest.startsWith("DEC")) {
            axis = ST4AxisId::DECLINATION;
            rest = rest.substring(3);
        } else {
            serial_->println("ERR:INVALID_AXIS#");
            return;
        }

        ST4Direction dir;
        if (rest.startsWith("+")) {
            dir = ST4Direction::PLUS;
            rest = rest.substring(1);
        } else if (rest.startsWith("-")) {
            dir = ST4Direction::MINUS;
            rest = rest.substring(1);
        } else {
            serial_->println("ERR:INVALID_DIR#");
            return;
        }

        rest.trim();
        uint32_t ms = rest.toInt();
        if (ms > 0) {
            controller_->pulseGuide(axis, dir, ms);
            serial_->println("OK#");
        } else {
            serial_->println("ERR:INVALID_DURATION#");
        }
    } else if (cmd == "POS?") {
        serial_->print("POS ");
        serial_->print(controller_->position(ST4AxisId::RA), 6);
        serial_->print(" ");
        serial_->print(controller_->position(ST4AxisId::DECLINATION), 6);
        serial_->println("#");
    } else if (cmd.startsWith("SYNC ")) {
        // Format: SYNC 12.345 45.678
        String rest = cmd.substring(5);
        int spaceIdx = rest.indexOf(' ');
        if (spaceIdx > 0) {
            String raStr = rest.substring(0, spaceIdx);
            String decStr = rest.substring(spaceIdx + 1);
            decStr.trim();
            // Validate: must start with digit, sign, or dot (reject garbage → 0.0)
            bool raValid = raStr.length() > 0 &&
                (isDigit(raStr[0]) || raStr[0] == '-' || raStr[0] == '+' || raStr[0] == '.');
            bool decValid = decStr.length() > 0 &&
                (isDigit(decStr[0]) || decStr[0] == '-' || decStr[0] == '+' || decStr[0] == '.');
            if (raValid && decValid) {
                double ra = raStr.toDouble();
                double dec = decStr.toDouble();
                controller_->sync(ra, dec);
                serial_->println("OK#");
            } else {
                serial_->println("ERR:INVALID_COORDS#");
            }
        } else {
            serial_->println("ERR:INVALID_COORDS#");
        }
    } else if (cmd == "STATUS?") {
        ST4State s = controller_->state();
        serial_->print("STATUS ");
        serial_->print(s.connected ? "CONNECTED" : "DISCONNECTED");
        serial_->print(" RA:");
        serial_->print(directionStr(s.ra.direction));
        serial_->print(":");
        serial_->print(s.ra.position, 6);
        serial_->print(" DEC:");
        serial_->print(directionStr(s.dec.direction));
        serial_->print(":");
        serial_->print(s.dec.position, 6);
        serial_->println("#");
    } else if (cmd == "VERSION?") {
        serial_->print("VERSION ");
        serial_->print(ST4Constants::VERSION);
        serial_->println("#");
    }
    // Unknown extended commands are silently ignored (matches original behavior)
}

void ST4Serial::update() {
    while (serial_->available()) {
        char c = serial_->read();
        if (c == '#') {
            if (bufferOverflow_) {
                // Discard the truncated command entirely
                bufferOverflow_ = false;
            } else {
                buffer_.trim();
                if (buffer_.length() > 0) {
                    processCommand(buffer_);
                }
            }
            buffer_ = "";
        } else if (c != '\r' && c != '\n') {
            if (buffer_.length() < 64) {
                buffer_ += c;
            } else {
                bufferOverflow_ = true;
            }
        }
    }
}