#include "PosTracker.hpp"
#include <SFML/System/Vector2.hpp>

using sf::Vector2f;
using sf::Transform;

void PosTracker::reset(Vector2f initialPos, float initialRot) {
    transform = transformFromPosRot(initialPos, initialRot);
}

void PosTracker::update(std::size_t value_left, std::size_t value_right) {
    auto leftDistance = (value_left - _lastLeftValue) * wheelIncrementDistance() * _wheel_dir_left;
    auto rightDistance = (value_right - _lastRightValue) * wheelIncrementDistance() * _wheel_dir_right;
    updateTransform(leftDistance, rightDistance);
    _lastLeftValue = value_left;
    _lastRightValue = value_right;
}

void PosTracker::updateTransform(float leftDistance, float rightDistance) {
    float linearStep =  (leftDistance + rightDistance) / 2.0;
    float angularStep = (leftDistance - rightDistance) / _wheelDistance * 180.0 / std::numbers::pi;

    auto orientation = Transform(sf::Transform().rotate(transform.getRotation()));
    auto new_ = transformFromPosRot(
        transform.getPosition() + orientation.transformPoint(sf::Vector2f(linearStep, 0.0)),
        transform.getRotation() + angularStep
    );
    transform = new_;
}

Vector2f PosTracker::get_pos() { return this->transform.getPosition(); }
float PosTracker::get_rot() { return this->transform.getRotation(); }

void PosTracker::hint_right(bool forward) {
    _wheel_dir_right = forward ? 1 : -1;
}
void PosTracker::hint_left(bool forward) {
    _wheel_dir_left = forward ? 1 : -1;
}