/****************************************************************************
 * Copyright (C) 2009-2015 EPAM Systems
 * 
 * This file is part of Indigo toolkit.
 * 
 * This file may be distributed and/or modified under the terms of the
 * GNU General Public License version 3 as published by the Free Software
 * Foundation and appearing in the file LICENSE.GPL included in the
 * packaging of this file.
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 ***************************************************************************/

#include <string.h>

#include "math/algebra.h"

using namespace indigo;

IMPL_ERROR(Matr3x3d, "Matr3x3d");

Matr3x3d::Matr3x3d ()
{
   memset(elements, 0, 9 * sizeof(double));
}

void Matr3x3d::copy (const Matr3x3d &matr)
{
   memcpy(elements, matr.elements, 9 * sizeof(double));
}

void Matr3x3d::identity ()
{
   memset(elements, 0, 9 * sizeof(double));
   elements[0] = elements[4] = elements[8] = 1.0;
}

void Matr3x3d::_givensRotation (double x0, double x1, double &c, double &s)
{
   if (fabs(x1) < 3e-16)
   {
      c = 1;
      s = 0;
   }
   else if (fabs (x1) > fabs (x0))
   {
      double t = - x0 / x1;
      s = 1.0 / sqrt(1 + t * t);
      c = s * t;
   }
   else
   {
      double t = - x1 / x0;
      c = 1.0 / sqrt(1 + t * t);
      s = c * t;
   }
}

void Matr3x3d::_qrStep (int n, double gc[], double gs[])
{
   double x, z;
   double mu;
   double c, s;
   
   Matr3x3d rot, tmp;
   int k;
   
   double d1 = elements[(n - 1) * 3 + (n - 1)];
   double d2 = elements[n * 3 + n]; 
   double sd = elements[n * 3 + (n - 1)];
   double dt = (d1 - d2) / 2.0;

   if (dt > 0)
   {
      mu = d2 - sd * (sd / (dt + sqrt(dt * dt + sd * sd)));
   }
   else if (fabs(dt) < 3e-15)
   {
      mu = d2 - fabs(sd);
   }
   else
   {
      mu = d2 + sd * (sd / ((-dt) + sqrt(dt * dt + sd * sd)));
   }
   x = elements[0] - mu;
   z = elements[3];

   if (n == 1)
   {
      _givensRotation(x, z, c, s);
      gc[0] = c;
      gs[0] = s;
      double e0 = elements[0],
             e3 = elements[3],
             e4 = elements[4];
      elements[0] = c * (c * e0 - s * e3) + s * (s * e4 - c * e3);
      elements[3] = c * (s * e0 + c * e3) - s * (s * e3 + c * e4);
      elements[4] = s * (s * e0 + c * e3) + c * (s * e3 + c * e4);
      return;
   }

   k = 0;
   if(fabs(elements[3]) < 3e-15*(fabs(elements[0])+fabs(elements[4])))
   {
      k = 1;
      gc[0] = 1.0;
      gs[0] = 0.0;
      x = elements[4] - mu;
      z = elements[7];
   }

   for (; k < n; k++)
   {
      _givensRotation(x, z, c, s);
      gc[k] = c;
      gs[k] = s;

      rot.identity();
      rot.elements[k * 3 + k] = c;
      rot.elements[(k + 1) * 3 + (k + 1)] = c;
      rot.elements[(k + 1) * 3 + k] = -s;
      rot.elements[k * 3 + (k + 1)] = s;
      
      rot.transpose();
      rot.matrixMatrixMultiply(*this, tmp);
      rot.transpose();
      tmp.matrixMatrixMultiply(rot, *this);

      x = elements[(k + 1) * 3 + k];
      z = elements[(k + 2) * 3 + k];
   }
}

void Matr3x3d::getTransposed (Matr3x3d &matr_out) const
{
   matr_out.copy(*this);
   matr_out.transpose();
}

void Matr3x3d::matrixVectorMultiply (const Vec3f &a, Vec3f &b) const
{
   b.x = (float)(elements[0] * a.x + elements[1] * a.y + elements[2] * a.z);
   b.y = (float)(elements[3] * a.x + elements[4] * a.y + elements[5] * a.z);
   b.z = (float)(elements[6] * a.x + elements[7] * a.y + elements[8] * a.z);
}

void Matr3x3d::transpose ()
{
   double tmp;
   __swap(elements[3], elements[1], tmp);
   __swap(elements[6], elements[2], tmp);
   __swap(elements[7], elements[5], tmp);
}

void Matr3x3d::matrixMatrixMultiply (const Matr3x3d &m, Matr3x3d &matrix_out) const
{
   int i, j, k;
   for (i = 0; i < 3; i++)
   {
      for (j = 0; j < 3; j++)
      {
         matrix_out.elements[i * 3 + j] = 0;
         for (k = 0; k < 3; k++)
         {
            matrix_out.elements[i * 3 + j] += elements[i * 3 + k] * m.elements[k * 3 + j];
         }
      }
   }
}

void Matr3x3d::eigenSystem (Matr3x3d &evec_out)
{
   Matr3x3d Q;
   int i, j, k, s;
   
   //Householder reduction to tridiagonal matrix
   double x0, x1, nrm;
   double alpha, beta, tau;
   x0 = elements[3];
   x1 = elements[6];
   nrm = fabs(x1);
   if (nrm < 3e-16)
   {
      tau = 0;
   }
   else
   {
      alpha = x0;
      beta = - (alpha >= 0.0 ? +1.0 : -1.0) * sqrt(alpha * alpha + nrm * nrm) ;
      tau = (beta - alpha) / beta ;

      x1 = x1 / (alpha - beta);
      x0 = beta;
   }
   evec_out.identity();
   evec_out.elements[4] -= tau;
   evec_out.elements[5] -= tau * x1;
   evec_out.elements[7] -= tau * x1;
   evec_out.elements[8] -= tau * x1 * x1;
   evec_out.matrixMatrixMultiply(*this, Q);
   Q.matrixMatrixMultiply(evec_out, *this);
   //////////////////////////////////////////////////////////////////////////
   
   //QR Iterations
   i = 2;
   s = 0;
   while (i > 0 && s++ < 100)
   {
      if (fabs(elements[i * 3 + i - 1]) < 3e-15*(fabs(elements[(i-1)*3 + (i - 1)]) + fabs(elements[i * 3 + i])))
      {
         i--;
         s = 0;
         continue;
      }
     
      double gc[2];
      double gs[2];

      _qrStep(i, gc, gs);

      //Apply Givens rotation to transformation matrix
      for (j = 0; j < i; j++)
      {
         double c = gc[j], s = gs[j];
         for (k = 0; k < 3; k++)
         {
            x0 = evec_out.elements[k * 3 + j];
            x1 = evec_out.elements[k * 3 + j + 1];
            evec_out.elements[k * 3 + j] = x0 * c - x1 * s;
            evec_out.elements[k * 3 + j + 1] = x0 * s + x1 * c;
         }
      }
   }
   //////////////////////////////////////////////////////////////////////////
   
   //Sort eigenvalues in descending order
   for (i = 0; i < 2; i++)
   {
      for (j = 0; j < 2 - i; j++)
      {
         if (elements[j * 3 + j] < elements[(j + 1) * 3 + (j + 1)])
         {
            __swap(elements[j * 3 + j], elements[(j + 1) * 3 + (j + 1)], nrm);
            __swap(evec_out.elements[0 * 3 + j], evec_out.elements[0 * 3 + (j + 1)], nrm);
            __swap(evec_out.elements[1 * 3 + j], evec_out.elements[1 * 3 + (j + 1)], nrm);
            __swap(evec_out.elements[2 * 3 + j], evec_out.elements[2 * 3 + (j + 1)], nrm);
         }
      }
   }
   //////////////////////////////////////////////////////////////////////////
}