// ST4Serial unit tests
#include <unity.h>
#include "mock_state.h"
#include "ST4Controller.h"
#include "ST4Serial.h"

static ST4Controller* ctrl;
static ST4Serial* serial;

void setUp() {
    MockState::reset();
    ctrl = new ST4Controller();
    ctrl->begin();
    serial = new ST4Serial();
    serial->begin(*ctrl, Serial, true);
    // Clear the INITIALIZED# output from begin()
    MockState::serialOutput.clear();
}

void tearDown() {
    delete serial;
    delete ctrl;
    serial = nullptr;
    ctrl = nullptr;
}

// Helper: send a command string and process it
static void sendCommand(const char* cmd) {
    MockState::setSerialInput(cmd);
    Serial.resetReadPos();
    serial->update();
}

void test_connect_command() {
    sendCommand("CONNECT#");
    TEST_ASSERT_TRUE(ctrl->isConnected());
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("OK#"));
}

void test_disconnect_command() {
    sendCommand("CONNECT#");
    MockState::serialOutput.clear();
    sendCommand("DISCONNECT#");
    TEST_ASSERT_FALSE(ctrl->isConnected());
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("OK#"));
}

void test_ra_plus() {
    sendCommand("CONNECT#");
    MockState::serialOutput.clear();
    sendCommand("RA+#");
    TEST_ASSERT_EQUAL(ST4Direction::PLUS, ctrl->axisDirection(ST4AxisId::RA));
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("OK#"));
}

void test_ra_minus() {
    sendCommand("CONNECT#");
    MockState::serialOutput.clear();
    sendCommand("RA-#");
    TEST_ASSERT_EQUAL(ST4Direction::MINUS, ctrl->axisDirection(ST4AxisId::RA));
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("OK#"));
}

void test_ra_stop() {
    sendCommand("CONNECT#");
    sendCommand("RA+#");
    MockState::serialOutput.clear();
    sendCommand("RA0#");
    TEST_ASSERT_EQUAL(ST4Direction::STOP, ctrl->axisDirection(ST4AxisId::RA));
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("OK#"));
}

void test_dec_plus() {
    sendCommand("CONNECT#");
    MockState::serialOutput.clear();
    sendCommand("DEC+#");
    TEST_ASSERT_EQUAL(ST4Direction::PLUS, ctrl->axisDirection(ST4AxisId::DECLINATION));
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("OK#"));
}

void test_dec_minus() {
    sendCommand("CONNECT#");
    MockState::serialOutput.clear();
    sendCommand("DEC-#");
    TEST_ASSERT_EQUAL(ST4Direction::MINUS, ctrl->axisDirection(ST4AxisId::DECLINATION));
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("OK#"));
}

void test_dec_stop() {
    sendCommand("CONNECT#");
    sendCommand("DEC+#");
    MockState::serialOutput.clear();
    sendCommand("DEC0#");
    TEST_ASSERT_EQUAL(ST4Direction::STOP, ctrl->axisDirection(ST4AxisId::DECLINATION));
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("OK#"));
}

void test_pulse_command() {
    sendCommand("CONNECT#");
    MockState::serialOutput.clear();
    sendCommand("PULSE RA+ 500#");
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("OK#"));
}

void test_pos_query() {
    sendCommand("CONNECT#");
    ctrl->sync(12.345, 45.678);
    MockState::serialOutput.clear();
    sendCommand("POS?#");
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("POS"));
}

void test_sync_command() {
    sendCommand("CONNECT#");
    MockState::serialOutput.clear();
    sendCommand("SYNC 12.345 45.678#");
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("OK#"));
    TEST_ASSERT_DOUBLE_WITHIN(1e-3, 12.345, ctrl->position(ST4AxisId::RA));
    TEST_ASSERT_DOUBLE_WITHIN(1e-3, 45.678, ctrl->position(ST4AxisId::DECLINATION));
}

void test_status_query() {
    sendCommand("CONNECT#");
    MockState::serialOutput.clear();
    sendCommand("STATUS?#");
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("STATUS"));
}

void test_version_query() {
    MockState::serialOutput.clear();
    sendCommand("VERSION?#");
    TEST_ASSERT_NOT_EQUAL(std::string::npos,
        MockState::serialOutput.find(ST4Constants::VERSION));
}

void test_buffer_overflow() {
    // Send >64 chars then '#' -- should discard the truncated command without crashing
    std::string overflow(70, 'A');
    overflow += '#';
    sendCommand(overflow.c_str());
    // No crash is the pass condition. Output should not contain "OK#" for this garbage
    // (the overflow flag causes discard, so processCommand is never called)
}

void test_sync_garbage_rejected() {
    sendCommand("CONNECT#");
    MockState::serialOutput.clear();
    sendCommand("SYNC garbage data#");
    TEST_ASSERT_NOT_EQUAL(std::string::npos, MockState::serialOutput.find("ERR:INVALID_COORDS#"));
}

void test_partial_buffering() {
    // Send "CON" then "NECT#" across two update() calls
    MockState::setSerialInput("CON");
    Serial.resetReadPos();
    serial->update();
    // No command processed yet
    TEST_ASSERT_FALSE(ctrl->isConnected());

    MockState::setSerialInput("NECT#");
    Serial.resetReadPos();
    serial->update();
    TEST_ASSERT_TRUE(ctrl->isConnected());
}

int main() {
    UNITY_BEGIN();
    RUN_TEST(test_connect_command);
    RUN_TEST(test_disconnect_command);
    RUN_TEST(test_ra_plus);
    RUN_TEST(test_ra_minus);
    RUN_TEST(test_ra_stop);
    RUN_TEST(test_dec_plus);
    RUN_TEST(test_dec_minus);
    RUN_TEST(test_dec_stop);
    RUN_TEST(test_pulse_command);
    RUN_TEST(test_pos_query);
    RUN_TEST(test_sync_command);
    RUN_TEST(test_status_query);
    RUN_TEST(test_version_query);
    RUN_TEST(test_buffer_overflow);
    RUN_TEST(test_sync_garbage_rejected);
    RUN_TEST(test_partial_buffering);
    return UNITY_END();
}