#include "mg_obstacles/sdf.hpp"
#include <glm/gtx/norm.hpp>

namespace mg {
    double boxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t) {
        glm::dvec2 d = glm::abs(t - pos) - size;
        return glm::length(glm::max(d, 0.0)) + glm::min(glm::max(d.x, d.y), 0.0);
    }

    double circleSdf(glm::dvec2 pos, glm::dvec2 t) { return glm::length(pos - t); }

    bool inCircleSdf(glm::dvec2 pos, double rad, glm::dvec2 t, double radius) {
        return glm::length2(pos - t) < glm::pow(rad + radius, 2);
    }

    bool inBoxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t, double radius) {
        const glm::dvec2 d = glm::abs(t - pos) - size;
        // return (glm::length2(glm::max(d, 0.0))) < glm::pow(radius - glm::min(glm::max(d.x, d.y), 0.0), 2);
        return boxSdf(pos, size, t) - radius < 0;
    }

    double boxToBox(const glm::dvec2 pos,
                    const glm::dvec2 size,
                    const glm::dvec2 t,
                    const glm::dvec2 sizet,
                    const glm::dmat2 rot_mat) {

        const glm::dmat2 irot_mat = glm::transpose(rot_mat);

        glm::dvec2 pointA1 = rot_mat * (glm::dvec2{ -sizet.x, -sizet.y } * 0.5) + t;
        glm::dvec2 pointA2 = rot_mat * (glm::dvec2{ sizet.x, -sizet.y } * 0.5) + t;
        glm::dvec2 pointA3 = rot_mat * (glm::dvec2{ -sizet.x, sizet.y } * 0.5) + t;
        glm::dvec2 pointA4 = rot_mat * (glm::dvec2{ sizet.x, sizet.y } * 0.5) + t;

        double distance = boxSdf(pos, size * 0.5, pointA1);

        distance = glm::min(boxSdf(pos, size * 0.5, pointA2), distance);
        distance = glm::min(boxSdf(pos, size * 0.5, pointA3), distance);
        distance = glm::min(boxSdf(pos, size * 0.5, pointA4), distance);

        glm::dvec2 pointB1 = irot_mat * (glm::dvec2{ -size.x, -size.y } * 0.5 + pos - t);
        glm::dvec2 pointB2 = irot_mat * (glm::dvec2{ size.x, -size.y } * 0.5 + pos - t);
        glm::dvec2 pointB3 = irot_mat * (glm::dvec2{ -size.x, size.y } * 0.5 + pos - t);
        glm::dvec2 pointB4 = irot_mat * (glm::dvec2{ size.x, size.y } * 0.5 + pos - t);

        distance = glm::min(boxSdf({}, sizet * 0.5, pointB1), distance);
        distance = glm::min(boxSdf({}, sizet * 0.5, pointB2), distance);
        distance = glm::min(boxSdf({}, sizet * 0.5, pointB3), distance);
        distance = glm::min(boxSdf({}, sizet * 0.5, pointB4), distance);

        return distance;
    }
}