#include "m33.hpp"

m33 T(m33 m1)
{
  m33 m2(m1.m[0][0],m1.m[0][1],m1.m[0][2],
         m1.m[1][0],m1.m[1][1],m1.m[1][2],
         m1.m[2][0],m1.m[2][1],m1.m[2][2]);
  return m2;
}

m33 operator+(const m33 &m1, const m33 &m2)
{
  m33 m3;
  m3.m[0][0] = m1.m[0][0] + m2.m[0][0];
  m3.m[1][0] = m1.m[1][0] + m2.m[1][0];
  m3.m[2][0] = m1.m[2][0] + m2.m[2][0];
  m3.m[0][1] = m1.m[0][1] + m2.m[0][1];
  m3.m[1][1] = m1.m[1][1] + m2.m[1][1];
  m3.m[2][1] = m1.m[2][1] + m2.m[2][1];
  m3.m[0][2] = m1.m[0][2] + m2.m[0][2];
  m3.m[1][2] = m1.m[1][2] + m2.m[1][2];
  m3.m[2][2] = m1.m[2][2] + m2.m[2][2];
  return m3;
}

m33 operator-(const m33 &m1, const m33 &m2)
{
  m33 m3;
  m3.m[0][0] = m1.m[0][0] - m2.m[0][0];
  m3.m[1][0] = m1.m[1][0] - m2.m[1][0];
  m3.m[2][0] = m1.m[2][0] - m2.m[2][0];
  m3.m[0][1] = m1.m[0][1] - m2.m[0][1];
  m3.m[1][1] = m1.m[1][1] - m2.m[1][1];
  m3.m[2][1] = m1.m[2][1] - m2.m[2][1];
  m3.m[0][2] = m1.m[0][2] - m2.m[0][2];
  m3.m[1][2] = m1.m[1][2] - m2.m[1][2];
  m3.m[2][2] = m1.m[2][2] - m2.m[2][2];
  return m3;
}

m33 operator*(const m33 &m1, const m33 &m2)
{
  m33 m3;
  m3.m[0][0] = m1.m[0][0]*m2.m[0][0] + m1.m[0][1]*m2.m[1][0] + m1.m[0][2]*m2.m[2][0];
  m3.m[0][1] = m1.m[0][0]*m2.m[0][1] + m1.m[0][1]*m2.m[1][1] + m1.m[0][2]*m2.m[2][1];
  m3.m[0][2] = m1.m[0][0]*m2.m[0][2] + m1.m[0][1]*m2.m[1][2] + m1.m[0][2]*m2.m[2][2];

  m3.m[1][0] = m1.m[1][0]*m2.m[0][0] + m1.m[1][1]*m2.m[1][0] + m1.m[1][2]*m2.m[2][0];
  m3.m[1][1] = m1.m[1][0]*m2.m[0][1] + m1.m[1][1]*m2.m[1][1] + m1.m[1][2]*m2.m[2][1];
  m3.m[1][2] = m1.m[1][0]*m2.m[0][2] + m1.m[1][1]*m2.m[1][2] + m1.m[1][2]*m2.m[2][2];

  m3.m[2][0] = m1.m[2][0]*m2.m[0][0] + m1.m[2][1]*m2.m[1][0] + m1.m[2][2]*m2.m[2][0];
  m3.m[2][1] = m1.m[2][0]*m2.m[0][1] + m1.m[2][1]*m2.m[1][1] + m1.m[2][2]*m2.m[2][1];
  m3.m[2][2] = m1.m[2][0]*m2.m[0][2] + m1.m[2][1]*m2.m[1][2] + m1.m[2][2]*m2.m[2][2];
  return m3;
}

m33 operator*(const m33 &m1, const float &s)
{
  m33 m3;
  m3.m[0][0] = m1.m[0][0] * s;
  m3.m[0][1] = m1.m[0][1] * s;
  m3.m[0][2] = m1.m[0][2] * s;
  m3.m[1][0] = m1.m[1][0] * s;
  m3.m[1][1] = m1.m[1][1] * s;
  m3.m[1][2] = m1.m[1][2] * s;
  m3.m[2][0] = m1.m[2][0] * s;
  m3.m[2][1] = m1.m[2][1] * s;
  m3.m[2][2] = m1.m[2][2] * s;
  return m3;
}

m33 operator*(const float &s, const m33 &m1)
{
  m33 m3;
  m3.m[0][0] = s * m1.m[0][0];
  m3.m[0][1] = s * m1.m[0][1];
  m3.m[0][2] = s * m1.m[0][2];
  m3.m[1][0] = s * m1.m[1][0];
  m3.m[1][1] = s * m1.m[1][1];
  m3.m[1][2] = s * m1.m[1][2];
  m3.m[2][0] = s * m1.m[2][0];
  m3.m[2][1] = s * m1.m[2][1];
  m3.m[2][2] = s * m1.m[2][2];
  return m3;
}

m33 Identity()
{
  m33 m1;
  m1.m[0][0] = m1.m[1][1] = m1.m[2][2] = 1.0;
  m1.m[1][0] = m1.m[2][1] = m1.m[0][2] = 0.0;
  m1.m[0][1] = m1.m[1][2] = m1.m[2][0] = 0.0;
  return m1;
}

m33 Rx(float t)
{
  m33 m1;
  m1.m[0][0] = 1.0;
  m1.m[1][0] = m1.m[2][0] = m1.m[0][1] = m1.m[0][2] = 0.0;
  m1.m[1][1] = m1.m[2][2] = cos(t);
  m1.m[1][2] = -(m1.m[2][1] = sin(t));
  return m1;
}

m33 Ry(float t)
{
  m33 m1;
  m1.m[1][1] = 1.0;
  m1.m[0][1] = m1.m[2][1] = m1.m[1][0] = m1.m[1][2] = 0.0;
  m1.m[0][0] = m1.m[2][2] = cos(t);
  m1.m[2][0] = -(m1.m[0][2] = sin(t));
  return m1;
}

m33 Rz(float t)
{
  m33 m1;
  m1.m[2][2] = 1.0;
  m1.m[0][2] = m1.m[1][2] = m1.m[2][0] = m1.m[2][1] = 0.0;
  m1.m[0][0] = m1.m[1][1] = cos(t);
  m1.m[0][1] = -(m1.m[1][0] = sin(t));
  return m1;
}

m33 R(float dx, float dy, float dz, float t)
{
  m33 m1;
  float r[3];
  float l = sqrt(dx*dx + dy*dy + dz*dz);

  r[0] = dx/l;
  r[1] = dy/l;
  r[2] = dz/l;

  m1.m[0][0] = r[0]*r[0] + cos(t)*(1.0 - r[0]*r[0]);
  m1.m[1][1] = r[1]*r[1] + cos(t)*(1.0 - r[1]*r[1]);
  m1.m[2][2] = r[2]*r[2] + cos(t)*(1.0 - r[2]*r[2]);

  m1.m[1][0] = r[1]*r[0] - cos(t)*r[1]*r[0] + r[2]*sin(t);
  m1.m[0][1] = r[0]*r[1] - cos(t)*r[0]*r[1] - r[2]*sin(t);

  m1.m[2][0] = r[2]*r[0] - cos(t)*r[2]*r[0] - r[1]*sin(t);
  m1.m[0][2] = r[0]*r[2] - cos(t)*r[0]*r[2] + r[1]*sin(t);

  m1.m[2][1] = r[2]*r[1] - cos(t)*r[2]*r[1] + r[0]*sin(t);
  m1.m[1][2] = r[1]*r[2] - cos(t)*r[1]*r[2] - r[0]*sin(t);

  return m1;
}

Vect3d operator*(const m33 &ma, const Vect3d &va)
{
  Vect3d vb;
  vb.x = ma.m[0][0] * va.x + ma.m[0][1] * va.y + ma.m[0][2] * va.z;
  vb.y = ma.m[1][0] * va.x + ma.m[1][1] * va.y + ma.m[1][2] * va.z;
  vb.z = ma.m[2][0] * va.x + ma.m[2][1] * va.y + ma.m[2][2] * va.z;
  return vb;
}

Vect3d solve_system(const m33 &A, const Vect3d &b)
{
  // basic application of Kramer's rule.  This is probably unstable or
  // something, but I need a inverter now!
  Vect3d X;
  float det = A.m[0][0]*A.m[1][1]*A.m[2][2] + A.m[0][1]*A.m[1][2]*A.m[2][0] +
              A.m[0][2]*A.m[1][0]*A.m[2][1] - A.m[0][2]*A.m[1][1]*A.m[2][0] -
              A.m[0][1]*A.m[1][0]*A.m[2][2] - A.m[0][0]*A.m[1][2]*A.m[2][1];
  X.x = b.x*A.m[1][1]*A.m[2][2] + A.m[0][1]*A.m[1][2]*b.z +
        A.m[0][2]*b.y*A.m[2][1] - A.m[0][2]*A.m[1][1]*b.z -
        A.m[0][1]*b.y*A.m[2][2] - b.x*A.m[1][2]*A.m[2][1];
  X.y = A.m[0][0]*b.y*A.m[2][2] + b.x*A.m[1][2]*A.m[2][0] +
        A.m[0][2]*A.m[1][0]*b.z - A.m[0][2]*b.y*A.m[2][0] -
        b.x*A.m[1][0]*A.m[2][2] - A.m[0][0]*A.m[1][2]*b.z;
  X.z = A.m[0][0]*A.m[1][1]*b.z + A.m[0][1]*b.y*A.m[2][0] +
        b.x*A.m[1][0]*A.m[2][1] - b.x*A.m[1][1]*A.m[2][0] -
        A.m[0][1]*A.m[1][0]*b.z - A.m[0][0]*b.y*A.m[2][1];
  X /= det;
  return X;
}

m33 orthonormalize(m33 m)
{
  Vect3d c0(m.m[0][0], m.m[1][0], m.m[2][0]);
  Vect3d c1(m.m[0][1], m.m[1][1], m.m[2][1]);
  Vect3d c2(m.m[0][2], m.m[1][2], m.m[2][2]);

  c0 = c0 * (1.0 / sqrt(c0 * c0));

  c1 = c1 - (c0 * (c0 * c1));
  c1 = c1 * (1.0 / sqrt(c1 * c1));

  c2 = c2 - (c0 * (c0 * c2));
  c2 = c2 - (c1 * (c1 * c2));
  c2 = c2 * (1.0 / sqrt(c2 * c2));

  m.m[0][0]=c0.x;  m.m[1][0]=c0.y;  m.m[2][0]=c0.z;
  m.m[0][1]=c1.x;  m.m[1][1]=c1.y;  m.m[2][1]=c1.z;
  m.m[0][2]=c2.x;  m.m[1][2]=c2.y;  m.m[2][2]=c2.z;

  return m;
}

#define ROTATE(a,i,j,k,l) g=a.m[i][j]; h=a.m[k][l]; a.m[i][j]=g-s*(h+g*tau); a.m[k][l]=h+s*(g-h*tau);
void eigen(m33 *vout, Vect3d *dout, m33 a)
{
  int n=3;
  int j,iq,ip,i;
  float tresh,theta,tau,t,sm,s,h,g,c;
  Vect3d b,z,d;
  m33 v;

  v = Identity();
  for(ip=0; ip<n; ip++)
  {
    b[ip] = a.m[ip][ip];
    d[ip] = a.m[ip][ip];
    z[ip] = 0.0;
  }

  for(i=0; i<50; i++)
  {
    sm=0.0;
    for(ip=0;ip<n;ip++) for(iq=ip+1;iq<n;iq++) sm+=fabs(a.m[ip][iq]);
    if (sm == 0.0)
    {
      *vout = v;
      *dout = d;
      return;
    }

    if (i < 3) tresh=0.2*sm/(n*n); else tresh=0.0;

    for(ip=0; ip<n; ip++)
    for(iq=ip+1; iq<n; iq++)
    {
      g = 100.0*fabs(a.m[ip][iq]);
      if (i>3 && fabs(d[ip])+g==fabs(d[ip]) && fabs(d[iq])+g==fabs(d[iq]))
        a.m[ip][iq]=0.0;
      else if (fabs(a.m[ip][iq]) > tresh)
      {
        h = d[iq]-d[ip];
        if (fabs(h)+g == fabs(h)) t=(a.m[ip][iq])/h;
        else
        {
    	    theta=0.5*h/(a.m[ip][iq]);
          t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
          if (theta < 0.0) t = -t;
    	  }
        c=1.0/sqrt(1+t*t);
        s=t*c;
        tau=s/(1.0+c);
        h=t*a.m[ip][iq];
        z[ip] -= h;
        z[iq] += h;
        d[ip] -= h;
        d[iq] += h;
        a.m[ip][iq]=0.0;
        for(j=0;j<ip;j++) { ROTATE(a,j,ip,j,iq); }
        for(j=ip+1;j<iq;j++) { ROTATE(a,ip,j,j,iq); }
        for(j=iq+1;j<n;j++) { ROTATE(a,ip,j,iq,j); }
        for(j=0;j<n;j++) { ROTATE(v,j,ip,j,iq); }
      }
    }

    for(ip=0;ip<n;ip++)
	  {
	    b[ip] += z[ip];
  	  d[ip] = b[ip];
	    z[ip] = 0.0;
  	}
  }

  fprintf(stderr, "eigen: too many iterations in Jacobi transform.\n");

  return;
}