#include "glcamera.h"
#include "glversion.h"
#include <stdio.h>
 
GlCamera::GlCamera()
{
    e1 = Vec3f(1, 0, 0);
    e2 = Vec3f(0, 1, 0);
    e3 = Vec3f(0, 0, 1);
    isKeepProportionXY = false;
    centerRotationZ=1;
 
}
 
GlCamera::GlCamera(unsigned int width,unsigned int height)
{
    e1 = Vec3f(1, 0, 0);
    e2 = Vec3f(0, 1, 0);
    e3 = Vec3f(0, 0, 1);
    isKeepProportionXY = false;
    centerRotationZ=1;
    setHeight(height);
    setWidth(width);
 
}
 
 
void GlCamera::setKeepProportionXY(bool keepProportionXY)
{
    this->isKeepProportionXY=keepProportionXY;
}
 
 
 
 
Vec3f GlCamera::getScale()
{
    return this->scale;
}
 
void GlCamera::setScale(float sx, float sy, float sz)
{
    this->scale.set(sx, sy, sz);
 
}
 
void GlCamera::setScale(Vec3f s)
{
    this->scale = s;
 
}
 
void GlCamera::setScale(float s)
{
    this->scale.set(s, s, s);
 
}
 
void GlCamera::multScale(float s)
{
    this->scale.scale(s);
 
}
 
void GlCamera::multScale(float sx, float sy, float sz)
{
    Vec3f scale(sx, sy, sz);
    this->scale.componentMul(scale);
 
}
 
void GlCamera::multScale(Vec3f s)
{
    this->scale.componentMul(s);
 
}
 
void GlCamera::setRotationCenter(float centerRotationZ)
{
    this->centerRotationZ = centerRotationZ;
}
 
void GlCamera::reset()
{
    system.setCoordinateSystem(systemInit);
    scale=scaleInit;
    centerRotationZ=centerRotationZInit;
}
 
GlCoordinateSystem * GlCamera::getSystem() {
    return & system;
}
 
GlCoordinateSystem * GlCamera::getSystemInitMovement() {
    return & systemInitMovement;
}
 
GlCoordinateSystem * GlCamera::getSystemInit() {
    return & systemInit;
}
 
Vec3f GlCamera::getX()
{
    return system.getX();
}
 
Vec3f GlCamera::getY()
{
    return system.getY();
}
 
Vec3f GlCamera::getZ()
{
    return system.getZ();
}
 
Vec3f GlCamera::getPosition()
{
    return system.getPosition();
}
 
void GlCamera::setPosition(Vec3f &pos)
{
    system.setPosition(pos);
}
 
std::ostream& operator<<(std::ostream& stream, GlCamera &m)
{
 
    Vec3f c = m.getRotationCenter();
    Vec3f p = m.system.getPosition();
    Vec3f Y = m.system.getY();
    std::cout << "Camera             " << std::endl;
    std::cout << "getPosition       :" << p << std::endl;
    std::cout << "getRotationCenter :" << c << std::endl;
    std::cout << "Y                 :" << Y << std::endl;
    std::cout << "centerRotationZ   :" << m.centerRotationZ << std::endl;
    std::cout << "scale X           :" << m.scale.getX() << std::endl;
    std::cout << "scale Y           :" << m.scale.getY() << std::endl;
    std::cout << "scale Z           :" << m.scale.getZ() << std::endl;
    std::cout << "height            :" << m.height << std::endl;
    std::cout << "width             :" << m.width << std::endl;
 
    return stream;
}
 
void GlCamera::setBestView(Vec3f &objectCenterPosistion, float size)
{
    setBestView(FRONT_VIEW,objectCenterPosistion,size);
}
 
void GlCamera::setBestView(CameraView v,Vec3f &objectCenterPosistion, float size)
{
    Vec3f pos;
    switch (v)
    {
    case FRONT_VIEW:
        pos.addScaled(objectCenterPosistion, size, e3);
        system.setPosition(pos);
        system.setX(e1);
        system.setY(e2);
        system.setZ(e3);
        break;
    case UP_VIEW:
        pos.addScaled(objectCenterPosistion, size, e2);
        system.setPosition(pos);
        system.setX(e1);
        system.setY(-1*e3);
        system.setZ(e2);
        break;
    case DOWN_VIEW:
        pos.addScaled(objectCenterPosistion, size, -1*e2);
        system.setPosition(pos);
        system.setX(e1);
        system.setY(e3);
        system.setZ(-1*e2);
        break;
    case LEFT_VIEW:
        pos.addScaled(objectCenterPosistion, size, -1*e1);
        system.setPosition(pos);
        system.setX(e3);
        system.setY(-1*e2);
        system.setZ(-1*e1);
        break;
    case RIGHT_VIEW:
        pos.addScaled(objectCenterPosistion, size, e1);
        system.setPosition(pos);
        system.setX(-1*e3);
        system.setY(e2);
        system.setZ(e1);
        break;
    case BACK_VIEW:
        pos.addScaled(objectCenterPosistion, size, -1*e3);
        system.setPosition(pos);
        system.setX(-1*e1);
        system.setY(e2);
        system.setZ(-1*e3);
        break;
    case FREE_VIEW:
        break;
    }
    setRotationCenter(size);
    float s = 1.0f / size;
    scale.set(s, s, s);
}
 
 
void GlCamera::setViewport()
{
    glViewport(0, 0, this->width, this->height);
}
 
int GlCamera::getHeight()
{
    return this->height;
}
 
int GlCamera::getWidth()
{
    return this->width;
}
 
void GlCamera::setHeight(int h)
{
    this->height = h;
    setViewport();
}
 
void GlCamera::setWidth(int w)
{
    this->width = w;
    setViewport();
}
 
void GlCamera::setSize(int h, int w)
{
    this->height = h;
    this->width = w;
    setViewport();
}
 
void GlCamera::startMovement()
{
    systemInitMovement.setCoordinateSystem(system);
}
 
void GlCamera::updateMovementTranslation(Vec3f &translation)
{
    Vec3f initpos = systemInitMovement.getPosition();
    setPosition(initpos, translation);
}
 
void GlCamera::setPosition(Vec3f &pos, Vec3f &translation)
{
    Vec3f translationx = system.getX();
    translationx.scale(translation.getX());
    Vec3f translationy = system.getY();
    translationy.scale(translation.getY());
    Vec3f finalpos = Vec3f(pos);
    finalpos.add(translationx);
    finalpos.add(translationy);
    system.setPosition(finalpos);
}
 
void GlCamera::setTranslation(Vec3f &translation)
{
    Vec3f pos = system.getPosition();
    Vec3f finalpos;
    finalpos.add(pos, translation);
    system.setPosition(finalpos);
}
 
void GlCamera::rotate(Mat3f & rotartionMat)
{
    // C => Center point
    // oP => old Position point
    // nP => new Position point
 
    //CoP = centerRotationZ * z
    //CnP = rotartionMat * z
    Vec3f CoP;
    Vec3f CnP;
    Vec3f z = system.getZ();
    CoP.scale(centerRotationZ, z);
    rotartionMat.verctMul(CoP, CnP);
 
    Vec3f rotCenter = getRotationCenter();
    Vec3f pos;
    pos.add(rotCenter, CnP);
    setPosition(pos);
 
    Vec3f X = system.getX();
    Vec3f Y = system.getY();
    Vec3f Z = system.getZ();
 
    Vec3f V;
    rotartionMat.verctMul(X, V);
    system.setX(V);
    rotartionMat.verctMul(Y, V);
    system.setY(V);
    rotartionMat.verctMul(Z, V);
    system.setZ(V);
}
 
void GlCamera::rotateX(float angle)
{
    Vec3f X = system.getX();
    Mat3f rotartionMat;
    rotartionMat.setRotation(X, angle);
    rotate(rotartionMat);
}
 
void GlCamera::rotateY(float angle)
{
    Vec3f Y = system.getY();
    Mat3f rotartionMat;
    rotartionMat.setRotation(Y, angle);
    rotate(rotartionMat);
}
 
void GlCamera::rotateZ(float angle)
{
    Vec3f Z = system.getZ();
    Mat3f rotartionMat;
    rotartionMat.setRotation(Z, angle);
    rotate(rotartionMat);
}
 
void GlCamera::setInitial()
{
    systemInit.setCoordinateSystem(system);
    scaleInit=scale;
    centerRotationZInit=centerRotationZ;
}
 
 
 
Vec3f GlCamera::getRotationCenter()
{
    //  vector pc = position to center
    Vec3f Z = system.getZ();
    Vec3f pc;
    pc.scale(-centerRotationZ, Z);
    //  coordinate of rotation centrer
    Vec3f c;
    Vec3f pos = system.getPosition();
    c.add(pos, pc);
    return c;
}
 
void GlCamera::setOpenglProjectionMatrixInternal()
{
 
    const float margin = 0.3;
    halfsizex = 1.0f + margin;
 
    if (isKeepProportionXY)
        halfsizey = halfsizex*((float) height / (float) width);
    else
        halfsizey = 1.0f + margin;
 
    glOrtho(-halfsizex, halfsizex, -halfsizey, halfsizey, -0.25 * centerRotationZ*scale.getZ(), 2.5 * centerRotationZ*scale.getZ());
}
 
void GlCamera::setOpenglPojectionMatrix()
{
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    setOpenglProjectionMatrixInternal();
 
}
 
void GlCamera::pushMatrix() {
    glPushMatrix();
}
 
void GlCamera::popMatrix() {
    glPopMatrix();
}
 
static void normalize(float v[3])
{
    float r;
 
    r = sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
    if (r == 0.0) return;
 
    v[0] /= r;
    v[1] /= r;
    v[2] /= r;
}
 
static void cross(float v1[3], float v2[3], float result[3])
{
    result[0] = v1[1]*v2[2] - v1[2]*v2[1];
    result[1] = v1[2]*v2[0] - v1[0]*v2[2];
    result[2] = v1[0]*v2[1] - v1[1]*v2[0];
}
 
static void lookAt(GLdouble eyex, GLdouble eyey, GLdouble eyez, GLdouble centerx,
                   GLdouble centery, GLdouble centerz, GLdouble upx, GLdouble upy,
                   GLdouble upz)
{
    float forward[3], side[3], up[3];
    GLfloat m[4][4];
 
    forward[0] = centerx - eyex;
    forward[1] = centery - eyey;
    forward[2] = centerz - eyez;
 
    up[0] = upx;
    up[1] = upy;
    up[2] = upz;
 
    normalize(forward);
 
    /* Side = forward x up */
    cross(forward, up, side);
    normalize(side);
 
    /* Recompute up as: up = side x forward */
    cross(side, forward, up);
 
 
    m[0][0] = side[0];
    m[1][0] = side[1];
    m[2][0] = side[2];
    m[3][0] = 0;
 
    m[0][1] = up[0];
    m[1][1] = up[1];
    m[2][1] = up[2];
    m[3][1] = 0;
 
    m[0][2] = -forward[0];
    m[1][2] = -forward[1];
    m[2][2] = -forward[2];
    m[3][3] = 0;
 
    m[0][3] = 0;
    m[1][3] = 0;
    m[2][3] = 0;
    m[3][3] = 1;
    glMultMatrixf(&m[0][0]);
    glTranslated(-eyex, -eyey, -eyez);
}
 
void GlCamera::setOpenglModelViewMatrix()
{
    glScaled(scale.getX(), scale.getY(), scale.getZ());
    Vec3f c = getRotationCenter();
    Vec3f p = system.getPosition();
    Vec3f Y = system.getY();
    lookAt(p.getX(), p.getY(), p.getZ(), c.getX(), c.getY(), c.getZ(), Y.getX(), Y.getY(), Y.getZ());
}
 
float GlCamera::getWorldX(int pixelX)
{
    const Vec3f p = system.getPosition();
    float XPixelSize = getXPixelSize();
    // tips : (this->width/2)*XPixelSize+XPixel0=p.getX()
    float XPixel0 = p.getX()-((float)this->width / 2.0f) * XPixelSize;
 
    return XPixelSize * pixelX + XPixel0;
}
 
float GlCamera::getWorldY(int pixelY)
{
    const Vec3f p = system.getPosition();
    float YPixelSize = -getYPixelSize();
    // tips : (this->height/2)*YPixelSize+YPixel0=p.getY()
    float YPixel0 = p.getY()-((float)this->height / 2.0f) * YPixelSize;
 
    return YPixelSize * pixelY + YPixel0;
}
 
float GlCamera::getXPixelSize()
{
    return (2.0f * halfsizex / (this->width * this->scale.getX()));
}
 
float GlCamera::getYPixelSize()
{
    return (2.0f * halfsizey / (this->height * this->scale.getY()));
}
 
float GlCamera::XPixelToGl(int x)
{
    const float XPixelSize = 2.0f * halfsizex / this->width;
    return (x - (float)this->width / 2.0f)*XPixelSize;
}
 
float GlCamera::YPixelToGl(int y)
{
    const float YPixelSize = 2.0f * halfsizey / this->height;
    return ((float)this->height / 2.0f - y)*YPixelSize;
}
 
float GlCamera::getGlHeight()
{
    return  2.0f * halfsizey;
}
float GlCamera::getGlWidth()
{
    return  2.0f * halfsizex;
}