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

#include "Point6D.h"

#include <math.h>

#include "data/Matrix6x6.h"
#include "data/Point3D.h"
#include "io/OutStream.h"
#include "units/Units.h"
#include "utils/AuvMath.h"

/// Default constructior
Point6D::Point6D( )
{
    *this = 0;
}

/// Single value constructior
Point6D::Point6D( const double& init )
{
    *this = init;
}

/// Multi value constructior
Point6D::Point6D( const double& x, const double& y, const double& z,
                  const double& roll, const double& pitch, const double& heading )
{
    setX( x );
    setY( y );
    setZ( z );
    setRoll( roll );
    setPitch( pitch );
    setHeading( heading );
}

/// Copy Constructor
Point6D::Point6D( const Point6D& value )
{
    *this = value;
}

/// Return a pointer to a new copy
Point6D* Point6D::copy() const
{
    return new Point6D( *this );
}

// as a Str
Str Point6D::toString() const
{
    return "[" + Str( value_[0] ) + ","
           + Str( value_[1] ) + ","
           + Str( value_[2] ) + ","
           + Str( value_[3] ) + ","
           + Str( value_[4] ) + ","
           + Str( value_[5] ) + "]";
}

Point6D& Point6D::operator=( const Point6D & rhs )
{
    setX( rhs.getX() );
    setY( rhs.getY() );
    setZ( rhs.getZ() );
    setRoll( rhs.getRoll() );
    setPitch( rhs.getPitch() );
    setHeading( rhs.getHeading() );
    return *this;
}

Point6D& Point6D::operator=( const double rhs )
{
    setX( rhs );
    setY( rhs );
    setZ( rhs );
    setRoll( rhs );
    setPitch( rhs );
    setHeading( rhs );
    return *this;
}

Point6D& Point6D::operator+=( const Point6D & rhs )
{
    setX( getX() + rhs.getX() );
    setY( getY() + rhs.getY() );
    setZ( getZ() + rhs.getZ() );
    setRoll( getRoll() + rhs.getRoll() );
    setPitch( getPitch() + rhs.getPitch() );
    setHeading( getHeading() + rhs.getHeading() );
    return *this;
}

Point6D& Point6D::operator+=( const Point3D & rhs )
{
    setX( getX() + rhs.getX() );
    setY( getY() + rhs.getY() );
    setZ( getZ() + rhs.getZ() );
    return *this;
}

Point6D& Point6D::operator+=( const double rhs )
{
    setX( getX() + rhs );
    setY( getY() + rhs );
    setZ( getZ() + rhs );
    setRoll( getRoll() + rhs );
    setPitch( getPitch() + rhs );
    setHeading( getHeading() + rhs );
    return *this;
}

Point6D& Point6D::operator-=( const Point6D & rhs )
{
    setX( getX() - rhs.getX() );
    setY( getY() - rhs.getY() );
    setZ( getZ() - rhs.getZ() );
    setRoll( getRoll() - rhs.getRoll() );
    setPitch( getPitch() - rhs.getPitch() );
    setHeading( getHeading() - rhs.getHeading() );
    return *this;
}

Point6D& Point6D::operator-=( const double rhs )
{
    setX( getX() - rhs );
    setY( getY() - rhs );
    setZ( getZ() - rhs );
    setRoll( getRoll() - rhs );
    setPitch( getPitch() - rhs );
    setHeading( getHeading() - rhs );
    return *this;
}

Point6D& Point6D::operator*=( const Point6D & rhs )
{
    setX( getX() * rhs.getX() );
    setY( getY() * rhs.getY() );
    setZ( getZ() * rhs.getZ() );
    setRoll( getRoll() * rhs.getRoll() );
    setPitch( getPitch() * rhs.getPitch() );
    setHeading( getHeading() * rhs.getHeading() );
    return *this;
}

Point6D& Point6D::operator*=( const double rhs )
{
    setX( getX() * rhs );
    setY( getY() * rhs );
    setZ( getZ() * rhs );
    setRoll( getRoll() * rhs );
    setPitch( getPitch() * rhs );
    setHeading( getHeading() * rhs );
    return *this;
}

Point6D& Point6D::addProduct( const Point6D& lhs, const double rhs )
{
    setX( getX() + lhs.getX() * rhs );
    setY( getY() + lhs.getY() * rhs );
    setZ( getZ() + lhs.getZ() * rhs );
    setRoll( getRoll() + lhs.getRoll() * rhs );
    setPitch( getPitch() + lhs.getPitch() * rhs );
    setHeading( getHeading() + lhs.getHeading() * rhs );
    return *this;
}

Point6D& Point6D::addProduct( const Point3D& lhs, const double rhs )
{
    setX( getX() + lhs.getX() * rhs );
    setY( getY() + lhs.getY() * rhs );
    setZ( getZ() + lhs.getZ() * rhs );
    return *this;
}

Point6D& Point6D::addProduct( Matrix6x6& lhs, Point6D& rhs )
{
    const double* l = lhs.getPtr1d();
    const double* r = rhs.getPtr1d();

    setX( getX() + l[ 0 ] * r[ 0 ] + l[ 1 ] * r[ 1 ] + l[ 2 ] * r[ 2 ] + l[ 3 ] * r[ 3 ] + l[ 4 ] * r[ 4 ] + l[ 5 ] * r[ 5 ] );
    setY( getY() + l[ 6 ] * r[ 0 ] + l[ 7 ] * r[ 1 ] + l[ 8 ] * r[ 2 ] + l[ 9 ] * r[ 3 ] + l[ 10 ] * r[ 4 ] + l[ 11 ] * r[ 5 ] );
    setZ( getZ() + l[ 12 ] * r[ 0 ] + l[ 13 ] * r[ 1 ] + l[ 14 ] * r[ 2 ] + l[ 15 ] * r[ 3 ] + l[ 16 ] * r[ 4 ] + l[ 17 ] * r[ 5 ] );
    setP( getP() + l[ 18 ] * r[ 0 ] + l[ 19 ] * r[ 1 ] + l[ 20 ] * r[ 2 ] + l[ 21 ] * r[ 3 ] + l[ 22 ] * r[ 4 ] + l[ 23 ] * r[ 5 ] );
    setQ( getQ() + l[ 24 ] * r[ 0 ] + l[ 25 ] * r[ 1 ] + l[ 26 ] * r[ 2 ] + l[ 27 ] * r[ 3 ] + l[ 28 ] * r[ 4 ] + l[ 29 ] * r[ 5 ] );
    setR( getR() + l[ 30 ] * r[ 0 ] + l[ 31 ] * r[ 1 ] + l[ 32 ] * r[ 2 ] + l[ 33 ] * r[ 3 ] + l[ 34 ] * r[ 4 ] + l[ 35 ] * r[ 5 ] );
    return *this;
}

Point6D& Point6D::toAbs()
{
    setX( fabs( getX() ) );
    setY( fabs( getY() ) );
    setZ( fabs( getZ() ) );
    setRoll( fabs( getRoll() ) );
    setPitch( fabs( getPitch() ) );
    setHeading( fabs( getHeading() ) );
    return *this;
}

double& Point6D::operator[]( int index )
{
    if( index < 0 || index > 5 )
    {
        index = 0;
    }
    return value_[ index ];
}

const double Point6D::absSum() const
{
    return fabs( getX() ) + fabs( getY() ) + fabs( getZ() )
           + fabs( getRoll() ) + fabs( getPitch() ) + fabs( getHeading() );
}

const double Point6D::sum() const
{
    return getX() + getY() + getZ() + getRoll() + getPitch() + getHeading();
}

const double Point6D::sumSquared() const
{
    return getX() * getX() + getY() * getY() + getZ() * getZ() + getRoll() * getRoll() + getPitch() * getPitch() + getHeading() * getHeading();
}