/**
 * @file TestableUTest.cpp
 * @author Gustice
 * @brief
 */

#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN
#include "doctest/doctest.h"
#include <iostream>
#include <numbers>

#include "watchFace.hpp"

class StepperMotorMock : public IStepperMotor {
  public:
    void moveBy(float angle, uint8_t profile = 0) override {
        lastAngle = angle;
        lastProfile = profile;
    }
    void moveBy(float angle, uint8_t profile, float accel, float speed, std::function<void(int diff)> profileDone = nullptr) override {
        lastAngle = angle;
        lastProfile = profile;
    }
    float getScaledPos() const override {
        return nextPos;
    }
    uint16_t getQuanted() const override {
        return 0;
    }

    float lastAngle{};
    uint8_t lastProfile{};
    float lastAccel{};
    float lastSpeed{};
    float nextPos{};
};

class LoggerMock : public ILogging {
    void output(Severity severity, std::string message) override {
        // do nothing
    }
};

void onFin(uint8_t idx) {
}

class WatchFaceFixture {
    using PositionStore = StepperMotor::Position<50, 5>;

  public:
  protected:
    static constexpr int MicroSteps = 32;
    StepperMotorMock motor;
    LoggerMock logger;
    WatchMotor sut{motor, onFin, logger};

  private:
};

TEST_CASE_FIXTURE(WatchFaceFixture, "Moving simple steps") {
    auto next = 0*std::numbers::pi/3;
    motor.nextPos = next;
    sut.moveTo(1, 0, 0, 1, 1);
    auto delta = 1*std::numbers::pi/3;
    CHECK(motor.lastAngle == doctest::Approx(delta).epsilon(0.000625)); // MicroStep tolerance
    
    motor.nextPos = 1.0F/6;
    sut.moveTo(2, 0, 0, 1, 1);
    CHECK(motor.lastAngle == doctest::Approx(delta).epsilon(0.000625)); // MicroStep tolerance

    motor.nextPos = 2.0F/6;
    sut.moveTo(3, 0, 0, 1, 1);
    CHECK(motor.lastAngle == doctest::Approx(delta).epsilon(0.000625)); // MicroStep tolerance

    motor.nextPos = 3.0F/6;
    sut.moveTo(4, 0, 0, 1, 1);
    CHECK(motor.lastAngle == doctest::Approx(delta).epsilon(0.000625)); // MicroStep tolerance

    motor.nextPos = 4.0F/6;
    sut.moveTo(5, 0, 0, 1, 1);
    CHECK(motor.lastAngle == doctest::Approx(delta).epsilon(0.000625)); // MicroStep tolerance

    motor.nextPos = 5.0F/6;
    sut.moveTo(6, 0, 0, 1, 1);
    CHECK(motor.lastAngle == doctest::Approx(delta).epsilon(0.000625)); // MicroStep tolerance

    motor.nextPos = 6.0F/6;
    sut.moveTo(1, 0, 0, 1, 1);
    CHECK(motor.lastAngle == doctest::Approx(delta).epsilon(0.000625)); // MicroStep tolerance

    motor.nextPos = 1.0F/6;
    sut.moveTo(2, 0, 0, 1, 1);
    CHECK(motor.lastAngle == doctest::Approx(delta).epsilon(0.000625)); // MicroStep tolerance
}

TEST_CASE_FIXTURE(WatchFaceFixture, "Moving multiple turns") {
    auto next = 0*std::numbers::pi/3;
    motor.nextPos = next;
    sut.moveTo(1, 0, 0, 1, 1);
    auto delta = 1*std::numbers::pi/3;
    CHECK(motor.lastAngle == doctest::Approx(delta).epsilon(0.000625)); // MicroStep tolerance
    
    motor.nextPos = 1.0F/6;
    sut.moveTo(1, 0, 0, 1, 1);
    CHECK(motor.lastAngle == doctest::Approx(6*delta).epsilon(0.000625)); // MicroStep tolerance

    motor.nextPos = 1.0F/6;
    sut.moveTo(1, 0, 7, 1, 1);
    CHECK(motor.lastAngle == doctest::Approx(12*delta).epsilon(0.000625)); // MicroStep tolerance
}