
#include <WFX/renderer/Camera.h>

#include <WFX/math/Matrix4.h>
#include <WFX/renderer/Renderer.h>

using namespace WFX;
using namespace WFX::renderer;
using namespace WFX::math;

Camera::Camera() : Position(0,0,-6), View(0,0,0), Up(0, 1, 0) {
    PMode = PM_PERSPECTIVE;
    RMode = RM_SOLID;
    FOVY = 45.0f;
    Far = 150.0f;
    Near = 1.0f;
}

Camera::~Camera() {
}

void Camera::init(math::Vector3& p, math::Vector3& v, math::Vector3& u) {
    Position = p;
    View = v;
    Up = u;
}

void Camera::walk(Real r) {
    Vector3 v = View - Position;
    v.normalize();
    Position.X += v.X * r;
    Position.Y += v.Y * r;
    Position.Z += v.Z * r;
    View.X += v.X * r;
    View.Y += v.Y * r;
    View.Z += v.Z * r;
}

void Camera::strafe(Real r) {
    Vector3 v = View - Position;
    v = v.cross(Up);
    v.normalize();
    Position.X += v.X * r;
    Position.Z += v.Z * r;
    View.X += v.X * r;
    View.Z += v.Z * r;
}

void Camera::rotate(Real angle, Vector3& axis) {
    //new view
    Vector3 nv;
    //view direction
    Vector3 v = View - Position;
    //calc sine and cosine only once
    Real cosa = Math::cos(angle);
    Real sina = Math::sin(angle);
    //Find X 
    nv.X  = (cosa + (1 - cosa) * axis.X * axis.X)           * v.X;
    nv.X += ((1 - cosa) * axis.X * axis.Y - axis.Z * sina)  * v.Y;
    nv.X += ((1 - cosa) * axis.X * axis.Y + axis.Y * sina)  * v.Z;
    //Find Y
    nv.Y  = ((1 - cosa) * axis.X * axis.Y + axis.Z * sina)  * v.X;
    nv.Y += (cosa + (1 - cosa) * axis.Y * axis.Y)           * v.Y;
    nv.Y += ((1 - cosa) * axis.Y * axis.Z - axis.X * sina)  * v.Z;
    //Find Y
    nv.Z  = ((1 - cosa) * axis.X * axis.Z - axis.Y * sina)  * v.X;
    nv.Z += ((1 - cosa) * axis.Y * axis.Z + axis.X * sina)  * v.Y;
    nv.Z += (cosa + (1 - cosa) * axis.Z * axis.Z)           * v.Z;

    //set new view
    View = Position + nv;
    
}

void Camera::updateFrustum() {
    //if (PMode == PM_ORTHOGRAPHIC) {
        //FIXME...
    //} else if (PMode == PM_PERSPECTIVE) {
        Matrix4 proj;                               // This will hold our projection matrix
        Matrix4 modl;                               // This will hold our modelview matrix
                
        Renderer* rend = Renderer::getInstancePtr();

        rend->getProjectionMatrix(proj);
        rend->getModelViewMatrix(modl);
       
        Matrix4 mul = modl * proj;
        Real* ptr = (Real*)&mul;

        //RIGHT side of frustum
        CullPlanes[FRUSTUM_RIGHT].setNormal(ptr[3] - ptr[0],
                                            ptr[7] - ptr[4],
                                            ptr[11] - ptr[8]);
        CullPlanes[FRUSTUM_RIGHT].setConstant(ptr[15] - ptr[12]);
        CullPlanes[FRUSTUM_RIGHT].normalize();
        
        //LEFT side of frustum
        CullPlanes[FRUSTUM_LEFT].setNormal(ptr[3] + ptr[0],
                                           ptr[7] + ptr[4],
                                           ptr[11] + ptr[8]);
        CullPlanes[FRUSTUM_LEFT].setConstant(ptr[15] + ptr[12]);
        CullPlanes[FRUSTUM_LEFT].normalize();
        
        //BOTTOM side of frustum
        CullPlanes[FRUSTUM_BOTTOM].setNormal(ptr[3] + ptr[1],
                                             ptr[7] + ptr[5],
                                             ptr[11] + ptr[9]);
        CullPlanes[FRUSTUM_BOTTOM].setConstant(ptr[15] + ptr[13]);
        CullPlanes[FRUSTUM_BOTTOM].normalize();
        
        //TOP side of frustum
        CullPlanes[FRUSTUM_TOP].setNormal(ptr[3] - ptr[1],
                                          ptr[7] - ptr[5],
                                          ptr[11] - ptr[9]);
        CullPlanes[FRUSTUM_TOP].setConstant(ptr[15] - ptr[13]);
        CullPlanes[FRUSTUM_TOP].normalize();
        
        //FAR side of frustum
        CullPlanes[FRUSTUM_FAR].setNormal(ptr[3] - ptr[2],
                                          ptr[7] - ptr[6],
                                          ptr[11] - ptr[10]);
        CullPlanes[FRUSTUM_FAR].setConstant(ptr[15] - ptr[14]);
        CullPlanes[FRUSTUM_FAR].normalize();
        
        //NEAR side of frustum
        CullPlanes[FRUSTUM_NEAR].setNormal(ptr[3] + ptr[2],
                                           ptr[7] + ptr[6],
                                           ptr[11] + ptr[10]);
        CullPlanes[FRUSTUM_NEAR].setConstant(ptr[15] + ptr[14]);
        CullPlanes[FRUSTUM_NEAR].normalize();
        
         
    //}
}

bool Camera::culled(scene::BoundingVolume* b) {
    //b->printDebug();
    //LOGN("culled? ... ");
   
    for (UInt i=FRUSTUM_NEAR;i<FRUSTUM_MAX;i++) {
        if (b->whichSide(CullPlanes[i]) == Plane::NEGATIVE_SIDE) {
            //LOG("Yes");
            return true;
        }
    }
    //LOG("Nope");
    return false;
}

bool Camera::pointInFrustum(WFX::math::Vector3& p) {
    for (UInt i=FRUSTUM_NEAR;i<FRUSTUM_MAX;i++) {
        if (CullPlanes[i].side(p) == Plane::NEGATIVE_SIDE) {
            return false;
        }
    }
    return true;
}

void Camera::printDebug() {
    LOG("Camdebug");

}


