#pragma once
#include "Vec2.h"

namespace MC {

// Represents position (mm) + heading (degrees, 0=east, CCW positive)
struct Transform2D {
    Vec2   pos;
    double heading = 0.0; // degrees

    Transform2D() = default;
    Transform2D(Vec2 pos, double heading) : pos(pos), heading(heading) {}
    Transform2D(double x, double y, double heading) : pos(x, y), heading(heading) {}

    // Apply this transform to a local point
    Vec2 transformPoint(const Vec2& local) const {
        return pos + local.rotated(heading);
    }

    // Compose: apply other transform in local space of this one
    Transform2D operator*(const Transform2D& other) const {
        return {
            transformPoint(other.pos),
            heading + other.heading
        };
    }

    // Normalise heading to [0, 360)
    static double normalizeAngle(double deg) {
        while (deg <    0.0) deg += 360.0;
        while (deg >= 360.0) deg -= 360.0;
        return deg;
    }

    double normalizedHeading() const { return normalizeAngle(heading); }

    // Return the transform reversed (flip direction, used for piece connections)
    Transform2D reversed() const {
        return { pos, normalizeAngle(heading + 180.0) };
    }
};

} // namespace MC
