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

void setUp() { MockState::reset(); }
void tearDown() {}

void test_initial_position_zero() {
    ST4Tracker tracker;
    tracker.begin();
    TEST_ASSERT_DOUBLE_WITHIN(1e-9, 0.0, tracker.position());
}

void test_position_accumulation() {
    ST4Tracker tracker;
    tracker.begin();
    double rate = 2.5;
    tracker.start(rate);
    // Advance 1 second
    MockState::advanceTime(1000000);
    double pos = tracker.position();
    TEST_ASSERT_DOUBLE_WITHIN(1e-6, 2.5, pos);
}

void test_stop_freezes_position() {
    ST4Tracker tracker;
    tracker.begin();
    tracker.start(3.0);
    MockState::advanceTime(1000000); // 1 second -> pos = 3.0
    tracker.stop();
    double posAtStop = tracker.position();
    // Advance more time -- position should not change
    MockState::advanceTime(5000000);
    double posLater = tracker.position();
    TEST_ASSERT_DOUBLE_WITHIN(1e-9, posAtStop, posLater);
    TEST_ASSERT_DOUBLE_WITHIN(1e-6, 3.0, posLater);
}

void test_set_position() {
    ST4Tracker tracker;
    tracker.begin();
    tracker.setPosition(42.0);
    TEST_ASSERT_DOUBLE_WITHIN(1e-9, 42.0, tracker.position());
}

void test_direction_sign_positive() {
    ST4Tracker tracker;
    tracker.begin();
    tracker.start(1.0);
    MockState::advanceTime(1000000);
    TEST_ASSERT_TRUE(tracker.position() > 0.0);
}

void test_direction_sign_negative() {
    ST4Tracker tracker;
    tracker.begin();
    tracker.start(-1.0);
    MockState::advanceTime(1000000);
    TEST_ASSERT_TRUE(tracker.position() < 0.0);
}

void test_slew_rate_while_moving() {
    ST4Tracker tracker;
    tracker.begin();
    tracker.start(5.0);
    TEST_ASSERT_DOUBLE_WITHIN(1e-9, 5.0, tracker.slewRate());
}

void test_slew_rate_after_stop() {
    ST4Tracker tracker;
    tracker.begin();
    tracker.start(5.0);
    tracker.stop();
    TEST_ASSERT_DOUBLE_WITHIN(1e-9, 0.0, tracker.slewRate());
}

void test_is_moving() {
    ST4Tracker tracker;
    tracker.begin();
    TEST_ASSERT_FALSE(tracker.isMoving());
    tracker.start(1.0);
    TEST_ASSERT_TRUE(tracker.isMoving());
    tracker.stop();
    TEST_ASSERT_FALSE(tracker.isMoving());
}

void test_rate_change_accumulates() {
    ST4Tracker tracker;
    tracker.begin();
    tracker.start(2.0);
    MockState::advanceTime(1000000); // 1s at rate 2.0 -> 2.0
    tracker.start(4.0); // changes rate, accumulates previous delta
    MockState::advanceTime(1000000); // 1s at rate 4.0 -> +4.0
    tracker.stop();
    double pos = tracker.position();
    TEST_ASSERT_DOUBLE_WITHIN(1e-6, 6.0, pos);
}

void test_set_position_stops_movement() {
    ST4Tracker tracker;
    tracker.begin();
    tracker.start(1.0);
    MockState::advanceTime(1000000);
    tracker.setPosition(100.0);
    TEST_ASSERT_FALSE(tracker.isMoving());
    TEST_ASSERT_DOUBLE_WITHIN(1e-9, 100.0, tracker.position());
}

int main() {
    UNITY_BEGIN();
    RUN_TEST(test_initial_position_zero);
    RUN_TEST(test_position_accumulation);
    RUN_TEST(test_stop_freezes_position);
    RUN_TEST(test_set_position);
    RUN_TEST(test_direction_sign_positive);
    RUN_TEST(test_direction_sign_negative);
    RUN_TEST(test_slew_rate_while_moving);
    RUN_TEST(test_slew_rate_after_stop);
    RUN_TEST(test_is_moving);
    RUN_TEST(test_rate_change_accumulates);
    RUN_TEST(test_set_position_stops_movement);
    return UNITY_END();
}