#include "m2_coord.h"

#include <cmath>
#include <cstdio>

namespace fmt::m2coord {

std::string tile_dir(int tile_x, int tile_y) {
    char buf[16];
    std::snprintf(buf, sizeof(buf), "%06u", tile_id(tile_x, tile_y));
    return buf;
}

namespace {
Mat3 mul(const Mat3& a, const Mat3& b) {
    Mat3 r{};
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            for (int k = 0; k < 3; ++k)
                r.m[i * 3 + j] += a.m[i * 3 + k] * b.m[k * 3 + j];
    return r;
}
Mat3 rot_x(double a) {
    double c = std::cos(a), s = std::sin(a);
    return {{1, 0, 0, 0, c, -s, 0, s, c}};
}
Mat3 rot_y(double a) {
    double c = std::cos(a), s = std::sin(a);
    return {{c, 0, s, 0, 1, 0, -s, 0, c}};
}
Mat3 rot_z(double a) {
    double c = std::cos(a), s = std::sin(a);
    return {{c, -s, 0, s, c, 0, 0, 0, 1}};
}
} // namespace

Mat3 ypr_basis(double yaw_deg, double pitch_deg, double roll_deg) {
    const double d2r = 3.14159265358979323846 / 180.0;
    return mul(mul(rot_y(yaw_deg * d2r), rot_x(pitch_deg * d2r)), rot_z(roll_deg * d2r));
}

Mat3 object_basis_godot(double yaw_deg, double pitch_deg, double roll_deg) {
    // Conjugate the Metin2-space rotation through the Z-up -> Y-up axis swap
    // C = rotate(-90deg, X): (x,y,z) -> (x,z,-y).   R_godot = C * R_m2 * C^-1
    const Mat3 C{{1, 0, 0, 0, 0, 1, 0, -1, 0}};
    const Mat3 Ci{{1, 0, 0, 0, 0, -1, 0, 1, 0}};
    return mul(mul(C, ypr_basis(yaw_deg, pitch_deg, roll_deg)), Ci);
}

bool parse_tile_dir(const std::string& name, int& tile_x, int& tile_y) {
    if (name.size() != 6) return false;
    for (char c : name)
        if (c < '0' || c > '9') return false;
    long id = std::strtol(name.c_str(), nullptr, 10);
    tile_x = int(id / 1000);
    tile_y = int(id % 1000);
    return true;
}

} // namespace fmt::m2coord