/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT 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 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT 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 MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef CPOSE2D_H
#define CPOSE2D_H

#include <MRPT/MRML/CPose.h>

namespace MRML
{
	/** A class used to store a 2D pose.
	 *    A class used to store a 2D pose, including the 2D coordinate
	 *      point and a heading (phi) angle. It is used in many situations,
	 *      from defining a robot pose, maps relative poses, sensors,...
	 *
	 *  For a complete description of Points/Poses, see MRML::CPoseOrPoint, or refer
	 *    to the <a href="http://babel.isa.uma.es/mrpt/index.php/2D_3D_Geometry">2D/3D Geometry tutorial</a> in the wiki.
	 *

<div align=center>

<table class=MsoTableGrid border=1 cellspacing=0 cellpadding=0
 style='border-collapse:collapse;border:none'>
 <tr>
  <td width=576 colspan=2 style='width:432.2pt;border:solid windowtext 1.0pt;
  background:#E6E6E6;padding:0cm 5.4pt 0cm 5.4pt'>
  <p   align=center style='text-align:center'>MRML::CPose2D</p>
  </td>
 </tr>
 <tr>
  <td width=288 style='width:216.1pt;border:solid windowtext 1.0pt;border-top:
  none;padding:0cm 5.4pt 0cm 5.4pt'>
  <p   align=center style='text-align:center'>Homogeneous
  transfomation matrix</p>
  </td>
  <td width=288 style='width:216.1pt;border-top:none;border-left:none;
  border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt;
  padding:0cm 5.4pt 0cm 5.4pt'>
  <p   align=center style='text-align:center'>Spatial
  representation</p>
  </td>
 </tr>
 <tr style='height:108.3pt'>
  <td width=288 style='width:216.1pt;border:solid windowtext 1.0pt;border-top:
  none;padding:0cm 5.4pt 0cm 5.4pt;height:108.3pt'>
  <div align=center>
  <table  Table border=0 cellspacing=0 cellpadding=0 width="67%"
   style='width:67.92%;border-collapse:collapse'>
   <tr style='height:20.6pt'>
    <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>cos<span
    style='font-family:Symbol'>j</span></p>
    </td>
    <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>-sin<span
    style='font-family:Symbol'>j</span></p>
    </td>
    <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>0</p>
    </td>
    <td width=47 style='width:34.9pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>x</p>
    </td>
   </tr>
   <tr style='height:20.6pt'>
    <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>sin<span
    style='font-family:Symbol'>j</span></p>
    </td>
    <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>cos<span
    style='font-family:Symbol'>j</span></p>
    </td>
    <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>0</p>
    </td>
    <td width=47 style='width:34.9pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>y</p>
    </td>
   </tr>
   <tr style='height:20.6pt'>
    <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>0</p>
    </td>
    <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>0</p>
    </td>
    <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>1</p>
    </td>
    <td width=47 style='width:34.9pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>0</p>
    </td>
   </tr>
   <tr style='height:20.6pt'>
    <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>0</p>
    </td>
    <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>0</p>
    </td>
    <td width=46 style='width:34.85pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>0</p>
    </td>
    <td width=47 style='width:34.9pt;padding:0cm 5.4pt 0cm 5.4pt;height:20.6pt'>
    <p   align=center style='text-align:center'>1</p>
    </td>
   </tr>
  </table>
  </div>
  <p   align=center style='text-align:center'></p>
  </td>
  <td width=288 style='width:216.1pt;border-top:none;border-left:none;
  border-bottom:solid windowtext 1.0pt;border-right:solid windowtext 1.0pt;
  padding:0cm 5.4pt 0cm 5.4pt;height:108.3pt'>
  <p   align=center style='text-align:center'><img src="CPose2D.gif"></p>
  </td>
 </tr>
</table>

</div>

	 *
	 * \sa CPoseOrPoint,CPoint2D
	 */
	class CPose2D : public CPose
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CPose2D )

	 protected:
		UTILS::CMatrixDouble	aux_HM;

	 public:
		 /** Constructor from an initial value of the pose.
		  */
		 CPose2D(const double& x=0,const double&y=0,const double& phi=0);

		 /** Constructor from a CPoint2D object.
		   */
		 CPose2D(const CPoint2D &);

		 /** Aproximation!! Avoid its use, since information is lost.
		   */
		 CPose2D(const CPose3D &);

		 /** The orientation of the pose, in radians.
		  */
		 double phi;

		 /** Returns the corresponding 4x4 homogeneous
		   *  transformation matrix for the point(translation),
		   *  or pose (translation+orientation).
		   * \sa getInverseHomogeneousMatrix
		   */
		 const UTILS::CMatrixDouble&  getHomogeneousMatrix() const;

		 /** Returns the corresponding 4x4 homogeneous
		   *  transformation matrix for the point(translation),
		   *  or pose (translation+orientation).
		   * \sa getInverseHomogeneousMatrix
		   */
		 void  getHomogeneousMatrix(const UTILS::CMatrixDouble *& ptrHM ) const;

		 /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator.
		  */
		 CPose2D  operator + (const CPose2D& D) const ;

		 /** The operator \f$ a = this \oplus D \f$ is the pose compounding operator.
		  */
		 CPose3D  operator + (const CPose3D& D) const ;

		 /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding operator.
		   */
		 CPoint2D operator + (const CPoint2D& u) const ;

		 /** The operator \f$ u' = this \oplus u \f$ is the pose/point compounding operator.
		   */
		 CPoint3D operator + (const CPoint3D& u) const ;

		 /** The operator \f$ D = this \ominus b \f$  is the pose inverse compounding operator,
		   *   the resulting pose D fulfils:  \f$ this = b \oplus D \f$, that is: \f$ b = a \oplus (b \ominus a) \f$
		   */
		 CPose2D  operator - (const CPose2D& b) const ;

		 /** Scalar sum of components: This is diferent from poses
		  *    composition, which is implemented as "+" operators in "CPose" derived classes.
		  */
		 void AddComponents(CPose2D &p);

		 /** Scalar multiplication.
		  */
		 void operator *=(const double &s);

		 /** Forces "phi" to be in the range [-pi,pi];
		   */
		 void  normalizePhi();

	}; // End of class def.

	DEFINE_SERIALIZABLE_POST( CPose2D )

	std::ostream& operator << (std::ostream& o, const CPose2D& p);

} // End of namespace

#endif
