#include "MLVector.h"
//#include "MLLine.h"

const MLVector MLVector::invalid = MLVector(0, 0, 0, false);
const MLVector MLVector::nullVector = MLVector(0, 0, 0, true);

MLVector::MLVector(void)
	:m_x(0.0), m_y(0.0), m_z(0.0), m_valid(true)
{

}

MLVector::MLVector( double vx, double vy, double vz /*= 0.0*/, bool valid_in /*= true*/ )
	: m_x(vx), m_y(vy), m_z(vz)
{
	valid = valid_in;
}


MLVector::~MLVector(void)
{
}

void MLVector::set(double vx, double vy, double vz) 
{
	x = vx;
	y = vy;
	z = vz;
	valid = true;
}

/**
 * Sets a new position for the vector in polar coordinates.
 *
 * \param radius the radius or the distance
 *
 * \param angle the angle in rad
 */
void MLVector::setPolar(double radius, double angle)
{
    x = radius * cos(angle);
    y = radius * sin(angle);
    z = 0.0;
    //valid = MLMath::isNormal(radius) && MLMath::isNormal(angle);
}

bool MLVector::isValid() const 
{
	return valid;
}

bool MLVector::isInside( const MLBox& b ) const
{
	/*
	MLVector bMin = b.getMinimum();
	MLVector bMax = b.getMaximum();

	return (x >= bMin.x && x <= bMax.x && y >= bMin.y && y <= bMax.y && z
		>= bMin.z && z <= bMax.z);
		*/
	return false;
}

void MLVector::setX(double x)
{
	m_x = x;
}

double MLVector::getX()  const
{
	return m_x;
}

double& MLVector::getX()
{
	return m_x;
}

void MLVector::setY(double y) 
{
	m_y = y;
}

double MLVector::getY() const
{
	return m_y;
}

double& MLVector::getY()
{
	return m_y;
}

void MLVector::setZ(double z) 
{
	m_z = z;
}

double MLVector::getZ() const
{
	return m_z;
}

double& MLVector::getZ()
{
	return m_z;
}

void MLVector::setValid(bool valid)
{
	m_valid = valid;
}

bool MLVector::getValid() const
{
	return m_valid;
}

/**
 * \return True if this vector and the given vector are almost equal
 * (see RS::PointTolerance).
 *
 * \param tol Tolerance in X, Y and Z.

bool MLVector::equalsFuzzy(const MLVector& v, double tol) const 
{
    return (fabs(x-v.x)<tol &&
            fabs(y-v.y)<tol &&
            fabs(z-v.z)<tol);
} */

/**
 * \return The distance between this and the given coordinate.
 */
double MLVector::getDistanceTo(const MLVector& v) const 
{
    if (!valid || !v.valid) {
        return RMAXDOUBLE;
    } else {
        return (*this - v).getMagnitude();
    }
}

/**
 * \return The distance between this and the given coordinate on the XY plane.
 */
double MLVector::getDistanceTo2d(const MLVector& v) const
{
    if (!valid || !v.valid) {
        return RMAXDOUBLE;
    } else {
        return (*this - v).getMagnitude2d();
    }
}

/**
 * \return true if this vector is within the given range (2d).
 */
bool MLVector::isInWindow(const MLVector& firstCorner,
        const MLVector& secondCorner) 
{
    double minX = MIN(firstCorner.x, secondCorner.x);
    double maxX = MAX(firstCorner.x, secondCorner.x);
    double minY = MIN(firstCorner.y, secondCorner.y);
    double maxY = MAX(firstCorner.y, secondCorner.y);

    return (x >= minX && x <= maxX && y >= minY && y <= maxY);
}


void MLVector::setAngle(double a) 
{
    double m = getMagnitude();
    setPolar(m, a);
}

/**
 * \return The angle from zero to this vector (in rad).
 */
double MLVector::getAngle() const 
{
    double ret = 0.0;
    double m = getMagnitude2d();

    if (m > 1.0e-6)
	{
        double dp = getDotProduct(*this, MLVector(1.0, 0.0));
        if (dp / m >= 1.0)
		{
            ret = 0.0;
        } 
		else if (dp / m < -1.0)
		{
            ret = M_PI;
        } 
		else
		{
            ret = acos(dp / m);
        }

        if (y < 0.0)
		{
            ret = 2*M_PI - ret;
        }
    }
    return ret;
}

/**
 * \return Angle between this vector and XY plane (horizontal plane).
 */
double MLVector::getAngleToPlaneXY() const 
{
    MLVector n(0, 0, 1);

    if (getMagnitude() < 1.0e-4) 
	{
        return M_PI / 2;
    }
	else if ((getDotProduct(*this, n) / (getMagnitude() * 1)) > 1.0) 
	{
        return 0.0;
    }
	else 
	{
        return M_PI / 2 - acos(getDotProduct(*this, n) / (getMagnitude() * 1));
    }
}

/**
 * \return The angle from this and the given coordinate (in rad).
 */
double MLVector::getAngleTo(const MLVector& v) const
{
    if (!valid || !v.valid) 
	{
        return RNANDOUBLE;
    }
	else
	{
        return (v - (*this)).getAngle();
    }
}


/**
 * Sets the vector magnitude without chaning the direction.
 */
void MLVector::setMagnitude2d(double m) 
{
    double a = getAngle();
    setPolar(m, a);
}

/**
 * \return Magnitude (length) of the vector.
*/
double MLVector::getMagnitude() const 
{
    if (!valid)
	{
        return RNANDOUBLE;
    }
    // Note that the z coordinate is also needed for 2d
    //   (due to definition of crossP())
	return 0;// sqrt(MLMath::pow(x, 2) + MLMath::pow(y, 2) + MLMath::pow(z, 2));
} 

/**
 * \return Magnitude (length) of the vector projected to the x/y plane (2d).
 */
double MLVector::getMagnitude2d() const 
{
    if (!valid) 
	{
        return RNANDOUBLE;
    }
	return 0;// sqrt(MLMath::pow(x, 2) + MLMath::pow(y, 2));
}

/**
 * \return Square of magnitude (length).
 */
double MLVector::getSquaredMagnitude() const
{
    if (!valid) 
	{
        return RNANDOUBLE;
    }

	return 0;// MLMath::pow(x, 2) + MLMath::pow(y, 2) + MLMath::pow(z, 2);
}

/**
 * Linear interpolation between this and \c v by fraction 't'.
 */
MLVector MLVector::getLerp(const MLVector& v, double t) const 
{
    return MLVector(x + (v.x - x) * t, y + (v.y - y) * t, z + (v.z - z) * t);
}

/**
 * \return Unit vector for this vector.
 */
MLVector MLVector::getUnitVector() const
{
    return *this / getMagnitude();
}

/**
 * binary + operator.
 */
MLVector MLVector::operator +(const MLVector& v) const 
{
    return MLVector(x + v.x, y + v.y, z + v.z, valid && v.valid);
}

/**
 * binary - operator.
 */
MLVector MLVector::operator -(const MLVector& v) const 
{
    return MLVector(x - v.x, y - v.y, z - v.z, valid && v.valid);
}

/**
 * binary * operator.
 */
MLVector MLVector::operator *(double s) const
{
    return MLVector(x * s, y * s, z * s, valid);
}

/**
 * binary / operator.
 */
MLVector MLVector::operator /(double s) const 
{
    return MLVector(x / s, y / s, z / s, valid);
}

/**
 * unary - operator.
 */
MLVector MLVector::operator -() const 
{
    return getNegated();
}

/**
 * \return New vector with negated components.
 */
MLVector MLVector::getNegated() const 
{
    return MLVector(-x, -y, -z, valid);
}

/**
 * Normalizes this vector and returns a reference to this vector.
 */
MLVector MLVector::normalize() 
{
    *this = getNormalized();
    return *this;
}

/**
 * \return A new unit vector with the same direction as this vector.
*/
MLVector MLVector::getNormalized() const 
{
	/*
    double l = getMagnitude();
    if (l<MLCore::PointTolerance) 
	{
        return MLVector::invalid;
    }
    return *this / l;*/
	return *this;
} 

/**
 * += operator. The result is only valid if both vectors are valid.
 */
void MLVector::operator +=(const MLVector& v) 
{
    x += v.x;
    y += v.y;
    z += v.z;
    valid = valid && v.valid;
}

/**
 * -= operator. The result is only valid if both vectors are valid.
 */
void MLVector::operator -=(const MLVector& v) 
{
    x -= v.x;
    y -= v.y;
    z -= v.z;
    valid = valid && v.valid;
}

/**
 * *= operator
 */
void MLVector::operator *=(double s) 
{
    x *= s;
    y *= s;
    z *= s;
}

/**
 * /= operator
 */
void MLVector::operator /=(double s)
{
    x /= s;
    y /= s;
    z /= s;
}

/**
 * == operator
 */
bool MLVector::operator ==(const MLVector& v) const 
{
    if (valid == true && v.valid == true) 
	{
        return x == v.x && y == v.y && z == v.z;
    } 
	else if (valid == false && v.valid == false) 
	{
        return true;
    }
    return false;
}

bool MLVector::operator !=(const MLVector& v) const
{
	return !operator==(v);
}

/**
 * Scalarproduct (dot product).
 */
double MLVector::getDotProduct(const MLVector& v1, const MLVector& v2) 
{
    return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}

/**
 * \return Cross product of two vectors.
 */
MLVector MLVector::getCrossProduct(const MLVector& v1, const MLVector& v2)
{
    return MLVector(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x
            * v2.y - v1.y * v2.x, v1.valid && v2.valid);
}

MLVector MLVector::createPolar(double radius, double angle)
{
	MLVector ret;
	ret.setPolar(radius, angle);
	return ret;
}


/**
 * \return A vector with the minimum components from the vectors v1 and v2.
 * These might be mixed components from both vectors.
 */
MLVector MLVector::getMinimum(const MLVector& v1, const MLVector& v2) 
{
    return MLVector(MIN(v1.x, v2.x), MIN(v1.y, v2.y), MIN(v1.z, v2.z), v1.valid && v2.valid);
}

/**
 * \return A vector with the maximum values from the vectors v1 and v2
 */
MLVector MLVector::getMaximum(const MLVector& v1, const MLVector& v2)
{
    return MLVector(MAX(v1.x, v2.x), MAX(v1.y, v2.y), MAX(v1.z, v2.z), v1.valid && v2.valid);
}

/**
 * Convenience function.
 * \return (v1 + v2) / 2.0
 */
MLVector MLVector::getAverage(const MLVector& v1, const MLVector& v2)
{
    return (v1+v2)/2.0;
}

/**
 * Rotates this vector around 0/0 by the given angle.
 */
MLVector MLVector::rotate(double rotation)
{
    if (!valid) 
	{
        return *this;
    }

    double r = getMagnitude2d();
    double a = getAngle() + rotation;

    x = cos(a) * r;
    y = sin(a) * r;

    return *this;
}

/**
 * Rotates this vector around the given center by the given angle.
 */
MLVector MLVector::rotate(double rotation, const MLVector& center)
{
    *this = center + (*this - center).rotate(rotation);
    return *this;
}


/**
 * Scales this vector by the given factor with the given center.
 */
MLVector MLVector::scale(double factor, const MLVector& center) 
{
    return scale(MLVector(factor, factor, factor), center);
}

/**
 * Scales this vector by the given factors with the given center.
 */
MLVector MLVector::scale(const MLVector& factors, const MLVector& center) 
{
    if (center==MLVector()) 
	{
        x *= factors.x;
        y *= factors.y;
        z *= factors.z;
        return *this;
    }

    *this = center + (*this - center).scale(factors);
    return *this;
}


/**
 * Mirrors this vector at the given axis.
 */
MLVector MLVector::mirror(const MLLine& axis)
{
   /* double phi1 = axis.startPoint.getAngleTo(*this);
    double phi2 = axis.startPoint.getAngleTo(axis.endPoint) - phi1;
    double r1 = axis.startPoint.getDistanceTo(*this);
    double r2 = axis.endPoint.getDistanceTo(*this);

    if (r1 < 1.0e-6 || r2 < 1.0e-6) 
	{
        // point touches one axis point
    } 
	else 
	{
        setPolar(r1, phi1 + 2* phi2 );
        (*this) += axis.startPoint;
    }
	*/
    return *this;
}

MLVector MLVector::mirror(const MLVector& axis1, const MLVector& axis2) 
{
   return *this;
   //return mirror(MLLine(axis1, axis2));
}


/**
 * Mirrors this vector at the Y-axis.
 */
MLVector MLVector::flipHorizontal() 
{
    return mirror(MLVector(0,0,0), MLVector(0,1,0));
}

/**
 * Mirrors this vector at the X-axis.
 */
MLVector MLVector::flipVertical() 
{
    return mirror(MLVector(0,0,0), MLVector(1,0,0));
}