// whereami.h
// Glenn G. Chappell
// 27 Sep 2012
//
// For CS 381 Fall 2012
// Header for Camera-Position Computation
// There is no associated source file
// Requires GLUT

#ifndef FILE_WHEREAMI_H_INCLUDED
#define FILE_WHEREAMI_H_INCLUDED

// OpenGL/GLUT includes - DO THESE FIRST
#include <cstdlib>       // Do this before GL/GLUT includes
using std::exit;
#ifndef __APPLE__
# include <GL/glut.h>    // GLUT stuff, includes OpenGL headers as well
#else
# include <GLUT/glut.h>  // Apple puts glut.h in a different place
#endif

// Other includes
#include <vector>        // For std::vector


// whereAmI
// Given matrix cmat, in OpenGL format, representing transformation
//  composed only of translations & rotations, returns 1st 3 components
//  of cmat^(-1)*<0,0,0,1>. That is, finds pt that cmat takes to origin.
//  Thus, if cmat is the camera transformation matrix, then we return
//  the camera position in world coordinates.
// Uses OpenGL. Assumes matrix mode is GL_MODEL_VIEW.
inline std::vector<GLdouble> whereAmI(const GLdouble * cmat)
{
    // Save old model/view (assume we are in model/view mode!)
    glPushMatrix();

    // Inverses of translation part & rotation part of cmat
    GLdouble transmatinv[16];
    GLdouble rotmatinv[16];

    // Make transmatinv
    glLoadIdentity();
    glGetDoublev(GL_MODELVIEW_MATRIX, transmatinv);
    transmatinv[12] = -cmat[12];
    transmatinv[13] = -cmat[13];
    transmatinv[14] = -cmat[14];

    // Make rotmatinv
    glLoadIdentity();
    glGetDoublev(GL_MODELVIEW_MATRIX, rotmatinv);
    for(int i = 0; i < 3; ++i)
    {
        for (int j = 0; j < 3; ++j)
        {
            rotmatinv[i*4+j] = cmat[j*4+i];
        }
    }

    // Compute cmatinv: inverse of cmat
    glLoadMatrixd(rotmatinv);
    glMultMatrixd(transmatinv);
    GLdouble cmatinv[16];
    glGetDoublev(GL_MODELVIEW_MATRIX, cmatinv);

    // Return 1st 3 components of right-most column of cmatinv
    // This column is cmatinv * <0,0,0,1>
    std::vector<GLdouble> v(3);
    v[0] = cmatinv[12];
    v[1] = cmatinv[13];
    v[2] = cmatinv[14];

    // Restore old model/view
    glPopMatrix();

    return v;
}


#endif //#ifndef FILE_WHEREAMI_H_INCLUDED