/**
 * ------------------------------------
 * @author: Weipeng Kong
 * @date: 2021/11/17
 * @email: yjxkwp@foxmail.com
 * @site: https://donot.fit
 * @description:
 * ------------------------------------
**/

#include "../include/Octree/AABB.h"
#include "Eigen/Dense"


/********************* TOOLS ************************************/

namespace AABBTools {
    static bool AXISTEST(double rad, double p0, double p1) {
        return (std::min(p0, p1) > rad || std::max(p0, p1) < -rad);
    }

    inline double vec_min_component(double a, double b, double c) {
        return std::min(std::min(a, b), c);
    }

    inline double vec_max_component(double a, double b, double c) {
        return std::max(std::max(a, b), c);
    }

    bool directionTest(double a, double b, double c, double hs) {
        return (vec_min_component(a, b, c) > hs || vec_max_component(a, b, c) < -hs);
    }

    bool planeBoxOverlap(const Eigen::Vector3d &normal, double d, const Eigen::Vector3d &hs) {
        Eigen::Vector3d vmin = {(normal[0] > 0.0) ? -hs[0] : +hs[0],
                                (normal[1] > 0.0) ? -hs[1] : +hs[1],
                                (normal[2] > 0.0) ? -hs[2] : +hs[2]};

        if (normal.dot(vmin) + d > 0.0) return false;
        Eigen::Vector3d vmax = -vmin;
        if (normal.dot(vmax) + d >= 0.0) return true;
        return false;
    }
}

/*********************************************************/



Octree::AABB::AABB(const Eigen::Vector3d &min, const Eigen::Vector3d &max) : min(min), max(max) {

}

Octree::AABB::AABB(double minx, double maxx, double miny, double maxy, double minz, double maxz) :
        min(minx, miny, minz), max(maxx, maxy, maxz) {
}


Octree::AABB::AABB(const std::array<double, 6> &aabb) :
        AABB(aabb[0], aabb[1], aabb[2], aabb[3], aabb[4], aabb[5]) {
}


Eigen::Vector3d Octree::AABB::size() const {
    return max - min;
}


Eigen::Vector3d Octree::AABB::get_center() const {
    return (max + min) / 2.0;
}

Eigen::Vector3d Octree::AABB::get_min() const {
    return min;
}

Eigen::Vector3d Octree::AABB::get_max() const {
    return max;
}

bool Octree::AABB::intersect_triangle(const Eigen::Vector3d &a,
                                         const Eigen::Vector3d &b,
                                         const Eigen::Vector3d &c) const {
    using Eigen::Vector3d;

    Vector3d center = this->get_center();
    Vector3d hs = this->size() / 2;
    Vector3d v0 = a - center;
    Vector3d v1 = b - center;
    Vector3d v2 = c - center;

    // EDGE 0
    Vector3d e0 = v1 - v0;
    Vector3d ea = e0.cwiseAbs();
    Vector3d e_v0 = e0.cross(v0);
    Vector3d e_v1 = e0.cross(v1);
    Vector3d e_v2 = e0.cross(v2);

    if (AABBTools::AXISTEST(ea[2] * hs[1] + ea[1] * hs[2], e_v0[0], e_v2[0])) return false; // X
    if (AABBTools::AXISTEST(ea[2] * hs[0] + ea[0] * hs[2], e_v0[1], e_v2[1])) return false; // Y
    if (AABBTools::AXISTEST(ea[1] * hs[0] + ea[0] * hs[1], e_v1[2], e_v2[2])) return false; // Z

    // EDGE 1
    Vector3d e1 = v2 - v1;
    ea = e1.cwiseAbs();
    e_v0 = e1.cross(v0);
    e_v1 = e1.cross(v1);
    e_v2 = e1.cross(v2);

    if (AABBTools::AXISTEST(ea[2] * hs[1] + ea[1] * hs[2], e_v0[0], e_v2[0])) return false;
    if (AABBTools::AXISTEST(ea[2] * hs[0] + ea[0] * hs[2], e_v0[1], e_v2[1])) return false;
    if (AABBTools::AXISTEST(ea[1] * hs[0] + ea[0] * hs[1], e_v0[2], e_v1[2])) return false;

    // EDGE 2
    Vector3d e2 = v0 - v2;
    ea = e2.cwiseAbs();
    e_v0 = e2.cross(v0);
    e_v1 = e2.cross(v1);
    e_v2 = e2.cross(v2);

    if (AABBTools::AXISTEST(ea[2] * hs[1] + ea[1] * hs[2], e_v0[0], e_v1[0])) return false;
    if (AABBTools::AXISTEST(ea[2] * hs[0] + ea[0] * hs[2], e_v0[1], e_v1[1])) return false;
    if (AABBTools::AXISTEST(ea[1] * hs[0] + ea[0] * hs[1], e_v1[2], e_v2[2])) return false;


    // Bullet 1:
    //   First test overlap in the {x,y,z}-directions.
    //   Find min, max of the triangle each direction, and test for overlap in that
    //   direction -- this is equivalent to testing a minimal AABB around the triangle against the AABB.
    if (AABBTools::directionTest(v0[0], v1[0], v2[0], hs[0])) return false; // Test in X-direction.
    if (AABBTools::directionTest(v0[1], v1[1], v2[1], hs[1])) return false; // Test in Y-direction.
    if (AABBTools::directionTest(v0[2], v1[2], v2[2], hs[2])) return false; // Test in Z-direction.


    // Bullet 2:
    //   Test if the box intersects the plane of the triangle. Compute plane equation of triangle: normal*x+d=0.
    Vector3d normal = e0.cross(e1);
    double d = -normal.dot(v0);  // plane eq: normal.x+d=0
    if (!AABBTools::planeBoxOverlap(normal, d, hs)) return false;

    return true; // box and triangle overlaps
}


bool Octree::AABB::contains(const Eigen::Vector3d &v) const {
    return (v.x() >= min.x() && v.y() >= min.y() && v.z() >= min.z()) &&
           (v.x() <= max.x() && v.y() <= max.y() && v.z() <= max.z());
}

bool Octree::AABB::fully_contains_triangle(const Eigen::Vector3d &a, const Eigen::Vector3d &b,
                                              const Eigen::Vector3d &c) const {
    return contains(a) && contains(b) && contains(c);
}

std::array<Octree::AABB, 8> Octree::AABB::half_divide() const {
    std::array<AABB, 8> aabbs;
    auto half_size = this->size() / 2.0;
    for (int i = 0; i < 8; ++i) {
        Eigen::Vector3d sub_min = {min[0] + (((i & 4) > 0) ? half_size[0] : 0),
                                   min[1] + (((i & 2) > 0) ? half_size[1] : 0),
                                   min[2] + (((i & 1) > 0) ? half_size[2] : 0)};
        Eigen::Vector3d sub_max = sub_min + half_size;
        aabbs[i] = AABB(sub_min, sub_max);
    }
    return std::move(aabbs);
}

void Octree::AABB::extend(double step) {
    this->min -= Eigen::Vector3d(step, step, step);
    this->max += Eigen::Vector3d(step, step, step);
}