#include "InstructionFactory.h"
#include "InstructionOptions.h"
#include "../animations/Animation.h"

#include "../objects/Line.h"
#include "../objects/Circle.h"
#include "../objects/Rectangle.h"
#include "../objects/Spiral.h"
#include "../objects/CompositeObject.h"

#include <chrono>
#include <cmath>
#include <cassert>

#include <boost/date_time/time.hpp>

#include <json/value.h>

namespace laser { namespace holodeck {

const Color MetaDataColor(Color::LIGHTBLUE);

static void ShiftPoints(std::vector<Point> & points, Point base)
{
	for (Point & p: points)
		p += base;
}

/*!
 * \brief determine characteristic values for a rectangle
 * \image html calculateRectangleCharacteristics.svg
 * \param[in] p1 midpoint of one short side of the rectangle
 * \param[in] p2 midpoint of the other short side
 * \param[out] angle angle between x-axis and line between \a p1 and \a p2
 * \param[out] length \f$ \mbox{length} = \sqrt{(\mbox{p1}_x - \mbox{p2}_x)^2 + (\mbox{p1}_y - \mbox{p2}_y)^2} \f$
 * \param[out] start start point of axis aligned bounding box
 * \param[out] mid mid point of axis aligned bounding box
 * \param[out] end end point of axis aligned bounding box
 */
static void calculateRectangleCharacteristics(Point p1, Point p2, float &angle, float &length, Point &start, Point &mid, Point &end)
{
	// calculate rotation
	int dx = p2.x() - p1.x();
	int dy = p2.y() - p1.y();
	angle = atan2(dy, dx);

	// length of wall
	length = sqrt(sqr(dx) + sqr(dy));

	mid = (p1 + p2) / 2;
	start = mid - Point(0, length / 2);
	end   = mid + Point(0, length / 2);
}

static ObjectPtr getDigit(const Json::Value &root, unsigned int i, Point p = Point(0, 0), double rotation = 0.0)
{
	int id = root.get("turkers",
								Json::Value())
						  .get(i,
								Json::Value(-1))
						  .asInt();
	CompositeObjectPtr digit = opts::Digit::get(id);
	if (digit) {
		digit->rotate(rotation, Point(250, 500));
		digit->move(p);
		digit->setColor(MetaDataColor);
	}
	return digit;
}

static ObjectPtr MovingIndicator(const Point & p1, double angle)
{
	const double right = p1.x() + opts::IndicatorWidth * 0.5;
	const double left = p1.x() - opts::IndicatorWidth * 0.5;
	const double top = p1.y() - opts::IndicatorHeight * 0.5;
	const double bottom = p1.y() + opts::IndicatorHeight * 0.5;

	CompositeObjectPtr group = CompositeObject::construct();
	Line *lA0 = new Line(Point(left, top), p1),
		 *lA1 = new Line(p1, Point(right, top)),
		 *lB0 = new Line(right, p1.y(), p1.x(), bottom),
		 *lB1 = new Line(p1.x(), bottom, left, p1.y());

	group->add(lA0);
	group->add(lA1);
	group->add(lB0);
	group->add(lB1);

	group->setColor(MetaDataColor);
	group->rotate(angle, p1);

	std::shared_ptr<int> storedStep(new int);
	*storedStep = 3;
	group->addAnimation([=](Object *){
		// What about static's semantic?
		int step = *storedStep;
		lA0->setVisible((bool)(step & 1) != (bool)(step & 2));
		lA1->setVisible((bool)(step & 1) != (bool)(step & 2));

		lB0->setVisible(step & 2);
		lB1->setVisible(step & 2);
		*storedStep = (step + 1) & 3;
	}, Animation::msecs(200));

	return group;
}

////////////////////////////////////////////////////////////

ObjectPtr InstructionFactory::Wall(const Json::Value &root, Point p1, Point p2, Point p3, Point p4)
{
	return Table(root, p1, p2, p3, p4);
}

// Hannes asked to define walls by four points. I keep that one for MovingWalls, Door, etc.
static ObjectPtr TwoPointWall(const Json::Value & root, Point p1, Point p2)
{
	CompositeObjectPtr group = CompositeObject::construct();
	group->add(new Line(p1, p2, true));

	float alpha;
	float length;
	Point midPoint;
	Point start;
	Point end;

	calculateRectangleCharacteristics(p1, p2, alpha, length, start, midPoint, end);

	Point p1p2 = p2 - p1;
	ObjectPtr turkerId = getDigit(root, 0);
	if(turkerId){
		turkerId->rotate(alpha);
		turkerId->move(midPoint - p1p2 / 2);
		turkerId->move(Point(-p1p2.y(), p1p2.x()).norm() * 100);

		group->add(turkerId);
	}

	return group;
}

ObjectPtr InstructionFactory::MovingWall(const Json::Value &root, Point p1, Point p2)
{
	CompositeObjectPtr group = std::dynamic_pointer_cast<CompositeObject>(TwoPointWall(root, p1, p2));

	Point direction = p2 - p1;
	Point mid = p1 + direction * 0.5;
	Point spacer = direction.perpendicular().norm() * opts::IndicatorDistance;

	group->add(MovingIndicator(mid + spacer, direction.angle()));

	return group;
}

ObjectPtr InstructionFactory::Door(const Json::Value &root, Point p1, Point p2)
{
	CompositeObjectPtr door = std::dynamic_pointer_cast<CompositeObject>(TwoPointWall(root, p1, p2));
	door->add(new Circle(p2, 750));

	return door;
}

ObjectPtr InstructionFactory::Table(const Json::Value &root, Point p1, Point p2, Point p3, Point p4)
{
	CompositeObjectPtr group = CompositeObject::construct();
	const Point center = (p1 + p2 + p3 + p4) * 0.25;
	group->add(new Rectangle(p1, p2, p3, p4, false));
	group->add(getDigit(root, 0, center));

	return group;
}

ObjectPtr InstructionFactory::Player(const Json::Value &root, Point p)
{
	// No turkerId needed here
	(void)root;
	ObjectPtr c(new Circle(p, 1000));
	c->setPermanent(true);
	return c;
}

ObjectPtr InstructionFactory::Switch(const Json::Value &root, Point p1, Point p2)
{
	CompositeObjectPtr group = CompositeObject::construct();

	group->add(new Circle(p2, opts::SwitchHandleSize));

	Point stickDirection(p2 - p1);
	Point attachmentPoint = p2 - stickDirection.norm() * opts::SwitchHandleSize;
	group->add(new Line(p1, attachmentPoint));
	group->add(getDigit(root, 0, p1 + stickDirection / 2, stickDirection.angle()));

	return group;
}

ObjectPtr InstructionFactory::Beam(const Json::Value &root, Point p1, Point p2)
{
	CompositeObjectPtr group = CompositeObject::construct();

	float alpha;
	float length;
	Point midPoint;
	Point start;
	Point end;
	Point dir(p2 - p1);

	calculateRectangleCharacteristics(p1, p2, alpha, length, start, midPoint, end);

	Rectangle *bigRect = new Rectangle(midPoint.x() - 1000,
									   midPoint.y() - length/2,
									   2000, // beam is 1000 thick
									   length, // and as long as requested
									   false);
	bigRect->rotate(alpha-M_PI_2, midPoint);
	group->add(bigRect);
	group->add(getDigit(root, 0, midPoint + dir.norm().perpendicular() * 1000, dir.angle()));

	return group;
}

static ObjectPtr Portal(const Json::Value &root, Point p1, Point p2, bool active)
{
	(void)root;
	CompositeObjectPtr group = CompositeObject::construct();

	float alpha;
	float length;
	Point mid;
	Point start;
	Point end;

	calculateRectangleCharacteristics(p1, p2, alpha, length, start, mid, end);

	if (active)
	{
		group->add(new Line(start, start + Point(length / 3, 0)));
		group->add(new Spiral(mid.x(), mid.y() + length/8, 200, length/4, 3, active));
		group->add(new Line(end - Point(length/3, 0), end));
	}
	else
	{
		group->add(new Line(start, end));
		group->add(new Spiral(mid.x(), mid.y() + length/8, 200, length/4, 3, active));
	}

	group->rotate(alpha, mid);

	return group;
}

ObjectPtr InstructionFactory::PortalInactive(const Json::Value &root, Point p1, Point p2)
{ return Portal(root, p1, p2, false); }

ObjectPtr InstructionFactory::PortalActive(const Json::Value &root, Point p1, Point p2)
{ return Portal(root, p1, p2, true); }

ObjectPtr InstructionFactory::Zipline(const Json::Value &root, Point p1, Point p2)
{
	CompositeObjectPtr group = CompositeObject::construct();
	const Point midaxis(p2 - p1);
	const Point direction = midaxis.norm();
	const Point perpendic = direction.perpendicular();

	// Need 5 turkers
	auto addTurkerDigit = [&](const int index, const Point & pos) {
		group->add(getDigit(root, index, pos, direction.angle()));
	};

	addTurkerDigit(0, p1 - direction.norm() * (opts::ZipLineOuterCircle + opts::Number0Right) - perpendic * opts::Number0Bottom);
	addTurkerDigit(1, p1 - direction.norm() * (opts::ZipLineOuterCircle + opts::Number0Right));

	group->add(new Circle(p1, opts::ZipLineOuterCircle));
	group->add(new Circle(p1, opts::ZipLineInnerCircle));

	group->add(new Line(p1, p2));

	group->add(new Circle(p2, opts::ZipLineInnerCircle));
	group->add(new Circle(p2, opts::ZipLineOuterCircle));

	addTurkerDigit(2, p2 + direction.norm() * opts::ZipLineOuterCircle - perpendic * opts::Number0Bottom);
	addTurkerDigit(3, p2 + direction.norm() * opts::ZipLineOuterCircle);

	addTurkerDigit(4, p1 + midaxis * 0.5);

	return group;
}

ObjectPtr InstructionFactory::ZiplineWithStep(const Json::Value &root, Point p1, Point p2, Point p3, Point p4)
{
	CompositeObjectPtr group = std::dynamic_pointer_cast<CompositeObject>(Zipline(root, p1, p2));

	const Point direction(p2 - p1);

	auto fromMidLineTo = [&](const Point & target){
		// Project p1-target-vector on p1-p2-vector
		Point b_proj = direction * (direction.dot(target - p1) / sqr(direction.abs()));
		// find foot point and measure distance
		return target - (p1 + b_proj);
	};

	const Point toP3 = fromMidLineTo(p3);
	const Point toP4 = fromMidLineTo(p4);
	const Point leftStart = p1 + (direction.norm() * opts::ZipLineOuterCircle);
	const Point rightEnd = p2 - (direction.norm() * opts::ZipLineOuterCircle);
	ObjectPtr step(new Rectangle(leftStart + toP4, rightEnd + toP4, rightEnd + toP3, leftStart + toP3));
	group->add(step);

	const Point indicatorLine = p1 + toP3 + toP3.norm() * opts::IndicatorDistance;
	group->add(MovingIndicator(indicatorLine + direction * 0.25, direction.angle()));
	group->add(MovingIndicator(indicatorLine + direction * 0.75, direction.angle()));

	// No digit here. Let the guy with the horizontal stick think he is in charge

	return group;
}

ObjectPtr InstructionFactory::Stool(const Json::Value &root, Point p1, Point p2, Point p3, Point p4)
{
	//return std::make_shared<Rectangle>(p1, p2, p3, p4, false);

	// They just look the same
	return Table(root, p1, p2, p3, p4);
}

ObjectPtr InstructionFactory::Corpse(const Json::Value &root, Point head, Point chest)
{
	CompositeObjectPtr corpse = CompositeObject::construct();

	const double bodyAspectRatio = 0.702;
	const double spine = (head - chest).abs(); // bodyHeight
	const double bodyWidth = bodyAspectRatio * spine * 2;
	const double armWidth = 0.10 * spine;

	Point armStart(bodyWidth * (3.0 / 8), -spine * 1.0/3);
	Point ellbow(bodyWidth * 0.5, -spine);
	Point upperArm(ellbow - armStart);
	Point armShift(armWidth, 0);

	ObjectPtr turkerId = getDigit(root, 0, Point(0, -spine * 0.3));
	if(turkerId) corpse->add(turkerId);

	ObjectPtr body(new Circle(0, 0, spine, M_PI));
	body->scale(bodyAspectRatio, 1.0);
	corpse->add(body);

	corpse->add(new Circle(Point(0, -spine), 0.4 * spine, radians(158), radians(382)));
	corpse->add(new Line(armStart, ellbow));
	corpse->add(new Line(ellbow, ellbow + upperArm.scaled(-1, 1)));
	corpse->add(new Line(ellbow + upperArm.scaled(-1, 1) + armShift, ellbow + armShift));
	corpse->add(new Line(ellbow + armShift, armStart + armShift));

	corpse->rotate(M_PI_2 + (head - chest).angle());
	corpse->move(chest);

	return CompositeObject::construct(corpse);  // Safely store transform on corpse
}

ObjectPtr InstructionFactory::Water(const Json::Value &root, Point p)
{
	(void)root;
	CompositeObjectPtr group = CompositeObject::construct();

	group->add(new Circle(p, 2000, 3.8*M_PI_4, 9*M_PI_4));
	group->add(new Line(p.x() - 1750, p.y()+500, p.x(), p.y() + 4000));
	group->add(new Line(p.x() + 1750, p.y()+500, p.x(), p.y() + 4000));

	return group;
}

ObjectPtr InstructionFactory::Poke(const Json::Value &root, Point p1, Point p2)
{
	(void)root;
	CompositeObjectPtr group = CompositeObject::construct();

	float alpha;
	float length;
	Point mid;
	Point start;
	Point end;

	calculateRectangleCharacteristics(p1, p2, alpha, length, start, mid, end);

	group->add(new Rectangle(start.x(), start.y()-500, length, 1000, false));
	group->add(new Rectangle(start.x()+3000, start.y()-3000, length-3000, 2500, false));
	group->add(new Rectangle(mid.x()+2000, start.y(), length/2-2000, 2500, false));
	group->add(new Line(start.x()+3000, start.y() -2000, mid.x()+2000, start.y() -2000));
	group->add(new Line(start.x()+3000, start.y() -1000, mid.x()+2000, start.y() -1000));
	group->rotate(alpha, mid);

	group->addAnimation([](Object *o) {
							o->setVisible(!o->visible());
						}, std::chrono::milliseconds(50));

	return group;
}

ObjectPtr InstructionFactory::Footwear(const Json::Value &root, Point p)
{
	(void)root;
	CompositeObjectPtr group = CompositeObject::construct();
	CompositeObjectPtr groupA = CompositeObject::construct();
	CompositeObjectPtr groupB = CompositeObject::construct();

	groupA->add(new Circle(p, 1000));
	groupA->add(new Circle(p + Point(1000, 1000), 300));

	std::vector<Point> points;
	points.emplace_back(+ 300,  700);
	points.emplace_back(+ 200, 1400);
	points.emplace_back(    0,  700);
	points.emplace_back(- 200, 1400);
	points.emplace_back(- 300,  700);
	points.emplace_back(- 500, 1400);
	points.emplace_back(- 700,  700);
	//points.emplace_back(+ 500,  700);
	ShiftPoints(points, p);

	groupA->add(new Polygon(points, false));

	groupA->move(-p);
	groupA->scale(1, 2);
	groupA->move(p);

	groupA->move(-1500, 0);

	groupB->add(new Circle(p, 1000));
	groupB->add(new Circle(p + Point(1000, 1000), 300));

	points.clear();
	points.emplace_back(+ 300,  700);
	points.emplace_back(+ 200, 1400);
	points.emplace_back(    0,  700);
	points.emplace_back(- 200, 1400);
	points.emplace_back(- 300,  700);
	points.emplace_back(- 500, 1400);
	points.emplace_back(- 700,  700);
	//points.emplace_back(+ 500, 700);
	ShiftPoints(points, p);

	groupB->add(new Polygon(points, false));

	groupB->move(-p);
	groupB->scale(-1, 2);
	groupB->move(p);

	groupB->move(1500, 0);

	group->add(groupA);
	group->add(groupB);

	return group;
}

ObjectPtr InstructionFactory::Heat(const Json::Value &root, Point p)
{
	(void)root;
	CompositeObjectPtr group = CompositeObject::construct();

	group->add(new Circle(p, 2000, 3.8*M_PI_4, 9*M_PI_4));

	std::vector<Point> points;

	points.emplace_back(-1750,  500);
	points.emplace_back(-1500, 1000);
	points.emplace_back(-1000,  750);
	points.emplace_back(    0, 2000);
	points.emplace_back(  500, 1000);
	points.emplace_back( 1250, 4000);
	points.emplace_back( 1750,  500);
	ShiftPoints(points, p);

	group->add(new Polygon(points, false, false, false));

    return group;
}

ObjectPtr InstructionFactory::Elevator(const Json::Value &root, Point p1, Point p2, Point p3)
{
	(void)root;
	CompositeObjectPtr group = CompositeObject::construct();

	Point s12 = p2 - p1;
	Point s13 = p3 - p1;
	Point s23 = p3 - p2;
	Point arrowOneBottom = p1 + s12 / 5   + s23 / 5;
	Point arrowOneTop    = p1 + s12 / 5   + s23 * 0.8;
	Point arrowTwoBottom = p1 + s12 * 0.8 + s23 / 5;
	Point arrowTwoTop    = p1 + s12 * 0.8 + s23 * 0.8;

	Point arrowOneTopTipRight = arrowOneTop - s13 * 0.1;
	Point arrowOneTopTipLeft  = arrowOneTop + s13.scaled(0.1, -0.1);
	Point arrowTwoBottomTipRight = arrowTwoBottom + s13 * 0.1;
	Point arrowTwoBottomTipLeft = arrowTwoBottom + s13.scaled(-0.1, 0.1);

	group->add(new Rectangle(p1, p2, p3, p3 - s12, false));
	group->add(new Line(arrowOneBottom, arrowOneTop));
	group->add(new Line(arrowOneTop, arrowOneTopTipRight));
	group->add(new Line(arrowOneTop, arrowOneTopTipLeft));
	group->add(new Line(arrowTwoBottom, arrowTwoTop));
	group->add(new Line(arrowTwoBottom, arrowTwoBottomTipRight));
	group->add(new Line(arrowTwoBottom, arrowTwoBottomTipLeft));

	return group;
}

ObjectPtr InstructionFactory::Guardrail(const Json::Value &root, Point p1, Point p2, Point p3, Point p4)
{
	(void)root;
	CompositeObjectPtr group = CompositeObject::construct();

	float alpha;
	float length;
	Point midPoint;
	Point start;
	Point end;

	calculateRectangleCharacteristics(p1, p2, alpha, length, start, midPoint, end);

	Point p1p2 = p2 - p1;
	ObjectPtr turkerId = getDigit(root, 0);
	if(turkerId){
		turkerId->rotate(alpha);
		turkerId->move(midPoint - p1p2 / 2);
		turkerId->move(Point(-p1p2.y(), p1p2.x()).norm() * 100);
		group->add(turkerId);
	}
	group->add(new Rectangle(p1, p2, p3, p4, false));
	return group;
}

ObjectPtr InstructionFactory::BlueprintWall(const Json::Value &root, Point p1, Point p2)
{
	// No turker for this one
	(void)root;

	Point start = p1 + (p2 - p1) / 10;
	Point end   = p2 + (p1 - p2) / 10;

	CompositeObjectPtr group = CompositeObject::construct();

	group->add(new Line(p1, start));
	group->add(new Line(end, p2));

	return group;
}

ObjectPtr InstructionFactory::MovingWallWarning(const Json::Value &root, Point p1, Point p2)
{
	int normalX = root.get("direction", Json::Value()).get("x", Json::Value()).asInt();
	int normalY = root.get("direction", Json::Value()).get("y", Json::Value()).asInt();
//	int countdown = root.get("countdown", Json::Value(5000)).asInt();
	Point arrowTop = (p1 + p2) / 2 + Point(normalX, normalY).norm() * 1000;
	Point arrowEnd = arrowTop
					 - Point(normalX, normalY).norm() * 500
					 + (p2 - p1).norm() * 250;

	CompositeObjectPtr group = CompositeObject::construct();

	group->add(new Line(p1, p2));

	std::vector<Point> arrowPoints {(p1 + p2) / 2, arrowTop, arrowEnd};

	ObjectPtr arrow = std::make_shared<Polygon>(arrowPoints, false, false, false);
	arrow->addAnimation([] (Object *me) {
										static bool visible = true;
										visible = !visible;
										me->setVisible(visible);
									},
						std::chrono::milliseconds(500)
	);
	group->add(arrow);

	return group;
}

ObjectPtr InstructionFactory::MoveTurker(const Json::Value &root, BlinkFrequency freq, Point p1, Point p2)
{
	(void)root;
	CompositeObjectPtr group = CompositeObject::construct();
	group->add(new /*long*/ Line(p1, p2));
	Point end = p2 + (p1 - p2)/3;
	Line* shortLine = new Line(p2, end);
	shortLine->rotate(30.0f/180.0f*M_PI, p2);
	group->add(shortLine);
	std::chrono::milliseconds blinkfrequency;
	switch (freq) {
	case LOW:
		blinkfrequency = std::chrono::milliseconds(1000);
		break;
	case MEDIUM:
		blinkfrequency = std::chrono::milliseconds(500);
		break;
	case HIGH:
		blinkfrequency = std::chrono::milliseconds(100);
		break;
	default:
		break;
	}

	if (freq != NO)
	{
			group->addAnimation([] (Object* me)
			{
				static bool visible = false;
				visible = !visible;
				me->setVisible(visible);
			}, blinkfrequency);
	}

	return group;
}

ObjectPtr InstructionFactory::MoveTurkerLowFreq(const Json::Value &root, Point p1, Point p2)
{
	return MoveTurker(root, LOW, p1, p2);
}

ObjectPtr InstructionFactory::MoveTurkerMidFreq(const Json::Value &root, Point p1, Point p2)
{
	return MoveTurker(root, MEDIUM, p1, p2);
}

ObjectPtr InstructionFactory::MoveTurkerHighFreq(const Json::Value &root, Point p1, Point p2)
{
	return MoveTurker(root, HIGH, p1, p2);
}

ObjectPtr InstructionFactory::MoveTurkerNoFreq(const Json::Value &root, Point p1, Point p2)
{
	return MoveTurker(root, NO, p1, p2);
}

ObjectPtr InstructionFactory::MoveDoorClockwise(const Json::Value &root, BlinkFrequency freq, Point p1, Point p2)
{
	(void)root;
	Point p1p2 = p2 - p1;
	double distance = p1p2.abs();
	double radius = distance / M_SQRT2;
	//std::cout << radius << std::endl;
	Point halfway = (p1 + p2) / 2.0f;
	//ststd::cout << halfway << std::endl;
	Point center = halfway + p1p2.perpendicular().norm() * sqrt(sqr(radius) - sqr(distance/2));
	//ststd::cout << center << std::endl;
	double startAngle = (p1 - center).angle();
	//ststd::cout << startAngle << std::endl;
	double endAngle = (p2 - center).angle();
	if (endAngle < startAngle)
	{
		endAngle += 2*M_PI;
	}
	//ststd::cout << endAngle << std::endl;
	Point arrowEnd = p2 + (p2 - center) / 3.0f;

	CompositeObjectPtr group = CompositeObject::construct();
	group->add(new Circle(center, radius, startAngle, endAngle));
	Line* shortLine = new Line(p2, arrowEnd);
	shortLine->rotate(-60.0f/180.0f*M_PI, p2);
	group->add(shortLine);
	std::chrono::milliseconds blinkfrequency;
	switch (freq) {
	case LOW:
		blinkfrequency = std::chrono::milliseconds(1000);
		break;
	case MEDIUM:
		blinkfrequency = std::chrono::milliseconds(500);
		break;
	case HIGH:
		blinkfrequency = std::chrono::milliseconds(100);
		break;
	default:
		break;
	}

	if (freq != NO)
	{
			group->addAnimation([] (Object* me)
			{
				static bool visible = false;
				visible = !visible;
				me->setVisible(visible);
			}, blinkfrequency);
	}

	return group;
}

ObjectPtr InstructionFactory::MoveDoorCounterClockwise(const Json::Value &root, BlinkFrequency freq, Point p1, Point p2)
{
	(void)root;
	Point p1p2 = p2 - p1;
	double distance = p1p2.abs();
	double radius = distance / M_SQRT2;
	//std::cout << radius << std::endl;
	Point halfway = (p1 + p2) / 2.0f;
	//std:std::cout << halfway << std::endl;
	Point center = halfway - p1p2.perpendicular().norm() * sqrt(sqr(radius) - sqr(distance/2));
	//std:std::cout << center << std::endl;
	double startAngle = (p1 - center).angle();
	//std:std::cout << startAngle << std::endl;
	double endAngle = (p2 - center).angle();
	if (endAngle < startAngle)
	{
		endAngle += 2*M_PI;
	}
	//std:std::cout << endAngle << std::endl;
	Point arrowEnd = p2 + (p2 - center) / 3.0f;

	CompositeObjectPtr group = CompositeObject::construct();
	group->add(new Circle(center, radius, endAngle, startAngle));
	Line* shortLine = new Line(p2, arrowEnd);
	shortLine->rotate(60.0f/180.0f*M_PI, p2);
	group->add(shortLine);
	std::chrono::milliseconds blinkfrequency;
	switch (freq) {
	case LOW:
		blinkfrequency = std::chrono::milliseconds(1000);
		break;
	case MEDIUM:
		blinkfrequency = std::chrono::milliseconds(500);
		break;
	case HIGH:
		blinkfrequency = std::chrono::milliseconds(100);
		break;
	default:
		break;
	}

	if (freq != NO)
	{
			group->addAnimation([] (Object* me)
			{
				static bool visible = false;
				visible = !visible;
				me->setVisible(visible);
			}, blinkfrequency);
	}

	return group;
}

ObjectPtr InstructionFactory::MoveDoorClockwiseNoFreq(const Json::Value &root, Point p1, Point p2)
{
	return MoveDoorClockwise(root, NO, p1, p2);
}

ObjectPtr InstructionFactory::MoveDoorClockwiseLowFreq(const Json::Value &root, Point p1, Point p2)
{
	return MoveDoorClockwise(root, LOW, p1, p2);
}

ObjectPtr InstructionFactory::MoveDoorClockwiseMidFreq(const Json::Value &root, Point p1, Point p2)
{
	return MoveDoorClockwise(root, MEDIUM, p1, p2);
}

ObjectPtr InstructionFactory::MoveDoorClockwiseHighFreq(const Json::Value &root, Point p1, Point p2)
{
	return MoveDoorClockwise(root, HIGH, p1, p2);
}

ObjectPtr InstructionFactory::MoveDoorCounterClockwiseNoFreq(const Json::Value &root, Point p1, Point p2)
{
	return MoveDoorCounterClockwise(root, NO, p1, p2);

}

ObjectPtr InstructionFactory::MoveDoorCounterClockwiseLowFreq(const Json::Value &root, Point p1, Point p2)
{
	return MoveDoorCounterClockwise(root, LOW, p1, p2);

}

ObjectPtr InstructionFactory::MoveDoorCounterClockwiseMidFreq(const Json::Value &root, Point p1, Point p2)
{
	return MoveDoorCounterClockwise(root, MEDIUM, p1, p2);

}

ObjectPtr InstructionFactory::MoveDoorCounterClockwiseHighFreq(const Json::Value &root, Point p1, Point p2)
{
	return MoveDoorCounterClockwise(root, HIGH, p1, p2);
}

ObjectPtr InstructionFactory::TurkerLabel(const Json::Value &root, Point p1, Point p2)
{
	ObjectPtr digit = getDigit(root, 0, p1);
	if (digit)
		digit->rotate((p2 - p1).angle(), p1);
	return digit;
}

}} // namespace laser::holodeck


#ifndef IMAGE_REPROJECTION_PLUGINS_SPHERE_SURFACE_MODEL_HPP
#define IMAGE_REPROJECTION_PLUGINS_SPHERE_SURFACE_MODEL_HPP

#include <cmath>
#include <string>

#include <image_reprojection/surface_model.hpp>
#include <image_reprojection_plugins/SphereStamped.h>
#include <topic_tools/shape_shifter.h>

#include <boost/thread/locks.hpp>
#include <boost/thread/shared_mutex.hpp>

#include <opencv2/core/core.hpp>

namespace image_reprojection_plugins {

class SphereSurfaceModel : public image_reprojection::SurfaceModel {
public:
  SphereSurfaceModel() {}

  virtual ~SphereSurfaceModel() {}

  virtual void update(const topic_tools::ShapeShifter &surface) override {
    const SphereStampedConstPtr sphere = surface.instantiate<SphereStamped>();
    CV_Assert(sphere);
    update(*sphere);
  }

  void update(const SphereStamped &sphere) {
    CV_Assert(sphere.radius > 0.);

    boost::unique_lock<boost::shared_mutex> write_lock(mutex_);

    frame_id_ = sphere.header.frame_id;
    center_ = cv::Vec3f(sphere.center.x, sphere.center.y, sphere.center.z);
    radius_ = sphere.radius;
  }

  virtual std::string getFrameId() const override {
    boost::shared_lock<boost::shared_mutex> read_lock(mutex_);
    return frame_id_;
  }

private:
  virtual void onInit() override {}

  virtual void onIntersection(const cv::Vec3f &src_origin, const cv::Mat &src_direction,
                              cv::Mat &dst, cv::Mat &mask) const override {
    boost::shared_lock<boost::shared_mutex> read_lock(mutex_);
    multiraySphereIntersection(src_origin, src_direction, dst, mask);
  }

  void multiraySphereIntersection(const cv::Vec3f &src_origin, const cv::Mat &src_direction,
                                  cv::Mat &dst, cv::Mat &mask) const {
    dst.create(src_direction.size(), CV_32FC3);
    mask.forEach<uchar>([this, &src_origin, &src_direction, &dst](uchar &m, const int *const pos) {
      if (m != 0) {
        const cv::Vec3f &sd = *src_direction.ptr<cv::Vec3f>(pos[0], pos[1]);
        cv::Vec3f &d = *dst.ptr<cv::Vec3f>(pos[0], pos[1]);
        m = raySphereIntersection(src_origin, sd, d) ? 1 : 0;
      }
    });
  }

  bool raySphereIntersection(const cv::Vec3f &src_origin, const cv::Vec3f &src_direction,
                             cv::Vec3f &dst) const {
    // intersection point (x) can be described as
    //   x = p + t * d  (t >= 0)
    //   |x - c| = r
    // where p: ray origin, d: ray direction, c: center of sphere, r: radius of sphere
    //   |d|^2 * t^2 + 2 * dot(d, p - c) * t + |p - c|^2 - r^2 = 0

    // position of ray origin with respect to center of sphere
    const cv::Vec3f o = src_origin - center_;

    // coefficients
    const double a = src_direction.dot(src_direction);
    const double b = src_direction.dot(o);
    const double c = o.dot(o) - radius_ * radius_;
    const double D = b * b - a * c; // discriminant
    if (/* no intersection */ D < 0.) {
      return false;
    }

    // intersection point
    const double sD = std::sqrt(D);
    const double t0 = (-b + sD) / a, t1 = (-b - sD) / a;
    if (t0 >= 0. && t1 >= 0.) {
      dst = src_origin + std::min(t0, t1) * src_direction;
      return true;
    } else if (t0 >= 0. && t1 < 0.) {
      dst = src_origin + t0 * src_direction;
      return true;
    } else if (t0 < 0. && t1 >= 0.) {
      dst = src_origin + t1 * src_direction;
      return true;
    }

    return false;
  }

private:
  mutable boost::shared_mutex mutex_;
  std::string frame_id_;
  cv::Vec3f center_;
  double radius_;
};

} // namespace image_reprojection_plugins

#endif


#include <stdlib.h>
#include <math.h>
#include <string>
#include <GL/glut.h>
#include <GL/freeglut_ext.h>

#include <libpng/png.h>
#include <live2d/Live2D.h>
#include <live2d/util/UtSystem.h>
#include <live2d/Live2DModelOpenGL.h>
#include <live2d/util/Json.h>

#include <opencv2/opencv.hpp>

#include <dlib/opencv.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>

using namespace std;

live2d::Live2DModelOpenGL* live2DModel;
cv::VideoCapture* camera;
dlib::frontal_face_detector detector;
dlib::shape_predictor pose_model;

// live2d parameters
float x_rotate = 0.0f;
float y_rotate = 0.0f;
float z_rotate = 0.0f;

float left_eye = 1.0f;
float right_eye = 1.0f;

float eyebrow_left = 0.0f;
float eyebrow_right = 0.0f;

float mouth_open = 0.0f;

double meter(double A, double B, double C, double x, double y)
{
	double diversion = A * x + B * y + C;
	return diversion / sqrt(pow(A, 2) + pow(B, 2));
}

double eyebrow_move(dlib::point &p1, dlib::point &p2, double slope, double last, double rate)
{
	double bias = p1.y() - slope * p1.x();
	double distance = sqrt(pow(p1.x() - p2.x(), 2) + pow(p1.y() - p2.y(), 2));
	double diversion = meter(slope, -1, bias, p2.x(), p2.y());
	double result = (diversion / distance - 0.45) * 6;
	result = last * (1 - rate) + result * rate;
	return result;
}

double eye_open(dlib::point &p1, dlib::point &p2, dlib::point &v11, dlib::point &v12, dlib::point &v21, dlib::point &v22, double last, double rate)
{
	double distance = sqrt(pow(p1.x() - p2.x(), 2) + pow(p1.y() - p2.y(), 2));
	double slope = (double)(p2.y() - p1.y()) / (double)(p2.x() - p1.x());
	double bias = p2.y() - slope * p2.x();
	double d1 = abs(meter(slope, -1, bias, v11.x(), v11.y()));
	double d2 = abs(meter(slope, -1, bias, v12.x(), v12.y()));
	double diversion = d1 > d2 ? d1 : d2;
	d1 = abs(meter(slope, -1, bias, v21.x(), v21.y()));
	d2 = abs(meter(slope, -1, bias, v22.x(), v22.y()));
	diversion += d1 > d2 ? d1 : d2;
	double ratio = (diversion / distance - 0.18) * 8;
	ratio = ((int)(ratio * 10.0)) / 10.0;
	ratio = last * (1 - rate) + ratio * rate;
	return ratio;
}

void display(void)
{
	cv::Mat buff, temp;
	*camera >> temp;
	cv::GaussianBlur(temp, buff, cv::Size(5, 5), 5, 5);
	cv::flip(buff, temp, 1);

	dlib::cv_image<dlib::bgr_pixel> cimg(temp);
	vector<dlib::rectangle> faces = detector(cimg);
	vector<dlib::full_object_detection> shapes;

	for (unsigned long i = 0; i < faces.size(); ++i)
		shapes.push_back(pose_model(cimg, faces[i]));

	if (!shapes.empty()) {
		for (int i = 0; i < 68; i++) {
			cv::circle(temp, cvPoint(shapes[0].part(i).x(), shapes[0].part(i).y()), 2, cv::Scalar(0, 0, 255), -1);
		}

		int div_x = shapes[0].part(16).x() - shapes[0].part(0).x();
		int div_y = shapes[0].part(16).y() - shapes[0].part(0).y();
		double center_x = shapes[0].part(0).x() + div_x / 2.0;
		double center_y = shapes[0].part(0).y() + div_y / 2.0;
		double slope = (double)(div_y) / (double)(div_x);
		double bias = center_y - slope * center_x;
		double x_proj = (slope * (shapes[0].part(30).y() - bias) + shapes[0].part(30).x()) / (1 + pow(slope, 2));
		double y_proj = slope * x_proj + bias;
		double diversion = sqrt(pow(x_proj - shapes[0].part(0).x(), 2) + pow(y_proj - shapes[0].part(0).y(), 2));
		double distance = sqrt(pow(shapes[0].part(16).x() - shapes[0].part(0).x(), 2) + pow(shapes[0].part(16).y() - shapes[0].part(0).y(), 2));

		double rate = 0.5;
		// Ax+By+C/sqrt(A^2+B^2)
		x_rotate = x_rotate * (1 - rate) + asin(diversion / distance - 0.5) * 3.14 * 40.0 * rate;

		// nose to eye around 1/6 head
		diversion = meter(slope, -1, bias, shapes[0].part(30).x(), shapes[0].part(30).y());
		diversion = diversion + 1.0 / 6 * distance;
		y_rotate = y_rotate * (1 - rate) + asin(diversion / distance) * 3.14 * 40.0 * rate;

		z_rotate = z_rotate * (1 - rate) + atan(slope) * 3.14 * 40 * rate;

		// eye
		left_eye = eye_open(shapes[0].part(36), shapes[0].part(39), shapes[0].part(37), shapes[0].part(38), shapes[0].part(40), shapes[0].part(41), left_eye, rate);
		right_eye = eye_open(shapes[0].part(42), shapes[0].part(45), shapes[0].part(43), shapes[0].part(44), shapes[0].part(46), shapes[0].part(47), right_eye, rate);

		// eyebrow
		eyebrow_left = eyebrow_move(shapes[0].part(17), shapes[0].part(19), slope, eyebrow_left, rate);
		eyebrow_right = eyebrow_move(shapes[0].part(26), shapes[0].part(24), slope, eyebrow_right, rate);

		// mouth
		diversion = sqrt(pow(shapes[0].part(62).x() - shapes[0].part(66).x(), 2) + pow(shapes[0].part(62).y() - shapes[0].part(66).y(), 2));
		distance = sqrt(pow(shapes[0].part(60).x() - shapes[0].part(64).x(), 2) + pow(shapes[0].part(60).y() - shapes[0].part(64).y(), 2));
		mouth_open = (diversion / distance - 0.15) * 2;
	}
	else
	{
		x_rotate = 0.0f;
		y_rotate = 0.0f;
		z_rotate = 0.0f;
		left_eye = 1.0f;
		right_eye = 1.0f;
		eyebrow_left = 0.0f;
		eyebrow_right = 0.0f;
		mouth_open = 0.0f;
	}

	glClear(GL_COLOR_BUFFER_BIT);
	//double t = (live2d::UtSystem::getUserTimeMSec() / 1000.0) * 2 * 3.14;
	//live2DModel->setParamFloat("PARAM_ANGLE_Z", (float)(30 * sin(t / 3.0)));
	live2DModel->setParamFloat("PARAM_ANGLE_X", x_rotate);
	live2DModel->setParamFloat("PARAM_ANGLE_Y", y_rotate);
	live2DModel->setParamFloat("PARAM_ANGLE_Z", z_rotate);

	live2DModel->setParamFloat("PARAM_EYE_L_OPEN", left_eye);
	live2DModel->setParamFloat("PARAM_EYE_R_OPEN", right_eye);

	live2DModel->setParamFloat("PARAM_BROW_L_Y", eyebrow_left);
	live2DModel->setParamFloat("PARAM_BROW_R_Y", eyebrow_right);

	live2DModel->setParamFloat("PARAM_MOUTH_OPEN_Y", mouth_open);

	live2DModel->update();
	live2DModel->draw();

	//Display it all on the screen
	cv::imshow("Feature points", temp);
	glFlush();
}

void timer(int value) {

	glutPostRedisplay();
	glutTimerFunc(30, timer, 0);
}

int loadGLTexture(const char* path)
{
	unsigned int id;

	FILE            *fp;
	png_structp     png_ptr;
	png_infop       info_ptr;
	unsigned int   width, height;
	int             bit_depth, color_type, interlace_type;
	unsigned char   *image;

	errno_t error;

	if ((error = fopen_s(&fp, path, "rb")) != 0) {
		printf("file not exists!");
		return -1;
	}

	png_ptr = png_create_read_struct(
		PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
	info_ptr = png_create_info_struct(png_ptr);

	png_init_io(png_ptr, fp);
	png_read_info(png_ptr, info_ptr);
	png_get_IHDR(png_ptr, info_ptr, &width, &height,
		&bit_depth, &color_type, &interlace_type,
		NULL, NULL);
	int rb = png_get_rowbytes(png_ptr, info_ptr);
	image = new unsigned char[height * rb];
	unsigned char **recv = new unsigned char*[height];
	for (int i = 0; i < height; i++)
		recv[i] = &image[i * rb];
	png_read_image(png_ptr, recv);
	png_read_end(png_ptr, info_ptr);

	//premultiplied alpha

	png_destroy_read_struct(&png_ptr, &info_ptr, NULL);
	fclose(fp);
	delete[] recv;

	glEnable(GL_TEXTURE_2D);
	glGenTextures(1, &id);

	glBindTexture(GL_TEXTURE_2D, id);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);

	/*glTexImage2D(
	GL_TEXTURE_2D , 0 , GL_RGBA , width , height ,
	0 , GL_RGBA , GL_UNSIGNED_BYTE , image
	);*/
	gluBuild2DMipmaps(GL_TEXTURE_2D, GL_RGBA, width, height, GL_RGBA, GL_UNSIGNED_BYTE, image);
	return id;
}

void Live2DInit(void)
{
	glClearColor(0.0, 0.0, 1.0, 1.0);

	std::string path = "res/Epsilon/";
	live2DModel = live2d::Live2DModelOpenGL::loadModel((char*)((path + "Epsilon.moc").c_str()));

	const char* TEXTURES[] = {
		(char*)((path + "Epsilon.2048/texture_00.png").c_str()),
		NULL ,
	};

	for (int i = 0; i < 1000; i++) {
		if (!TEXTURES[i]) break;

		int tex = loadGLTexture(TEXTURES[i]);

		if (tex < 0) printf("failed");
		live2DModel->setTexture(i, tex);
	}
	live2DModel->setPremultipliedAlpha(false);
}

void OpenCVInit(void)
{
	camera = new cv::VideoCapture(0);
	if (!camera->isOpened())
	{
		cerr << "Unable to connect to camera" << endl;
	}

	detector = dlib::get_frontal_face_detector();
	dlib::deserialize("shape_predictor_68_face_landmarks.dat") >> pose_model;
}

void resize(int w, int h)
{
	glViewport(0, 0, w, h);

	glLoadIdentity();

	float aspect = (float)w / h;
	float sx = 2.0 / live2DModel->getCanvasWidth();
	float sy = -2.0 / live2DModel->getCanvasWidth() * aspect;
	float x = -1;
	float y = 1;
	float matrix[] = {
		sx , 0 , 0 , 0 ,
		0 , sy ,0 , 0 ,
		0 , 0 , 1 , 0 ,
		x , y , 0 , 1
	};

	live2DModel->setMatrix(matrix);
}

int main(int argc, char *argv[])
{
	live2d::Live2D::init();
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_RGBA);
	glutInitWindowSize(600, 600);
	glutCreateWindow("Model");
	Live2DInit();
	OpenCVInit();
	glutDisplayFunc(display);
	glutReshapeFunc(resize);
	glutTimerFunc(100, timer, 0);
	glutMainLoop();
	return 0;
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "volume.h"
#include "cross.h"
#include <Eigen/Geometry>
template <
  typename DerivedV,
  typename DerivedT,
  typename Derivedvol>
IGL_INLINE void igl::volume(
  const Eigen::PlainObjectBase<DerivedV>& V,
  const Eigen::PlainObjectBase<DerivedT>& T,
  Eigen::PlainObjectBase<Derivedvol>& vol)
{
  using namespace Eigen;
  const int m = T.rows();
  vol.resize(m,1);
  for(int t = 0;t<m;t++)
  {
    const RowVector3d & a = V.row(T(t,0));
    const RowVector3d & b = V.row(T(t,1));
    const RowVector3d & c = V.row(T(t,2));
    const RowVector3d & d = V.row(T(t,3));
    vol(t) = -(a-d).dot((b-d).cross(c-d))/6.;
  }
}

template <
  typename DerivedA,
  typename DerivedB,
  typename DerivedC,
  typename DerivedD,
  typename Derivedvol>
IGL_INLINE void igl::volume(
  const Eigen::PlainObjectBase<DerivedA> & A,
  const Eigen::PlainObjectBase<DerivedB> & B,
  const Eigen::PlainObjectBase<DerivedC> & C,
  const Eigen::PlainObjectBase<DerivedD> & D,
  Eigen::PlainObjectBase<Derivedvol> & vol)
{
  const auto & AmD = A-D;
  const auto & BmD = B-D;
  const auto & CmD = C-D;
  Eigen::PlainObjectBase<DerivedA> BmDxCmD;
  cross(BmD.eval(),CmD.eval(),BmDxCmD);
  const auto & AmDdx = (AmD.array() * BmDxCmD.array()).rowwise().sum();
  vol = -AmDdx/6.;
}

template <
  typename VecA,
  typename VecB,
  typename VecC,
  typename VecD>
IGL_INLINE typename VecA::Scalar igl::volume_single(
  const VecA & a,
  const VecB & b,
  const VecC & c,
  const VecD & d)
{
  return -(a-d).dot((b-d).cross(c-d))/6.;
}

template <
  typename DerivedL,
  typename Derivedvol>
IGL_INLINE void igl::volume(
  const Eigen::PlainObjectBase<DerivedL>& L,
  Eigen::PlainObjectBase<Derivedvol>& vol)
{
  using namespace Eigen;
  const int m = L.rows();
  typedef typename Derivedvol::Scalar ScalarS;
  vol.resize(m,1);
  for(int t = 0;t<m;t++)
  {
    const ScalarS u = L(t,0);
    const ScalarS v = L(t,1);
    const ScalarS w = L(t,2);
    const ScalarS U = L(t,3);
    const ScalarS V = L(t,4);
    const ScalarS W = L(t,5);
    const ScalarS X = (w - U + v)*(U + v + w);
    const ScalarS x = (U - v + w)*(v - w + U);
    const ScalarS Y = (u - V + w)*(V + w + u);
    const ScalarS y = (V - w + u)*(w - u + V);
    const ScalarS Z = (v - W + u)*(W + u + v);
    const ScalarS z = (W - u + v)*(u - v + W);
    const ScalarS a = sqrt(x*Y*Z);
    const ScalarS b = sqrt(y*Z*X);
    const ScalarS c = sqrt(z*X*Y);
    const ScalarS d = sqrt(x*y*z);
    vol(t) = sqrt(
       (-a + b + c + d)*
       ( a - b + c + d)*
       ( a + b - c + d)*
       ( a + b + c - d))/
       (192.*u*v*w);
  }
}
#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
// generated by autoexplicit.sh
template void igl::volume<Eigen::Matrix<double, -1, 6, 0, -1, 6>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 6, 0, -1, 6> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
template Eigen::Matrix<double, 1, 3, 1, 1, 3>::Scalar igl::volume_single<Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, 3, 1, 1, 3> >(Eigen::Matrix<double, 1, 3, 1, 1, 3> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&, Eigen::Matrix<double, 1, 3, 1, 1, 3> const&);
template Eigen::Matrix<double, 3, 1, 0, 3, 1>::Scalar igl::volume_single<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&);
template void igl::volume<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
template void igl::volume<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
#endif


/**
 * \file HadamardMixture.hxx
 */

#include <ATK/Delay/FeedbackDelayNetworkFilter.hxx>
#include <ATK/Delay/HadamardMixture.h>

#include <Eigen/Dense>

#include <ATK/Core/TypeTraits.h>
#include <ATK/Utility/fmath.h>

namespace ATK
{
  template<typename DataType_, unsigned int order>
  class HadamardMixture<DataType_, order>::MixtureImpl
  {
  public:
    using Vector = Eigen::Matrix<DataType, nb_channels, 1>;
    using Matrix = Eigen::Matrix<DataType, nb_channels, nb_channels>;

    Vector mix(const Vector& x) const
    {
      return transition * x;
    }

  protected:
    const Matrix transition = create();

    static Matrix create()
    {
      return (DataType_(1 / fmath::pow(2, order / 2.)) * recursive_create<order>()).template cast<DataType_>();
    }

    template<unsigned int recursive_order>
    static Eigen::Matrix<typename TypeTraits<DataType_>::Scalar, (1U<<recursive_order), (1U<<recursive_order)> recursive_create()
    {
      if constexpr(recursive_order == 0)
      {
        return Eigen::Matrix<typename TypeTraits<DataType_>::Scalar, 1U, 1U>::Constant(1);
      }
      else
      {
        constexpr auto big_size = (1U << recursive_order);
        constexpr auto small_size = (1U << (recursive_order - 1));
        Eigen::Matrix<typename TypeTraits<DataType_>::Scalar, big_size, big_size> cur_transition;

        auto M_1 = recursive_create<recursive_order - 1>();
        cur_transition.block(0, 0, small_size, small_size) = M_1;
        cur_transition.block(0, small_size, small_size, small_size) = M_1;
        cur_transition.block(small_size, 0, small_size, small_size) = -M_1;
        cur_transition.block(small_size, small_size, small_size, small_size) = M_1;
        return cur_transition;
      }
    }
  };
}


#include "Core.h"
#include "random.h"
#include <boost/random.hpp>
#include <ctime>

float angle_clamp(const float rads) {
	const float P = float(2.0*M_PI);
	float clamped = rads;

	while (clamped < 0.0f) {
		clamped += P;
	}

	while (clamped > 2*P) {
		clamped -= P;
	}

	return clamped;
}

float getAngle( const vec2 &a, const vec2 &b ) {
	float dx = a.x - b.x;
	float dy = a.y - b.y;
	float rads = atan2f(dy, dx);
	float clamped = angle_clamp(rads);
	return clamped;
}

vec3 GetRandomVector(float length) {
	return vec3(FRAND_RANGE(-1.0f,+1.0f),
	            FRAND_RANGE(-1.0f,+1.0f),
	            FRAND_RANGE(-1.0f,+1.0f)).getNormal() * length;
}

string toString(const any &value) {
	string typeName = value.type().name();

	if (typeName == typeid(vec2).name()) {
		return vec2::toString(any_cast<vec2>(value));
	} else if (typeName == typeid(vec3).name()) {
		return vec3::toString(any_cast<vec3>(value));
	} else if (typeName == typeid(vec4).name()) {
		return vec4::toString(any_cast<vec4>(value));
	} else if (typeName == typeid(mat4).name()) {
		return mat4::toString(any_cast<mat4>(value));
	} else if (typeName == typeid(mat3).name()) {
		return mat3::toString(any_cast<mat3>(value));
	} else if (typeName == typeid(float).name()) {
		return ftos(any_cast<float>(value));
	} else if (typeName == typeid(int).name()) {
		return itos(any_cast<int>(value));
	} else if (typeName == typeid(size_t).name()) {
		return sizet_to_string(any_cast<size_t>(value));
	} else if (typeName == typeid(bool).name()) {
		return any_cast<bool>(value) ? "true" : "false";
	} else {
		return "???";
	}
}

vec3 calcTriNorm(const vec3 &a, const vec3 &b, const vec3 &c) {
	vec3 v1 = a - b;
	vec3 v2 = b - c;
	vec3 n = v1.cross(v2);
	return n.getNormal();
}

vec3 calcTriNorm(const Triangle &tri) {
	return calcTriNorm(tri.a, tri.b, tri.c);
}

float SampleLogNormal(float mean, float sigma) {
	// Create a Mersenne twister random number generator
	// that is seeded once with #seconds since 1970
	static mt19937 rng(static_cast<unsigned> (std::time(0)));

	// select gamma probability distribution
	lognormal_distribution<float> dist(mean, sigma);

	// bind random number generator to distribution, forming a function
	variate_generator<mt19937&, lognormal_distribution<float> >  sampler(rng, dist);

	// sample from the distribution
	return sampler();
}

float SampleNormal(float mean, float sigma) {
	// Create a Mersenne twister random number generator
	// that is seeded once with #seconds since 1970
	static mt19937 rng(static_cast<unsigned> (std::time(0)));

	// select Gaussian probability distribution
	normal_distribution<float> norm_dist(mean, sigma);

	// bind random number generator to distribution, forming a function
	variate_generator<mt19937&, normal_distribution<float> >  normal_sampler(rng, norm_dist);

	// sample from the distribution
	return normal_sampler();
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    A.change_dim(5, 5); A= 0.0;
    {
	mtl::mat::inserter<Matrix>   ins(A);
	ins[0][0] << 7; ins[1][1] << 8; ins[1][3] << 2; ins[1][4] << 3;
	ins[2][2] << 2; ins[3][3] << 4; ins[4][4] << 9;
    }

    double xa[] = {1, 2, 3, 4, 5};
    mtl::dense_vector<double> x(xa), b;

    b= A * x;
    x= 0.0;

    // Check whether entries on the lower triangle are ignored for solving
    Matrix U(A); // Copy of the upper triangular
    {
	mtl::mat::inserter<Matrix>   ins(A);
	ins[4][1] << 7; ins[3][2] << 6;
    }

    cout << name << "\nA = \n" << A << "b = " << b << "\n";

    invert_diagonal(A);

    x= upper_trisolve(A, b, mtl::tag::inverse_diagonal());
    cout << "x = upper_trisolve(A, b) ==" << x << "\n\n";
    MTL_THROW_IF(std::abs(x[2] - 3.0) > 0.0001, mtl::runtime_error("Wrong result in upper_trisolve!"));

    Matrix B(trans(A)); // Diagonal already inverted

    x= xa;

    b= trans(U) * x;    // Take transposed of original matrix
    x= 0.0;

    cout << "B = \n" << B << "b = " << b << "\n";

    // invert_diagonal(B);

    x= lower_trisolve(B, b, mtl::tag::inverse_diagonal());
    cout << "x = lower_trisolve(B, b) ==" << x << "\n\n";
    MTL_THROW_IF(std::abs(x[2] - 3.0) > 0.0001, mtl::runtime_error("Wrong result in lower_trisolve!"));

}

int main(int, char**)
{
    using namespace mtl;

    dense2D<double>                                      dr;
    dense2D<double, mat::parameters<col_major> >      dc;
    morton_dense<double, recursion::morton_z_mask>       mzd;
    morton_dense<double, recursion::doppled_2_row_mask>  d2r;
    compressed2D<double>                                 cr;
    compressed2D<double, mat::parameters<col_major> > cc;

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(cr, "Compressed row major");
    test(cc, "Compressed column major");

    return 0;
}


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * UdU' Factorisation of Covariance Filter.
 *
 * For efficiency UD_scheme requires to know the maximum q_size of the predict_model
 * ISSUES:
 *  observe functions: returned rcond is the minimum of each sequential update, an overall conditioning would be better
 */
#include "UDFlt.hpp"
#include "matSup.hpp"
#include <boost/limits.hpp>

/* Filter namespace */
namespace Bayesian_filter
{
	using namespace Bayesian_filter_matrix;

UD_scheme::
UD_scheme (std::size_t x_size, std::size_t q_maxsize, std::size_t z_initialsize) :
		Kalman_state_filter(x_size),
		q_max(q_maxsize),
		UD(x_size,x_size+q_max),
		s(Empty), Sd(Empty),
		d(x_size+q_max), dv(x_size+q_max), v(x_size+q_max),
		a(x_size), b(x_size),
		h1(x_size), w(x_size),
		znorm(Empty),
		zpdecol(Empty),
		Gz(Empty),
		GIHx(Empty)
/* Initialise filter and set the size of things we know about
 */
{
	last_z_size = 0;	// Matrices conform to z_initialsize, they are left Empty if z_initialsize==0
	observe_size (z_initialsize);
}

UD_scheme&
 UD_scheme::operator= (const UD_scheme& a)
/* Optimise copy assignment to only copy filter state
 * Precond: matrix size conformance
 */
{
	Kalman_state_filter::operator=(a);
	q_max = a.q_max;
	UD = a.UD;
	return *this;
}

void
 UD_scheme::init ()
/* Initialise from a state and state coveriance
 * Computes UD factor from initial covaiance
 * Predcond:
 *  X
 * Postcond:
 *  X
 *  UD=X, d is PSD
 */
{
					// Factorise X into left partition of UD
	std::size_t x_size = UD.size1();
	UD.sub_matrix(0,x_size, 0,x_size) .assign (X);
	Float rcond = UdUfactor (UD, x_size);
	rclimit.check_PSD(rcond, "Initial X not PSD");
}

void
 UD_scheme::update ()
/* Defactor UD back into X
 * Precond:
 *  UD
 * Postcond:
 *  X=UD  PSD iff UD is PSD
 */
{
	UdUrecompose (X, UD);
}

UD_scheme::Float
 UD_scheme::predict (Linrz_predict_model& f)
/* Prediction using a diagonalised noise q, and its coupling G
 *  q can have order less then x and a matching G so GqG' has order of x
 * Precond:
 *	UD
 * Postcond:
 *  UD is PSD
 */
{
	x = f.f(x);			// Extended Kalman state predict is f(x) directly

						// Predict UD from model
	Float rcond = predictGq (f.Fx, f.G, f.q);
	rclimit.check_PSD(rcond, "X not PSD in predict");
	return rcond;
}

UD_scheme::Float
 UD_scheme::predictGq (const Matrix& Fx, const Matrix& G, const FM::Vec& q)
/* MWG-S prediction from Bierman  p.132
 *  q can have order less then x and a matching G so GqG' has order of x
 * Precond:
 *  UD
 * Postcond:
 *  UD
 *
 * Return:
 *		reciprocal condition number, -1 if negative, 0 if semi-definite (including zero)
 */
{
	std::size_t i,j,k;
	const std::size_t n = x.size();
	const std::size_t Nq = q.size();
	const std::size_t N = n+Nq;
	Float e;
					// Check preallocated space for q size
	if (Nq > q_max)
		error (Logic_exception("Predict model q larger than preallocated space"));

	if (n > 0)		// Simplify reverse loop termination
	{
						// Augment d with q, UD with G
		for (i = 0; i < Nq; ++i)		// 0..Nq-1
		{
			d[i+n] = q[i];
		}
		for (j = 0; j < n; ++j)		// 0..n-1
		{
			Matrix::Row UDj(UD,j);
			Matrix::const_Row  Gj(G,j);
			for (i = 0; i < Nq; ++i)		// 0..Nq-1
				UDj[i+n] = Gj[i];
		}

						// U=Fx*U and diagonals retrieved
		for (j = n-1; j > 0; --j)		// n-1..1
		{
						// Prepare d(0)..d(j) as temporary
			for (i = 0; i <= j; ++i)	// 0..j
				d[i] = Float(UD(i,j));	// ISSUE mixed type proxy assignment

						// Lower triangle of UD is implicitly empty
			for (i = 0; i < n; ++i) 	// 0..n-1
			{
				Matrix::Row UDi(UD,i);
				Matrix::const_Row Fxi(Fx,i);
				UDi[j] = Fxi[j];
				for (k = 0; k < j; ++k)	// 0..j-1
					UDi[j] += Fxi[k] * d[k];
			}
		}
		d[0] = Float(UD(0,0));	// ISSUE mixed type proxy assignment

						//  Complete U = Fx*U
		for (j = 0; j < n; ++j)			// 0..n-1
		{
			UD(j,0) = Fx(j,0);
		}

						// The MWG-S algorithm on UD transpose
		j = n-1;
		do {							// n-1..0
			Matrix::Row UDj(UD,j);
			e = 0;
			for (k = 0; k < N; ++k)		// 0..N-1
			{
				v[k] = Float(UDj[k]);	// ISSUE mixed type proxy assignment
				dv[k] = d[k] * v[k];
				e += v[k] * dv[k];
			}
			// Check diagonal element
			if (e > 0)
			{
				// Positive definite
				UDj[j] = e;

				Float diaginv = 1 / e;
				for (k = 0; k < j; ++k)	// 0..j-1
				{
					Matrix::Row UDk(UD,k);
					e = 0;
					for (i = 0; i < N; ++i)	// 0..N-1
						e += UDk[i] * dv[i];
					e *= diaginv;
					UDj[k] = e;

					for (i = 0; i < N; ++i)	// 0..N-1
						UDk[i] -= e * v[i];
				}
			}//PD
			else if (e == 0)
			{
				// Possibly semi-definite, check not negative
				UDj[j] = e;

				// 1 / e is infinite
				for (k = 0; k < j; ++k)	// 0..j-1
				{
					Matrix::Row UDk(UD,k);
					for (i = 0; i < N; ++i)	// 0..N-1
					{
						e = UDk[i] * dv[i];
						if (e != 0)
							goto Negative;
					}
					// UD(j,k) unaffected
				}
			}//PD
			else
			{
				// Negative
				goto Negative;
			}
		} while (j-- > 0); //MWG-S loop

						// Transpose and Zero lower triangle
		for (j = 1; j < n; ++j)			// 0..n-1
		{
			Matrix::Row UDj(UD,j);
			for (i = 0; i < j; ++i)
			{
				UD(i,j) = UDj[i];
				UDj[i] = 0;			// Zeroing unnecessary as lower only used as a scratch
			}
		}

	}

	// Estimate the reciprocal condition number from upper triangular part
	return UdUrcond(UD,n);

Negative:
	return -1;
}

void
 UD_scheme::observe_size (std::size_t z_size)
/* Optimised dynamic observation sizing
 */
{
	if (z_size != last_z_size) {
		last_z_size = z_size;

		s.resize(z_size, false);
		Sd.resize(z_size, false);
		znorm.resize(z_size, false);
	}
}

Bayes_base::Float
 UD_scheme::observe (Linrz_uncorrelated_observe_model& h, const Vec& z)
/* Standard linrz observe
 *  Uncorrelated observations are applied sequentially in the order they appear in z
 *  The sequential observation updates state x
 *  Therefore the model of each observation needs to be computed sequentially. Generally this
 *  is inefficient and observe (UD_sequential_observe_model&) should be used instead
 * Precond:
 *	 UD
 *	 Zv is PSD
 * Postcond:
 *  UD is PSD
 * Return: Minimum rcond of all sequential observe
 */
{
	const std::size_t z_size = z.size();
	Float s, S;			// Innovation and covariance

								// Dynamic sizing
	observe_size (z_size);
								// Apply observations sequentially as they are decorrelated
	Float rcondmin = std::numeric_limits<Float>::max();
	for (std::size_t o = 0; o < z_size; ++o)
	{
								// Observation model, extracted for a single z element
		const Vec& zp = h.h(x);
		h.normalise(znorm = z, zp);
		noalias(h1) = row(h.Hx, o);
								// Check Z precondition
		if (h.Zv[o] < 0)
			error (Numeric_exception("Zv not PSD in observe"));
								// Update UD and extract gain
		Float rcond = observeUD (w, S, h1, h.Zv[o]);
		rclimit.check_PSD(rcond, "S not PD in observe");	// -1 implies S singular
		if (rcond < rcondmin) rcondmin = rcond;
								// State update using normalised non-linear innovation
		s = znorm[o] - zp[o];
		noalias(x) += w * s;
								// Copy s and Sd
		UD_scheme::s[o] = s;
		UD_scheme::Sd[o] = S;
	}
	return rcondmin;
}

Bayes_base::Float
 UD_scheme::observe (Linrz_correlated_observe_model& /*h*/, const Vec& /*z*/)
/* No solution for Correlated noise and Linearised model
 */
{
	error (Logic_exception("observe no Linrz_correlated_observe_model solution"));
	return 0;	// never reached
}

Bayes_base::Float
 UD_scheme::observe (Linear_correlated_observe_model& h, const Vec& z)
/* Special Linear Hx observe for correlated Z
 *  Z must be PD and will be decorrelated
 * Applies observations sequentially in the order they appear in z
 * Creates temporary Vec and Matrix to decorrelate z,Z
 * Precondition:
 *  UD
 *  Z is PSD
 * Postcondition:
 *  UD is PSD
 * Return: Minimum rcond of all sequential observe
 */
{
	std::size_t i, j, k;
	const std::size_t x_size = x.size();
	const std::size_t z_size = z.size();
	Float s, S;			// Innovation and covariance

					// Dynamic sizing
	observe_size (z_size);
	if (z_size != zpdecol.size()) {
		zpdecol.resize(z_size, false);
		Gz.resize(z_size,z_size, false);
		GIHx.resize(z_size, x_size, false);
	}

					// Factorise process noise as GzG'
	{	Float rcond = FM::UdUfactor (Gz, h.Z);
		rclimit.check_PSD(rcond, "Z not PSD in observe");
	}

								// Observation prediction and normalised observation
	const Vec& zp = h.h(x);
	h.normalise(znorm = z, zp);

	if (z_size > 0)
	{							// Solve G* GIHx = Hx for GIHx in-place
		GIHx = h.Hx;
		for (j = 0; j < x_size; ++j)
		{
			i = z_size-1;
			do {
				for (k = i+1; k < z_size; ++k)
				{
					GIHx(i,j) -= Gz(i,k) * GIHx(k,j);
				}
			} while (i-- > 0);
		}

		zpdecol = zp;			// Solve G zp~ = z, G z~ = z  for zp~,z~ in-place
		i = z_size-1;
		do {
			for (k = i+1; k < z_size; ++k)
			{
				znorm[i] -= Gz(i,k) * znorm[k];
				zpdecol[i] -= Gz(i,k) * zpdecol[k];
			}
		} while (i-- > 0);
	}//if (z_size>0)

								// Apply observations sequential as they are decorrelated
	Float rcondmin = std::numeric_limits<Float>::max();
	for (std::size_t o = 0; o < z_size; ++o)
	{
		h1 = row(GIHx,o);
								// Update UD and extract gain
		Float rcond = observeUD (w, S, h1, Gz(o,o));
		rclimit.check_PSD(rcond, "S not PD in observe");	// -1 implies S singular
		if (rcond < rcondmin) rcondmin = rcond;
								// State update using linear innovation
		s = znorm[o]-zpdecol[o];
		noalias(x) += w * s;
								// Copy s and Sd
		UD_scheme::s[o] = s;
		UD_scheme::Sd[o] = S;
	}
	return rcondmin;
}

Bayes_base::Float
 UD_scheme::observe (UD_sequential_observe_model& h, const Vec& z)
/* Special observe using observe_model_sequential for fast uncorrelated linrz operation
 * Uncorrelated observations are applied sequentially in the order they appear in z
 * The sequential observation updates state x. Therefore the model of
 * each observation needs to be computed sequentially
 * Precondition:
 *  UD
 *  Z is PSD
 * Postcondition:
 *  UD is PSD
 * Return: Minimum rcond of all sequential observe
 */
{
	std::size_t o;
	const std::size_t z_size = z.size();
	Float s, S;			// Innovation and covariance

								// Dynamic sizing
	observe_size (z_size);
								// Apply observations sequentially as they are decorrelated
	Float rcondmin = std::numeric_limits<Float>::max();
	for (o = 0; o < z_size; ++o)
	{
								// Observation prediction and model
		const Vec& zp = h.ho(x, o);
		h.normalise(znorm = z, zp);
								// Check Z precondition
		if (h.Zv[o] < 0)
			error (Numeric_exception("Zv not PSD in observe"));
								// Update UD and extract gain
		Float rcond = observeUD (w, S, h.Hx_o, h.Zv[o]);
		rclimit.check_PSD(rcond, "S not PD in observe");	// -1 implies S singular
		if (rcond < rcondmin) rcondmin = rcond;
								// State update using non-linear innovation
		s = znorm[o]-zp[o];
		noalias(x) += w * s;
								// Copy s and Sd
		UD_scheme::s[o] = s;
		UD_scheme::Sd[o] = S;
	}
	return rcondmin;
}

UD_scheme::Float
 UD_scheme::observeUD (FM::Vec& gain, Float & alpha, const Vec& h, const Float r)
/** Linear UD factorisation update
 *  Bierman UdU' factorisation update. Bierman p.100
 * Input
 *  h observation coefficients
 *  r observation variance
 * Output
 *  gain  observation Kalman gain
 *  alpha observation innovation variance
 * Variables with physical significance
 *  gamma becomes covariance of innovation
 * Precondition:
 *  UD
 *  r is PSD (not checked)
 * Postcondition:
 *  UD (see return value)
 * Return:
 *  reciprocal condition number of UD, -1 if alpha singular (negative or zero)
 */
{
	std::size_t i,j,k;
	const std::size_t n = UD.size1();
	Float gamma, alpha_jm1, lamda;
	// a(n) is U'a
	// b(n) is Unweighted Kalman gain

					// Compute b = DU'h, a = U'h
	a = h;
	for (j = n-1; j >= 1; --j)	// n-1..1
	{
		for (k = 0; k < j; ++k)	// 0..j-1
		{
			a[j] += UD(k,j) * a[k];
		}
		b[j] = UD(j,j) * a[j];
	}
	b[0] = UD(0,0) * a[0];

					// Update UD(0,0), d(0) modification
	alpha = r + b[0] * a[0];
	if (alpha <= 0) goto alphaNotPD;
	gamma = 1 / alpha;
	UD(0,0) *= r * gamma;
					// Update rest of UD and gain b
	for (j = 1; j < n; ++j)		// 1..n-1
	{
					// d modification
		alpha_jm1 = alpha;	// alpha at j-1
		alpha += b[j] * a[j];
		lamda = -a[j] * gamma;
		if (alpha <= 0) goto alphaNotPD;
		gamma = 1 / alpha;
		UD(j,j) *= alpha_jm1 * gamma;
					// U modification
		for (i = 0; i < j; ++i)		// 0..j-1
		{
			Float UD_jm1 = UD(i,j);
			UD(i,j) = UD_jm1 + lamda * b[i];
			b[i] += b[j] * UD_jm1;
		}
	}
					// Update gain from b
	noalias(gain) = b * gamma;
 	// Estimate the reciprocal condition number from upper triangular part
	return UdUrcond(UD,n);

alphaNotPD:
	return -1;
}

}//namespace


// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <boost/math/special_functions/fpclassify.hpp> // isnan
#include <boost/math/special_functions/log1p.hpp>
#include <cmath>
#include <vinecopulib/misc/tools_eigen.hpp>

namespace vinecopulib {
inline GumbelBicop::GumbelBicop()
{
  family_ = BicopFamily::gumbel;
  parameters_ = Eigen::VectorXd(1);
  parameters_lower_bounds_ = Eigen::VectorXd(1);
  parameters_upper_bounds_ = Eigen::VectorXd(1);
  parameters_ << 1;
  parameters_lower_bounds_ << 1;
  parameters_upper_bounds_ << 50;
}

inline double
GumbelBicop::generator(const double& u)
{
  return std::pow(std::log(1 / u), this->parameters_(0));
}

inline double
GumbelBicop::generator_inv(const double& u)
{
  return std::exp(-std::pow(u, 1 / this->parameters_(0)));
}

inline double
GumbelBicop::generator_derivative(const double& u)
{
  double theta = double(this->parameters_(0));
  return std::pow(std::log(1 / u), theta - 1) * (-theta / u);
}

// inline double GumbelBicop::generator_derivative2(const double &u)
//{
//    double theta = double(this->parameters_(0));
//    return (theta - 1 - std::log(u)) * std::pow(std::log(1 / u), theta - 2) *
//           (theta / std::pow(u, 2));
//}

inline Eigen::VectorXd
GumbelBicop::pdf_raw(const Eigen::MatrixXd& u)
{
  double theta = static_cast<double>(parameters_(0));
  double thetha1 = 1.0 / theta;
  auto f = [theta, thetha1](const double& u1, const double& u2) {
    double t1 = std::pow(-std::log(u1), theta) + std::pow(-std::log(u2), theta);
    double temp = -std::pow(t1, thetha1) + (2 * thetha1 - 2.0) * std::log(t1) +
                  (theta - 1.0) * std::log(std::log(u1) * std::log(u2)) -
                  std::log(u1 * u2) +
                  boost::math::log1p((theta - 1.0) * std::pow(t1, -thetha1));
    return std::exp(temp);
  };
  return tools_eigen::binaryExpr_or_nan(u, f);
}

inline Eigen::VectorXd
GumbelBicop::hinv1_raw(const Eigen::MatrixXd& u)
{
  double theta = double(this->parameters_(0));
  double u1, u2;
  Eigen::VectorXd hinv = Eigen::VectorXd::Zero(u.rows());
  for (int j = 0; j < u.rows(); ++j) {
    u1 = u(j, 1);
    u2 = u(j, 0);
    if ((boost::math::isnan)(u1) | (boost::math::isnan)(u2)) {
      hinv(j) = std::numeric_limits<double>::quiet_NaN();
    } else {
      hinv(j) = qcondgum(&u1, &u2, &theta);
    }
  }

  return hinv;
}

inline Eigen::MatrixXd
GumbelBicop::tau_to_parameters(const double& tau)
{
  auto par = Eigen::VectorXd::Constant(1, 1.0 / (1 - std::fabs(tau)));
  return par.cwiseMax(parameters_lower_bounds_)
    .cwiseMin(parameters_upper_bounds_);
}

inline double
GumbelBicop::parameters_to_tau(const Eigen::MatrixXd& parameters)
{
  return (parameters(0) - 1) / parameters(0);
}

inline Eigen::VectorXd
GumbelBicop::get_start_parameters(const double tau)
{
  Eigen::VectorXd par = tau_to_parameters(tau);
  par = par.cwiseMax(parameters_lower_bounds_);
  par = par.cwiseMin(parameters_upper_bounds_);
  return par;
}
}

// This is copy&paste from the VineCopula package
inline double
qcondgum(double* q, double* u, double* de)
{
  double a, p, z1, z2, con, de1, dif;
  double mxdif;
  int iter;

  p = 1 - *q;
  z1 = -log(*u);
  con = log(1. - p) - z1 + (1. - *de) * log(z1);
  de1 = *de - 1.;
  a = pow(2. * pow(z1, *de), 1. / (*de));
  mxdif = 1;
  iter = 0;
  dif = .1; // needed in case first step leads to NaN
  while ((mxdif > 1.e-6) && (iter < 20)) {
    double g = a + de1 * log(a) + con;
    double gp = 1. + de1 / a;
    if ((boost::math::isnan)(g) || (boost::math::isnan)(gp) ||
        (boost::math::isnan)(g / gp)) {
      // added for de>50
      dif /= -2.;
    } else {
      dif = g / gp;
    }
    a -= dif;
    iter++;
    int it = 0;
    while ((a <= z1) && (it < 20)) {
      dif /= 2.;
      a += dif;
      ++it;
    }
    mxdif = fabs(dif);
  }
  z2 = pow(pow(a, *de) - pow(z1, *de), 1. / (*de));
  return (exp(-z2));
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2020 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef GEOMETRY_TEST_EXPECTATION_LIMITS_HPP
#define GEOMETRY_TEST_EXPECTATION_LIMITS_HPP

#include <boost/geometry/util/math.hpp>

#include <ostream>

// Structure to manage expectations: there might be small variations in area, for different
// types or options, which are all acceptable. With tolerance this is inconvenient.
// The values are stored as doubles, but the member functions accept any type,
// also for example Boost.MultiPrecision types
struct expectation_limits
{
    expectation_limits(double expectation)
        : m_lower_limit(expectation)
        , m_upper_limit(expectation)
    {
    }

    expectation_limits(double lower_limit, double upper_limit)
        : m_lower_limit(lower_limit)
        , m_upper_limit(upper_limit)
    {
    }

    double get() const { return m_lower_limit; }

    bool is_zero() const { return m_lower_limit < 1.0e-8; }

    bool has_two_limits() const { return m_lower_limit < m_upper_limit; }

    template<typename T>
    bool contains_logarithmic(const T& value, double tolerance) const
    {
      return abs(log(value) - std::log(m_lower_limit)) < tolerance;
    }

    template<typename T>
    bool contains(const T& value, double percentage, bool logarithmic = false) const
    {
        if (m_upper_limit < 1.0e-8)
        {
            return value < 1.0e-8;
        }
        if (logarithmic)
        {
            return contains_logarithmic(value, percentage);
        }

        // Note the > and <= and percentages, this is to make it exactly equivalent to
        // BOOST_CHECK_CLOSE(m_lower_limit, value, percentage) (if lower == upper)
        // But for two limits and optional slivers, >= is needed (for 0.00)
        double const fraction = percentage / 100.0;
        double const lower_limit = m_lower_limit * (1.0 - fraction);
        double const upper_limit = m_upper_limit * (1.0 + fraction);
        return has_two_limits()
                ? value >= lower_limit && value <= upper_limit
                : value > lower_limit && value <= upper_limit;
    }

    expectation_limits operator+(const expectation_limits& a) const
    {
        return this->has_two_limits() || a.has_two_limits()
                ? expectation_limits(this->m_lower_limit + a.m_lower_limit,
                                     this->m_upper_limit + a.m_upper_limit)
                : expectation_limits(this->m_lower_limit + a.m_lower_limit);
    }

    friend std::ostream &operator<<(std::ostream &os, const expectation_limits& lim)
    {
        if (lim.has_two_limits())
        {
            os << "[" << lim.m_lower_limit << " .. " << lim.m_upper_limit << "]";
        }
        else
        {
            os << lim.m_lower_limit;
        }
        return os;
    }

private :
    double const m_lower_limit;
    double const m_upper_limit;
};

inline expectation_limits optional_sliver(double upper_limit = 1.0e-4)
{
    return expectation_limits(0, upper_limit);
}

#endif // GEOMETRY_TEST_EXPECTATION_LIMITS_HPP


///////////////////////////////////////////////////////////////////////////////
// kernel::example::scalar_nw.cpp                                            //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/foreach.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/math/special_functions/fpclassify.hpp> //needed?
#include <boost/math/tools/precision.hpp>
#include <boost/typeof/typeof.hpp>

#include <boost/fusion/sequence/intrinsic/at_key.hpp>
#include <boost/fusion/include/at_key.hpp>
#include <boost/fusion/container/map.hpp>
#include <boost/fusion/include/map.hpp>
#include <boost/fusion/include/map_fwd.hpp>

#include <boost/statistics/detail/fusion/functor/at_key.hpp>

#include <boost/statistics/detail/kernel/kernels/scalar/gaussian.hpp>
#include <boost/statistics/detail/kernel/estimation/meta_nw_visitor_unary.hpp>
#include <boost/statistics/detail/kernel/estimation/estimator.hpp>
#include <libs/statistics/detail/kernel/example/scalar_nw.h>

void example_scalar_nw(std::ostream& out){

    out << "-> example_scalar_nw : ";
    using namespace boost;

    namespace kernel = boost::statistics::detail::kernel;

    // This example shows how to compute a Nadaraya-Watson estimate of E[y|x].
    // The type used for each data-unit, here, is a fusion map whose x and y
    // components are accessed using keys

    // Types
    typedef double                                          val_;
    typedef std::vector<val_>                               vals_;
    typedef mpl::int_<0>                                    key_x_;
    typedef mpl::int_<1>                                    key_y_;
    typedef fusion::pair<key_x_,val_>                       x_;
    typedef fusion::pair<key_y_,val_>                       y_;
    typedef statistics::detail::fusion::functor::at_key<key_x_> at_key_x_;
    typedef statistics::detail::fusion::functor::at_key<key_y_> at_key_y_;
    typedef fusion::map<x_,y_>                              data_unit_;
    typedef std::vector<data_unit_>                          dataset_;
    // The rationale for data_range_ is it's cheap to copy
    typedef sub_range<dataset_>                             data_range_;

    typedef mt19937                                         urng_;
    typedef normal_distribution<val_>                       norm_;
    typedef variate_generator<urng_&,norm_>                 gen_;
    typedef kernel::scalar::gaussian_kernel<val_>                  gauss_k_;
    typedef kernel::meta_nw_visitor_unary<
        at_key_x_,
        at_key_y_
    > meta_nw_visitor_u_;
    typedef meta_nw_visitor_u_::apply<
        gauss_k_,
        val_
    >::type  nw_visitor_u_;
    typedef nw_visitor_u_::nw_visitor_type              nw_visitor_;
    typedef nw_visitor_u_::rp_visitor_type              rp_visitor_;

    // Constants
    const val_ bandwidth = 0.5;
    const val_ eps = math::tools::epsilon<val_>();
    const unsigned n = 10;

    // Initialization
    vals_ vec_rp; vec_rp.reserve(n);
    vals_ vec_nw; vec_nw.reserve(n);
    dataset_ dataset;
    dataset.reserve(n);
    {
        urng_ urng;
        norm_ norm;
        gen_ gen(urng,norm);
        val_ one = static_cast<val_>(1);
        for(unsigned i = 0; i<n; i++){
            dataset.push_back(
                data_unit_(
                    fusion::make_pair<key_x_>(gen()),
                    fusion::make_pair<key_y_>(one)
                )
            );
        }
    }

    // Computes nw = E[y|x] for each x in the dataset. The density (rp) is
    // obtained as a by-product. Here, y = 1, so we should have
    // rp = nw (un-normalized).
    BOOST_FOREACH(data_unit_& u,dataset){
        nw_visitor_ nw_visitor = std::for_each(
            boost::begin(dataset),
            boost::end(dataset),
            nw_visitor_u_(
                bandwidth,
                fusion::at_key<key_x_>(u)
            )
        );
        val_ u_nw = nw_visitor.unnormalized_estimate();
        vec_nw.push_back(u_nw);
        rp_visitor_ rp_visitor = nw_visitor.rp_visitor();
        val_ rp = rp_visitor.estimate();
        BOOST_ASSERT(fabs(rp-u_nw)<eps);
    }

    // Same as above using estimator

    typedef kernel::estimator<
        data_range_,
        meta_nw_visitor_u_::apply,
        gauss_k_
    > estimator_;
    estimator_ estimator(bandwidth);
    estimator.train(
        data_range_(dataset)
    ); // * step 1 *

    BOOST_FOREACH(data_unit_& u,dataset){
        // -> these steps are independent of step2, they're just a test
        val_ x = fusion::at_key<key_x_>(u);
        BOOST_AUTO( nw_v , estimator.visit(x) );
        val_ u_nw = nw_v.unnormalized_estimate();
        BOOST_AUTO( rp_v , nw_v.rp_visitor() );
        val_ rp = rp_v.estimate();
        BOOST_ASSERT(fabs(rp-u_nw)<eps);
        // <-

        estimator.predict(x); // * step 2 *

    }

    out << "<-" << std::endl;
}


#define ARMA_DONT_USE_WRAPPER

#include <armadillo>

#include "../include/calc_asymptotic_variance.hpp"

using namespace std;
using namespace arma;

//' Calculate the asymptotic variance for the predicted y values
//'
//' @param Sigma_ll Sigma_ll matrix for the whole genome
//' @param Sigma_ls Sigma_ls matrix for the whole genome
//' @param Sigma_ss Sigma_ss matrix for the whole genome
//' @param Sigma_ss_blockwise a one-dimensional field containing blockwise Sigma_ss matrices
//' @param sigma2_s estimated value of sigma^2_s
//' @param n sample size for observed data (not the reference panel)
//' @param Xl_test genotypes matrix for large effect SNPs for test subjects
//' @param Xs_test genotypes matrix for small effect SNPs for test subjects
//' @return variance of predicted y values

arma::mat calc_asymptotic_variance(const arma::mat& Sigma_ll,
                                   const arma::mat& Sigma_ls,
                                   const arma::mat& Sigma_ss,
                                   const arma::field <arma::mat >& Sigma_ss_blockwise,
                                   double sigma2_s,
                                   unsigned int n,
                                   const arma::mat& Xl_test,
                                   const arma::mat& Xs_test){
  arma::mat Ainv = calc_A_inverse(Sigma_ss_blockwise, sigma2_s, n);
  arma::mat var_bl = calc_var_betal(Sigma_ll,
                                    Sigma_ls,
                                    Sigma_ss,
                                    Ainv,
                                    n);
  arma::mat var_bs = calc_var_betas(Sigma_ss,
                                    Sigma_ls,
                                    Ainv,
                                    sigma2_s,
                                    n,
                                    var_bl);
  arma::mat result = Xl_test * var_bl * arma::trans(Xl_test) + Xs_test * var_bs * arma::trans(Xs_test);
  return(result);
}

//' Calculate A inverse matrix
//'
//' @details (sigma^{-2}n^{-1} I_ms + Sigma_ss) = A. Here, we use the block diagonal structure
//' @param field a one-dimensional armadillo field for the entire genome. Each entry corresponds to a single LD block
//' @param sigma2_s estimate of sigma^2_s
//' @param n sample size
//' @return A inverse matrix

arma::mat calc_A_inverse(const arma::field <arma::mat >& field,
                         double sigma2_s,
                         unsigned int n)  {
  unsigned int n_blocks = field.n_elem;
  arma::field <arma::mat> inv_field(n_blocks);
  for( unsigned int i = 0; i < n_blocks; i++) {
    //make the diagonal matrix
    unsigned int m_s = field(i).n_rows;
    inv_field(i) = arma::inv_sympd(arma::eye(m_s, m_s) / (n * sigma2_s) + field(i));
  }
  arma::mat result = BlockDiag(inv_field);
  return result;
}

//' Calculate variance of coefficient estimator for large effects
//'
//' @param Sigma_ll Sigma_ll constructed for one LD block
//' @param Sigma_ls Sigma_ls constructed for one LD block
//' @param Sigma_ss Sigma_ss constructed for one LD block
//' @param A_inverse inverse of (sigma^{-2}n^{-1} I_ms + Sigma_ss)
//' @param n sample size
//' @return covariance matrix

arma::mat calc_var_betal(const arma::mat& Sigma_ll,
                         const arma::mat& Sigma_ls,
                         const arma::mat& Sigma_ss,
                         const arma::mat& A_inverse,
                         unsigned int n){
  //calculate second matrix
  arma::mat big = Sigma_ll - Sigma_ls * A_inverse * arma::trans(Sigma_ls);
  //invert and divide by n
  arma::mat result = arma::inv_sympd(big) / n;
  return (result);
}

//' Calculate variance of coefficient estimator for small effects
//'
//' @param Sigma_ss Sigma_ss matrix
//' @param Sigma_ls Sigma_ls matrix
//' @param A_inverse A inverse matrix
//' @param sigma2_s estimated value of sigma^2_s
//' @param n sample size
//' @param var_bl variance of beta hat l
//' @return covariance matrix

  arma::mat calc_var_betas(const arma::mat& Sigma_ss,
                           const arma::mat& Sigma_ls,
                           const arma::mat& A_inverse,
                           double sigma2_s,
                           unsigned int n,
                           const arma::mat& var_bl){
  arma::mat small = arma::trans(Sigma_ls) - Sigma_ss * A_inverse * arma::trans(Sigma_ls);
  arma::mat term2 = small * var_bl * arma::trans(small);
  arma::mat term1 = Sigma_ss - Sigma_ss * A_inverse * Sigma_ss;
  arma::mat result = sigma2_s * sigma2_s * n * (term1 + term2);
  return (result);
}

//' Construct a block diagonal matrix from a collection of matrices
//'
//' @param x a field of matrices, possibly of different sizes. Some matrices may have no rows and no columns
//' @return a block diagonal matrix
//' @reference https://stackoverflow.com/questions/29198893/block-diagonal-matrix-armadillo

arma::mat BlockDiag( const arma::field<arma::mat>& x ) {

  unsigned int len = x.n_elem;
  int drow = 0;
  int dcol = 0;
  arma::ivec rvec(len);
  arma::ivec cvec(len);
  //get dimensions of each matrix in the field
  for(unsigned int i = 0; i < len; i++) {
    rvec(i) = x(i).n_rows ;
    cvec(i) = x(i).n_cols ;
    drow += rvec(i);
    dcol += cvec(i);
  }
  //initialize matrix to be returned
  arma::mat X(drow, dcol, fill::zeros);
  int idx_row = 0;
  int idx_col = 0;
  // place matrices at correct places
  for(unsigned int i=0; i < len; i++) {
    if (rvec(i) > 0 && cvec(i) > 0){
      X.submat(idx_row,
               idx_col,
               idx_row + rvec(i) - 1,
               idx_col + cvec(i) - 1) = x(i) ;
      idx_row = idx_row + rvec(i) ;
      idx_col = idx_col + cvec(i);
    }
  }
  return(X);
}

//' Construct a n by p matrix from field containing matrices with n rows, but possibly fewer columns.
//'
//' @param x a field of matrices, possibly of different sizes, but all with the same number of rows.
//' @return a matrix

arma::mat ConcatenateColumns( const arma::field<arma::mat>& x ) {

  unsigned int len = x.n_elem;
  //unsigned int nrow = x(1).n_rows;//problem!
  int dcol = 0;

  arma::ivec cvec(len);
  arma::ivec rvec(len);
  //get number of columns of each matrix in the field
  for(unsigned int i = 0; i < len; i++) {
    cvec(i) = x(i).n_cols ;
    rvec(i) = x(i).n_rows;
    dcol += cvec(i);
  }
  unsigned int nrow = max(rvec);
  //initialize matrix to be returned
  arma::mat X(nrow, dcol, fill::zeros);
  cout << "number of rows: " << nrow << endl;
  cout << "number of columns: " << dcol << endl;

  int idx_col = 0;
  // place matrices at correct places
  for(unsigned int i=0; i < len; i++) {
    if (cvec(i) > 0){
      X.submat(0,
               idx_col,
               nrow - 1,
               idx_col + cvec(i) - 1) = x(i) ;
      idx_col = idx_col + cvec(i);
    }
  }
  return(X);
}

//' Assemble one set of five matrices for one chromosome
//'
//' @details Input is a two-dimensional arma::field, say from one chromosome, where each cell contains an arma::mat
//'     Specifically, it is a k by 5 arma::field, where k is the number of blocks on the chromosome of interest.
//' @param field a two-dimensional arma::field. See details.
//' @return a one-dimensional field containing exactly five arma::mat matrices: Sigma_ss, Sigma_sl, Sigma_ll, geno_s, geno_l

arma::field <arma::mat> assembleMatrices(const arma::field < arma::mat>& field){
  arma::field <arma::mat> result(5);
  cout << "field has this number of rows: " << field.n_rows << endl;
  result(0) = BlockDiag(field.col(0));
  cout << "result(0) has this dimension: " << result(0).n_rows << " rows & " << result(0).n_cols << " columns" << endl;
  result(1) = BlockDiag(field.col(1));
  cout << "result(1) has this dimension: " << result(1).n_rows << " rows & " << result(1).n_cols << " columns" << endl;
  result(2) = BlockDiag(field.col(2));
  cout << "result(2) has this dimension: " << result(2).n_rows << " rows & " << result(2).n_cols << " columns" << endl;
  result(3) = ConcatenateColumns(field.col(3));
  cout << "result(3) has this dimension: " << result(3).n_rows << " rows & " << result(3).n_cols << " columns" << endl;
  result(4) = ConcatenateColumns(field.col(4));
  return result;
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/math/basic_statistics.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

namespace scitbx { namespace math { namespace {

  struct basic_statistics_wrappers
  {
    typedef basic_statistics<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("basic_statistics", no_init)
        .def(init<af::const_ref<double> const&>((arg("values"))))
        .def_readonly("n", &w_t::n)
        .def_readonly("min", &w_t::min)
        .def_readonly("max", &w_t::max)
        .def_readonly("max_absolute", &w_t::max_absolute)
        .def_readonly("sum", &w_t::sum)
        .def_readonly("mean", &w_t::mean)
        .def_readonly("mean_absolute_deviation_from_mean",
          &w_t::mean_absolute_deviation_from_mean)
        .def_readonly("biased_variance", &w_t::biased_variance)
        .def_readonly("biased_standard_deviation",
          &w_t::biased_standard_deviation)
        .def_readonly("bias_corrected_variance", &w_t::bias_corrected_variance)
        .def_readonly("bias_corrected_standard_deviation",
          &w_t::bias_corrected_standard_deviation)
        .def_readonly("skew", &w_t::skew)
        .def_readonly("kurtosis", &w_t::kurtosis)
        .def_readonly("kurtosis_excess", &w_t::kurtosis_excess)
      ;
    }
  };

} // namespace <anonymous>

namespace boost_python {

  void wrap_basic_statistics()
  {
    basic_statistics_wrappers::wrap();
  }

}}} // namespace scitbx::math::boost_python


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[expand
//` Shows the usage of expand

#include <iostream>
#include <list>

#include <boost/geometry.hpp>

int main()
{
    typedef boost::geometry::model::d2::point_xy<short int> point_type;
    typedef boost::geometry::model::box<point_type> box_type;

    using boost::geometry::expand;

    box_type box = boost::geometry::make_inverse<box_type>(); /*< expand is usually preceded by a call to assign_inverse or make_inverse  >*/

    expand(box, point_type(0, 0));
    expand(box, point_type(1, 2));
    expand(box, point_type(5, 4));
    expand(box, boost::geometry::make<box_type>(3, 3, 5, 5));

    std::cout << boost::geometry::dsv(box) << std::endl;

    return 0;
}

//]

//[expand_output
/*`
Output:
[pre
((0, 0), (5, 5))
]
*/
//]


// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.gray
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Copyright Christopher Kormanyos 2015 - 2016.
// Copyright Paul A. Bristow 2015.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

//! \brief Example program showing fixed-point text-based Mandelbrot calculation with high resolution.

// Below are snippets of code that are included into Quickbook file fixed_point.qbk.

#include <algorithm>
#include <ctime>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <numeric>
#include <vector>

#include <boost/cstdint.hpp>
#include <boost/fixed_point/fixed_point.hpp>
#include <boost/gil/extension/io/jpeg_io.hpp>
#include <boost/gil/image.hpp>
#include <boost/gil/typedefs.hpp>

#define MANDELBROT_01_AQUA_FULL
//#define MANDELBROT_02_YELLOW_TOP
//#define MANDELBROT_03_FUSCHIA_SWIRL
//#define MANDELBROT_04_BLACK_WHITE_SEAHORSES

// Declare a base class for the Mandelbrot configuration.
class mandelbrot_configuration_base
{
public:
  virtual ~mandelbrot_configuration_base() { }

  long double x_lo() const { return my_x_lo; }
  long double x_hi() const { return my_x_hi; }
  long double y_lo() const { return my_y_lo; }
  long double y_hi() const { return my_y_hi; }

  virtual boost::uint_fast16_t max_iterations() const = 0;

  virtual int mandelbrot_fractional_resolution() const = 0;

  virtual long double step() const = 0;

  virtual boost::uint_fast32_t width () const = 0;
  virtual boost::uint_fast32_t height() const = 0;

  virtual boost::uint_fast32_t red_hue  () const = 0;
  virtual boost::uint_fast32_t green_hue() const = 0;
  virtual boost::uint_fast32_t blue_hue () const = 0;

protected:
  const long double my_x_lo;
  const long double my_x_hi;
  const long double my_y_lo;
  const long double my_y_hi;

  mandelbrot_configuration_base(const long double xl, const long double xh,
                                const long double yl, const long double yh) : my_x_lo(xl), my_x_hi(xh),
                                                                              my_y_lo(yl), my_y_hi(yh) { }
private:
  mandelbrot_configuration_base() : my_x_lo(0.0L), my_x_hi(0.0L),
                                    my_y_lo(0.0L), my_y_hi(0.0L) { }
};

// Make a template class that represents the Mandelbrot configuration.
// This class automatically creates sensible parameters based on
// the resolution of the fixed-point type supplied in the template
// parameter. If a custom pixel count is required, the step()
// method can be modified accordingly.
template<const int TotalDigits,
         const boost::uint_fast16_t MaxIterations,
         const int MandelbrotFractionalResolution,
         const boost::uint_fast32_t RedHue,
         const boost::uint_fast32_t GreenHue,
         const boost::uint_fast32_t BlueHue>
class mandelbrot_configuration : public mandelbrot_configuration_base
{
public:
  static_assert(    (RedHue   < UINT32_C(256))
                 && (GreenHue < UINT32_C(256))
                 && (BlueHue  < UINT32_C(256)), "The color hue parameters must be 255 or smaller.");

  typedef boost::fixed_point::negatable<16, 16 + 1 - TotalDigits> fixed_point_type;

  mandelbrot_configuration(const long double xl, const long double xh,
                           const long double yl, const long double yh)
    : mandelbrot_configuration_base(xl, xh, yl, yh) { }

  virtual ~mandelbrot_configuration() { }

private:
  virtual boost::uint_fast16_t max_iterations() const { return MaxIterations; }

  virtual int mandelbrot_fractional_resolution() const { return MandelbrotFractionalResolution; }

  virtual long double step() const { return 1.0L / (UINT64_C(1) << -mandelbrot_fractional_resolution()); }

  virtual boost::uint_fast32_t width () const { return static_cast<boost::uint_fast32_t>((x_hi() - x_lo()) / step()); }
  virtual boost::uint_fast32_t height() const { return static_cast<boost::uint_fast32_t>((y_hi() - y_lo()) / step()); }

  virtual boost::uint_fast32_t red_hue  () const { return RedHue; }
  virtual boost::uint_fast32_t green_hue() const { return GreenHue; }
  virtual boost::uint_fast32_t blue_hue () const { return BlueHue; }
};

// This class generated the rows of the mandelbrot iteration.
// The coordinates are set up according to the Mandelbrot configuration.
template<typename NumericType>
class mandelbrot_generator
{
public:
  mandelbrot_generator(const mandelbrot_configuration_base& config)
    : mandelbrot_configuration_object(config),
      mandelbrot_image               (config.width(), config.height()),
      mandelbrot_view                (boost::gil::rgb8_view_t()),
      mandelbrot_iteration_matrix    (mandelbrot_configuration_object.width(),
                                      std::vector<boost::uint_fast16_t>(mandelbrot_configuration_object.height())),
      mandelbrot_color_histogram     (static_cast<std::size_t>(config.max_iterations() + 1U), UINT32_C(0))
  {
    mandelbrot_view = boost::gil::view(mandelbrot_image);
  }

  ~mandelbrot_generator() { }

  void generate_mandelbrot_image()
  {
    // Setup the x-axis coordinates.
    std::vector<NumericType> x_values(mandelbrot_configuration_object.width());

    // Initialize the x-axis coordinates (one time only).
    {
      NumericType x_step(mandelbrot_configuration_object.x_lo());

      for(NumericType& x : x_values)
      {
        x = x_step;

        x_step += mandelbrot_configuration_object.step();
      }
    }

    // Initialize the y-axis coordinate.
    NumericType y(mandelbrot_configuration_object.y_hi());

    // TBD: The iteration through rows can be distributed in multithreading.

    // Loop through all the rows of pixels on the vertical
    // y-axis in the direction of decreasing y-value.
    for(boost::uint_fast32_t row = UINT32_C(0); row < mandelbrot_configuration_object.height(); ++row, y -= mandelbrot_configuration_object.step())
    {
      // Loop through this column of pixels on the horizontal
      // x-axis in the direction of increasing x-value.
      boost::uint_fast32_t col = UINT32_C(0);

      std::for_each(x_values.cbegin(),
                    x_values.cend(),
      [&y, &col, &row, this](const NumericType& x)
      {
        const NumericType cr(x);
        const NumericType ci(y);

        NumericType zr(0);
        NumericType zi(0);

        NumericType zr_sqr(0);
        NumericType zi_sqr(0);

        // Use an optimized complex-numbered multiplication scheme.
        // Thereby reduce the main work of the Mandelbrot iteration to
        // three real-valued multiplications and several real-valued
        // addition/subtraction operations.

        boost::uint_fast16_t i = UINT16_C(0);

        // Perform the iteration sequence for generating the Mandelbrot set.
        // Herein lies the work of the program.
        // TBD: This can easily be distributed to parallel processes.

        while(   (i < mandelbrot_configuration_object.max_iterations())
              && ((zr_sqr + zi_sqr) < 4))
        {
          zi *= zr;
          zi  = (zi + zi) + ci;

          zr = (zr_sqr - zi_sqr) + cr;

          zr_sqr = zr * zr;
          zi_sqr = zi * zi;

          ++i;
        }

        mandelbrot_iteration_matrix[col][row] = i;

        ++mandelbrot_color_histogram[i];

        ++col;
      });

      std::cout << "Calculating Mandelbrot image at row "
                << std::setw(6)
                << (row + 1U)
                << " of "
                << std::setw(6)
                << mandelbrot_configuration_object.height()
                << " total. Have patience."
                << "\r";
    }

    const boost::uint_fast32_t total = boost::uint_fast32_t(mandelbrot_configuration_object.width()) * mandelbrot_configuration_object.height();

    // Perform color-stretching using the histogram approach.
    // Convert the histogram entries such that a given entry contains
    // the sum of its own entries plus all previous entries. This provides
    // a set of scale factors for the color. The histogram approach
    // automatically scales to the distribution of pixels in the image.

    std::accumulate(mandelbrot_color_histogram.begin(),
                    mandelbrot_color_histogram.end(),
                    boost::uint_fast32_t(0U),
    [&total](boost::uint_fast32_t& sum, boost::uint_fast32_t& histogram_entry) -> boost::uint_fast32_t
    {
      sum += histogram_entry;

      histogram_entry = UINT32_C(0xFF) - static_cast<boost::uint_fast32_t>((boost::uint64_t(sum) * 0xFFU) / total);

      return sum;
    });

    for(boost::uint_fast32_t row = UINT32_C(0); row < mandelbrot_configuration_object.height(); ++row)
    {
      for(boost::uint_fast32_t col = UINT32_C(0); col < mandelbrot_configuration_object.width(); ++col)
      {
        const boost::uint_fast32_t color = mandelbrot_color_histogram[mandelbrot_iteration_matrix[col][row]];

        // Mix the color supplied in the template hue parameters.
        const boost::uint8_t rh = static_cast<boost::uint8_t>((mandelbrot_configuration_object.red_hue  () * color) / UINT32_C(255));
        const boost::uint8_t gh = static_cast<boost::uint8_t>((mandelbrot_configuration_object.green_hue() * color) / UINT32_C(255));
        const boost::uint8_t bh = static_cast<boost::uint8_t>((mandelbrot_configuration_object.blue_hue () * color) / UINT32_C(255));

        const boost::gil::rgb8_pixel_t the_color  = boost::gil::rgb8_pixel_t(rh, gh, bh);

        mandelbrot_view(col, row) = boost::gil::rgb8_pixel_t(the_color);
      }
    }

    boost::gil::jpeg_write_view("mandelbrot.jpg", mandelbrot_view);

    std::cout << std::endl
              << "The ouptput file mandelbrot.jpg has been written"
              << std::endl;
  }

private:
  const mandelbrot_configuration_base&           mandelbrot_configuration_object;
  boost::gil::rgb8_image_t                       mandelbrot_image;
  boost::gil::rgb8_view_t                        mandelbrot_view;
  std::vector<std::vector<boost::uint_fast16_t>> mandelbrot_iteration_matrix;
  std::vector<boost::uint_fast32_t>              mandelbrot_color_histogram;
};

int main()
{
  #if defined MANDELBROT_01_AQUA_FULL

    // This is the classic full immage rendered in aqua tones (and black).
    typedef mandelbrot_configuration<128, UINT16_C(2000), -11,
                                     UINT32_C(80),
                                     UINT32_C(255),
                                     UINT32_C(255)> mandelbrot_configuration_type;

    const mandelbrot_configuration_type mandelbrot_configuration_object(-2.000L, +0.500L,
                                                                        -1.000L, +1.000L);

  #elif defined MANDELBROT_02_YELLOW_TOP

    // This is an upper part of the image rendered in yellow and black tones.
    typedef mandelbrot_configuration<128, UINT16_C(2000), -13,
                                     UINT32_C(255),
                                     UINT32_C(255),
                                     UINT32_C(0)> mandelbrot_configuration_type;

    const mandelbrot_configuration_type mandelbrot_configuration_object(-0.1208L - 0.1616L, -0.1208L + 0.1616L,
                                                                        +0.7607L - 0.1616L, +0.7607L + 0.1616L);

  #elif defined MANDELBROT_03_FUSCHIA_SWIRL

    // This is a fanning image rendered in fuschia tones (and black).
    typedef mandelbrot_configuration<128, UINT16_C(10000), -22,
                                     UINT32_C(255),
                                     UINT32_C(0),
                                     UINT32_C(210)> mandelbrot_configuration_type;

    const mandelbrot_configuration_type mandelbrot_configuration_object(-0.749730L - 0.0002315L, -0.749730L + 0.0002315L,
                                                                        -0.046608L - 0.0002315L, -0.046608L + 0.0002315L);

  #elif defined MANDELBROT_04_BLACK_WHITE_SEAHORSES

    // This is a swirly seahorse image rendered in black-and-white tones.
    typedef mandelbrot_configuration<128, UINT16_C(10000), -46,
                                     UINT32_C(255),
                                     UINT32_C(255),
                                     UINT32_C(255)> mandelbrot_configuration_type;
    const mandelbrot_configuration_type mandelbrot_configuration_object(-0.745398360667L - 1.25E-11L, -0.745398360667L + 1.25E-11L,
                                                                      +0.112504634996L - 1.25E-11L, +0.112504634996L + 1.25E-11L);

  #else

    #error: Mandelbrot imag type is not defined!

  #endif

  typedef mandelbrot_configuration_type::fixed_point_type mandelbrot_numeric_type;

  typedef mandelbrot_generator<mandelbrot_numeric_type> mandelbrot_generator_type;

  const std::clock_t start = std::clock();

  mandelbrot_generator_type* the_mandelbrot_generator = new mandelbrot_generator_type(mandelbrot_configuration_object);

  the_mandelbrot_generator->generate_mandelbrot_image();

  const float elapsed = (float(std::clock()) - float(start)) / CLOCKS_PER_SEC;

  std::cout << "Time for calculation: "
            << elapsed
            << "s"
            << std::endl;

  delete the_mandelbrot_generator;
}