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

using namespace WFX;
using namespace WFX::math;

void Sphere::computeFromVertices(UInt count, math::Vector3* verts) {
//    LOG("SPHERE: computeFromVertices");
    if (count == 0) {
        Center = Vector3::ZERO;
        Radius = 0.0;
        return;
    }

    //init min and max vertices
    Vector3 kMin = verts[0];
    Vector3 kMax = kMin;

    //find largest and smalles X, Y and Z
    for (UInt i=0;i<count;i++) {
        if (kMin[0] > verts[i][0])
            kMin[0] = verts[i][0];
        if (kMin[1] > verts[i][1])
            kMin[1] = verts[i][1];
        if (kMin[2] > verts[i][2])
            kMin[2] = verts[i][2];
        if (kMax[0] < verts[i][0])
            kMax[0] = verts[i][0];
        if (kMax[1] < verts[i][1])
            kMax[1] = verts[i][1];
        if (kMax[2] < verts[i][2])
            kMax[2] = verts[i][2];
    }

    Center = (kMin+kMax);
    Center *= 0.5;

    Real fRadiusSqr = 0.0;
    for (UInt i=1;i<count;i++) {
        Vector3 kDiff = verts[i] - Center;
        Real fLengthSqr = kDiff.squaredLength();
        if (fLengthSqr > fRadiusSqr) {
            fRadiusSqr = fLengthSqr;
        }
    }
    Radius = Math::sqrt(fRadiusSqr);

}

Plane::Side Sphere::whichSide(const math::Plane& plane) const {
    Real fPseudoDistance = plane.distance(Center);
    if (fPseudoDistance <= -Radius) {
        return Plane::NEGATIVE_SIDE;
    } else if (fPseudoDistance >= Radius) {
        return Plane::POSITIVE_SIDE;
    } else {
        return Plane::NO_SIDE;
    }
}

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

scene::BoundingVolume* Sphere::copy() {
    Sphere* r = new Sphere();
    r->Center = Center;
    r->Radius = Radius;
    return r;
}

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

void Sphere::mergeMe(scene::BoundingVolume* bv) {
    bv->mergeSphere(*this);
}

void Sphere::mergeSphere(Sphere& s) {
    static const Real tolerance = 1e-06;

    Vector3 diff = Center - s.Center;

    Real fLengthSqr = diff.squaredLength();
    Real fDeltaRad = s.Radius - Radius;
    Real fDeltaRadSqr = fDeltaRad*fDeltaRad;
    Real fLength, fAlpha;

    if (fDeltaRad >= 0.0) {
        if (fDeltaRadSqr >= fLengthSqr) {
            //encloses the sphere
            Center = s.Center;
            Radius = s.Radius;
        } else {
            fLength = Math::sqrt(fLengthSqr);
            if (fLength > tolerance) {
                fAlpha = (fLength - fDeltaRad)/(2.0*fLength);
                Center = s.Center + diff*fAlpha;
            }
            Radius = 0.5*(s.Radius+fLength+Radius);
        }
    } else if (fDeltaRadSqr < fLengthSqr) {
        fLength = Math::sqrt(fLengthSqr);
        if (fLength > tolerance) {
            fAlpha = (fLength - fDeltaRad)/(2.0*fLength);
            Center = s.Center + diff*fAlpha;
        }
        Radius = 0.5*(s.Radius+fLength+Radius);
    }
}

void Sphere::mergeCube(math::Cube& c) {
    LOG("Sphere::mergeCube");
    Real hw = c.getWidth()/2;

    std::vector<Vector3> Points(8);
    Points[0].init(c.getCenter().X - hw, c.getCenter().Y + hw, c.getCenter().Z + hw);
    Points[1].init(c.getCenter().X - hw, c.getCenter().Y + hw, c.getCenter().Z - hw);
    Points[2].init(c.getCenter().X + hw, c.getCenter().Y + hw, c.getCenter().Z - hw);
    Points[3].init(c.getCenter().X + hw, c.getCenter().Y + hw, c.getCenter().Z + hw);

    Points[4].init(c.getCenter().X - hw, c.getCenter().Y - hw, c.getCenter().Z + hw);
    Points[5].init(c.getCenter().X - hw, c.getCenter().Y - hw, c.getCenter().Z - hw);
    Points[6].init(c.getCenter().X + hw, c.getCenter().Y - hw, c.getCenter().Z - hw);
    Points[7].init(c.getCenter().X + hw, c.getCenter().Y - hw, c.getCenter().Z + hw);

    Vector3 newCent = Center + c.getCenter();
    newCent /= 2;

    Vector3 diff = Center - newCent;
    Real newRad = Radius + diff.length();
   
    for (UInt i=0; i<8; i++) {
        diff = Center - Points[i];
        Real len = diff.length();
        if (len > newRad) newRad = len;
    }
    
    Center = newCent;
    Radius = newRad;
}

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

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

void Sphere::printDebug() {
    LOG("SPHERE ( " << Center.X << "," << Center.Y << "," << Center.Z << ") " << Radius);
}


