/* ImLib3D
 * Copyright (c) 2001, ULP-IPB Strasbourg.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/*! \file   ShapeAnalysis.hpp */

#ifndef _ShapeAnalysis_hpp
#define _ShapeAnalysis_hpp

#include<ImLib3D/ImageProcessor.hpp>
#include<newmat/newmatap.h>
#include<newmat/newmatio.h>
#include<newmat/newmat.h>
using namespace NEWMAT;


namespace IP3D
{
/*! #&CenterOfGravity&#     Computes mass, center of mass
     <UL>Args:
      <LI>image of weighted shape  type:ImageType
      <LI>mass                     type:double
      <LI>center of mass           type:Vect3Df
     </UL>
~*/
	template <class ImageType>
	void CenterOfGravity(const ImageType& src,double &mass,Vect3Df &centerOfGravity)
	{
		mass = src.Sum();
		centerOfGravity.x=0;
		centerOfGravity.y=0;
		centerOfGravity.z=0;
		SymmetricMatrix inertiaTensor(3); //! for object orientation
		typename ImageType::const_iteratorXYZ p;
		for (p=src.begin();p!=src.end();++p)
		{
			centerOfGravity.x += (*p)*p.x;
			centerOfGravity.y += (*p)*p.y;
			centerOfGravity.z += (*p)*p.z;
		}
		centerOfGravity.x /= mass;
		centerOfGravity.y /= mass;
		centerOfGravity.z /= mass;
	}
};

namespace IP3D
{
/*! #&PrincipalAxes&#     Computes  principal axes of weighted shape, 
    optionally mass and center of mass,
     <UL>Args:
      <LI>image of weighted shape  type:ImageType
      <LI>axes                     type:vector<Vect3Df>
      <LI>mass                     type:double
      <LI>center of mass           type:Vect3Df
     </UL>
~*/
	template <class ImageType>
	void PrincipalAxes(const ImageType& src,vector<Vect3Df>& pprincipalAxes,
					  double *_mass=NULL,
					  Vect3Df *_centerOfGravity=NULL)
	{
		double mass;
		Vect3Df centerOfGravity;
		if (_mass)
			mass=*_mass;
		else
			mass = src.Sum();
		if (!_centerOfGravity)
		{
			centerOfGravity.x=0;
			centerOfGravity.y=0;
			centerOfGravity.z=0;
		}
		SymmetricMatrix inertiaTensor(3); //! for object orientation
		typename ImageType::const_iteratorXYZ p;
		for (p=src.begin();p!=src.end();++p)
		{
			if (!_centerOfGravity)
			{
				centerOfGravity.x += (*p)*p.x;
				centerOfGravity.y += (*p)*p.y;
				centerOfGravity.z += (*p)*p.z;
			}
			inertiaTensor(1,1)+= (*p)*p.x*p.x;
			inertiaTensor(2,2)+= (*p)*p.y*p.y;
			inertiaTensor(3,3)+= (*p)*p.z*p.z;
			inertiaTensor(2,1)+= (*p)*p.x*p.y;
			inertiaTensor(3,1)+= (*p)*p.x*p.z;
			inertiaTensor(3,2)+= (*p)*p.z*p.y;
		}
		if (!_centerOfGravity)
		{
			centerOfGravity.x /= mass;
			centerOfGravity.y /= mass;
			centerOfGravity.z /= mass;
		}
		inertiaTensor(1,1) -= centerOfGravity.x*centerOfGravity.x*mass;
		inertiaTensor(2,2) -= centerOfGravity.y*centerOfGravity.y*mass;
		inertiaTensor(3,3) -= centerOfGravity.z*centerOfGravity.z*mass;
			
		inertiaTensor(2,1) -= centerOfGravity.x*centerOfGravity.y*mass;
		inertiaTensor(3,1) -= centerOfGravity.x*centerOfGravity.z*mass;
		inertiaTensor(3,2) -= centerOfGravity.z*centerOfGravity.y*mass;
			
		inertiaTensor(2,1) *= (-1);
		inertiaTensor(3,1) *= (-1);
		inertiaTensor(3,2) *= (-1);

		vector<Vect3Df> &principalAxes=pprincipalAxes;
		principalAxes.clear();
		principalAxes.resize(3);
		Vect3Df &comp1=principalAxes[0];
		Vect3Df &comp2=principalAxes[1];
		Vect3Df &comp3=principalAxes[2];
		
		DiagonalMatrix eigenValues;
		Matrix eigenVectors;
		Jacobi(inertiaTensor, eigenValues, eigenVectors);//more stable
		comp1.x = eigenVectors(1,1);
		comp1.y = eigenVectors(2,1);
		comp1.z = eigenVectors(3,1);
			
		comp2.x = eigenVectors(1,2);
		comp2.y = eigenVectors(2,2);
		comp2.z = eigenVectors(3,2);
			
		comp3.x = eigenVectors(1,3);
		comp3.y = eigenVectors(2,3);
		comp3.z = eigenVectors(3,3);
		
		// Sort in descending order
		if (eigenValues(1,1) < eigenValues(2,2)) // switch 1 and 2 ?
		{
			swap(comp1,comp2);
			
			float t=eigenValues(1,1);
			eigenValues(1,1)=eigenValues(2,2);
			eigenValues(2,2)=t;
		}
		// now there are only 3 possibilities left:
		if (eigenValues(1,1) < eigenValues(3,3)){swap(comp1,comp3);}
		else 
			if (eigenValues(2,2) < eigenValues(3,3)){swap(comp2,comp3);}
	}
};

namespace IP3D
{
/*! #&BoundingBox&#     Finds bounding box of a mask
     <UL>Args:
      <LI>Source Image  type:Mask3D
      <LI>Bounding Box  type:RectZone3Di
     </UL>
~*/
		void BoundingBox(const Mask3D& mask, RectZone3Di &bbox);
};


#endif //_ShapeAnalysis_hpp
