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

static ST4Controller* ctrl;

void setUp() {
    MockState::reset();
    ctrl = new ST4Controller();
    ctrl->begin();
}

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

void test_not_connected_guard() {
    // Before connect(), move() should be a no-op
    ctrl->move(ST4AxisId::RA, ST4Direction::PLUS);
    TEST_ASSERT_EQUAL(ST4Direction::STOP, ctrl->axisDirection(ST4AxisId::RA));
    TEST_ASSERT_FALSE(ctrl->axisActive(ST4AxisId::RA));
}

void test_connect_sets_led_high() {
    ctrl->connect();
    TEST_ASSERT_EQUAL(HIGH, MockState::gpioStates[ST4_PIN_LED]);
    TEST_ASSERT_TRUE(ctrl->isConnected());
}

void test_disconnect_sets_led_low() {
    ctrl->connect();
    ctrl->disconnect();
    TEST_ASSERT_EQUAL(LOW, MockState::gpioStates[ST4_PIN_LED]);
    TEST_ASSERT_FALSE(ctrl->isConnected());
}

void test_move_ra_plus() {
    ctrl->connect();
    ctrl->move(ST4AxisId::RA, ST4Direction::PLUS);
    TEST_ASSERT_EQUAL(ST4Direction::PLUS, ctrl->axisDirection(ST4AxisId::RA));
    TEST_ASSERT_TRUE(ctrl->axisActive(ST4AxisId::RA));
}

void test_move_dec_minus() {
    ctrl->connect();
    ctrl->move(ST4AxisId::DECLINATION, ST4Direction::MINUS);
    TEST_ASSERT_EQUAL(ST4Direction::MINUS, ctrl->axisDirection(ST4AxisId::DECLINATION));
}

void test_rate_calculation_ra_plus() {
    ctrl->connect();
    ctrl->move(ST4AxisId::RA, ST4Direction::PLUS);
    // RA PLUS: 9 * RA_PER_SECOND = 9 / 3600
    MockState::advanceTime(1000000); // 1 second
    double expected = 9.0 * ST4Constants::RA_PER_SECOND;
    double pos = ctrl->position(ST4AxisId::RA);
    TEST_ASSERT_DOUBLE_WITHIN(1e-9, expected, pos);
}

void test_rate_calculation_ra_minus() {
    ctrl->connect();
    ctrl->move(ST4AxisId::RA, ST4Direction::MINUS);
    MockState::advanceTime(1000000);
    // RA MINUS: -7 * RA_PER_SECOND
    double expected = -7.0 * ST4Constants::RA_PER_SECOND;
    double pos = ctrl->position(ST4AxisId::RA);
    TEST_ASSERT_DOUBLE_WITHIN(1e-9, expected, pos);
}

void test_rate_calculation_dec_plus() {
    ctrl->connect();
    ctrl->move(ST4AxisId::DECLINATION, ST4Direction::PLUS);
    MockState::advanceTime(1000000);
    double expected = 8.0 * ST4Constants::DEGREES_PER_SECOND;
    double pos = ctrl->position(ST4AxisId::DECLINATION);
    TEST_ASSERT_DOUBLE_WITHIN(1e-9, expected, pos);
}

void test_state_snapshot() {
    ctrl->connect();
    ctrl->move(ST4AxisId::RA, ST4Direction::PLUS);
    ST4State s = ctrl->state();
    TEST_ASSERT_TRUE(s.connected);
    TEST_ASSERT_TRUE(s.ra.active);
    TEST_ASSERT_EQUAL(ST4Direction::PLUS, s.ra.direction);
    TEST_ASSERT_FALSE(s.dec.active);
    TEST_ASSERT_EQUAL(ST4Direction::STOP, s.dec.direction);
}

void test_sync() {
    ctrl->connect();
    ctrl->sync(10.0, 20.0);
    TEST_ASSERT_DOUBLE_WITHIN(1e-9, 10.0, ctrl->position(ST4AxisId::RA));
    TEST_ASSERT_DOUBLE_WITHIN(1e-9, 20.0, ctrl->position(ST4AxisId::DECLINATION));
}

void test_stop_all() {
    ctrl->connect();
    ctrl->move(ST4AxisId::RA, ST4Direction::PLUS);
    ctrl->move(ST4AxisId::DECLINATION, ST4Direction::MINUS);
    ctrl->stopAll();
    TEST_ASSERT_EQUAL(ST4Direction::STOP, ctrl->axisDirection(ST4AxisId::RA));
    TEST_ASSERT_EQUAL(ST4Direction::STOP, ctrl->axisDirection(ST4AxisId::DECLINATION));
    TEST_ASSERT_FALSE(ctrl->axisActive(ST4AxisId::RA));
    TEST_ASSERT_FALSE(ctrl->axisActive(ST4AxisId::DECLINATION));
}

void test_stop_axis() {
    ctrl->connect();
    ctrl->move(ST4AxisId::RA, ST4Direction::PLUS);
    ctrl->move(ST4AxisId::DECLINATION, ST4Direction::MINUS);
    ctrl->stopAxis(ST4AxisId::RA);
    TEST_ASSERT_EQUAL(ST4Direction::STOP, ctrl->axisDirection(ST4AxisId::RA));
    TEST_ASSERT_EQUAL(ST4Direction::MINUS, ctrl->axisDirection(ST4AxisId::DECLINATION));
}

void test_pulse_guide_not_connected() {
    bool result = ctrl->pulseGuide(ST4AxisId::RA, ST4Direction::PLUS, 500);
    TEST_ASSERT_FALSE(result);
}

void test_pulse_guide_connected() {
    ctrl->connect();
    bool result = ctrl->pulseGuide(ST4AxisId::RA, ST4Direction::PLUS, 500);
    TEST_ASSERT_TRUE(result);
}

void test_set_rates() {
    ST4RateConfig customRates = {4.0, 3.0, 2.0, 2.0};
    ctrl->setRates(customRates);
    ctrl->connect();
    ctrl->move(ST4AxisId::RA, ST4Direction::PLUS);
    MockState::advanceTime(1000000);
    double expected = 4.0 * ST4Constants::RA_PER_SECOND;
    double pos = ctrl->position(ST4AxisId::RA);
    TEST_ASSERT_DOUBLE_WITHIN(1e-9, expected, pos);
}

int main() {
    UNITY_BEGIN();
    RUN_TEST(test_not_connected_guard);
    RUN_TEST(test_connect_sets_led_high);
    RUN_TEST(test_disconnect_sets_led_low);
    RUN_TEST(test_move_ra_plus);
    RUN_TEST(test_move_dec_minus);
    RUN_TEST(test_rate_calculation_ra_plus);
    RUN_TEST(test_rate_calculation_ra_minus);
    RUN_TEST(test_rate_calculation_dec_plus);
    RUN_TEST(test_state_snapshot);
    RUN_TEST(test_sync);
    RUN_TEST(test_stop_all);
    RUN_TEST(test_stop_axis);
    RUN_TEST(test_pulse_guide_not_connected);
    RUN_TEST(test_pulse_guide_connected);
    RUN_TEST(test_set_rates);
    return UNITY_END();
}