

//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/
//_/_/
//_/_/  This file is part of CMSDK version 2.0.0.170915_Beta4 (15 September 2017)
//_/_/  
//_/_/  developed by
//_/_/  
//_/_/  Communicative Machines Ltd. 
//_/_/    http://cmlabs.com/
//_/_/  Copyright (c) 2017
//_/_/  
//_/_/  in collaboration with 
//_/_/  
//_/_/  Icelandic Institute for Intelligent Machines ses
//_/_/    Menntavegur 1
//_/_/    101 Reykjavik
//_/_/    ICELAND
//_/_/    http://www.iiim.is/
//_/_/
//_/_/  and is released under the following
//_/_/  
//_/_/  EXTENDED BSD License
//_/_/
//_/_/  Redistribution and use in source and binary forms, with or without
//_/_/  modification, is permitted provided that the following conditions 
//_/_/  are met:
//_/_/
//_/_/  - Redistributions of source code must retain the above copyright 
//_/_/    and collaboration notice, this list of conditions and the following 
//_/_/    disclaimer.
//_/_/
//_/_/  - Redistributions in binary form must reproduce the above copyright 
//_/_/    notice, this list of conditions and the following disclaimer in the 
//_/_/    documentation and/or other materials provided with the distribution.
//_/_/
//_/_/  - Neither the name of its copyright holders nor the names of its 
//_/_/    contributors may be used to endorse or promote products derived from 
//_/_/    this software without specific prior written permission.
//_/_/
//_/_/  - CADIA Clause: The license granted in and to the software under this 
//_/_/    agreement is a limited-use license. The software may not be used in 
//_/_/    furtherance of: (i) intentionally causing bodily injury or severe 
//_/_/    emotional harm to any person; (ii) invading the personal privacy or 
//_/_/    violating the human rights of any person; or (iii) committing or 
//_/_/    preparing for any act of war. 
//_/_/
//_/_/  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
//_/_/  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
//_/_/  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 
//_/_/  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER 
//_/_/  OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
//_/_/  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
//_/_/  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 
//_/_/  PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
//_/_/  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
//_/_/  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
//_/_/  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//_/_/
//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/
#include "MathClasses.h"

namespace cmlabs {

/////////////////////////////////////////////////////////////////////
// Size Class
/////////////////////////////////////////////////////////////////////

Size::Size() { w=0; h=0; d=0; }
Size::Size(double width, double height, double depth) { w=width; h=height; d=depth; }
Size::~Size() {}

bool Size::equals(const Size &s) const {
	return (
		(this->d == s.d) &&
		(this->w == s.w) &&
		(this->h == s.h) );
}

bool Size::operator==(const Size &s) const {
	return equals(s);
}


double Size::getHeight() const { return h; }
double Size::getWidth() const { return w; }
double Size::getDepth() const { return d; }

bool Size::setHeight(double height) { h = height; return true; }
bool Size::setWidth(double width) { w = width; return true; }
bool Size::setDepth(double depth) { d = depth; return true; }

bool Size::isNonZero() const {
	return ( (w != 0) || (h != 0) || (d != 0) );
}

double Size::getArea() const {
	return ( getWidth() * getHeight() );
}

double Size::getDiagonalLenth() const {
	return sqrt( pow(getWidth(), 2) + pow(getHeight(), 2) );
}

std::string Size::print() {
	return utils::StringFormat("(w: %.3f, h: %.3f, d: %.3f)", w, h, d);
}


/////////////////////////////////////////////////////////////////////
// Point Class
/////////////////////////////////////////////////////////////////////

Point::Point() { x=0; y=0; z=0; }
Point::Point(int px, int py, int pz, Size psize) { x=px; y=py; z=pz; size=psize; }
Point::~Point() {}

Point::operator PointFloat() const {
	PointFloat pf;
	pf.setSize(this->size);
	pf.setX(this->x);
	pf.setY(this->y);
	pf.setZ(this->z);
	return pf;
}

int Point::operator[](int n) const {
	switch(n) {
		case 0:
			return x;
		case 1:
			return y;
		case 2:
			return z;
	}
	return 0;
}

bool Point::operator==(const Point &p) const {
	return (
		(this->x == p.x) &&
		(this->y == p.y) &&
		(this->z == p.z) &&
		(this->size == p.size) );
}

bool Point::operator==(const PointFloat &p) const {
	return (
		(this->x == (int)p.x) &&
		(this->y == (int)p.y) &&
		(this->z == (int)p.z) &&
		(this->size == p.size) );
}

Point Point::operator-(const Point &p) const {
	Point np = *this;
	np.x -= p.x;
	np.y -= p.y;
	np.z -= p.z;
	return np;
}
PointFloat Point::operator-(const PointFloat &p) const {
	PointFloat np = *this;
	np.x -= p.x;
	np.y -= p.y;
	np.z -= p.z;
	return np;
}
Point Point::operator+(const Point &p) const {
	Point np = *this;
	np.x += p.x;
	np.y += p.y;
	np.z += p.z;
	return np;
}
PointFloat Point::operator+(const PointFloat &p) const {
	PointFloat np = *this;
	np.x += p.x;
	np.y += p.y;
	np.z += p.z;
	return np;
}
Point Point::operator*(const Point &p) const {
	Point np = *this;
	np.x *= p.x;
	np.y *= p.y;
	np.z *= p.z;
	return np;
}
PointFloat Point::operator*(const PointFloat &p) const {
	PointFloat np = *this;
	np.x *= p.x;
	np.y *= p.y;
	np.z *= p.z;
	return np;
}
Point Point::operator-(double a) const {
	Point np = *this;
	np.x = (int)(np.x - a);
	np.y = (int)(np.y - a);
	np.z = (int)(np.z - a);
	return np;
}
Point Point::operator+(double a) const {
	Point np = *this;
	np.x = (int)(np.x + a);
	np.y = (int)(np.y + a);
	np.z = (int)(np.z + a);
	return np;
}
Point Point::operator*(double a) const {
	Point np = *this;
	np.x = (int)(np.x * a);
	np.y = (int)(np.y * a);
	np.z = (int)(np.z * a);
	return np;
}


int Point::getX() const { return x; }
int Point::getY() const { return y; }
int Point::getZ() const { return z; }
Size Point::getSize() const { return size; }

bool Point::set(int xx, int yy, int zz) { x = xx; y = yy; z = zz; return true; }
bool Point::setX(int n) { x = n; return true; }
bool Point::setY(int n) { y = n; return true; }
bool Point::setZ(int n) { z = n; return true; }
bool Point::setSize(Size s) { size = s; return true; }

double Point::getDistanceTo(Point &p) const {
	return sqrt( pow((double)(getX()-p.getX()), 2) + pow((double)(getX()-p.getX()), 2) );
}
double Point::getDistanceTo(PointFloat &p) const {
	return sqrt( pow((double)(getX()-p.getX()), 2) + pow((double)(getX()-p.getX()), 2) );
}

std::string Point::print() {
	if (size.isNonZero())
		return utils::StringFormat("(%d,%d,%d x %s)", x, y, z, size.print().c_str());
	else
		return utils::StringFormat("(%d,%d,%d)", x, y, z);
}


/////////////////////////////////////////////////////////////////////
// PointFloat Class
/////////////////////////////////////////////////////////////////////

PointFloat::PointFloat() { x=0; y=0; z=0; }
PointFloat::PointFloat(double px, double py, double pz, Size psize) { x=px; y=py; z=pz; size=psize; }
PointFloat::~PointFloat() {}

PointFloat::operator Point() const {
	Point p;
	p.setSize(this->size);
	p.setX((int)this->x);
	p.setY((int)this->y);
	p.setZ((int)this->z);
	return p;
}

double PointFloat::operator[](int n) const {
	switch(n) {
		case 0:
			return x;
		case 1:
			return y;
		case 2:
			return z;
	}
	return 0;
}

bool PointFloat::operator==(const Point &p) const {
	return (
		(this->x == p.x) &&
		(this->y == p.y) &&
		(this->z == p.z) &&
		(this->size == p.size) );
}

bool PointFloat::operator==(const PointFloat &p) const {
	return (
		(this->x == p.x) &&
		(this->y == p.y) &&
		(this->z == p.z) &&
		(this->size == p.size) );
}

PointFloat PointFloat::operator-(const Point &p) const {
	PointFloat np = *this;
	np.x -= p.x;
	np.y -= p.y;
	np.z -= p.z;
	return np;
}
PointFloat PointFloat::operator-(const PointFloat &p) const {
	PointFloat np = *this;
	np.x -= p.x;
	np.y -= p.y;
	np.z -= p.z;
	return np;
}
PointFloat PointFloat::operator+(const Point &p) const {
	PointFloat np = *this;
	np.x += p.x;
	np.y += p.y;
	np.z += p.z;
	return np;
}
PointFloat PointFloat::operator+(const PointFloat &p) const {
	PointFloat np = *this;
	np.x += p.x;
	np.y += p.y;
	np.z += p.z;
	return np;
}
PointFloat PointFloat::operator*(const Point &p) const {
	PointFloat np = *this;
	np.x *= p.x;
	np.y *= p.y;
	np.z *= p.z;
	return np;
}
PointFloat PointFloat::operator*(const PointFloat &p) const {
	PointFloat np = *this;
	np.x *= p.x;
	np.y *= p.y;
	np.z *= p.z;
	return np;
}
PointFloat PointFloat::operator-(double a) const {
	PointFloat np = *this;
	np.x -= a;
	np.y -= a;
	np.z -= a;
	return np;
}
PointFloat PointFloat::operator+(double a) const {
	PointFloat np = *this;
	np.x += a;
	np.y += a;
	np.z += a;
	return np;
}
PointFloat PointFloat::operator*(double a) const {
	PointFloat np = *this;
	np.x *= a;
	np.y *= a;
	np.z *= a;
	return np;
}

double PointFloat::getX() const { return x; }
double PointFloat::getY() const { return y; }
double PointFloat::getZ() const { return z; }
Size PointFloat::getSize() const { return size; }

bool PointFloat::set(double xx, double yy, double zz) { x = xx; y = yy; z = zz; return true; }
bool PointFloat::setX(double n) { x = n; return true; }
bool PointFloat::setY(double n) { y = n; return true; }
bool PointFloat::setZ(double n) { z = n; return true; }
bool PointFloat::setSize(Size s) { size = s; return true; }

double PointFloat::getDistanceTo(const Point &p) const {
	return sqrt( pow((getX()-p.getX()), 2) + pow((getX()-p.getX()), 2) );
}
double PointFloat::getDistanceTo(const PointFloat &p) const {
	return sqrt( pow((getX()-p.getX()), 2) + pow((getX()-p.getX()), 2) );
}

std::string PointFloat::print() {
	if (size.isNonZero()) {
		if (z != 0)
			return utils::StringFormat("(%.3f,%.3f,%.3f x %s)", x, y, z, (char*)size.print().c_str());
		else
			return utils::StringFormat("(%.3f,%.3f x %s)", x, y, (char*)size.print().c_str());
	}
	else {
		if (z != 0)
			return utils::StringFormat("(%.3f,%.3f,%.3f)", x, y, z);
		else
			return utils::StringFormat("(%.3f,%.3f)", x, y);
	}
}





/////////////////////////////////////////////////////////////////////
// Line Class
/////////////////////////////////////////////////////////////////////

Line::Line() {
	lineWidth = 0;
}

Line::Line(PointFloat startpoint, PointFloat endpoint, double width) {
	startPoint = startpoint;
	endPoint = endpoint;
	lineWidth = width;
}

Line::~Line() {
}

PointFloat Line::getStartPoint() const {
	return startPoint;
}
PointFloat Line::getEndPoint() const {
	return endPoint;
}
double Line::getLineWidth() const {
	return lineWidth;
}

bool Line::setStartPoint(PointFloat point) {
	startPoint = point;
	return true;
}
bool Line::setEndPoint(PointFloat point) {
	endPoint = point;
	return true;
}
bool Line::setLineWidth(double width) {
	lineWidth = width;
	return true;
}

std::string Line::print() {
	if (lineWidth != 0)
		return utils::StringFormat("%s-%sx(W: %.3f)", (char*) startPoint.print().c_str(), (char*) endPoint.print().c_str(), lineWidth);
	else
		return utils::StringFormat("%s-%s", (char*) startPoint.print().c_str(), (char*) endPoint.print().c_str());
}



/////////////////////////////////////////////////////////////////////
// PolyLine Class
/////////////////////////////////////////////////////////////////////

PolyLine::PolyLine() {
}

PolyLine::~PolyLine() {}

uint32 PolyLine::getLineCount() const {
	return (uint32)lines.size();
}

Line PolyLine::getLine(uint32 pos) const {
	if (pos > lines.size())
		return Line();
	return lines[pos];
}
bool PolyLine::addLine(Line line) {
	lines.push_back(line);
	return true;
}
bool PolyLine::replaceLine(uint32 pos, Line newline) {
	lines[pos] = newline;
	return true;
}
bool PolyLine::removeLine(uint32 pos) {
	lines[pos] = Line();
	return true;
}

std::string PolyLine::print() {
	if (lines.size() == 0)
		return "PolyLine: (empty)";
	std::string str = "PolyLine: ";
	uint32 n;
	for (n=0; n<=lines.size()-1; n++) {
		str += utils::StringFormat("%s,", (char*)lines[n].print().c_str());
	}
	str += utils::StringFormat("%s", (char*)lines[n].print().c_str());
	return str;
}























/////////////////////////////////////////////////////////////////////
// Box Class
/////////////////////////////////////////////////////////////////////

Box::Box() {
	lineWidth = 0;
	orientation = 0;
}

Box::Box(double x, double y, double w, double h, double linewidth) {
	upperLeft = PointFloat(x, y);
	size = Size(w, h);

	if (size.getWidth() < 0) {
		size.setWidth(-1*size.getWidth());
		upperLeft.setX(upperLeft.getX()-size.getWidth());
	}

	if (size.getHeight() < 0) {
		size.setHeight(-1*size.getHeight());
		upperLeft.setY(upperLeft.getY()-size.getHeight());
	}

	lineWidth = linewidth;
	orientation = 0;
}

Box::Box(PointFloat upperleft, Size boxsize, double linewidth) {
	upperLeft = upperleft;
	size = boxsize;

	if (size.getWidth() < 0) {
		size.setWidth(-1*size.getWidth());
		upperLeft.setX(upperLeft.getX()-size.getWidth());
	}

	if (size.getHeight() < 0) {
		size.setHeight(-1*size.getHeight());
		upperLeft.setY(upperLeft.getY()-size.getHeight());
	}

	lineWidth = linewidth;
	orientation = 0;
}

Box::Box(PointFloat upperleft, PointFloat lowerright, double linewidth) {
	upperLeft = upperleft;
	size = Size(lowerright.getX() - upperleft.getX(), lowerright.getY() - upperleft.getY());

	if (size.getWidth() < 0) {
		size.setWidth(-1*size.getWidth());
		upperLeft.setX(upperLeft.getX()-size.getWidth());
	}

	if (size.getHeight() < 0) {
		size.setHeight(-1*size.getHeight());
		upperLeft.setY(upperLeft.getY()-size.getHeight());
	}

	lineWidth = linewidth;
	orientation = 0;
}

Box::~Box() {
}


double Box::getCMX() const {
	return getCM().getX();
}

double Box::getCMY() const {
	return getCM().getY();
}

PointFloat Box::getCM() const {
	return PointFloat(upperLeft.getX()+(size.getWidth()/2), upperLeft.getY()+(size.getHeight()/2));
}

PointFloat Box::getUpperLeft() const {
	return upperLeft;
}

PointFloat Box::getUpperRight() const {
	return PointFloat(upperLeft.getX()+size.getWidth(), upperLeft.getY());
}

PointFloat Box::getLowerLeft() const {
	return PointFloat(upperLeft.getX(), upperLeft.getY()+size.getHeight());
}

PointFloat Box::getLowerRight() const {
	return PointFloat(upperLeft.getX()+size.getWidth(), upperLeft.getY()+size.getHeight());
}

double Box::getUpperY() const {
	return upperLeft.getY();
}

double Box::getLowerY() const {
	return upperLeft.getY() + size.getHeight();
}

double Box::getLeftX() const {
	return upperLeft.getX();
}

double Box::getRightX() const {
	return upperLeft.getX() + size.getWidth();
}


double Box::getLineWidth() const {
	return lineWidth;
}

Size Box::getSize() const {
	return size;
}

double Box::getArea() const {
	return ( size.w * size.h );
}

double Box::getWidth() const {
	return size.getWidth();
}

double Box::getHeight() const {
	return size.getHeight();
}


bool Box::move(double dx, double dy) {
	return set(getLeftX() + dx, getUpperY() + dy, getWidth(), getHeight());
}

bool Box::moveTo(double x, double y) {
	return set(x, y, getWidth(), getHeight());
}

bool Box::grow(double dw, double dh) {
	double w = getWidth() + dw;
	double h = getHeight() + dh;
	return setSize(Size(w, h));
}



Box Box::getConstrainedCopy(const Box& box) {
	Box newbox = *this;
	newbox.constrainTo(box);
	return newbox;
}

Box Box::getConstrainedCopy(double x, double y, double w, double h) {
	Box newbox = *this;
	newbox.constrainTo(x, y, w, h);
	return newbox;
}

bool Box::constrainTo(const Box& box) {
	return constrainTo(box.upperLeft.x, box.upperLeft.y, box.size.w, box.size.h);
}

bool Box::constrainTo(double x, double y, double w, double h) {
	if (upperLeft.x < x)
		upperLeft.x = 0;
	if (upperLeft.y < y)
		upperLeft.y = 0;
	if (upperLeft.x + size.w > w)
		size.w = w - upperLeft.x;
	if (upperLeft.y + size.h > h)
		size.h = h - upperLeft.y;
	return true;
}

Box Box::getDoubleSizeSameCenter() {
	Box box;
	box.upperLeft.x = upperLeft.x - (size.w / 2.0);
	box.upperLeft.y = upperLeft.y - (size.h / 2.0);
	box.size.w = size.w * 2.0;
	box.size.h = size.h * 2.0;
	return box;
}

bool Box::set(double x, double y, double w, double h, double linewidth) {
	upperLeft = PointFloat(x, y);
	size = Size(w, h);

	if (size.getWidth() < 0) {
		size.setWidth(-1*size.getWidth());
		upperLeft.setX(upperLeft.getX()-size.getWidth());
	}

	if (size.getHeight() < 0) {
		size.setHeight(-1*size.getHeight());
		upperLeft.setY(upperLeft.getY()-size.getHeight());
	}

	lineWidth = linewidth;
	return true;
}


bool Box::setUpperLeft(const PointFloat& point) {
	upperLeft = point;
	return true;
}

bool Box::setSize(const Size& boxsize) {
	size = boxsize;
	return true;
}

bool Box::setLineWidth(double width) {
	lineWidth = width;
	return true;
}


bool Box::isPointWithin(int x, int y) const {
	if ( x > this->getRightX() )
		return false;
	else if ( x < this->getLeftX() )
		return false;
	else if ( y < this->getUpperY() )
		return false;
	else if ( y > this->getLowerY() )
		return false;
	return true;
}

bool Box::isPointWithin(const PointFloat& point) const {
	if ( point.getX() > this->getRightX() )
		return false;
	else if ( point.getX() < this->getLeftX() )
		return false;
	else if ( point.getY() < this->getUpperY() )
		return false;
	else if ( point.getY() > this->getLowerY() )
		return false;
	return true;
}

PointFloat Box::getCentreMass() const {
	return PointFloat(upperLeft.getX()+(size.getWidth()/2), upperLeft.getY()+(size.getHeight()/2));
}

bool Box::hasZeroSize() const {
	return (!size.isNonZero());
}

bool Box::equals(const Box &otherbox) const {
	return (
		(this->size == otherbox.size) &&
		(this->upperLeft == otherbox.upperLeft) &&
		(this->orientation == otherbox.orientation) );
}

bool Box::equals(const Box &otherbox, double maxerror) const {
	if (maxerror == 0.0)
		maxerror = -0.001;
	return (percentOverlap(otherbox) <= maxerror);
}

bool Box::growToBoundingBox(const Box &otherbox) {
	Box bb = getBoundingBox(otherbox);
	if (bb.hasZeroSize())
		return false;
	this->upperLeft = bb.upperLeft;
	this->size = bb.size;
	return true;
}

bool Box::growToIncludePoint(const Point& point, int padX, int padY) {
	double dif;
	if ( ( dif = upperLeft.x - (point.x - padX) ) > 0 ) {
		upperLeft.x -= dif;
		size.w += dif;
	}
	if ( ( dif = upperLeft.y - (point.y - padY) ) > 0 ) {
		upperLeft.y -= dif;
		size.h += dif;
	}

	if ( ( dif = point.x + padX - (upperLeft.x + size.w) ) > 0 ) {
		size.w += dif;
	}
	if ( ( dif = point.y + padY - (upperLeft.y + size.h) ) > 0 ) {
		size.h += dif;
	}
	return true;
}

Box Box::getBoundingBox(const Box &otherbox) const {

	double left, right, top, bottom;

	if (this->getLeftX() <= otherbox.getLeftX())
		left = this->getLeftX();
	else
		left = otherbox.getLeftX();

	if (this->getRightX() <= otherbox.getRightX())
		right = otherbox.getRightX();
	else
		right = this->getRightX();

	if (this->getUpperY() <= otherbox.getUpperY())
		top = this->getUpperY();
	else
		top = otherbox.getUpperY();

	if (this->getLowerY() <= otherbox.getLowerY())
		bottom = otherbox.getLowerY();
	else
		bottom = this->getLowerY();

	return Box(PointFloat(left, top), PointFloat(right, bottom));
}

Box Box::getOverlapBox(const Box &otherbox) const {

	Box zeroOverlap = Box();

	// Rule out zero overlap first
	if ( otherbox.getLeftX() > this->getRightX() )
		return zeroOverlap;
	else if ( otherbox.getRightX() < this->getLeftX() )
		return zeroOverlap;
	else if ( otherbox.getLowerY() < this->getUpperY() )
		return zeroOverlap;
	else if ( otherbox.getUpperY() > this->getLowerY() )
		return zeroOverlap;

	// Now we know there is an overlap

	double left, right, top, bottom;

	if (this->getLeftX() <= otherbox.getLeftX())
		left = otherbox.getLeftX();
	else
		left = this->getLeftX();

	if (this->getRightX() <= otherbox.getRightX())
		right = this->getRightX();
	else
		right = otherbox.getRightX();

	if (this->getUpperY() <= otherbox.getUpperY())
		top = otherbox.getUpperY();
	else
		top = this->getUpperY();

	if (this->getLowerY() <= otherbox.getLowerY())
		bottom = this->getLowerY();
	else
		bottom = otherbox.getLowerY();

	return Box(PointFloat(left, top), PointFloat(right, bottom));
}

double Box::percentOverlap(const Box &otherbox) const {

	Box overlap = getOverlapBox(otherbox);
	if (!overlap.getSize().isNonZero())
		return 0.0;

	double myArea = size.getArea();
	double overlapArea = overlap.getSize().getArea();
	
	
	if ( (myArea > overlapArea) && (myArea != 0.0) )
		return ( overlapArea / myArea );
	else if (overlapArea != 0.0)
		return ( myArea / overlapArea );
	else
		return 0;
}






std::string Box::print() {
	if (lineWidth != 0)
		return utils::StringFormat("%s-%sx(W: %.3f)", (char*) getUpperLeft().print().c_str(), (char*) getLowerRight().print().c_str(), lineWidth);
	else
		return utils::StringFormat("%s-%s", (char*) getUpperLeft().print().c_str(), (char*) getLowerRight().print().c_str());
}



















/////////////////////////////////////////////////////////////////////
// Vector2D Class
/////////////////////////////////////////////////////////////////////

Vector2D::Vector2D() { x=0; y=0; }
Vector2D::Vector2D(double x, double y) { this->x=x; this->y=y; }
Vector2D::~Vector2D() {}

double Vector2D::operator[](int n) const {
	switch(n) {
		case 0:
			return x;
		case 1:
			return y;
	}
	return 0;
}

bool Vector2D::operator==(const Vector2D &v) const {
	return (
		(this->x == v.x) &&
		(this->y == v.y) );
}

Vector2D Vector2D::operator-(const Vector2D &v) const {
	Vector2D np = *this;
	np.x -= v.x;
	np.y -= v.y;
	return np;
}

Vector2D Vector2D::operator+(const Vector2D &v) const {
	Vector2D np = *this;
	np.x += v.x;
	np.y += v.y;
	return np;
}

double Vector2D::operator*(const Vector2D &v) const {
	return (x*v.x) + (y*v.y);
}

Vector2D Vector2D::operator-(double a) const {
	Vector2D np = *this;
	np.x -= a;
	np.y -= a;
	return np;
}

Vector2D Vector2D::operator+(double a) const {
	Vector2D np = *this;
	np.x += a;
	np.y += a;
	return np;
}

Vector2D Vector2D::operator*(double a) const {
	Vector2D np = *this;
	np.x *= a;
	np.y *= a;
	return np;
}

const Vector2D& Vector2D::operator-=(const Vector2D &v) {
	this->x -= v.x;
	this->y -= v.y;
	return *this;
}

const Vector2D& Vector2D::operator+=(const Vector2D &v) {
	this->x += v.x;
	this->y += v.y;
	return *this;
}

const Vector2D& Vector2D::operator-=(double a) {
	this->x -= a;
	this->y -= a;
	return *this;
}

const Vector2D& Vector2D::operator+=(double a) {
	this->x += a;
	this->y += a;
	return *this;
}

const Vector2D& Vector2D::operator*=(double a) {
	this->x *= a;
	this->y *= a;
	return *this;
}



double Vector2D::getX() const { return x; }
double Vector2D::getY() const { return y; }

bool Vector2D::set(const PointFloat& p1, const PointFloat& p2) {
	return set(p1.x, p1.y, p2.x, p2.y);
}

bool Vector2D::set(double x1, double y1, double x2, double y2) {
	return set(x2-x1, y2-y1);
}

bool Vector2D::set(double xx, double yy) { x = xx; y = yy; return true; }
bool Vector2D::setX(double n) { x = n; return true; }
bool Vector2D::setY(double n) { y = n; return true; }

std::string Vector2D::print() {
	return utils::StringFormat("(%.3f,%.3f)", x, y);
}

double Vector2D::length() const {
	return sqrt((x*x) + (y*y));
}

double Vector2D::det(const Vector2D &v) const {
	return (x*v.y) - (y*v.x);
}

bool Vector2D::isOrthogonalWith(const Vector2D &v) const {
	return (*this*v == 0);
}

bool Vector2D::isParallelWith(const Vector2D &v) const {
	return (det(v) == 0);
}

Vector2D Vector2D::getProjectionOn(const Vector2D &v) const {
	Vector2D unit = v.getUnitVector();
	return unit*(*this*unit);
}

Vector2D Vector2D::getUnitVector() const {
	return *this*(1.0/length());
}

Vector2D Vector2D::getOrthogonalVector() const {
	return Vector2D(0-y, x);
}

double Vector2D::getAngle(const Vector2D &v) const {
	double lengths = this->length() * v.length();
	if (lengths == 0)
		return 0;
	return acos((*this*v)/lengths);
}

double Vector2D::getArea(const Vector2D &v) const {
	return fabs(det(v));
}




/////////////////////////////////////////////////////////////////////
// Vector3D Class
/////////////////////////////////////////////////////////////////////

Vector3D::Vector3D() { x=0; y=0; z=0; }
Vector3D::Vector3D(double x, double y, double z) { this->x=x; this->y=y; this->z=z; }
Vector3D::~Vector3D() {}

double Vector3D::operator[](int n) const {
	switch(n) {
		case 0:
			return x;
		case 1:
			return y;
		case 2:
			return z;
	}
	return 0;
}

bool Vector3D::operator==(const Vector3D &v) const {
	return (
		(this->x == v.x) &&
		(this->y == v.y) &&
		(this->z == v.z) );
}

Vector3D Vector3D::operator-(const Vector3D &v) const {
	Vector3D np = *this;
	np.x -= v.x;
	np.y -= v.y;
	np.z -= v.z;
	return np;
}

Vector3D Vector3D::operator+(const Vector3D &v) const {
	Vector3D np = *this;
	np.x += v.x;
	np.y += v.y;
	np.z += v.z;
	return np;
}

double Vector3D::operator*(const Vector3D &v) const {
	return (x*v.x) + (y*v.y) + (z*v.z);
}

Vector3D Vector3D::operator-(double a) const {
	Vector3D np = *this;
	np.x -= a;
	np.y -= a;
	np.z -= a;
	return np;
}

Vector3D Vector3D::operator+(double a) const {
	Vector3D np = *this;
	np.x += a;
	np.y += a;
	np.z += a;
	return np;
}

Vector3D Vector3D::operator*(double a) const {
	Vector3D np = *this;
	np.x *= a;
	np.y *= a;
	np.z *= a;
	return np;
}

const Vector3D& Vector3D::operator-=(const Vector3D &v) {
	this->x -= v.x;
	this->y -= v.y;
	this->z -= v.z;
	return *this;
}

const Vector3D& Vector3D::operator+=(const Vector3D &v) {
	this->x += v.x;
	this->y += v.y;
	this->z += v.z;
	return *this;
}

const Vector3D& Vector3D::operator-=(double a) {
	this->x -= a;
	this->y -= a;
	this->z -= a;
	return *this;
}

const Vector3D& Vector3D::operator+=(double a) {
	this->x += a;
	this->y += a;
	this->z += a;
	return *this;
}

const Vector3D& Vector3D::operator*=(double a) {
	this->x *= a;
	this->y *= a;
	this->z *= a;
	return *this;
}



double Vector3D::getX() const { return x; }
double Vector3D::getY() const { return y; }
double Vector3D::getZ() const { return z; }

bool Vector3D::set(const PointFloat& p1, const PointFloat& p2) {
	return set(p1.x, p1.y, p1.z, p2.x, p2.y, p2.z);
}

bool Vector3D::set(double x1, double y1, double z1, double x2, double y2, double z2) {
	return set(x2-x1, y2-y1, z2-z1);
}

bool Vector3D::set(double xx, double yy, double zz) { x = xx; y = yy; z = zz; return true; }
bool Vector3D::setX(double n) { x = n; return true; }
bool Vector3D::setY(double n) { y = n; return true; }
bool Vector3D::setZ(double n) { z = n; return true; }

std::string Vector3D::print() {
	return utils::StringFormat("(%.3f,%.3f,%.3f)", x, y, z);
}

double Vector3D::length() const {
	return sqrt((x*x) + (y*y) + (z*z));
}

Vector3D Vector3D::product(const Vector3D &v) const {
	return Vector3D(
		this->y*v.z - this->z*v.y,
		this->z*v.x - this->x*v.z,
		this->x*v.y - this->y*v.x);
}

bool Vector3D::isOrthogonalWith(const Vector3D &v) const {
	return (*this*v == 0);
}

bool Vector3D::isParallelWith(const Vector3D &v) const {
	return (product(v).length() == 0);
}

Vector3D Vector3D::getProjectionOn(const Vector3D &v) const {
	Vector3D unit = v.getUnitVector();
	return unit*(*this*unit);
}

Vector3D Vector3D::getUnitVector() const {
	return *this*(1.0/length());
}

double Vector3D::getAngle(const Vector3D &v) const {
	double lengths = this->length() * v.length();
	if (lengths == 0)
		return 0;
	return acos((*this*v)/lengths);
}

double Vector3D::getArea(const Vector3D &v) const {
	return fabs(product(v).length());
}







Color::Color() {
	init(0, 0, 0);
}

Color::Color(uint8 red, uint8 green, uint8 blue) {
	init(red, green, blue);
}

Color::Color(const Color &color) {
	init(color.r, color.g, color.b);
	size = color.size;
}

Color::Color(const char* colorName) {
	init(colorName);
}

Color::~Color() {
}

bool Color::init(uint8 red, uint8 green, uint8 blue) {
	r = red;
	g = green;
	b = blue;
	size = 1;
	return true;
}

bool Color::init(const char* colorName) {
	
	if (!colorName || !strlen(colorName))
		return init(0, 0, 0);

	if (strchr(colorName, ',')) {
		std::vector<std::string> col = utils::TextListSplit(colorName, ",", true);
		if (col.size() < 3)
			return false;
		else if (col.size() == 3)
			return init(
				(uint8)utils::Ascii2Uint32(col[0].c_str()),
				(uint8)utils::Ascii2Uint32(col[1].c_str()),
				(uint8)utils::Ascii2Uint32(col[2].c_str())
			);
		else {
			size = utils::Ascii2Uint32(col[3].c_str());
			return init(
				(uint8)utils::Ascii2Uint32(col[0].c_str()),
				(uint8)utils::Ascii2Uint32(col[1].c_str()),
				(uint8)utils::Ascii2Uint32(col[2].c_str())
				);
		}
	}
	
	std::map<std::string, std::array<uint8, 3> >::const_iterator i = ColorMap.find(colorName);
	if (i != ColorMap.end())
		return init((*i).second.at(0), (*i).second.at(1), (*i).second.at(2));
	else
		return init(0, 0, 0);
}

//bool Color::setNoColor() {
//	r = g = b = -1;
//	return true;
//}
//
//bool Color::isValid() {
//	return (!isNoColor());
//}
//
//bool Color::isNoColor() {
//	return ((r < 0) || (g < 0) || (b < 0));
//}

Color Color::mixOn(Color& color, double weight) {
	// color is the background color
	if (weight > 1)
		weight = 1;
	if (weight < 0)
		weight = 0;
	Color resColor;
	resColor.r = (uint8)(color.r*(1 - weight) + this->r*weight);
	resColor.g = (uint8)(color.g*(1 - weight) + this->g*weight);
	resColor.b = (uint8)(color.b*(1 - weight) + this->b*weight);
	return resColor;
}

uint8 Color::getGreyValue() {
	return (uint8)(((double)r + g + b) / 3.0);
}

Color Color::getReverseColor() {
	Color resColor;
	resColor.r = 255 - r;
	resColor.g = 255 - g;
	resColor.b = 255 - b;
	return resColor;
}

double Color::distance(Color &otherColor) {
	return pow((double)pow((double)r - otherColor.r, 2) + pow((double)g - otherColor.g, 2) + pow((double)b - otherColor.b, 2), (1.0 / 3));
}

std::vector<Color> Color::createColorsMaxDifference(uint32 count, bool mix) {

	int rcount = (int)(count / 3);
	if (rcount <= 0) rcount = 1;
	int gcount = (int)(count / 3);
	if (gcount <= 0) gcount = 1;
	int bcount = count - rcount - gcount;
	if (bcount <= 0) bcount = 1;

	int n;
	std::vector<Color> col;
	int value = 0;
	for (n = 0; n<rcount; n++) {
		value = (int)((n + 1)*(255.0 / rcount));
		if (value >= 256) value = 255;
		col.push_back(Color(value, 0, 0));
	}
	for (n = 0; n<gcount; n++) {
		value = (int)((n + 1)*(255.0 / gcount));
		if (value >= 256) value = 255;
		col.push_back(Color(0, value, 0));
	}
	for (n = 0; n<bcount; n++) {
		value = (int)((n + 1)*(255.0 / bcount));
		if (value >= 256) value = 255;
		col.push_back(Color(0, 0, value));
	}

	if (mix)
		std::random_shuffle(col.begin(), col.end());
	return col;
}




/////////////////////////////////////////////////////////////////////
// RandomPathGenerator Class
/////////////////////////////////////////////////////////////////////

RandomPathGenerator::RandomPathGenerator() {
	lastPointTime = 0;
	srand((uint32)(uint64)this);
}

RandomPathGenerator::~RandomPathGenerator() {
}

bool RandomPathGenerator::setStartPoint(double x, double y) {
	lastPoint.x = x;
	lastPoint.y = y;
	lastPointTime = GetTimeNow();
	return true;
}

bool RandomPathGenerator::setDirection(double dx, double dy) {
	direction.x = dx;
	direction.y = dy;
	return true;
}

bool RandomPathGenerator::setVariation(double var) {
	variation.x = variation.y = var;
	return true;
}

PointFloat RandomPathGenerator::generateNextPoint() {
	double secs = (double)GetTimeAge(lastPointTime) / 1000000.0;
	//Interpolate next exact point from last point and timestamp
	PointFloat nextPoint, dynPoint;
	nextPoint.x = lastPoint.x + (secs * direction.x);
	nextPoint.y = lastPoint.y + (secs * direction.y);
	//Move exact point by random variation
	dynPoint.x = utils::RandomValue(nextPoint.x - variation.x, nextPoint.x + variation.x);
	dynPoint.y = utils::RandomValue(nextPoint.y - variation.y, nextPoint.y + variation.y);
	//Publish point
	lastPoint = nextPoint;
	lastPointTime = GetTimeNow();
	return dynPoint;
}



} // namespace cmlabs
