/****************************************************************************
 * 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 "math/algebra.h"
#include "base_cpp/array.h"
#include "base_cpp/tlscont.h"

using namespace indigo;

bool Transform3f::bestFit (int npoints, const Vec3f points[], const Vec3f goals[], float *sqsum_out)
{
   QS_DEF(Array<double>, X); //set of points
   QS_DEF(Array<double>, Y); //set of goals
   Matr3x3d R, RT, RTR, evectors_matrix;
   //
   Matr3x3d rotation;
   double scale;
   Vec3f translation;
   //

   bool res = 1;
   Vec3f vec, tmp;
   double cpoints[3] = {0.0}, cgoals[3] = {0.0}; // centroid of points, of goals
   int i, j, k;
   
   for (i = 0; i < npoints; i++)
   {
      cpoints[0] += points[i].x;
      cpoints[1] += points[i].y;
      cpoints[2] += points[i].z;
      cgoals[0] += goals[i].x;
      cgoals[1] += goals[i].y;
      cgoals[2] += goals[i].z;
   }
   for (i = 0; i < 3; i++)
   {
      cpoints[i] /= npoints; 
      cgoals[i] /= npoints;
   }
   X.resize(npoints * 3);
   Y.resize(npoints * 3);

   //move each set to origin
   for (i = 0; i < npoints; i++)
   {
      X[i * 3 + 0] = points[i].x - cpoints[0];
      X[i * 3 + 1] = points[i].y - cpoints[1];
      X[i * 3 + 2] = points[i].z - cpoints[2];
      Y[i * 3 + 0] = goals[i].x - cgoals[0];
      Y[i * 3 + 1] = goals[i].y - cgoals[1];
      Y[i * 3 + 2] = goals[i].z - cgoals[2];
   }

   if (npoints > 1)
   {
      /* compute R */
      for (i = 0; i < 3; i++) 
      {
         for (j = 0; j < 3; j++) 
         {
            R.elements[i * 3 + j] = 0.0;
            for (k = 0; k < npoints; k++)
            {
               R.elements[i * 3 + j] += Y[k * 3 + i] * X[k * 3 + j];
            }
         }
      }
      
      //Compute R^T * R

      R.getTransposed(RT);
      RT.matrixMatrixMultiply(R, RTR);

      RTR.eigenSystem(evectors_matrix);

      if (RTR.elements[0] > 2 * EPSILON)
      {
         float norm_b0,norm_b1,norm_b2;
         Vec3f a0, a1, a2;
         Vec3f b0, b1, b2;
         
         a0.set((float)evectors_matrix.elements[0], (float)evectors_matrix.elements[3], (float)evectors_matrix.elements[6]);
         a1.set((float)evectors_matrix.elements[1], (float)evectors_matrix.elements[4], (float)evectors_matrix.elements[7]);
         a2.cross(a0, a1);

         R.matrixVectorMultiply(a0, b0);
         R.matrixVectorMultiply(a1, b1);
         norm_b0 = b0.length();
         norm_b1 = b1.length();
         Line3f l1, l2;
         float sqs1, sqs2;
         l1.bestFit(npoints, points, &sqs1);
         l2.bestFit(npoints, goals, &sqs2);
         if( sqs1 < 2 * EPSILON && sqs2 < 2 * EPSILON)
         {
            Transform3f temp;
            temp.rotationVecVec(l1.dir, l2.dir);
            for (i = 0; i < 3; i++)
               for (j = 0; j < 3; j++)
                  rotation.elements[i * 3 + j] = temp.elements[j * 4 + i];
         }
         else
         {
            b0.normalize();
            b1.normalize();
            b2.cross(b0, b1);
            norm_b2 = b2.length();
            
            evectors_matrix.elements[2] = a2.x;
            evectors_matrix.elements[5] = a2.y;
            evectors_matrix.elements[8] = a2.z;
            evectors_matrix.transpose();
                        
            RTR.elements[0] = b0.x; RTR.elements[1] = b1.x; RTR.elements[2] = b2.x;
            RTR.elements[3] = b0.y; RTR.elements[4] = b1.y; RTR.elements[5] = b2.y;
            RTR.elements[6] = b0.z; RTR.elements[7] = b1.z; RTR.elements[8] = b2.z;
            RTR.matrixMatrixMultiply(evectors_matrix, rotation);
         }
      }
      else
      {
         res = 0;
      }
   }
   else
   {
      res = 0;
   }
   if (!res)
   {
      rotation.identity();
   }
 
   //Calc scale
   scale = 1.0;
   if (res && npoints > 1) 
   {
      float l1 = 0.0;
      float l2 = 0.0;
      Vec3f vx, vy; 
      for (i = 0; i < npoints; i++) 
      {
         Vec3f vx((float)X[i * 3 + 0], (float)X[i * 3 + 1], (float)X[i * 3 + 2]);
         Vec3f vy((float)Y[i * 3 + 0], (float)Y[i * 3 + 1], (float)Y[i * 3 + 2]);
         rotation.matrixVectorMultiply(vx, vec);
         l1 += Vec3f::dot(vy, vec);
         l2 += Vec3f::dot(vec, vec);
      }
      scale = l1 / l2;
   }

   X.clear();
   Y.clear();

   //Calc translation
   translation.set((float)cgoals[0], (float)cgoals[1], (float)cgoals[2]);
   tmp = Vec3f((float)cpoints[0], (float)cpoints[1], (float)cpoints[2]);
   rotation.matrixVectorMultiply(tmp, vec);
   vec.scale((float)scale);
   translation.sub(vec);
   
   identity();
   for (i = 0; i < 3; i++)
   {
      for (j = 0; j < 3; j++)
      {
         elements[i * 4 + j] = (float)rotation.elements[j * 3 + i];
      }
   }
   elements[15] = 1.0f;
   translate(translation);
   for (i = 0; i < 3; i++)
   {
      for (j = 0; j < 3; j++)
      {
         elements[i * 4 + j] *= (float)scale;
      }
   }

   //Deviation
   if (sqsum_out)
   {
      *sqsum_out = 0;
      float d = .0f;
      for (i = 0; i < npoints; i++)
      {
         vec.pointTransformation(points[i], *this);
         d = Vec3f::dist(vec, goals[i]);
         *sqsum_out += d * d;
      }
   }
   return true;
}

bool Plane3f::bestFit (int npoints, const Vec3f points[], float *sqsum_out)
{
   QS_DEF(Array<double>, m);
   m.clear_resize(npoints * 3);
   int i, j, k;
   Matr3x3d A, evec;
   Vec3f c;

   for (i = 0; i < npoints; i++)
   {
      c.add(points[i]);
   }
   c.scale(1.0f/npoints);

   for (i = 0; i < npoints; i ++)
   {
      m[3 * i + 0] = points[i].x - c.x;
      m[3 * i + 1] = points[i].y - c.y;
      m[3 * i + 2] = points[i].z - c.z;
   }
   
   for (i = 0; i < 3; i++)
   {
      for (j = 0; j < 3; j++)
      {
         A.elements[i * 3 + j] = 0;
         for (k = 0; k < npoints; k++)
         {
            A.elements[i * 3 + j] += m[k * 3 + i] * m[k * 3 + j];
         }
      }
   }

   A.eigenSystem(evec);
   _norm.x = (float)evec.elements[2];
   _norm.y = (float)evec.elements[5];
   _norm.z = (float)evec.elements[8];

   _d = - Vec3f::dot(_norm, c);

   if (sqsum_out != 0)
   {
      *sqsum_out = 0;
      for (i = 0; i < npoints; i++)
      {
         float d = distFromPoint(points[i]);
         *sqsum_out += d * d;
      }
   }
   return true;
}

bool Line3f::bestFit (int npoints,const Vec3f points[], float *sqsum_out)
{
   QS_DEF(Array<double>, m);
   Matr3x3d A;
   Matr3x3d evec;
   int i, j, k;
   m.clear_resize(npoints * 3);
   org = Vec3f(0, 0, 0);
   
   for (i = 0; i < npoints; i++)
   {
      org.add(points[i]);
   }
   org.scale(1.0f/npoints);
   
   for (i = 0; i < npoints; i ++)
   {
      m[3*i + 0] = points[i].x - org.x;
      m[3*i + 1] = points[i].y - org.y;
      m[3*i + 2] = points[i].z - org.z;
   }

   for (i = 0; i < 3; i++)
   {
      for (j = 0; j < 3; j++)
      {
         A.elements[i * 3 + j] = 0;
         for (k = 0; k < npoints; k++)
         {
            A.elements[i * 3 + j] += m[k * 3 + i] * m[k * 3 + j];
         }
      }
   }
   A.eigenSystem(evec);
   dir.x = (float)evec.elements[0];
   dir.y = (float)evec.elements[3];
   dir.z = (float)evec.elements[6];
   dir.normalize();

   if (sqsum_out != 0)
   {
      *sqsum_out = 0;
      for (int i = 0; i < npoints; i++)
      {
         float d = distFromPoint(points[i]);
         *sqsum_out += d*d;
      }
   }

   return true;
}