#include "utils.h"
#include <iostream>
#include "math.h"
#include <GL/glut.h>
#include <GL/gl.h>

using namespace std;

Vecteur3D prodVect(Vecteur3D v1,Vecteur3D v2) {
   Vecteur3D n;
   n.x = v1.y*v2.z-v1.z*v2.y;
   n.y = v1.z*v2.x-v1.x*v2.z;
   n.z = v1.x*v2.y-v1.y*v2.x;
   return n;
}

Vecteur3D vecteur(Point3D p1,Point3D p2) {
  Vecteur3D res;
  res.x=p2.x-p1.x;
  res.y=p2.y-p1.y;
  res.z=p2.z-p1.z;
  return res;
}

float prodScal(Vecteur3D v1,Vecteur3D v2) {
  return v1.x*v2.x+v1.y*v2.y+v1.z*v2.z;
}

Vecteur3D add(Vecteur3D v1,Vecteur3D v2) {
  Vecteur3D res;
  res.x=v1.x+v2.x;
  res.y=v1.y+v2.y;
  res.z=v1.z+v2.z;
  return res;
}

Vecteur3D scale(Vecteur3D v,float k) {
  Vecteur3D res;
  res.x=v.x*k;
  res.y=v.y*k;
  res.z=v.z*k;
  return res;
}

float norme(Vecteur3D v) {
  float res;
  res=v.x*v.x+v.y*v.y+v.z*v.z;
  return sqrt(res);
}

Vecteur3D normer(Vecteur3D v) {
  float n=norme(v);
  return scale(v,1.0/n);
}


GLboolean invert(GLfloat src[16], GLfloat inverse[16])
{
    double t;
    int i, j, k, swap;
    GLfloat tmp[4][4];

    for(i=0;i<16;i++)
      if (i%4==i/4)
        inverse[i]=1.0;
      else
        inverse[i]=0.0;

    for (i = 0; i < 4; i++) {
	for (j = 0; j < 4; j++) {
	    tmp[i][j] = src[i*4+j];
	}
    }

    for (i = 0; i < 4; i++) {
        /* look for largest element in column. */
        swap = i;
        for (j = i + 1; j < 4; j++) {
            if (fabs(tmp[j][i]) > fabs(tmp[i][i])) {
                swap = j;
            }
        }

        if (swap != i) {
            /* swap rows. */
            for (k = 0; k < 4; k++) {
                t = tmp[i][k];
                tmp[i][k] = tmp[swap][k];
                tmp[swap][k] = t;

                t = inverse[i*4+k];
                inverse[i*4+k] = inverse[swap*4+k];
                inverse[swap*4+k] = t;
            }
        }

        if (tmp[i][i] == 0) {
            /* no non-zero pivot.  the matrix is singular, which
	       shouldn't happen.  This means the user gave us a bad
	       matrix. */
            return GL_FALSE;
        }

        t = tmp[i][i];
        for (k = 0; k < 4; k++) {
            tmp[i][k] /= t;
            inverse[i*4+k] /= t;
        }
        for (j = 0; j < 4; j++) {
            if (j != i) {
                t = tmp[j][i];
                for (k = 0; k < 4; k++) {
                    tmp[j][k] -= tmp[i][k]*t;
                    inverse[j*4+k] -= inverse[i*4+k]*t;
                }
            }
        }
    }
    return GL_TRUE;
}

void positionObs(Point3D *obs) {
   float view[16];
   float inverse[16];
   float result[16];
   for(int i=0;i<16;i++) {
     result[i]=0.0;
   }
   result[3]=1.0;
   glPushMatrix();
   glGetFloatv(GL_MODELVIEW_MATRIX,view);
   invert(view,inverse);
   glLoadIdentity();
   glMultMatrixf(inverse);
   glMultMatrixf(result);
   glGetFloatv(GL_MODELVIEW_MATRIX,result);
   obs->x=result[0]/result[3];   
   obs->y=result[1]/result[3];   
   obs->z=result[2]/result[3];   
   glPopMatrix();
}

void tracerPlan() {
  glBegin(GL_POLYGON);
  glVertex3f(-2,-2,-2);
  glVertex3f(-2,2,-2);
  glVertex3f(2,2,-2);
  glVertex3f(2,-2,-2);
  glEnd();
  
}