/************************************************************************/
/* Correction for projecting on spherical surfaces                      */
/*
    task, date(mdy), name, email
    --------------------------------------------------------------------
    original, 030103 Willem de jonge, willem@argcargv.com
    many fixes, 120105, bob kuehne, rpk@blue-newt.com
    added opengl projection code, 121005, bob kuehne, rpk@blue-newt.com
*/
/************************************************************************/

#include <stdio.h>
#include <malloc.h>

#include <fstream>
#include <sstream>
#include <iomanip>
#include <iostream>
using namespace std;

#include "projectioncorrection.h"
#include "distortion_opengl.h"

#include "GL/glu.h"

#undef  PI
#define PI                  (3.1415926535897932f)
#define DEG2RAD(d) (d*(PI/180.0))

template<class T>
T sqr(const T& a) { return a*a; }

const char* inifile = "CaveUT.ini";

ProjectionDistortion::ProjectionDistortion()
{
    oo = &cout; // new ofstream( "foo.txt" );

    // view model
    m_viewFOVleft  = -45;
    m_viewFOVright =  45;
    m_viewFOVtop   =  30;
    m_viewFOVbottom= -30;

    // projector model
    m_projFOVpitch = 0.0;
    m_projFOVleft   = -45;
    m_projFOVright  =  45;
    m_projFOVtop    =  30;
    m_projFOVbottom = -30;
    m_projPos = Vector3(0,0,0);
    m_eyePos  = Vector3(0,0,0);

    // projection sphere
    m_radius  = 10;

    m_bufferInitialized = false;
    m_paramsChanged     = true;
    m_displayList       = 0;

    // load file data
    memset( &m_lastupdate, 0, sizeof( struct stat ) );
    stat( inifile, &m_lastupdate );

}

ProjectionDistortion::~ProjectionDistortion()
{
    //  oo->close();
    // delete oo;
    // we don't cleanup, let os/gl take care of this
    // this way we don't need to track contexts
    // fini();
}

void ProjectionDistortion::fini()
{
    if (m_bufferInitialized)
    {
        glJump.glDeleteTextures( 1, &m_buffer );
        m_bufferInitialized = false;
    }

    if (m_displayList)
    {
        glJump.glDeleteLists(m_displayList,1);
        m_displayList = 0;
    }
}

std::string shaderFromFile( std::string filename )
{
    std::string ss;
    std::ifstream sourceFile;
    sourceFile.open( filename.c_str(), std::ios::binary );
    if( sourceFile )
    {
        sourceFile.seekg(0, std::ios::end);
        int length = sourceFile.tellg();
        char *text = new char[ length + 1 ];
        sourceFile.seekg(0, std::ios::beg);
        sourceFile.read(text, length);
        sourceFile.close();
        text[ length ] = '\0';
        ss = std::string( text );
        delete [] text;
//    text[length] = '\0';
    }
    return( ss );
}

void checkStatus( GLJumpTable::GLhandleARB hh )
{
    GLint type;
    glJump.glGetObjectParameterivARB( hh, GL_OBJECT_TYPE_ARB, &type );
    
    if ( type == GL_PROGRAM_OBJECT_ARB )
    {
        GLint linkresult;
        glJump.glGetObjectParameterivARB( hh, GL_OBJECT_LINK_STATUS_ARB, &linkresult );
        cout << hh << " linked: " << boolalpha << (bool)linkresult << endl;
    }
    else if ( type == GL_SHADER_OBJECT_ARB )
    {
        GLint compileresult;
        glJump.glGetObjectParameterivARB( hh, GL_OBJECT_COMPILE_STATUS_ARB, &compileresult );
        cout << hh << " compiled: " << boolalpha << (bool)compileresult << endl;
    }
    
    GLint infolength = 0;
    glJump.glGetObjectParameterivARB( hh, GL_OBJECT_INFO_LOG_LENGTH_ARB, &infolength );
    char *foo  = new char[ infolength+1 ];
    glJump.glGetInfoLogARB( hh, infolength, &infolength, foo );
    if ( infolength > 0 )
    {
        cout << "infolog: " << foo << endl;
    }
}

GLJumpTable::GLhandleARB loadAndCompileShader( bool isvertex, std::string shadername )
{
     std::string shadersource = shaderFromFile( shadername );
     const char * shadersource_c = shadersource.c_str();

    GLJumpTable::GLhandleARB shaderhandle = glJump.glCreateShaderObjectARB( isvertex?
                                                (GL_VERTEX_SHADER_ARB):
                                                (GL_FRAGMENT_SHADER_ARB) );
    glJump.glShaderSourceARB( shaderhandle, 1, &shadersource_c, NULL );
    glJump.glCompileShaderARB( shaderhandle );
    checkStatus( shaderhandle );
    
    return( shaderhandle );
}


void ProjectionDistortion::recalculate( bool debug, bool usegl )
{
    // create shader code
    cout << shaderFromFile( "blend.vert" ) << endl;
    cout << shaderFromFile( "blend.frag" ) << endl;
    GLJumpTable::GLhandleARB vertexShader   = loadAndCompileShader( true, "blend.vert" );
    GLJumpTable::GLhandleARB fragmentShader = loadAndCompileShader( false, "blend.frag" );

    blendProgram = glJump.glCreateProgramObjectARB();
    //glJump.glAttachObjectARB( blendProgram, vertexShader );
    glJump.glAttachObjectARB( blendProgram, fragmentShader );
    glJump.glLinkProgramARB( blendProgram );
    checkStatus( blendProgram );
    glJump.glUseProgramObjectARB( blendProgram );

    // blend parameters
    glJump.glUniform4fARB(  glJump.glGetUniformLocationARB( blendProgram, "blendenabled"),
                            _blendEnabled, _blendDebug, 0.0, 1.0 );
    glJump.glUniform1iARB(  glJump.glGetUniformLocationARB( blendProgram, "tex0" ), 0 );
    glJump.glUniform4fARB(  glJump.glGetUniformLocationARB( blendProgram, "leftedge" ),
                            _leftBlend[ 0 ], _leftBlend[ 1 ], 0.0, 0.0 );
    glJump.glUniform1fARB(  glJump.glGetUniformLocationARB( blendProgram, "pfactor" ), _blendExp );
    glJump.glUniform4fARB(  glJump.glGetUniformLocationARB( blendProgram, "rightedge" ),
                            _rightBlend[ 0 ], _rightBlend[ 1 ], 0.0, 0.0 );
    glJump.glUniform3fARB( glJump.glGetUniformLocationARB( blendProgram, "blendInvGamma" ),
                            _gammaInv[ 0 ], _gammaInv[ 1 ] , _gammaInv[ 2 ] );

    // color parameters
    glJump.glUniform4fARB(  glJump.glGetUniformLocationARB( blendProgram, "finalColorScale" ),
                            _colorScale[ 0 ], _colorScale[ 1 ] , _colorScale[ 2 ], _colorScale[ 3 ] );
    glJump.glUniform4fARB( glJump.glGetUniformLocationARB( blendProgram, "finalColorBias" ),
                            _colorBias[ 0 ], _colorBias[ 1 ] , _colorBias[ 2 ], _colorBias[ 3 ] );

    checkStatus( blendProgram );
    glJump.glUseProgramObjectARB( 0 );

    // first time create display list
    if (!m_displayList)
        m_displayList = glJump.glGenLists(1);

    glJump.glNewList(m_displayList,GL_COMPILE);

    if ( !usegl )
    {
        float src_x,src_y,dst_x,dst_y;
        float dx,dy;

        // number horizontal/vertical quads
        int   nw = 4;
        int   nh = nw * m_height / m_width;
        
        src_x = 0.0;
        src_y = 0.0;
        
        dx = m_width / (float) nw;
        dy = m_height / (float) nh;
        
        glJump.glBegin(GL_QUADS);

        float tx,ty;

        for (int k=0; k<nw; k++)
        {
            src_x = k*dx;
            
            for (int l=0; l<nh; l++)
            {
                src_y = l*dy;
                
                tx = src_x; ty = src_y;
                correct(tx,ty,&dst_x,&dst_y);
                if( 0 ) // debug )
                {   
                    (*oo) << "lower left corner of quad : ";
                    (*oo) << k << " " << l << endl;
                    (*oo) << " origxy: "
                        << std::setw(10) << tx
                        << std::setw(10) << " " << ty << endl;
                    (*oo) << " newxy:  "
                        << std::setw(10) << dst_x << " "
                        << std::setw(10) << dst_y << endl;
                    (*oo) << " tc:     "
                        << std::setw(10) << tx/m_buffer_width << " "
                        << std::setw(10) << ty/m_buffer_height<< endl;
                }
                distortedVertex(tx,ty,dst_x,dst_y);
                
                tx = src_x; ty = src_y+dy;
                correct(tx,ty,&dst_x,&dst_y);
                distortedVertex(tx,ty,dst_x,dst_y);
                
                tx = src_x+dx; ty = src_y+dy;
                correct(tx,ty,&dst_x,&dst_y);
                distortedVertex(tx,ty,dst_x,dst_y);
                
                tx = src_x+dx; ty = src_y;
                correct(tx,ty,&dst_x,&dst_y);
                distortedVertex(tx,ty,dst_x,dst_y);
            }
        }
        
        glJump.glEnd();
    }
    else // opengl distortion correction
    {
        drawDome( dtor(m_viewFOVleft), dtor(m_viewFOVright), dtor(m_viewFOVbottom), dtor(m_viewFOVtop), 
                  dtor(m_sphereOverlap), m_radius );
    }

    glJump.glEndList();
    
    m_paramsChanged = false;
}

void ProjectionDistortion::correct(float src_x, float src_y, float *dst_x, float *dst_y)
{
    // r = ray of view model in reference system
    Ray r = v(src_x,src_y);

    // intersect r with sphere
    Vector3 I;
    if (intersect(m_radius,r,&I))
    {
        // s = ray translated to origin of projector
        Ray s(Vector3(0,0,0), I-m_projPos); // ray with origin in projector posistion
        
        // apply inverse projector model to s to get new x,y coordinate
        p_inv(s, dst_x, dst_y);
    }
    else
    {
        *dst_x = 0;
        *dst_y = 0;
    }
}

static void write_u16(FILE* f, int i)
{
    unsigned char b;
    b = i & 0xff;
    fwrite(&b,1,1,f);
    b = (i>>8) & 0xff;
    fwrite(&b,1,1,f);
}

bool saveTGA(const char* name, const void* img, int width, int height)
{
    if (!img)
        return false;

    FILE* f = fopen(name,"wb");

    if (!f)
        return false;

    unsigned char b;

    // ID length, 0 = no ID
    b = 0; fwrite(&b,1,1,f);

    // Color Map type. 0 = no color map
    b = 0; fwrite(&b,1,1,f);

    // Image type, 3 = uncompressed black and white
    b = 2; fwrite(&b,1,1,f);

    // Color Map Specification, 5 bytes zero
    b = 0;
    fwrite(&b,1,1,f);
    fwrite(&b,1,1,f);
    fwrite(&b,1,1,f);
    fwrite(&b,1,1,f);
    fwrite(&b,1,1,f);

    // X-origin
    write_u16(f,0);

    // Y-origin
    write_u16(f,0);

    // Width
    write_u16(f,width);

    // Heigth
    write_u16(f,height);

    // Pixel Depth
    b = 32; fwrite(&b,1,1,f);

    // Image Descriptor, 0x20 = first pixel is top left
    // 0x08 = 8 attribute = alpha bits
    b = 0x08; fwrite(&b,1,1,f);

    // write image data
    fwrite(img,width*height,4,f);

    if (fclose(f))
        return false;

    return true;
}

void ProjectionDistortion::distortionCorrect( bool debug, bool grid, bool usegl,
                                             std::string path, std::string iname, unsigned int nframe )
{
    static unsigned int ii = 1;

    // initialize backbuffer texture first time
    if (!m_bufferInitialized)
        initializeBuffer();

    // watch for file change in debug mode, check every n fps (resolution on timer is only 1s, so no need to check faster)
    if ( debug && ( 0 == ii% 30 ) )
    {
        struct stat now;
        stat( inifile, &now );
#ifdef __APPLE__
        if( now.st_mtimespec.tv_sec > m_lastupdate.st_mtimespec.tv_sec )
#else
        if( now.st_mtime > m_lastupdate.st_mtime )
#endif
        {
            m_lastupdate = now;
            cout << "reloading parameter file " << endl;
            loadParams();
        }
    }

    if (m_paramsChanged )
        recalculate( debug, usegl );

    glJump.glPushAttrib(GL_TEXTURE_BIT|GL_DEPTH_BUFFER_BIT|GL_POLYGON_BIT|GL_COLOR_BUFFER_BIT|GL_TRANSFORM_BIT);

    glJump.glMatrixMode(GL_MODELVIEW);
    glJump.glPushMatrix();
    glJump.glLoadIdentity();

    glJump.glActiveTextureARB(GL_TEXTURE0_ARB);
    glJump.glMatrixMode(GL_TEXTURE);
    glJump.glPushMatrix();
    glJump.glLoadIdentity();

    glJump.glMatrixMode(GL_PROJECTION);
    glJump.glPushMatrix();
    glJump.glLoadIdentity();

    glJump.glOrtho(0,m_width,0,m_height,-1,1);

    glJump.glDisable(GL_BLEND);
    glJump.glDisable(GL_ALPHA_TEST);
    glJump.glDisable(GL_DEPTH_TEST);
    glJump.glDisable(GL_CULL_FACE);

    // clear all texture units when multitexturing is available
    if (glJump.multitexture())
    {
        int n;
        glJump.glGetIntegerv(GL_MAX_TEXTURE_UNITS_ARB, &n);
        for (int i=0; i<n; i++)
        {
            glJump.glActiveTextureARB(GL_TEXTURE0_ARB+i);
            glJump.glDisable(GL_TEXTURE_2D);
        }
        glJump.glActiveTextureARB(GL_TEXTURE0_ARB);
    }
    
    // grab current frame buffer in texture
    glJump.glBindTexture( GL_TEXTURE_2D, m_buffer );
    glJump.glCopyTexSubImage2D( GL_TEXTURE_2D, 0, 0, 0, 0, 0, m_vp_width, m_vp_height );

	glJump.glClearColor( 0.0, 0.0, 0.0, 1.0 );
    glJump.glClear (GL_COLOR_BUFFER_BIT);

    if ( debug )
    {
        // write grabbed texture to tga file
        if ( 0 == ii%nframe )
        {
            void* imgBuf = malloc(m_buffer_width*m_buffer_height*4);
            glGetTexImage(GL_TEXTURE_2D, 0, GL_BGRA, GL_UNSIGNED_BYTE, imgBuf);
            std::stringstream ss;
            ss << path << "tex2d" << std::setw( 10 ) << std::setfill( '0' ) << ii << ".tga";
            if ( !iname.empty() )
            {
                ss.str( iname );
            }
            saveTGA( ss.str().c_str(), imgBuf, m_buffer_width, m_buffer_height);
            free(imgBuf);
        }
        ii++;
    }

    if ( !usegl )
    {
        glErrorCheck();
        glJump.glDisable(GL_TEXTURE_GEN_S);
        glJump.glDisable(GL_TEXTURE_GEN_T);
        glJump.glDisable(GL_TEXTURE_GEN_R);
        glJump.glDisable(GL_TEXTURE_GEN_Q);
        glJump.glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);
        glJump.glEnable(GL_TEXTURE_2D);
        glJump.glEnable(GL_BLEND);
        glJump.glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

        glJump.glCallList(m_displayList);

        if ( debug )
        {
            glJump.glDisable( GL_TEXTURE_2D );
            glJump.glLineWidth( 3.0 );
            glJump.glColor3f( 1.0, 1.0, 0.0 );
            glJump.glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
            glJump.glCallList( m_displayList );
        }
    }
    else // opengl correction
    {
        glJump.glMatrixMode( GL_PROJECTION );
        glJump.glLoadIdentity();

        float projFOVy = m_projFOVtop-m_projFOVbottom;
        float projAspect = (m_projFOVright-m_projFOVleft)/projFOVy;
        float nearplane =  0.1f;
        float farplane =  10.0f;
#if 0
        gluPerspective( projFOVy, projAspect, nearplane, farplane );
#else
        glJump.glFrustum( nearplane * tan( DEG2RAD( m_projFOVleft ) ),
                   nearplane * tan( DEG2RAD( m_projFOVright ) ),
                   nearplane * tan( DEG2RAD( m_projFOVbottom ) ),
                   nearplane * tan( DEG2RAD( m_projFOVtop ) ),
                   nearplane, farplane );
#endif

        glJump.glMatrixMode( GL_MODELVIEW );
        glJump.glLoadIdentity();

        float projoffset[ 3 ] = { m_projPos.x, m_projPos.y, m_projPos.z };
        float eyeoffset[ 3 ]  = { m_eyePos.x, m_eyePos.y, m_eyePos.z };
        float eyeFOVy = m_viewFOVtop-m_viewFOVbottom;
        float eyeAspect = (m_viewFOVright-m_viewFOVleft)/eyeFOVy;
        beginDistortionState();
        loadTextureProjection( projoffset, m_projFOVpitch,  m_projFOVyaw,
                               eyeoffset, m_viewFOVrotation, eyeFOVy, eyeAspect );

        // orient dome
        glJump.glMatrixMode( GL_MODELVIEW );
        glJump.glLoadIdentity();
        glJump.glTranslatef( -projoffset[0], -projoffset[1], -projoffset[2] ); // cam translate
        glJump.glRotatef( -m_projFOVpitch, 1,0,0 );  // cam rotate
        glJump.glRotatef( -m_projFOVyaw, 0,1,0 );  // cam rotate

        glJump.glUseProgramObjectARB( blendProgram );
        glJump.glCallList(m_displayList);
        glJump.glUseProgramObjectARB( 0 );

        if ( debug && grid )
        {
            glJump.glDisable( GL_TEXTURE_2D );
            glJump.glLineWidth( 3.0 );
            glJump.glColor3f( 1.0, 1.0, 0.0 );
            glJump.glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
            glJump.glCallList( m_displayList );
        }
        endDistortionState();
        glErrorCheck();
    }
    // restore transform stacks
    glJump.glMatrixMode(GL_PROJECTION);
    glJump.glPopMatrix();
    glJump.glActiveTextureARB(GL_TEXTURE0_ARB);
    glJump.glMatrixMode(GL_TEXTURE);
    glJump.glPopMatrix();
    glJump.glMatrixMode(GL_MODELVIEW);
    glJump.glPopMatrix();

    glJump.glPopAttrib();
}

void ProjectionDistortion::initializeBuffer()
{
    /*
     *  create buffer texture of at least viewport resolution
     *  to copy original image into
     */

    GLint viewport[4];
    glJump.glGetIntegerv(GL_VIEWPORT,viewport);

    m_vp_width  = viewport[2];
    m_vp_height = viewport[3];

    m_width  = m_vp_width;
    m_height = m_vp_height;

    m_buffer_width  = 1024;
    m_buffer_height = 768;
    
    while ( m_buffer_width < m_vp_width )
        m_buffer_width *= 2;
    while ( m_buffer_height < m_vp_height )
        m_buffer_height *= 2;

    glJump.glPushAttrib(GL_TEXTURE_BIT);

    glJump.glGenTextures( 1, &m_buffer );
    glJump.glBindTexture( GL_TEXTURE_2D, m_buffer );
    glJump.glTexImage2D( GL_TEXTURE_2D, 0, GL_RGB, m_buffer_width, m_buffer_height, 0, GL_RGB, GL_UNSIGNED_BYTE, 0 );
    glJump.glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glJump.glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);

    glJump.glPopAttrib();

    m_bufferInitialized = true;
}

void ProjectionDistortion::setViewModel(float FOVleft, float FOVright, float FOVtop, float FOVbottom, float FOVrotation )
{
    m_viewFOVleft   = FOVleft  ;
    m_viewFOVright  = FOVright ;
    m_viewFOVtop    = FOVtop   ;
    m_viewFOVbottom = FOVbottom;
    m_viewFOVrotation = FOVrotation;

    m_paramsChanged = true;
}

void ProjectionDistortion::getViewModel(float& FOVleft, float& FOVright, float& FOVtop, float& FOVbottom, float &FOVrotation )
{
    FOVleft   = m_viewFOVleft;
    FOVright  = m_viewFOVright;
    FOVtop    = m_viewFOVtop;
    FOVbottom = m_viewFOVbottom;
	FOVrotation = m_viewFOVrotation;
}

void ProjectionDistortion::setProjector(float x, float y, float z,
                                                float FOVleft, float FOVright,
                                                float FOVtop, float FOVbottom,
                                                float FOPpitch, float FOPyaw )
{
    m_projPos = Vector3(x, y, z);

    m_projFOVleft   = FOVleft  ;
    m_projFOVright  = FOVright ;
    m_projFOVtop    = FOVtop   ;
    m_projFOVbottom = FOVbottom;

    m_projFOVpitch = FOPpitch;
    m_projFOVyaw   = FOPyaw;

    m_paramsChanged = true;
}

void ProjectionDistortion::getProjector(float& x, float& y, float& z,
                                        float& FOVleft, float& FOVright,
                                        float& FOVtop, float& FOVbottom,
                                        float& FOVpitch, float& FOVyaw )
{
    x = m_eyePos.x;
    y = m_eyePos.y;
    z = m_eyePos.z;

    FOVleft   = m_projFOVleft  ;
    FOVright  = m_projFOVright ;
    FOVtop    = m_projFOVtop   ;
    FOVbottom = m_projFOVbottom;

    FOVpitch  = m_projFOVpitch;
    FOVyaw    = m_projFOVyaw;
}

void ProjectionDistortion::setEye(float x, float y, float z)
{
    m_eyePos = Vector3(x, y, z);

    m_paramsChanged = true;
}

void ProjectionDistortion::getEye(float& x, float& y, float& z)
{
    x = m_eyePos.x;
    y = m_eyePos.y;
    z = m_eyePos.z;
}

void ProjectionDistortion::setSphere(float radius, float overlap )
{
    m_radius = radius;
    m_sphereOverlap = overlap;

    m_paramsChanged = true;
}

void ProjectionDistortion::getSphere(float& radius)
{
    radius = m_radius;
}

void ProjectionDistortion::surface2Texcoords(float x, float y, float *u, float *v)
{
    *u = x / m_buffer_width;
    *v = y / m_buffer_height;
}

void ProjectionDistortion::surface2ScreenCoords(float x, float y, float *sx, float *sy)
{
    *sx = x;
    *sy = y;
}

bool ProjectionDistortion::intersect(const Plane &plane, const Ray &ray, Vector3 *i)
{
    float Vd = plane.n * ray.d;

    // check if ray is parallel to plane
    if (abs(Vd)<1.0e-9)
        return false;

    float Vo = -(plane.n*ray.p + plane.d);
    float t = Vo/Vd;

    *i = ray.p + t*ray.d;

    return true;
}

bool ProjectionDistortion::intersect(float radius, Ray &ray, Vector3 *i)
{
    Ray r = ray;
    r.normalize();
    
    Vector3 center(0,0,0);
    float B = 2*( r.d.x*(r.d.x-center.x) + r.d.y*(r.d.y-center.y) + r.d.z*(r.d.z-center.z)  );
    float C = sqr(r.p.x) + sqr(r.p.x) + sqr(r.p.x) - sqr(radius);
    
    float discr = B*B-4*C;
    if (discr>0.0)
    {
        float t = (-B - sqrt(discr))/2;
        if (t<0)
            t = (-B + sqrt(discr))/2;
        i->x = r.p.x + t*r.d.x;
        i->y = r.p.y + t*r.d.y;
        i->z = r.p.z + t*r.d.z;
        
        return true;
    }
    else
        return false;
}

//-------------------------------------------------------------------
// v, view model
//-------------------------------------------------------------------
Ray ProjectionDistortion::v(float x, float y)
{
    const float& w = m_width;
    const float& h = m_height;
    
    Ray ray;
    
    ray.p = m_eyePos;

    ray.d.x = tan(DEG2RAD(m_viewFOVleft  )) + (tan(DEG2RAD(m_viewFOVright)) - tan(DEG2RAD(m_viewFOVleft  ))) * x/w;
    ray.d.y = tan(DEG2RAD(m_viewFOVbottom)) + (tan(DEG2RAD(m_viewFOVtop  )) - tan(DEG2RAD(m_viewFOVbottom))) * y/h;
    ray.d.z = -1;
    
    return ray;
}

//-------------------------------------------------------------------
// p_inv, inverse projector model
//-------------------------------------------------------------------
void ProjectionDistortion::p_inv(Ray &ray, float *x, float *y)
{
    float &k = ray.d.x;
    float &l = ray.d.y;
    float &m = ray.d.z;

    const float& w = m_width;
    const float& h = m_height;

    float alpha = atan2f(k, -m);
    *x =  w * (tan(alpha)-tan(DEG2RAD(m_projFOVleft))) / (tan(DEG2RAD(m_projFOVright))-tan(DEG2RAD(m_projFOVleft)));

    float beta  = atan2f(l, -m);
    beta += DEG2RAD( m_projFOVpitch );
//  cout << m_projFOVpitch << " " <<  DEG2RAD( m_projFOVpitch )<< " " <<  beta << endl;
    *y =  h * (tan(beta)-tan(DEG2RAD(m_projFOVbottom))) / (tan(DEG2RAD(m_projFOVtop))-tan(DEG2RAD(m_projFOVbottom)));
}

void ProjectionDistortion::distortedVertex(float old_x, float old_y, float new_x, float new_y)
{
    // convert old image position into u,v coordinates and send to gl
    glJump.glTexCoord2f(old_x/m_buffer_width,old_y/m_buffer_height);

    // place vertex at new image position
    glJump.glVertex3f(new_x,new_y,0.0f);
}

void ProjectionDistortion::setViewDimensions( float ww, float hh )
{
    m_hack_width   = ww;
    m_hack_height  = hh;
}

void ProjectionDistortion::getViewDimensions( float &ww, float &hh )
{
    ww = m_hack_width;
    hh = m_hack_height;
}


void ProjectionDistortion::setColorScaleBias( float redScale, float greenScale, float blueScale,
        float redBias, float greenBias, float blueBias )
{
    cout << _colorScale[ 0 ] << " " << _colorScale[ 1 ] << " " << _colorScale[ 2 ] << endl;
    _colorScale[ 0 ] = redScale;
    _colorScale[ 1 ] = greenScale;
    _colorScale[ 2 ] = blueScale;
    _colorScale[ 3 ] = 1.0;
    _colorBias[ 0 ]  = redBias;
    _colorBias[ 1 ]  = greenBias;
    _colorBias[ 2 ]  = blueBias;
    _colorBias[ 3 ]  = 0.0;
}