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

#include "Point3D.h"

#include <math.h>

#include "data/Matrix3x3.h"
#include "io/OutStream.h"
#include "units/Units.h"
#include "utils/AuvMath.h"
#include "utils/RingBuffer.h"

/// Default constructior
Point3D::Point3D( )
{
    setX( __builtin_nan( "" ) );
    setY( __builtin_nan( "" ) );
    setZ( __builtin_nan( "" ) );
}

/// Single value constructior
Point3D::Point3D( const double& init )
{
    setX( init );
    setY( init );
    setZ( init );
}

/// Multi value constructior
Point3D::Point3D( const double& x, const double& y, const double& z )
{
    setX( x );
    setY( y );
    setZ( z );
}


/// Copy Constructor
Point3D::Point3D( const Point3D& value )
{
    setX( value.getX() );
    setY( value.getY() );
    setZ( value.getZ() );
}

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

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

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

Point3D& Point3D::operator=( const double rhs )
{
    setX( rhs );
    setY( rhs );
    setZ( rhs );
    return *this;
}

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

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

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

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

Point3D Point3D::operator-( const Point3D & rhs )
{
    Point3D Point3DSub;
    Point3DSub.setX( getX() - rhs.getX() );
    Point3DSub.setY( getY() - rhs.getY() );
    Point3DSub.setZ( getZ() - rhs.getZ() );
    return Point3DSub;
}

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

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

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

Point3D Point3D::operator*( const double rhs )
{
    Point3D Point3DMult;
    Point3DMult.setX( getX() * rhs );
    Point3DMult.setY( getY() * rhs );
    Point3DMult.setZ( getZ() * rhs );
    return Point3DMult;
}

bool Point3D::operator==( const Point3D & rhs )
{
    return ( getX() == rhs.getX() && getY() == rhs.getY() && getZ() == rhs.getZ() ) ||
           ( isnan( getX() ) && isnan( rhs.getX() ) && isnan( getY() ) && isnan( rhs.getY() ) && isnan( getZ() ) && isnan( rhs.getZ() ) );
}

bool Point3D::operator!=( const Point3D & rhs )
{
    return !( *this == rhs );
}

Point3D& Point3D::addProduct( Matrix3x3& lhs, Point3D& 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 ] );
    setY( getY() + l[ 3 ] * r[ 0 ] + l[ 4 ] * r[ 1 ] + l[ 5 ] * r[ 2 ] );
    setZ( getZ() + l[ 6 ] * r[ 0 ] + l[ 7 ] * r[ 1 ] + l[ 8 ] * r[ 2 ] );

    return *this;
}

Point3D& Point3D::toAbs()
{
    setX( fabs( getX() ) );
    setY( fabs( getY() ) );
    setZ( fabs( getZ() ) );
    return *this;
}

double Point3D::getMagnitude() const
{
    return sqrt( value_[0] * value_[0] + value_[1] * value_[1] + value_[2] * value_[2] );
}

bool Point3D::isNan() const
{
    return isnan( value_[0] ) || isnan( value_[1] ) || isnan( value_[2] );
}

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

const double Point3D::sum() const
{
    return getX() + getY() + getZ();
}

const double Point3D::sumSquared() const
{
    return getX() * getX() + getY() * getY() + getZ() * getZ();
}