/** \file
 *
 *  Contains the Point6D class definition.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 *
 */

#ifndef POINT6D_H_
#define POINT6D_H_

#include "data/I1D.h"
#include "utils/Str.h"

class Point3D;
class Matrix6x6;

/**
 *  Wraps six double precision (8-byte) floating point numbers.
 *
 *  The 6 numbers are normally accessed as

 *  \li x, y, z, pitch, roll, heading
 *  \n -OR-
 *  \li u, v, w, p, q, r
 *
 * \ingroup data
 */

class Point6D: public I1D<double>
{
public:
    /// X, and also U=dX/dt
    inline void setX( const double x )
    {
        value_[0] = x;
    }
    inline double getX() const
    {
        return value_[0];
    }

    /// Y, and also V=dY/dt
    inline void setY( const double y )
    {
        value_[1] = y;
    }
    inline double getY() const
    {
        return value_[1];
    }

    /// Z, and also W=dZ/dt
    inline void setZ( const double z )
    {
        value_[2] = z;
    }
    inline double getZ() const
    {
        return value_[2];
    }

    /// X axis rotation, also Roll, Phi, and P=dPhi/dt
    inline void setRoll( const double roll )
    {
        value_[3] = roll;
    }
    inline double getRoll() const
    {
        return value_[3];
    }

    /// Y axis rotation, also Pitch, Theta, and Q=dPtheta/dt
    inline void setPitch( const double pitch )
    {
        value_[4] = pitch;
    }
    inline double getPitch() const
    {
        return value_[4];
    }

    /// Z axis rotation, also Heading, Psi, and R=dPsi/dt
    inline void setHeading( const double heading )
    {
        value_[5] = heading;
    }
    inline double getHeading() const
    {
        return value_[5];
    }

    /// U -- alias for dX/dt
    inline void setU( const double u )
    {
        value_[0] = u;
    }
    inline double getU() const
    {
        return value_[0];
    }

    /// V -- alias for dY/dt
    inline void setV( const double v )
    {
        value_[1] = v;
    }
    inline double getV() const
    {
        return value_[1];
    }

    /// W -- alias for dZ/dt
    inline void setW( const double w )
    {
        value_[2] = w;
    }
    inline double getW() const
    {
        return value_[2];
    }

    /// P -- alias for dRoll/dt
    inline void setP( const double p )
    {
        value_[3] = p;
    }
    inline double getP() const
    {
        return value_[3];
    }

    /// Q -- alias for dPitch/dt
    inline void setQ( const double q )
    {
        value_[4] = q;
    }
    inline double getQ() const
    {
        return value_[4];
    }

    /// R -- alias for dHeading/dt
    inline void setR( const double r )
    {
        value_[5] = r;
    }
    inline double getR() const
    {
        return value_[5];
    }

    /// Default constructior
    Point6D( );

    /// Single value constructior
    Point6D( const double& init );

    /// Multi value constructior
    Point6D( const double& x, const double& y, const double& z,
             const double& roll, const double& pitch, const double& heading );

    /// Copy Constructor
    Point6D( const Point6D& value );
    /*
        /// Constructor from binary
        Point6D( const void *binary );
    */
    /// Destructor
    virtual ~Point6D()
    {}
    ;

    /// Return a pointer to a new copy
    Point6D* copy() const;

    // as a Str
    virtual Str toString() const;

    Point6D& operator=( const Point6D& rhs );

    Point6D& operator=( const double rhs );

    Point6D& operator+=( const Point6D& rhs );

    Point6D& operator+=( const Point3D& rhs );

    Point6D& operator+=( const double rhs );

    Point6D& operator-=( const Point6D& rhs );

    Point6D& operator-=( const double rhs );

    Point6D& operator*=( const Point6D& rhs );

    Point6D& operator*=( const double rhs );

    Point6D& addProduct( Matrix6x6& lhs, Point6D& rhs );

    Point6D& addProduct( const Point6D& lhs, const double rhs );

    Point6D& addProduct( const Point3D& lhs, const double rhs );

    Point6D& toAbs();

    double& operator[]( int index );

    const double absSum() const;

    const double sum() const;

    const double sumSquared() const;

    /// Implement abstract I1D method
    virtual double* getPtr1d()
    {
        return value_;
    }

    /// Implement abstract I2D method
    virtual int getM() const
    {
        return 6;
    }

protected:

    // The actual data
    double value_[6];

};

#endif /*POINT6D_H_*/