
#include <WFX/math/Cube.h>
#include <WFX/renderer/Renderer.h>
#include <WFX/scene/SceneGraphVisitor.h>

using namespace WFX;
using namespace WFX::math;
using namespace WFX::scene;

void Cube::transformBy(math::Matrix3& rot, math::Vector3& trans, Real scale) {
    Center *= scale;
    Center += trans;
    Width *= scale;
}

void Cube::computeFromVertices(UInt vcount, math::Vector3* verts) {
    if (vcount == 0) {
        Center = Vector3::ZERO;
        Width = 0.0;
        return;
    }

    //this block determines the center of the outer block
    //add all vertices to the center vertex
    for (UInt i=0; i<vcount; i++) {
        Center += verts[i];
    }
    //
    Center /= vcount;
    
    //the next block determines the cube dimensions
    Vector3 maxDims(0,0,0);
    for (UInt i=0; i<vcount; i++) {
        //determine distance to Center
        Vector3 currDims = verts[i] - Center;
        //use absolute Values...
        currDims.abs();
        //check whether any of the current dimensions
        //is larger than the max dimensions
        maxDims.X = (maxDims.X > currDims.X)?maxDims.X:currDims.X;
        maxDims.Y = (maxDims.Y > currDims.Y)?maxDims.Y:currDims.Y;
        maxDims.Z = (maxDims.Z > currDims.Z)?maxDims.Z:currDims.Z;

    }

    //set real width
    maxDims *= 2;
    
    //determine max dimension
    Width = (maxDims.X > maxDims.Y)?maxDims.X:maxDims.Y;
    Width = (Width > maxDims.Z)?Width:maxDims.Z;
    
}

math::Plane::Side Cube::whichSide(const math::Plane& plane) const {
    //first we determine the side the center of the cube is on
    //then we check whether each point of the cube lie on the same side
    //of the plane
    Vector3 n = plane.getNormal(); //plane normal
    Real D = plane.getConstant();
    Real s = Width/2; //half width

    Real pdist = plane.distance(Center);

    if (pdist > 0) {
        if (n.X*(Center.X - s) + n.Y*(Center.Y - s) + n.Z*(Center.Z - s) + D < 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X + s) + n.Y*(Center.Y - s) + n.Z*(Center.Z - s) + D < 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X - s) + n.Y*(Center.Y + s) + n.Z*(Center.Z - s) + D < 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X + s) + n.Y*(Center.Y + s) + n.Z*(Center.Z - s) + D < 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X - s) + n.Y*(Center.Y - s) + n.Z*(Center.Z + s) + D < 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X + s) + n.Y*(Center.Y - s) + n.Z*(Center.Z + s) + D < 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X - s) + n.Y*(Center.Y + s) + n.Z*(Center.Z + s) + D < 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X + s) + n.Y*(Center.Y + s) + n.Z*(Center.Z + s) + D < 0)
            return Plane::NO_SIDE;
        //if we reach this point the cube is completely on the positive side of the plane
        return Plane::POSITIVE_SIDE;
    } else if (pdist < 0) {
        if (n.X*(Center.X - s) + n.Y*(Center.Y - s) + n.Z*(Center.Z - s) + D > 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X + s) + n.Y*(Center.Y - s) + n.Z*(Center.Z - s) + D > 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X - s) + n.Y*(Center.Y + s) + n.Z*(Center.Z - s) + D > 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X + s) + n.Y*(Center.Y + s) + n.Z*(Center.Z - s) + D > 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X - s) + n.Y*(Center.Y - s) + n.Z*(Center.Z + s) + D > 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X + s) + n.Y*(Center.Y - s) + n.Z*(Center.Z + s) + D > 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X - s) + n.Y*(Center.Y + s) + n.Z*(Center.Z + s) + D > 0)
            return Plane::NO_SIDE;
        if (n.X*(Center.X + s) + n.Y*(Center.Y + s) + n.Z*(Center.Z + s) + D > 0)
            return Plane::NO_SIDE;
        //if we reach this point the cube is completely on the negative side of the plane
        return Plane::NEGATIVE_SIDE;
    } else {
        return Plane::NO_SIDE;
    }
       
}

BoundingVolume* Cube::copy() {
    return clone();
}

Cube* Cube::clone() {
    Cube* r = new Cube();
    r->Center = Center;
    r->Width = Width;
    return r;
}

void Cube::merge(BoundingVolume* bv) {
    bv->mergeMe(this);
}

void Cube::mergeMe(BoundingVolume* bv) {
    bv->mergeCube(*this);
}

void Cube::mergeSphere(math::Sphere& s) {
    THROW("Cube::mergeSphere not implemented");
}

void Cube::mergeCube(math::Cube& c) {
    THROW("Cube::mergeCube -> not yet implemented");
}

void Cube::draw(WFX::renderer::Renderer* r) {
    r->drawWireCube(*this);
}

void Cube::accept(WFX::scene::SceneGraphVisitor* v) {
    v->visitCube(*this);
}

void Cube::printDebug() {
    LOG("CUBE:");
    Center.printDebug();
    LOG("Width: " << Width);
}

