#ifndef GEOMETRYXFORM_HH
#define GEOMETRYXFORM_HH
//
// $Id: GeometryXform.hpp,v 1.31 2001/03/25 20:53:59 hobbs Exp $
//
// File: GeometryXform.hh
// Purpose: Provide a 3D coordinate transformation.  
//
// Created: 02-SEP-1997   John Hobbs
// Updated: 10-FEB-2001   Dhiman Chakraborty
//                        Added the yaw-pitch-roll convention as an option
//                        for specifying the Euler angles
// $Revision: 1.31 $
//
//

#include "PhysicsVectors/Rotation.h"
#include "PhysicsVectors/SpaceVector.h"
#include "PhysicsVectors/UnitVector.h"
#include "d0om/d0_Object.hpp"
#include "spacegeom/SpacePoint.h"
#include "spacegeom/SpaceXform.h"

#include <iostream>
#include <string>

namespace dgs {

/**   A 3D coordinate transformation.

      The transform is defined in cartesian coordinates as a
      translation followed by a rotation.  The units of the
      translation are assumed to be cm, the units of the rotation
      angles are radians.
          
       The Euler angle constructor with default convention ("x") uses the 
      (active form of the) convention found in Marion, "Classical Dynamics of 
      Particles and Systems", Academic Press 1970, pp 384-385.  The same 
      convention is also described in Goldstein, "Classical Mechanics", Addison
      Wesley 1980, pp 143-148.  The relation between the (axis,angle)
      description and the Euler angle description can be found in Goldstein,
      eqns 4-72 and 4-95.

      The rotations are (1) through an angle phi about the original Z
      axis followed by (2) an angle theta about the new X' axis,
      followed by (3) an angle psi about current Z'' axis.

      In this convention, rotations about the x-, y- and z-axes are
      given by (phi,theta,psi)=(x-rotation angle,0,0),
      (-PI/2,y-rotation angle,PI/2) and (0.0,0.0,z-rotation angle)
      respectively.

      This implements the general transformation interface of SpaceXform
      The internal format has the inverse rotation (local\_to\_global)
      stored in \_rotation, and the global\_to\_local rotation stored in
      \_inverse.

      Because of this internal convention, if one gets the Euler angles
      from the Rotation returned by get\_rotation(), the following transformation
      must be applied to get the original Euler angles back:

          psi -> -phi
          theta -> -theta
          phi -> -psi

      The Euler angle constructor with the "xyz" (or yaw-pitch-roll) 
      convention uses the (active form of the) that found in Goldstein,
      "Classical Mechanics", Addison Wesley 1980, pp 608-609 (appendix B).

      The rotations are (1) through a yaw angle phi about the original Z
      axis followed by (2) a pitch angle theta about the new Y' axis,
      followed by (3) a roll angle psi about the final X'' axis. This is 
      sometimes referred to as the 321 sequence, i.e., the first rotation is 
      about the 3 axis, etc.

      Because of this internal convention, if one gets the Euler angles
      from the Rotation returned by get\_rotation(), the following transformation
      must be applied to get the original Euler angles back:

          psi -> -phi
          theta -> -theta
          phi -> -psi
*/

class GeometryXform: public SpaceXform, public d0_Object {

public:

  ///  Default constructor.
  GeometryXform();

  /// Convention for Euler angles
  enum EulerConvention {x=0, xyz=1};

  /**  Construct transformation with origin vector and Euler angles with 
different choices of convention. The default is convention=1 corresponds to 
the"x" convention while convention=1 corresponds to the "xyz" convention 
(see Goldstein, "Classical Mechanics", Addison Wesley 1980, pp 608-609) */
  GeometryXform(double dx, double dy, double dz, double phi, double theta, double psi, EulerConvention cnv=x);

  ///  Construct transformation with offset vector and rotation axis and angle
  GeometryXform(const SpaceVector& offset,
		const SpaceVector& direction, const double angle);

  ///  Construct transformation with offset and rotation.
  GeometryXform(const SpaceVector& offset, 
		const Rotation& rotation);

  /// Construct transformation giving old and new basis vectors.
  GeometryXform(
     const SpaceVector old1, const SpaceVector old2, const SpaceVector old3,
     const SpaceVector new1, const SpaceVector new2, const SpaceVector new3);

  ///  Copy constructor.
  GeometryXform(const GeometryXform& initializer);

  ///  Assignment operator.
  GeometryXform operator=(const GeometryXform& initializer);

  /**  Apply transformation.

       Apply a given {\bf GeometryXform} to an existing {\bf
       GeometryXform} object.  This is how one adds transformations
       together.

  */
  GeometryXform apply(const GeometryXform& add_this) const;

  /**  Apply transformation.  (SpaceXform interface)

       Apply an existing {\bf GeometryXform} to a given {\bf
       SpacePoint} to get the corresponding {\bf SpacePoint}.

  */
  SpacePoint apply(const SpacePoint& move_this) const; 

  /**  Apply transformation.  (SpaceXform interface)

       Apply an existing {\bf GeometryXform} to a given {\bf
       SpacePointVector} to get the corresponding {\bf SpacePointVector}.

  */
  SpacePointVector apply(const SpacePointVector& move_this) const; 

  /**  Get the inverse transformation. (GeometryXform interface)

       Return the inverse of this transformation.  

  */

  GeometryXform& get_inverse() const;
  /**  Get the inverse transformation. (SpaceXform interface)

       Return a reference to the inverse of this transformation.  
       SpaceXform requires that the inverse in managed by this
       class.  Thus, we add a non-persistant attribute \_myInverse.
  */

  const SpaceXform& inverse() const; 

  /**  Invert transformation.

       {\bf invert} defines the inverse transformation operation.  If
       {\bf A}, {\bf B} and {\bf C} are {\bf GeometryXform}'s, then if

       \centerline{{\tt C = B.apply(A)},} 

       \centerline{{\tt B = C.invert(A)}.}

       One can also invert a {\bf SpacePoint} to get a {\bf SpacePoint}.

  */
  GeometryXform invert(const GeometryXform& remove_this) const;

  ///  Invert a {\bf SpacePoint} to get a {\bf SpacePoint}.
  SpacePoint invert(const SpacePoint& gpt) const;

  /**  Difference between transformations.

       {\bf diff} is defined such that if

       \centerline{{\tt C = B.apply(A)},}

       \centerline{{\tt A = C.diff(B)}.}
  */
  GeometryXform diff(const GeometryXform& base) const;

  /**  Generic access to tranformation offset.

       Returns specified coordinate of transformation offset.
       xform[0] = x coordinate, xform[1] = y coordinate, xform[2] = z
       coordinate.

  */
  double operator[](int i) const { return _origin[i]; }
  double operator ()(int i) const { return _origin[i]; }

  ///  Access the individual rotation matrix terms by index
  double operator ()(int i, int j) const { return _rotation[i][j]; }

  /// Access the individual terms of the inverse rotation matrix.
  double get_inverse(int i, int j) const { return _inverse[i][j]; }

  ///  Access offset.
  SpaceVector get_offset() const;

  ///  Access rotation.
  Rotation get_rotation() const;

  ///  Access rotation inverse.
  Rotation get_inverse_rotation() const;

  ///  and all three Euler angles, in order (convention = "x" or "xyz"
  ///  in Goldstein nomenclature - see comments at the top)
  void get_Euler(double &phi, double &theta, double &psi, EulerConvention cnv=x) const;

  ///  Access rotation by direction vector
  UnitVector get_axis() const;

  ///  and the corresponding angle
  double get_angle() const;


  // Comparisons
  friend bool operator ==(const GeometryXform& lhs, const GeometryXform& rhs);
  friend bool operator !=(const GeometryXform& lhs, const GeometryXform& rhs);
  friend bool operator <(const GeometryXform& lhs, const GeometryXform &rhs);

  /// Get source code version. Kept to allow following code changes
  const std::string& code_version() const { return _cvers; }

  ///  Stream insertion operator.
  friend std::ostream& operator <<(std::ostream& os, const GeometryXform& xf);

  /// Destructor.  Non-default needed to handle SpaceXform interface
  ~GeometryXform();

private:

  /// Define position of origin.
  double _origin[3];

  /// Rotation matrix corresponding to the Euler angles 
  double _rotation[3][3];

  /// Inverse of rotation matrix.  In the long run, do not store this...
  double _inverse[3][3];  //! DO NOT PERSIST THIS ATTRIBUTE

  /// Use this somewhat unsafe constructor only internally.
  GeometryXform(const double origin[3], const double rotation[3][3]);

  /// Compute the inverse of my rotation matrix.
  void compute_inverse();

  /// RCS version, defined automatically in constructor
  std::string _cvers;  //! CANNOT PERSIST THIS STATIC ATTRIBUTE.  THAT'S OK.

  /// Use this attribute to manage the inverse for the get_inverse method 
  /// We use this indirect method to avoid trouble with 'get\_inverse() const'.
  /// (Non-persistant.  Created upon demand.)
  mutable GeometryXform* _GXInv;  //! DO NOT PERSIST THIS ATTRIBUTE.

  /// Use this to create the locally managed inverse xform.
  void build_managed_inverse() const;

public:

  /// How many digits to the right of the decimal point when printing? (3)
  static int _ndecimal;
  
  D0_OBJECT_SETUP(GeometryXform);
  virtual void activate();
  //virtual void deactivate() const;

};

// Just the declarations, ma'am
bool operator ==(const GeometryXform& lhs, const GeometryXform& rhs);
bool operator !=(const GeometryXform& lhs, const GeometryXform& rhs);
bool operator <(const GeometryXform& lhs, const GeometryXform &rhs);
std::ostream& operator <<(std::ostream& os, const GeometryXform& xf);

}
#endif //GEOMETRYXFORM_HH