#include "Camera3D.h"
#include "../engine/Wrapper.h"
#include <cmath>
#include "../engine/Utils.h"

Camera3D::Camera3D()
{
}

Camera3D::~Camera3D()
{
}

const Vector3D& Camera3D::getFlatFront() const
{
    return flatFront;
}

const Vector3D& Camera3D::getFlatBack() const
{
    return flatBack;
}

const Vector3D& Camera3D::getFlatRight() const
{
    return flatRight;
}

const Vector3D& Camera3D::getFlatLeft() const
{
    return flatLeft;
}

const Vector3D& Camera3D::getFlatUp() const
{
    return flatUp;
}

const Vector3D& Camera3D::getFlatDown() const
{
    return flatDown;
}

void Camera3D::addToOldLengthAngle(float f)
{
    oldLengthAngle += f;
}

void Camera3D::storePosition()
{
    oldCamera.set(camera);
    oldLengthAngle = lengthAngle;
    oldWidthAngle = widthAngle;
}

void Camera3D::setPosition(float x, float y, float z, float length, float width)
{
    camera.set(x, y, z);
    lengthAngle = length;
    widthAngle = width;
}

float old = 0;
float current = 0;

void Camera3D::update(float lag)
{
    // -------------------------------------------------------------------------
    // calculate vectors for the view matrix
    // -------------------------------------------------------------------------
    
    // front
    front.setAngles(interpolate(lag, oldLengthAngle, lengthAngle), interpolate(lag, oldWidthAngle, widthAngle));
    
    // back
    back.setInverse(front);

    // right
    right.set(front);
    right.cross(0.0f, 1.0f, 0.0f);
    right.normalize();
    
    // left
    left.setInverse(right);
    
    // up
    up.set(front);
    up.cross(left);
    up.normalize();
    
    // down
    down.setInverse(up);
    
    Vector3D interCamera = oldCamera;
    interCamera.addMul(camera, lag);
    interCamera.addMul(oldCamera, -lag);
    
    view.set(0, 0, right.getX());
    view.set(0, 1, right.getY());
    view.set(0, 2, right.getZ());
    view.set(0, 3, right.dotInverse(interCamera));
    
    view.set(1, 0, up.getX());
    view.set(1, 1, up.getY());
    view.set(1, 2, up.getZ());
    view.set(1, 3, up.dotInverse(interCamera));
    
    view.set(2, 0, back.getX());
    view.set(2, 1, back.getY());
    view.set(2, 2, back.getZ());
    view.set(2, 3, back.dotInverse(interCamera));
    
    view.set(3, 0, 0.0f);
    view.set(3, 1, 0.0f);
    view.set(3, 0, 0.0f);
    view.set(3, 3, 1.0f);
   
    // -------------------------------------------------------------------------
    // calculate flat vectors for movement
    // -------------------------------------------------------------------------
    
    // front
    flatFront.set(front);
    flatFront.setY(0.0f);
    flatFront.normalize();
    
    // back
    flatBack.setInverse(flatFront);

    // right
    flatRight.set(flatFront);
    flatRight.cross(0.0f, 1.0f, 0.0f);
    flatRight.normalize();
    
    // left
    flatLeft.setInverse(flatRight);

    // up
    flatUp.set(0.0f, 1.0f, 0.0f);
    
    // down
    flatDown.setInverse(flatUp);
    
    // -------------------------------------------------------------------------
    // update frustum planes
    // -------------------------------------------------------------------------
    
    // http://cgvr.informatik.uni-bremen.de/teaching/cg_literatur/lighthouse3d_view_frustum_culling/index.html
    float tan = tanf((0.5f * Engine::getFieldOfView()) * M_PI / 180.0f);
    float aspect = (float) Engine::getWidth() / Engine::getHeight();
    
    float nearHigh = tan * Engine::getNearClip();
    float nearWidth = nearHigh * aspect;

    float farHigh = tan * Engine::getFarClip();
    float farWidth = farHigh * aspect;

    Vector3D fc = interCamera;
    fc.addMul(front, Engine::getFarClip());

    Vector3D ftl = fc;
    ftl.addMul(left, farWidth);
    ftl.addMul(up, farHigh);

    Vector3D fbl = fc;
    fbl.addMul(left, farWidth);
    fbl.addMul(down, farHigh);

    Vector3D fbr = fc;
    fbr.addMul(right, farWidth);
    fbr.addMul(down, farHigh);

    Vector3D nc = interCamera;
    nc.addMul(front, Engine::getNearClip());

    Vector3D ntl = nc;
    ntl.addMul(left, nearWidth);
    ntl.addMul(down, nearHigh);

    Vector3D ntr = nc;
    ntr.addMul(right, nearWidth);
    ntr.addMul(up, nearHigh);

    Vector3D nbr = nc;
    nbr.addMul(right, nearWidth);
    nbr.addMul(down, nearHigh);

    // generating planes with counter clockwise vector order
    frustumPlanes[0].set(fbl, ftl, fbr); // far
    frustumPlanes[1].set(ntl, ftl, fbl); // left
    frustumPlanes[2].set(fbr, ntr, nbr); // right
    frustumPlanes[3].set(fbl, fbr, nbr); // bottom
    frustumPlanes[4].set(ntr, ftl, ntl); // top
    frustumPlanes[5].set(nbr, ntr, ntl); // near
}

const float* Camera3D::getViewMatrix()
{
    return view.getValues();
}

bool Camera3D::isInFrustum(float x, float y, float z, float x2, float y2, float z2) const
{
    // some tolerance
    x -= 1;
    y -= 1;
    z -= 1;
    x2 += 1;
    y2 += 1;
    z2 += 1;
    //return true;
    // http://cgvr.informatik.uni-bremen.de/teaching/cg_literatur/lighthouse3d_view_frustum_culling/index.html
    // for each plane do ...
    for(int fp = 0; fp < 6; fp++) 
    {
        // reset counters for corners in and out
        int out = 0;
        int in = 0;
        // for each corner of the box do ...
        // get out of the cycle as soon as a box as corners
        // both inside and out of the frustum
        for(int i = 0; i < 8 && (in == 0 || out == 0); i++) 
        {
            // is the corner outside or inside
            if(frustumPlanes[fp].getSignedDistance(((i >> 2) & 1) ? x : x2, ((i >> 1) & 1) ? y : y2, (i & 1) ? z : z2) > 0)
            {
                out++;
            }
            else
            {
                in++;
            }
        }
        //if all corners are out
        if(in == 0)
        {
            return false;
        }
    }
    return true;
}