#include <iostream>
#include <iomanip>
#include <fstream>
#include <assert.h>
#include <cstdlib>
#include <string>
#include <vector>
#include <queue>
#include <boost/regex.hpp>
#include <boost/tokenizer.hpp>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include "commands.h"
#include "Point.h"
#include "Vector.h"
#include "ProjPoint.h"
#include "TransformationWithFrames.h"
#include "TexShape.h"

using namespace std;
using namespace boost;

const double	 pi = 3.1415926535897;
const char polypathstr[] = "poly";
const char polyextstr[] = ".poly";
const char texpathstr[] = "tex";
boost::filesystem::path polypath(polypathstr);
boost::filesystem::path polyext(polyextstr);

Command::~Command() { }

token Command::tokenize_cmd(string command) {
	char_separator<char> seq(" \\");
	return token(command, seq);
}

void Command::writePolygons(polygonList polygons, string filename) {
	boost::filesystem::create_directory(polypathstr);
	ofstream outfile((string(polypathstr) + "/" + filename).c_str());
	//cout << (string(polypathstr) + "/" + filename).c_str() << " created\n";
	BOOST_FOREACH(vector<Point> polygon, polygons) {
		if (polygon.size() < 1)
			continue;
		BOOST_FOREACH(Point p, polygon) {
			outfile << p << " ";
		}
		outfile << endl;
	}
	outfile.close();
}

/**
 * store the command name and filename argument
 */
token::iterator preparse(token tok, Command &c) {
	token::iterator itr = tok.begin();
	c.cname = *itr; // command name
	assert(++itr != tok.end());

	// check for file extension
	cmatch what;
	regex exp("(.*)(\\.poly)");
	if (regex_match(((string)*itr).c_str(), what, exp)) { // file extension found
		c.filename = what[0];
	} else { // append file extension
		c.filename = ((string)*itr).append(polyextstr);
	}
	return ++itr;
}

/**
 * convert a transform code in string to enum type
 */
code_t codeStrToEnum(string code) {
	int index = -1;
	do {
		if (string(codeStr[++index]) == code)
			break;
	} while ((code_t)index != END);
	assert((code_t)index != END);
	code_t enumcode = (code_t)index;
	return enumcode;
}

MidasPolygon::MidasPolygon(token tok) {
	token::iterator itr = preparse(tok, *this);
	// insert the 2d points
	for (; itr != tok.end(); ++itr) {
		double x = atof( ((string)*itr).c_str() );
		assert(++itr != tok.end());
		double y = atof( ((string)*itr).c_str() );
		int r = 255, g = 255, b = 255;//, a = 255;
		token::iterator backtrackItr = itr;
		if (++itr != tok.end()) {
			string color = (string)*itr;
			if (color.size() == 6) {
				string red = string("0x").append(color.substr(0,2));
				string green = string("0x").append(color.substr(2,2));
				string blue = string("0x").append(color.substr(4,2));
				//string alpha = string("0x").append(color.substr(6,2));
				//cout << red << " " << green << " " << blue << endl;
				char *endptr;
				r = strtol(red.c_str(), &endptr, 16);
				g = strtol(green.c_str(), &endptr, 16);
				b = strtol(blue.c_str(), &endptr, 16);
				//a = strtol(alpha.c_str(), &endptr, 16);
			} else
				itr = backtrackItr;
		} else
			itr = backtrackItr;
		//cout << r << " " << g << " " << b << endl;
		Point p(x, y, 0.0, r, g, b);
		//cout << p << "\n";
		points.push_back(p);
		//cout << points.back() << "\n\n";
		assert(itr != tok.end());
	}
	//cout << endl;
}

void MidasPolygon::execute() {
	//cout << "Executing Polygon\n";
	boost::filesystem::create_directory(polypathstr);
	ofstream file((string(polypathstr) + "/" + this->filename).c_str());
	BOOST_FOREACH(Point p, this->points) {
		file << p << " ";
	}
	file << "\n";
	file.close();
}

Assemble::Assemble(token tok) {
	token::iterator itr = preparse(tok, *this);

	for (; itr != tok.end(); ++itr) {
		boost::filesystem::path filepath = polypath / (*itr + polyextstr);
		assert(boost::filesystem::exists(filepath));
		this->files.push_back(*itr);
	}
}

void Assemble::execute() {
	//cout << "Executing Assemble\n";
	boost::filesystem::create_directory(polypathstr);
	ofstream newfile((string(polypathstr) + "/" + this->filename).c_str());
	BOOST_FOREACH(string thisfile, files) {
		ifstream curfile((polypath / (thisfile + polyextstr)).string().c_str());
		char c;
		while ((c = curfile.get()) != EOF)
			newfile << c;
		curfile.close();
	}
	newfile.close();
	//cout << (string(polypathstr) + "/" + this->filename).c_str() << " assembled\n";
}

Transform::Transform() {

}

Transform::Transform(polygonList polygons) {
	this->polygons = polygons;
}

Transform::Transform(projPolygonList projPolygons) {
	this->projPolygons = projPolygons;
}

/**
 * transform constructor with argument
 * param tok tokenized command
 */
Transform::Transform(token tok) {
	token::iterator itr = preparse(tok, *this);
	readPolygons(this->filename, &polygons, &texturedPolygons);
	readCodes(tok, itr);
}

void Transform::readCodes(token tok, token::iterator itr) {
	for(; itr != tok.end(); itr++) {
		code_t code = codeStrToEnum(*itr);
		itr++;
		assert(itr != tok.end());
		tfCmd[code] = new double(atof((*itr).c_str()));
	}
}

vector<Point> getCoords(token tok) {
	vector<Point> newPolygon;
	vector<string> coords; // put all coords string in a vector
	BOOST_FOREACH(string s, tok) {
		//cout << s << " ";
		coords.push_back(s);
		////cout << coords.back() << " ";
	}
	//cout << endl;

	assert(coords.size() % 4 == 0);
	for (unsigned int i = 0; i < coords.size(); i += 4) {
		double x = atof(coords[i].c_str());
		double y = atof(coords[i+1].c_str());
		double z = atof(coords[i+2].c_str());
		string color = coords[i+3];
		assert(color.size() == 6);
		string red = string("0x").append(color.substr(0,2));
		string green = string("0x").append(color.substr(2,2));
		string blue = string("0x").append(color.substr(4,2));
		//string alpha = string("0x").append(color.substr(6,2));
		//cout << red << " " << green << " " << blue << endl;
		char *endptr;
		int r = strtol(red.c_str(), &endptr, 16);
		int g = strtol(green.c_str(), &endptr, 16);
		int b = strtol(blue.c_str(), &endptr, 16);
		//int a = strtol(alpha.c_str(), &endptr, 16);
		newPolygon.push_back(Point(x, y, z, r, g, b));
	}
	return newPolygon;
}

void Command::readPolygons(string filename, polygonList* polygons, texturedPolygonList* texturedPolygons) {
	//cout << "Reading " << filename << ":\n";
	char c[512];
	assert(boost::filesystem::exists(polypath / filename));
	//cout << (string(polypathstr) + "/" + filename).c_str() << endl;
	ifstream inf((string(polypathstr) + "/" + filename).c_str());
	while(inf.good()) {
		inf.getline(c,512);
		if (c[0] == '#' || c[0] == '\n') // comment or blank lines
			continue;
		if (c[0] == '\0')
			break;

		string str(c);
		erase_all(str, ")"); // remove extra characters
		erase_all(str, "(");
		replace_all(str, ",", " ");
//		cout << str << endl;

		char_separator<char> sep(" "); // tokenize line
		tokenizer<char_separator<char> > tok(str, sep);

		token::iterator itr = tok.begin();
		if (*itr == string("t")) {	// if textured polygon
			TexShape newTexShape;
			newTexShape.texturefile = *(++itr);
			int count = atoi((++itr)->c_str());
			//cout << count << endl;
			for (int i = 0; i < count; i++) {
				if (c[0] == '#' || c[0] == '\n') // comment or blank lines
					continue;
				if (c[0] == '\0')
					break;
				inf.getline(c,512);
				str = string(c);
				erase_all(str, ")"); // remove extra characters
				erase_all(str, "(");
				replace_all(str, ",", " ");
				tok = token(str, sep);
				newTexShape.shape.push_back(getCoords(tok));

				inf.getline(c,512);
				str = string(c);
				erase_all(str, ")"); // remove extra characters
				erase_all(str, "(");
				replace_all(str, ",", " ");
				tok = token(str, sep);
				newTexShape.texture.push_back(getCoords(tok));
			}
			//newTexShape.print(cout);
			if (texturedPolygons) {
				texturedPolygons->push_back(newTexShape);
			}
			continue;
		}

		vector<Point> newPolygon = getCoords(tok);
		if (newPolygon.size() > 0) // make sure no empty polygon is inserted
			polygons->push_back(newPolygon);
	}
	BOOST_FOREACH(vector<Point> polygon, *polygons) {
		BOOST_FOREACH(Point p, polygon) {
			//cout << p << " ";
		}
		//cout << endl;
	}
	inf.close();
}

void Transform::execute() {
	//cout << "Executing Transform\n";
	transform(translate() * scale() * rotate(), false);
}

polygonList Transform::transform(Transformation t, bool project) {
	//cout << "Transformation matrix:\n";
	//cout << t << endl;
	if (!project) { // transforming only
		vector< vector<Point> >::iterator polygonItr = polygons.begin();
		for(; polygonItr != polygons.end(); polygonItr++) {
			vector<Point>::iterator pointItr = (*polygonItr).begin();
			for(; pointItr != (*polygonItr).end(); pointItr++) // apply matrix to each points
				(*polygonItr).insert((*polygonItr).erase(pointItr),  t.applied_to(*pointItr));
		}
		for (int i = 0; i < texturedPolygons.size(); i++) {
			TexShape *ts = &texturedPolygons[i];
			vector< vector<Point> >::iterator polygonItr = ts->shape.begin();
			for(; polygonItr != ts->shape.end(); polygonItr++) {
				vector<Point>::iterator pointItr = (*polygonItr).begin();
				for(; pointItr != (*polygonItr).end(); pointItr++) // apply matrix to each points
					(*polygonItr).insert((*polygonItr).erase(pointItr),  t.applied_to(*pointItr));
			}
		}
		//cout << "polygons: \n";
//		BOOST_FOREACH(vector<Point> polygon, polygons) {
//			BOOST_FOREACH(Point p, polygon) {
//				cout << p << " ";
//			}
//			cout << endl;
//		}
		//cout << "textured polygons:\n";
//		BOOST_FOREACH(TexShape ts, texturedPolygons) {
//			BOOST_FOREACH(vector<Point> shape, ts.shape) {
//				BOOST_FOREACH(Point p, shape) {
//					cout << p << " ";
//				}
//				cout << endl;
//			}
//		}
	} else { // camera / viewing transform
		vector< vector<ProjPoint> >::iterator polygonItr = projPolygons.begin();
		for(; polygonItr != projPolygons.end(); polygonItr++) {
			vector<ProjPoint>::iterator pointItr = (*polygonItr).begin();
			for(; pointItr != (*polygonItr).end(); pointItr++)
				(*polygonItr).insert((*polygonItr).erase(pointItr),  t.applied_to(*pointItr));
		}

//		BOOST_FOREACH(vector<ProjPoint> polygon, projPolygons) {
//			BOOST_FOREACH(ProjPoint p, polygon) {
//				//cout << p << " ";
//			}
//			//cout << endl;
//		}

		polygons.clear();
		BOOST_FOREACH(vector<ProjPoint> polygon, projPolygons) {
			vector<Point> newpolygon;
			BOOST_FOREACH(ProjPoint p, polygon) {
//				if (p.w() < 0)
//					p.setW(-1.0 * p.w());
				//cout << p << " ";
				Point pp = p.project();
				newpolygon.push_back(pp);
			}
			polygons.push_back(newpolygon);
			//cout << endl;
		}
	}
	return this->polygons;
}

Transformation Transform::translate() {
	double x = (tfCmd[XT])?*tfCmd[XT]:0.000;
	double y = (tfCmd[YT])?*tfCmd[YT]:0.000;
	double z = (tfCmd[ZT])?*tfCmd[ZT]:0.000;
	//cout << codeStr[XT] << ": " << x << endl;
	//cout << codeStr[YT] << ": " << y << endl;
	//cout << codeStr[ZT] << ": " << z << endl;
	return Transformation::translate_by(x, y, z);
}

Transformation Transform::scale() {
	double x = (tfCmd[XS])?*tfCmd[XS]:1.000;
	double y = (tfCmd[YS])?*tfCmd[YS]:1.000;
	double z = (tfCmd[ZS])?*tfCmd[ZS]:1.000;
	double u = (tfCmd[US])?*tfCmd[US]:1.000;
	//cout << codeStr[XS] << ": " << x << endl;
	//cout << codeStr[YS] << ": " << y << endl;
	//cout << codeStr[ZS] << ": " << z << endl;
	//cout << codeStr[US] << ": " << u << endl;
	return Transformation::scale_by(x, y, z) * Transformation::scale_by(u, u, u);
}

Transformation Transform::rotate() {
	double xd = (tfCmd[XD])?*tfCmd[XD]:0.000;
	double yd = (tfCmd[YD])?*tfCmd[YD]:0.000;
	double zd = (tfCmd[ZD])?*tfCmd[ZD]:0.000;
	double xr = (tfCmd[XR])?*tfCmd[XR]:0.000;
	double yr = (tfCmd[YR])?*tfCmd[YR]:0.000;
	double zr = (tfCmd[ZR])?*tfCmd[ZR]:0.000;
	//cout << codeStr[XD] << ": " << xd << endl;
	//cout << codeStr[YD] << ": " << yd << endl;
	//cout << codeStr[ZD] << ": " << zd << endl;
	//cout << codeStr[XR] << ": " << xr << endl;
	//cout << codeStr[YR] << ": " << yr << endl;
	//cout << codeStr[ZR] << ": " << zr << endl;
	return Transformation::x_rotate_by(xd) * Transformation::y_rotate_by(yd) *
		Transformation::z_rotate_by(zd) * Transformation::x_rotate_by(xr * 180.0 / pi) *
		Transformation::y_rotate_by(yr * 180.0 / pi) * Transformation::z_rotate_by(zr * 180.0 / pi);
}

/**
 * create newfile oldfile transforms
 */
Create::Create(token tok) {
	token::iterator itr = preparse(tok, *this);
	outputfile = filename;

	assert(itr != tok.end());
	// check for file extension
	cmatch what;
	regex exp("(.*)(\\.poly)");
	if (regex_match(((string)*itr).c_str(), what, exp)) { // file extension found
		filename = what[0];
	} else { // append file extension
		filename = ((string)*itr).append(polyextstr);
	}
	readPolygons(filename, &polygons, &texturedPolygons);
	readCodes(tok, ++itr);
}

void Create::execute() {
	//cout << "Executing Create\n";
	Transform::execute();
	writePolygons(polygons, this->outputfile);
	BOOST_FOREACH(TexShape ts, texturedPolygons) {
		Surface::writeSurface(ts, outputfile);
	}
}

View::View() {

}

View::View(token tok) {
	token::iterator itr = preparse(tok, *this);
	eyeX = atof((*itr).c_str());
	assert(++itr != tok.end());
	eyeY = atof((*itr).c_str());
	assert(++itr != tok.end());
	eyeZ = atof((*itr).c_str());
	assert(++itr != tok.end());
	upX = atof((*itr).c_str());
	assert(++itr != tok.end());
	upY = atof((*itr).c_str());
	assert(++itr != tok.end());
	upZ = atof((*itr).c_str());
	assert(++itr != tok.end());
	centerX = atof((*itr).c_str());
	assert(++itr != tok.end());
	centerY = atof((*itr).c_str());
	assert(++itr != tok.end());
	centerZ = atof((*itr).c_str());
	assert(++itr != tok.end());
	angle = atof((*itr).c_str());
	assert(++itr != tok.end());
	near = atof((*itr).c_str());
	assert(++itr != tok.end());
	far = atof((*itr).c_str());
	readPolygons(filename, &polygons, &texturedPolygons);
}

void View::convert() {
	//cout << "Converting points to projective space\n";
	BOOST_FOREACH(vector<Point> polygon, polygons) {
		vector<ProjPoint> newpolygon;
		BOOST_FOREACH(Point p, polygon) {
			ProjPoint pp(p);
			newpolygon.push_back(pp);
			//cout << pp << " ";
		}
		projPolygons.push_back(newpolygon);
		//cout << endl;
	}
}

/**
 * view file eyex eyey eyez roll upy pitch centerx centery centerz angle near far
 */
void View::execute() {
//	convert();
	//cout << "Executing View\n";
//	Frame f1(Vector(1, 0, 0), Vector(0, 1, 0), Vector(0, 0, 1), Point(centerX, centerY, centerZ));
//	Vector w = Point(eyeX, eyeY, eyeZ) - Point(centerX, centerY, centerZ);
//	w.normalize();
//	Frame f2(w, Vector(upX,1,0), Point(eyeX, eyeY, eyeZ));
//	Transform tf(projPolygons);
//	Transformation camera_t = Transformation::frame_to_frame(f1, f2);
//	Transformation viewing_t = Transformation::view(near, far, angle);
//	//cout << "Camera:\n" << camera_t << endl;
//	//cout << "Viewing:\n" << viewing_t << endl;
//	polygons = tf.transform(camera_t * viewing_t, true);
	//cout << "Final Points:\n";
//	BOOST_FOREACH(vector<Point> polygon, polygons) {
//		BOOST_FOREACH(Point p, polygon) {
//			//cout << p << " ";
//		}
//		//cout << endl;
//	}
}

Extrude::Extrude(token tok) {
	token::iterator itr = preparse(tok, *this);
	assert(itr != tok.end());

	cmatch what;
	regex exp("(.*)(\\.poly)");
	if (regex_match(((string)*itr).c_str(), what, exp)) { // file extension found
		newfile = what[0];
	} else { // append file extension
		newfile = ((string)*itr).append(polyextstr);
	}
	assert(++itr != tok.end());
	units = atof((*itr).c_str());
	readPolygons(filename, &polygons);
}

/**
 * extrude oldfile newfile units
 */

void Extrude::execute() {
	//cout << "Executing Extrude\n";
	vector<vector<Point> > newPolygons;

	BOOST_FOREACH(vector<Point> polygon, polygons) {
		newPolygons.push_back(polygon); // add the original base
		vector<Point> topFace;
		Vector normal;
		for (unsigned int i = 0; i < polygon.size(); i++) {
			if (i == 0) { // get the normal vector from first 3 points
				Vector v1;
				Vector v2;
				v1 = polygon.back() - polygon[0];
				//cout << "v1: " << v1 << endl;
				v2 = polygon[i+1] - polygon[i];
				v1.normalize();
				v2.normalize();
				//cout << "v2: " << v2 << endl;
				normal = cross(v1, v2);
				normal.normalize();
				//cout << "normal: " << normal << endl;
				normal *= units;
			}
			topFace.push_back(polygon[i] + normal); // add the extruded points
		}
//		BOOST_FOREACH(Point p, topFace) {
//			//cout << p << " ";
//		}
		//cout << endl;
		newPolygons.push_back(topFace); // add extruded face

		// add the extruded sides
		for (unsigned int i = 0; i < polygon.size(); i++) {
			vector<Point> newSideFace;
			newSideFace.push_back(polygon[i]); // push 2 base points
			if (i+1 < polygon.size())
				newSideFace.push_back(polygon[i+1]);
			else
				newSideFace.push_back(polygon[0]);
			if (i+1 < topFace.size()) // push 2 top face points
				newSideFace.push_back(topFace[i+1]);
			else
				newSideFace.push_back(topFace[0]);
			newSideFace.push_back(topFace[i]);

			newPolygons.push_back(newSideFace); // add extruded sides
		}

//		BOOST_FOREACH(vector<Point> polygon, newPolygons) {
//			BOOST_FOREACH(Point p, polygon) {
//				//cout << p << " ";
//			}
//			//cout << endl;
//		}

		writePolygons(newPolygons, this->newfile);

	}
}

Clip::Clip(token tok) {
	token::iterator itr = preparse(tok, *this);
	assert(itr != tok.end());

	cmatch what;
	regex exp("(.*)(\\.poly)");
	if (regex_match(((string)*itr).c_str(), what, exp)) { // file extension found
		newfile = what[0];
	} else { // append file extension
		newfile = ((string)*itr).append(polyextstr);
	}
	double px, py, pz, nx, ny, nz;
	assert(++itr != tok.end());
	px = atof((*itr).c_str());
	assert(++itr != tok.end());
	py = atof((*itr).c_str());
	assert(++itr != tok.end());
	pz = atof((*itr).c_str());
	assert(++itr != tok.end());
	nx = atof((*itr).c_str());
	assert(++itr != tok.end());
	ny = atof((*itr).c_str());
	assert(++itr != tok.end());
	nz = atof((*itr).c_str());
	p = Point(px, py, pz);
	vnorm = Vector(nx, ny, nz);
	vnorm.normalize();

	readPolygons(filename, &polygons);
}

void Clip::execute() {
	//cout << "Executing Clip" << endl;
	// get the clipping point
	//cout << "p: " << p << endl << "vnorm: " << vnorm << endl;
	polygonList clippedPolygons;
	BOOST_FOREACH(vector<Point> polygon, polygons) {
		vector<Point> clippedPolygon;
		Point lastPoint = polygon.back();
		BOOST_FOREACH(Point q, polygon) {
			float d1 = dot(vnorm, (q-p));
			float d2 = dot(vnorm, (lastPoint - p));
			//cout << "d1: " << d1 << " d2: " << d2 << endl;
			if (d1 >= 0.0 && d2 >= 0.0) {
				clippedPolygon.push_back(q);
				//cout << q << " is IN" << endl;
			}
			else if (d1 >= 0.0 && d2 < 0.0 || d1 < 0.0 && d2 >= 0.0) { // lastpoint on outside
				float t = d1 / (d1 - d2);
				Point I = q + t*(lastPoint - q);
				Point *outPoint; // determine which one is out to set color
				if (d1 >= 0.0) { // current is IN, last is OUT
					//cout << q << " is IN\n";
					outPoint = &lastPoint;
				} else { // current is OUT, last is IN
					//cout << q << " is OUT\n";
					outPoint = &q;
				}
				I.setA(outPoint->a()); // set color of intersection point
				I.setG(outPoint->g()); //  to the color of point on
				I.setR(outPoint->r()); //  outside
				I.setB(outPoint->b());
				//cout << I << " is an intersection\n";
				clippedPolygon.push_back(I); // add intersection point to polygon
				if (d1 >= 0.0) // add current point to polygon if IN
					clippedPolygon.push_back(q);
			} else if (d1 == 0.0) { // point is on the plane
				clippedPolygon.push_back(q);
				//cout << q << " is on the plane\n";
			} else {
				//cout << q << " is OUT\n";
			}
			lastPoint = q;
		}
		clippedPolygons.push_back(clippedPolygon);
	}

	//cout << "clipped polygons:\n";
//	BOOST_FOREACH(vector<Point> clippedPolygon, clippedPolygons) {
//		BOOST_FOREACH(Point cp, clippedPolygon) {
//			//cout << cp << " ";
//		}
//		//cout << endl;
//	}

	writePolygons(clippedPolygons, this->newfile);
}

Volume::Volume(token tok) {
	token::iterator itr = preparse(tok, *this);
	assert(itr != tok.end());

	cmatch what;
	regex exp("(.*)(\\.poly)");
	if (regex_match(((string)*itr).c_str(), what, exp)) { // file extension found
		newfile = what[0];
	} else { // append file extension
		newfile = ((string)*itr).append(polyextstr);
	}
	assert(++itr != tok.end());
	double px = atof((*itr).c_str());
	assert(++itr != tok.end());
	double py = atof((*itr).c_str());
	assert(++itr != tok.end());
	double pz = atof((*itr).c_str());
	assert(++itr != tok.end());
	double vx = atof((*itr).c_str());
	assert(++itr != tok.end());
	double vy = atof((*itr).c_str());
	assert(++itr != tok.end());
	double vz = atof((*itr).c_str());
	assert(++itr != tok.end());
	alpha = atof((*itr).c_str());
	assert(++itr != tok.end());
	resolution = atof((*itr).c_str());

	referencePoint = Point(px, py, pz);
	axis = Vector(vx, vy, vz);

	readPolygons(filename, &polygons);
}

void Volume::execute() {
	//cout << "Executing Volume\n";

	polygonList newPolygons;

	//cout << "\tTranslating to origin...\n";
	Point p1 = -referencePoint;
	Transformation translateTransform = Transformation::translate_by(p1.x(), p1.y(), p1.z());
	Transform* transformCmd = new Transform(polygons);
	polygons = transformCmd->transform(translateTransform, false);

	Transformation stepRotate = Transformation::rotate_by(resolution, axis);
	//cout << "\t" << stepRotate << endl;

	polygonList lastPolygons = polygons;
	double curRotated = 0.0;

	for (; curRotated <= alpha; curRotated += resolution) {
		//cout << "\tRotated by " << curRotated << " degrees\n";
		polygons = transformCmd->transform(stepRotate, false);
		for(unsigned int i = 0; i < polygons.size(); i++) {
			vector<Point> polygon = polygons[i];
			vector<Point> lastpolygon = lastPolygons[i];
			newPolygons.push_back(lastpolygon);
			if (curRotated + resolution >= alpha)
				newPolygons.push_back(polygon);

			for(unsigned int j = 0; j < polygon.size(); j++) {
				vector<Point> newSideFace;
				newSideFace.push_back(polygon[j]); // push 2 base points
				if (j+1 < polygon.size())
					newSideFace.push_back(polygon[j+1]);
				else
					newSideFace.push_back(polygon[0]);
				if (j+1 < lastpolygon.size()) // push 2 top face points
					newSideFace.push_back(lastpolygon[j+1]);
				else
					newSideFace.push_back(lastpolygon[0]);
				newSideFace.push_back(lastpolygon[j]);

				newPolygons.push_back(newSideFace); // add surfaces
			}

		}
		lastPolygons = polygons;
	}
	if (curRotated < alpha) {
		// generate one more volume to close surface
	}

	//cout << "\tTranslating back to original position...\n";
	Point p0 = referencePoint;
	translateTransform = Transformation::translate_by(p0.x(), p0.y(), p0.z());
	if (transformCmd) delete transformCmd;
	transformCmd = new Transform(newPolygons);
	newPolygons = transformCmd->transform(translateTransform, false);

	writePolygons(newPolygons, newfile);
}

Surface::Surface(token tok) {
	token::iterator itr = preparse(tok, *this);
	assert(itr != tok.end());
	texturefile = *itr;
	itr++;
	int level = atoi((*itr).c_str());
	assert(level >= 0);

	matrix<Point> patch(4,4);

	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			assert(++itr != tok.end());
			int x = atoi((*itr).c_str());
			assert(++itr != tok.end());
			int y = atoi((*itr).c_str());
			assert(++itr != tok.end());
			int z = atoi((*itr).c_str());
			patch(i,j) = Point(x,y,z);
		}
	}
	surface = new BezierPatch(patch);
	surface->refine(level);
}

void Surface::execute() {
	cout << "Executing Surface..." << endl;
	matrix<Point> patch = surface->flatten();
	for (unsigned int i = 0; i < patch.RowNo()-1; i++) {
		for (unsigned int j = 0; j < patch.ColNo()-1; j++) {
			vector<Point> polygon;
			vector<Point> texCoords;
			polygon.push_back(Point(patch(i,j))); // top left
			texCoords.push_back( Point(
					(double)j/((double)patch.ColNo()-1.0),
					1.0-(double)i/((double)patch.RowNo()-1.0) ) );
			polygon.push_back(Point(patch(i,j+1))); // top right
			texCoords.push_back( Point(
					(double)(j+1)/((double)patch.ColNo()-1.0),
					1.0-(double)i/((double)patch.RowNo()-1.0) ) );
			polygon.push_back(Point(patch(i+1,j+1))); // bottom right
			texCoords.push_back( Point(
					(double)(j+1)/((double)patch.ColNo()-1.0),
					1.0-(double)(i+1)/((double)patch.RowNo()-1.0) ) );
			polygon.push_back(Point(patch(i+1,j))); // bottom left
			texCoords.push_back( Point(
					(double)j/((double)patch.ColNo()-1.0),
					1.0-(double)(i+1)/((double)patch.RowNo()-1.0) ) );
			polygons.shape.push_back(polygon);
			polygons.texture.push_back(texCoords);
			polygons.texturefile = texturefile;
		}
	}
	writeSurface(polygons, filename, ios_base::trunc);
}

void Surface::writeSurface(TexShape polygons, string filename, ios_base::openmode mode) {
	boost::filesystem::create_directory(polypathstr);
	ofstream outfile((string(polypathstr) + "/" + filename).c_str(), mode);
	polygons.print(outfile);
	outfile.close();
	cout << (string(polypathstr) + "/" + filename).c_str() << " created\n";
}


//  (C) Copyright John Maddock 2008.
//  Use, modification and distribution are 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 BOOST_MATH_SPECIAL_NEXT_HPP
#define BOOST_MATH_SPECIAL_NEXT_HPP

#ifdef _MSC_VER
#pragma once
#endif
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/tools/traits.hpp>

#include <float.h>

#if !defined(_CRAYC) && !defined(__CUDACC__) && (!defined(__GNUC__) || (__GNUC__ > 3) || ((__GNUC__ == 3) && (__GNUC_MINOR__ > 3)))
#if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || defined(__SSE2__)
#include "xmmintrin.h"
#define BOOST_MATH_CHECK_SSE2
#endif
#endif

namespace boost{ namespace math{

   namespace concepts {

      class real_concept;
      class std_real_concept;

   }

namespace detail{

template <class T>
struct has_hidden_guard_digits;
template <>
struct has_hidden_guard_digits<float> : public std::false_type {};
template <>
struct has_hidden_guard_digits<double> : public std::false_type {};
template <>
struct has_hidden_guard_digits<long double> : public std::false_type {};
#ifdef BOOST_HAS_FLOAT128
template <>
struct has_hidden_guard_digits<__float128> : public std::false_type {};
#endif
template <>
struct has_hidden_guard_digits<boost::math::concepts::real_concept> : public std::false_type {};
template <>
struct has_hidden_guard_digits<boost::math::concepts::std_real_concept> : public std::false_type {};

template <class T, bool b>
struct has_hidden_guard_digits_10 : public std::false_type {};
template <class T>
struct has_hidden_guard_digits_10<T, true> : public std::integral_constant<bool, (std::numeric_limits<T>::digits10 != std::numeric_limits<T>::max_digits10)> {};

template <class T>
struct has_hidden_guard_digits
   : public has_hidden_guard_digits_10<T,
   std::numeric_limits<T>::is_specialized
   && (std::numeric_limits<T>::radix == 10) >
{};

template <class T>
inline const T& normalize_value(const T& val, const std::false_type&) { return val; }
template <class T>
inline T normalize_value(const T& val, const std::true_type&)
{
   BOOST_STATIC_ASSERT(std::numeric_limits<T>::is_specialized);
   BOOST_STATIC_ASSERT(std::numeric_limits<T>::radix != 2);

   boost::intmax_t shift = (boost::intmax_t)std::numeric_limits<T>::digits - (boost::intmax_t)ilogb(val) - 1;
   T result = scalbn(val, shift);
   result = round(result);
   return scalbn(result, -shift);
}

template <class T>
inline T get_smallest_value(std::true_type const&)
{
   //
   // numeric_limits lies about denorms being present - particularly
   // when this can be turned on or off at runtime, as is the case
   // when using the SSE2 registers in DAZ or FTZ mode.
   //
   static const T m = std::numeric_limits<T>::denorm_min();
#ifdef BOOST_MATH_CHECK_SSE2
   return (_mm_getcsr() & (_MM_FLUSH_ZERO_ON | 0x40)) ? tools::min_value<T>() : m;
#else
   return ((tools::min_value<T>() / 2) == 0) ? tools::min_value<T>() : m;
#endif
}

template <class T>
inline T get_smallest_value(std::false_type const&)
{
   return tools::min_value<T>();
}

template <class T>
inline T get_smallest_value()
{
#if defined(BOOST_MSVC) && (BOOST_MSVC <= 1310)
   return get_smallest_value<T>(std::integral_constant<bool, std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::has_denorm == 1)>());
#else
   return get_smallest_value<T>(std::integral_constant<bool, std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::has_denorm == std::denorm_present)>());
#endif
}

//
// Returns the smallest value that won't generate denorms when
// we calculate the value of the least-significant-bit:
//
template <class T>
T get_min_shift_value();

template <class T>
struct min_shift_initializer
{
   struct init
   {
      init()
      {
         do_init();
      }
      static void do_init()
      {
         get_min_shift_value<T>();
      }
      void force_instantiate()const{}
   };
   static const init initializer;
   static void force_instantiate()
   {
      initializer.force_instantiate();
   }
};

template <class T>
const typename min_shift_initializer<T>::init min_shift_initializer<T>::initializer;

template <class T>
inline T calc_min_shifted(const std::true_type&)
{
   BOOST_MATH_STD_USING
   return ldexp(tools::min_value<T>(), tools::digits<T>() + 1);
}
template <class T>
inline T calc_min_shifted(const std::false_type&)
{
   BOOST_STATIC_ASSERT(std::numeric_limits<T>::is_specialized);
   BOOST_STATIC_ASSERT(std::numeric_limits<T>::radix != 2);

   return scalbn(tools::min_value<T>(), std::numeric_limits<T>::digits + 1);
}

template <class T>
inline T get_min_shift_value()
{
   static const T val = calc_min_shifted<T>(std::integral_constant<bool, !std::numeric_limits<T>::is_specialized || std::numeric_limits<T>::radix == 2>());
   min_shift_initializer<T>::force_instantiate();

   return val;
}

template <class T, bool b = boost::math::tools::detail::has_backend_type<T>::value>
struct exponent_type
{
   typedef int type;
};

template <class T>
struct exponent_type<T, true>
{
   typedef typename T::backend_type::exponent_type type;
};

template <class T, class Policy>
T float_next_imp(const T& val, const std::true_type&, const Policy& pol)
{
   typedef typename exponent_type<T>::type exponent_type;

   BOOST_MATH_STD_USING
   exponent_type expon;
   static const char* function = "float_next<%1%>(%1%)";

   int fpclass = (boost::math::fpclassify)(val);

   if((fpclass == (int)FP_NAN) || (fpclass == (int)FP_INFINITE))
   {
      if(val < 0)
         return -tools::max_value<T>();
      return policies::raise_domain_error<T>(
         function,
         "Argument must be finite, but got %1%", val, pol);
   }

   if(val >= tools::max_value<T>())
      return policies::raise_overflow_error<T>(function, 0, pol);

   if(val == 0)
      return detail::get_smallest_value<T>();

   if((fpclass != (int)FP_SUBNORMAL) && (fpclass != (int)FP_ZERO) && (fabs(val) < detail::get_min_shift_value<T>()) && (val != -tools::min_value<T>()))
   {
      //
      // Special case: if the value of the least significant bit is a denorm, and the result
      // would not be a denorm, then shift the input, increment, and shift back.
      // This avoids issues with the Intel SSE2 registers when the FTZ or DAZ flags are set.
      //
      return ldexp(float_next(T(ldexp(val, 2 * tools::digits<T>())), pol), -2 * tools::digits<T>());
   }

   if(-0.5f == frexp(val, &expon))
      --expon; // reduce exponent when val is a power of two, and negative.
   T diff = ldexp(T(1), expon - tools::digits<T>());
   if(diff == 0)
      diff = detail::get_smallest_value<T>();
   return val + diff;
} // float_next_imp
//
// Special version for some base other than 2:
//
template <class T, class Policy>
T float_next_imp(const T& val, const std::false_type&, const Policy& pol)
{
   typedef typename exponent_type<T>::type exponent_type;

   BOOST_STATIC_ASSERT(std::numeric_limits<T>::is_specialized);
   BOOST_STATIC_ASSERT(std::numeric_limits<T>::radix != 2);

   BOOST_MATH_STD_USING
   exponent_type expon;
   static const char* function = "float_next<%1%>(%1%)";

   int fpclass = (boost::math::fpclassify)(val);

   if((fpclass == (int)FP_NAN) || (fpclass == (int)FP_INFINITE))
   {
      if(val < 0)
         return -tools::max_value<T>();
      return policies::raise_domain_error<T>(
         function,
         "Argument must be finite, but got %1%", val, pol);
   }

   if(val >= tools::max_value<T>())
      return policies::raise_overflow_error<T>(function, 0, pol);

   if(val == 0)
      return detail::get_smallest_value<T>();

   if((fpclass != (int)FP_SUBNORMAL) && (fpclass != (int)FP_ZERO) && (fabs(val) < detail::get_min_shift_value<T>()) && (val != -tools::min_value<T>()))
   {
      //
      // Special case: if the value of the least significant bit is a denorm, and the result
      // would not be a denorm, then shift the input, increment, and shift back.
      // This avoids issues with the Intel SSE2 registers when the FTZ or DAZ flags are set.
      //
      return scalbn(float_next(T(scalbn(val, 2 * std::numeric_limits<T>::digits)), pol), -2 * std::numeric_limits<T>::digits);
   }

   expon = 1 + ilogb(val);
   if(-1 == scalbn(val, -expon) * std::numeric_limits<T>::radix)
      --expon; // reduce exponent when val is a power of base, and negative.
   T diff = scalbn(T(1), expon - std::numeric_limits<T>::digits);
   if(diff == 0)
      diff = detail::get_smallest_value<T>();
   return val + diff;
} // float_next_imp

} // namespace detail

template <class T, class Policy>
inline typename tools::promote_args<T>::type float_next(const T& val, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   return detail::float_next_imp(detail::normalize_value(static_cast<result_type>(val), typename detail::has_hidden_guard_digits<result_type>::type()), std::integral_constant<bool, !std::numeric_limits<result_type>::is_specialized || (std::numeric_limits<result_type>::radix == 2)>(), pol);
}

#if 0 //def BOOST_MSVC
//
// We used to use ::_nextafter here, but doing so fails when using
// the SSE2 registers if the FTZ or DAZ flags are set, so use our own
// - albeit slower - code instead as at least that gives the correct answer.
//
template <class Policy>
inline double float_next(const double& val, const Policy& pol)
{
   static const char* function = "float_next<%1%>(%1%)";

   if(!(boost::math::isfinite)(val) && (val > 0))
      return policies::raise_domain_error<double>(
         function,
         "Argument must be finite, but got %1%", val, pol);

   if(val >= tools::max_value<double>())
      return policies::raise_overflow_error<double>(function, 0, pol);

   return ::_nextafter(val, tools::max_value<double>());
}
#endif

template <class T>
inline typename tools::promote_args<T>::type float_next(const T& val)
{
   return float_next(val, policies::policy<>());
}

namespace detail{

template <class T, class Policy>
T float_prior_imp(const T& val, const std::true_type&, const Policy& pol)
{
   typedef typename exponent_type<T>::type exponent_type;

   BOOST_MATH_STD_USING
   exponent_type expon;
   static const char* function = "float_prior<%1%>(%1%)";

   int fpclass = (boost::math::fpclassify)(val);

   if((fpclass == (int)FP_NAN) || (fpclass == (int)FP_INFINITE))
   {
      if(val > 0)
         return tools::max_value<T>();
      return policies::raise_domain_error<T>(
         function,
         "Argument must be finite, but got %1%", val, pol);
   }

   if(val <= -tools::max_value<T>())
      return -policies::raise_overflow_error<T>(function, 0, pol);

   if(val == 0)
      return -detail::get_smallest_value<T>();

   if((fpclass != (int)FP_SUBNORMAL) && (fpclass != (int)FP_ZERO) && (fabs(val) < detail::get_min_shift_value<T>()) && (val != tools::min_value<T>()))
   {
      //
      // Special case: if the value of the least significant bit is a denorm, and the result
      // would not be a denorm, then shift the input, increment, and shift back.
      // This avoids issues with the Intel SSE2 registers when the FTZ or DAZ flags are set.
      //
      return ldexp(float_prior(T(ldexp(val, 2 * tools::digits<T>())), pol), -2 * tools::digits<T>());
   }

   T remain = frexp(val, &expon);
   if(remain == 0.5f)
      --expon; // when val is a power of two we must reduce the exponent
   T diff = ldexp(T(1), expon - tools::digits<T>());
   if(diff == 0)
      diff = detail::get_smallest_value<T>();
   return val - diff;
} // float_prior_imp
//
// Special version for bases other than 2:
//
template <class T, class Policy>
T float_prior_imp(const T& val, const std::false_type&, const Policy& pol)
{
   typedef typename exponent_type<T>::type exponent_type;

   BOOST_STATIC_ASSERT(std::numeric_limits<T>::is_specialized);
   BOOST_STATIC_ASSERT(std::numeric_limits<T>::radix != 2);

   BOOST_MATH_STD_USING
   exponent_type expon;
   static const char* function = "float_prior<%1%>(%1%)";

   int fpclass = (boost::math::fpclassify)(val);

   if((fpclass == (int)FP_NAN) || (fpclass == (int)FP_INFINITE))
   {
      if(val > 0)
         return tools::max_value<T>();
      return policies::raise_domain_error<T>(
         function,
         "Argument must be finite, but got %1%", val, pol);
   }

   if(val <= -tools::max_value<T>())
      return -policies::raise_overflow_error<T>(function, 0, pol);

   if(val == 0)
      return -detail::get_smallest_value<T>();

   if((fpclass != (int)FP_SUBNORMAL) && (fpclass != (int)FP_ZERO) && (fabs(val) < detail::get_min_shift_value<T>()) && (val != tools::min_value<T>()))
   {
      //
      // Special case: if the value of the least significant bit is a denorm, and the result
      // would not be a denorm, then shift the input, increment, and shift back.
      // This avoids issues with the Intel SSE2 registers when the FTZ or DAZ flags are set.
      //
      return scalbn(float_prior(T(scalbn(val, 2 * std::numeric_limits<T>::digits)), pol), -2 * std::numeric_limits<T>::digits);
   }

   expon = 1 + ilogb(val);
   T remain = scalbn(val, -expon);
   if(remain * std::numeric_limits<T>::radix == 1)
      --expon; // when val is a power of two we must reduce the exponent
   T diff = scalbn(T(1), expon - std::numeric_limits<T>::digits);
   if(diff == 0)
      diff = detail::get_smallest_value<T>();
   return val - diff;
} // float_prior_imp

} // namespace detail

template <class T, class Policy>
inline typename tools::promote_args<T>::type float_prior(const T& val, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   return detail::float_prior_imp(detail::normalize_value(static_cast<result_type>(val), typename detail::has_hidden_guard_digits<result_type>::type()), std::integral_constant<bool, !std::numeric_limits<result_type>::is_specialized || (std::numeric_limits<result_type>::radix == 2)>(), pol);
}

#if 0 //def BOOST_MSVC
//
// We used to use ::_nextafter here, but doing so fails when using
// the SSE2 registers if the FTZ or DAZ flags are set, so use our own
// - albeit slower - code instead as at least that gives the correct answer.
//
template <class Policy>
inline double float_prior(const double& val, const Policy& pol)
{
   static const char* function = "float_prior<%1%>(%1%)";

   if(!(boost::math::isfinite)(val) && (val < 0))
      return policies::raise_domain_error<double>(
         function,
         "Argument must be finite, but got %1%", val, pol);

   if(val <= -tools::max_value<double>())
      return -policies::raise_overflow_error<double>(function, 0, pol);

   return ::_nextafter(val, -tools::max_value<double>());
}
#endif

template <class T>
inline typename tools::promote_args<T>::type float_prior(const T& val)
{
   return float_prior(val, policies::policy<>());
}

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type nextafter(const T& val, const U& direction, const Policy& pol)
{
   typedef typename tools::promote_args<T, U>::type result_type;
   return val < direction ? boost::math::float_next<result_type>(val, pol) : val == direction ? val : boost::math::float_prior<result_type>(val, pol);
}

template <class T, class U>
inline typename tools::promote_args<T, U>::type nextafter(const T& val, const U& direction)
{
   return nextafter(val, direction, policies::policy<>());
}

namespace detail{

template <class T, class Policy>
T float_distance_imp(const T& a, const T& b, const std::true_type&, const Policy& pol)
{
   BOOST_MATH_STD_USING
   //
   // Error handling:
   //
   static const char* function = "float_distance<%1%>(%1%, %1%)";
   if(!(boost::math::isfinite)(a))
      return policies::raise_domain_error<T>(
         function,
         "Argument a must be finite, but got %1%", a, pol);
   if(!(boost::math::isfinite)(b))
      return policies::raise_domain_error<T>(
         function,
         "Argument b must be finite, but got %1%", b, pol);
   //
   // Special cases:
   //
   if(a > b)
      return -float_distance(b, a, pol);
   if(a == b)
      return T(0);
   if(a == 0)
      return 1 + fabs(float_distance(static_cast<T>((b < 0) ? T(-detail::get_smallest_value<T>()) : detail::get_smallest_value<T>()), b, pol));
   if(b == 0)
      return 1 + fabs(float_distance(static_cast<T>((a < 0) ? T(-detail::get_smallest_value<T>()) : detail::get_smallest_value<T>()), a, pol));
   if(boost::math::sign(a) != boost::math::sign(b))
      return 2 + fabs(float_distance(static_cast<T>((b < 0) ? T(-detail::get_smallest_value<T>()) : detail::get_smallest_value<T>()), b, pol))
         + fabs(float_distance(static_cast<T>((a < 0) ? T(-detail::get_smallest_value<T>()) : detail::get_smallest_value<T>()), a, pol));
   //
   // By the time we get here, both a and b must have the same sign, we want
   // b > a and both positive for the following logic:
   //
   if(a < 0)
      return float_distance(static_cast<T>(-b), static_cast<T>(-a), pol);

   BOOST_ASSERT(a >= 0);
   BOOST_ASSERT(b >= a);

   int expon;
   //
   // Note that if a is a denorm then the usual formula fails
   // because we actually have fewer than tools::digits<T>()
   // significant bits in the representation:
   //
   (void)frexp(((boost::math::fpclassify)(a) == (int)FP_SUBNORMAL) ? tools::min_value<T>() : a, &expon);
   T upper = ldexp(T(1), expon);
   T result = T(0);
   //
   // If b is greater than upper, then we *must* split the calculation
   // as the size of the ULP changes with each order of magnitude change:
   //
   if(b > upper)
   {
      int expon2;
      (void)frexp(b, &expon2);
      T upper2 = ldexp(T(0.5), expon2);
      result = float_distance(upper2, b);
      result += (expon2 - expon - 1) * ldexp(T(1), tools::digits<T>() - 1);
   }
   //
   // Use compensated double-double addition to avoid rounding
   // errors in the subtraction:
   //
   expon = tools::digits<T>() - expon;
   T mb, x, y, z;
   if(((boost::math::fpclassify)(a) == (int)FP_SUBNORMAL) || (b - a < tools::min_value<T>()))
   {
      //
      // Special case - either one end of the range is a denormal, or else the difference is.
      // The regular code will fail if we're using the SSE2 registers on Intel and either
      // the FTZ or DAZ flags are set.
      //
      T a2 = ldexp(a, tools::digits<T>());
      T b2 = ldexp(b, tools::digits<T>());
      mb = -(std::min)(T(ldexp(upper, tools::digits<T>())), b2);
      x = a2 + mb;
      z = x - a2;
      y = (a2 - (x - z)) + (mb - z);

      expon -= tools::digits<T>();
   }
   else
   {
      mb = -(std::min)(upper, b);
      x = a + mb;
      z = x - a;
      y = (a - (x - z)) + (mb - z);
   }
   if(x < 0)
   {
      x = -x;
      y = -y;
   }
   result += ldexp(x, expon) + ldexp(y, expon);
   //
   // Result must be an integer:
   //
   BOOST_ASSERT(result == floor(result));
   return result;
} // float_distance_imp
//
// Special versions for bases other than 2:
//
template <class T, class Policy>
T float_distance_imp(const T& a, const T& b, const std::false_type&, const Policy& pol)
{
   BOOST_STATIC_ASSERT(std::numeric_limits<T>::is_specialized);
   BOOST_STATIC_ASSERT(std::numeric_limits<T>::radix != 2);

   BOOST_MATH_STD_USING
   //
   // Error handling:
   //
   static const char* function = "float_distance<%1%>(%1%, %1%)";
   if(!(boost::math::isfinite)(a))
      return policies::raise_domain_error<T>(
         function,
         "Argument a must be finite, but got %1%", a, pol);
   if(!(boost::math::isfinite)(b))
      return policies::raise_domain_error<T>(
         function,
         "Argument b must be finite, but got %1%", b, pol);
   //
   // Special cases:
   //
   if(a > b)
      return -float_distance(b, a, pol);
   if(a == b)
      return T(0);
   if(a == 0)
      return 1 + fabs(float_distance(static_cast<T>((b < 0) ? T(-detail::get_smallest_value<T>()) : detail::get_smallest_value<T>()), b, pol));
   if(b == 0)
      return 1 + fabs(float_distance(static_cast<T>((a < 0) ? T(-detail::get_smallest_value<T>()) : detail::get_smallest_value<T>()), a, pol));
   if(boost::math::sign(a) != boost::math::sign(b))
      return 2 + fabs(float_distance(static_cast<T>((b < 0) ? T(-detail::get_smallest_value<T>()) : detail::get_smallest_value<T>()), b, pol))
         + fabs(float_distance(static_cast<T>((a < 0) ? T(-detail::get_smallest_value<T>()) : detail::get_smallest_value<T>()), a, pol));
   //
   // By the time we get here, both a and b must have the same sign, we want
   // b > a and both positive for the following logic:
   //
   if(a < 0)
      return float_distance(static_cast<T>(-b), static_cast<T>(-a), pol);

   BOOST_ASSERT(a >= 0);
   BOOST_ASSERT(b >= a);

   boost::intmax_t expon;
   //
   // Note that if a is a denorm then the usual formula fails
   // because we actually have fewer than tools::digits<T>()
   // significant bits in the representation:
   //
   expon = 1 + ilogb(((boost::math::fpclassify)(a) == (int)FP_SUBNORMAL) ? tools::min_value<T>() : a);
   T upper = scalbn(T(1), expon);
   T result = T(0);
   //
   // If b is greater than upper, then we *must* split the calculation
   // as the size of the ULP changes with each order of magnitude change:
   //
   if(b > upper)
   {
      boost::intmax_t expon2 = 1 + ilogb(b);
      T upper2 = scalbn(T(1), expon2 - 1);
      result = float_distance(upper2, b);
      result += (expon2 - expon - 1) * scalbn(T(1), std::numeric_limits<T>::digits - 1);
   }
   //
   // Use compensated double-double addition to avoid rounding
   // errors in the subtraction:
   //
   expon = std::numeric_limits<T>::digits - expon;
   T mb, x, y, z;
   if(((boost::math::fpclassify)(a) == (int)FP_SUBNORMAL) || (b - a < tools::min_value<T>()))
   {
      //
      // Special case - either one end of the range is a denormal, or else the difference is.
      // The regular code will fail if we're using the SSE2 registers on Intel and either
      // the FTZ or DAZ flags are set.
      //
      T a2 = scalbn(a, std::numeric_limits<T>::digits);
      T b2 = scalbn(b, std::numeric_limits<T>::digits);
      mb = -(std::min)(T(scalbn(upper, std::numeric_limits<T>::digits)), b2);
      x = a2 + mb;
      z = x - a2;
      y = (a2 - (x - z)) + (mb - z);

      expon -= std::numeric_limits<T>::digits;
   }
   else
   {
      mb = -(std::min)(upper, b);
      x = a + mb;
      z = x - a;
      y = (a - (x - z)) + (mb - z);
   }
   if(x < 0)
   {
      x = -x;
      y = -y;
   }
   result += scalbn(x, expon) + scalbn(y, expon);
   //
   // Result must be an integer:
   //
   BOOST_ASSERT(result == floor(result));
   return result;
} // float_distance_imp

} // namespace detail

template <class T, class U, class Policy>
inline typename tools::promote_args<T, U>::type float_distance(const T& a, const U& b, const Policy& pol)
{
   //
   // We allow ONE of a and b to be an integer type, otherwise both must be the SAME type.
   //
   BOOST_STATIC_ASSERT_MSG(
      (boost::is_same<T, U>::value
      || (boost::is_integral<T>::value && !boost::is_integral<U>::value)
      || (!boost::is_integral<T>::value && boost::is_integral<U>::value)
      || (std::numeric_limits<T>::is_specialized && std::numeric_limits<U>::is_specialized
         && (std::numeric_limits<T>::digits == std::numeric_limits<U>::digits)
         && (std::numeric_limits<T>::radix == std::numeric_limits<U>::radix)
         && !std::numeric_limits<T>::is_integer && !std::numeric_limits<U>::is_integer)),
      "Float distance between two different floating point types is undefined.");

   BOOST_IF_CONSTEXPR (!boost::is_same<T, U>::value)
   {
      BOOST_IF_CONSTEXPR(boost::is_integral<T>::value)
      {
         return float_distance(static_cast<U>(a), b, pol);
      }
      else
      {
         return float_distance(a, static_cast<T>(b), pol);
      }
   }
   else
   {
      typedef typename tools::promote_args<T, U>::type result_type;
      return detail::float_distance_imp(detail::normalize_value(static_cast<result_type>(a), typename detail::has_hidden_guard_digits<result_type>::type()), detail::normalize_value(static_cast<result_type>(b), typename detail::has_hidden_guard_digits<result_type>::type()), std::integral_constant<bool, !std::numeric_limits<result_type>::is_specialized || (std::numeric_limits<result_type>::radix == 2)>(), pol);
   }
}

template <class T, class U>
typename tools::promote_args<T, U>::type float_distance(const T& a, const U& b)
{
   return boost::math::float_distance(a, b, policies::policy<>());
}

namespace detail{

template <class T, class Policy>
T float_advance_imp(T val, int distance, const std::true_type&, const Policy& pol)
{
   BOOST_MATH_STD_USING
   //
   // Error handling:
   //
   static const char* function = "float_advance<%1%>(%1%, int)";

   int fpclass = (boost::math::fpclassify)(val);

   if((fpclass == (int)FP_NAN) || (fpclass == (int)FP_INFINITE))
      return policies::raise_domain_error<T>(
         function,
         "Argument val must be finite, but got %1%", val, pol);

   if(val < 0)
      return -float_advance(-val, -distance, pol);
   if(distance == 0)
      return val;
   if(distance == 1)
      return float_next(val, pol);
   if(distance == -1)
      return float_prior(val, pol);

   if(fabs(val) < detail::get_min_shift_value<T>())
   {
      //
      // Special case: if the value of the least significant bit is a denorm,
      // implement in terms of float_next/float_prior.
      // This avoids issues with the Intel SSE2 registers when the FTZ or DAZ flags are set.
      //
      if(distance > 0)
      {
         do{ val = float_next(val, pol); } while(--distance);
      }
      else
      {
         do{ val = float_prior(val, pol); } while(++distance);
      }
      return val;
   }

   int expon;
   (void)frexp(val, &expon);
   T limit = ldexp((distance < 0 ? T(0.5f) : T(1)), expon);
   if(val <= tools::min_value<T>())
   {
      limit = sign(T(distance)) * tools::min_value<T>();
   }
   T limit_distance = float_distance(val, limit);
   while(fabs(limit_distance) < abs(distance))
   {
      distance -= itrunc(limit_distance);
      val = limit;
      if(distance < 0)
      {
         limit /= 2;
         expon--;
      }
      else
      {
         limit *= 2;
         expon++;
      }
      limit_distance = float_distance(val, limit);
      if(distance && (limit_distance == 0))
      {
         return policies::raise_evaluation_error<T>(function, "Internal logic failed while trying to increment floating point value %1%: most likely your FPU is in non-IEEE conforming mode.", val, pol);
      }
   }
   if((0.5f == frexp(val, &expon)) && (distance < 0))
      --expon;
   T diff = 0;
   if(val != 0)
      diff = distance * ldexp(T(1), expon - tools::digits<T>());
   if(diff == 0)
      diff = distance * detail::get_smallest_value<T>();
   return val += diff;
} // float_advance_imp
//
// Special version for bases other than 2:
//
template <class T, class Policy>
T float_advance_imp(T val, int distance, const std::false_type&, const Policy& pol)
{
   BOOST_STATIC_ASSERT(std::numeric_limits<T>::is_specialized);
   BOOST_STATIC_ASSERT(std::numeric_limits<T>::radix != 2);

   BOOST_MATH_STD_USING
   //
   // Error handling:
   //
   static const char* function = "float_advance<%1%>(%1%, int)";

   int fpclass = (boost::math::fpclassify)(val);

   if((fpclass == (int)FP_NAN) || (fpclass == (int)FP_INFINITE))
      return policies::raise_domain_error<T>(
         function,
         "Argument val must be finite, but got %1%", val, pol);

   if(val < 0)
      return -float_advance(-val, -distance, pol);
   if(distance == 0)
      return val;
   if(distance == 1)
      return float_next(val, pol);
   if(distance == -1)
      return float_prior(val, pol);

   if(fabs(val) < detail::get_min_shift_value<T>())
   {
      //
      // Special case: if the value of the least significant bit is a denorm,
      // implement in terms of float_next/float_prior.
      // This avoids issues with the Intel SSE2 registers when the FTZ or DAZ flags are set.
      //
      if(distance > 0)
      {
         do{ val = float_next(val, pol); } while(--distance);
      }
      else
      {
         do{ val = float_prior(val, pol); } while(++distance);
      }
      return val;
   }

   boost::intmax_t expon = 1 + ilogb(val);
   T limit = scalbn(T(1), distance < 0 ? expon - 1 : expon);
   if(val <= tools::min_value<T>())
   {
      limit = sign(T(distance)) * tools::min_value<T>();
   }
   T limit_distance = float_distance(val, limit);
   while(fabs(limit_distance) < abs(distance))
   {
      distance -= itrunc(limit_distance);
      val = limit;
      if(distance < 0)
      {
         limit /= std::numeric_limits<T>::radix;
         expon--;
      }
      else
      {
         limit *= std::numeric_limits<T>::radix;
         expon++;
      }
      limit_distance = float_distance(val, limit);
      if(distance && (limit_distance == 0))
      {
         return policies::raise_evaluation_error<T>(function, "Internal logic failed while trying to increment floating point value %1%: most likely your FPU is in non-IEEE conforming mode.", val, pol);
      }
   }
   /*expon = 1 + ilogb(val);
   if((1 == scalbn(val, 1 + expon)) && (distance < 0))
      --expon;*/
   T diff = 0;
   if(val != 0)
      diff = distance * scalbn(T(1), expon - std::numeric_limits<T>::digits);
   if(diff == 0)
      diff = distance * detail::get_smallest_value<T>();
   return val += diff;
} // float_advance_imp

} // namespace detail

template <class T, class Policy>
inline typename tools::promote_args<T>::type float_advance(T val, int distance, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   return detail::float_advance_imp(detail::normalize_value(static_cast<result_type>(val), typename detail::has_hidden_guard_digits<result_type>::type()), distance, std::integral_constant<bool, !std::numeric_limits<result_type>::is_specialized || (std::numeric_limits<result_type>::radix == 2)>(), pol);
}

template <class T>
inline typename tools::promote_args<T>::type float_advance(const T& val, int distance)
{
   return boost::math::float_advance(val, distance, policies::policy<>());
}

}} // boost math namespaces

#endif // BOOST_MATH_SPECIAL_NEXT_HPP


/* Hector -- A Simple Climate Model
   Copyright (C) 2014-2015  Battelle Memorial Institute

   Please see the accompanying file LICENSE.md for additional licensing
   information.
*/
/*
 *  forcing_component.cpp
 *  hector
 *
 *  Created by Ben on 02 March 2011.
 *
 */

#include <boost/array.hpp>
#include <math.h>

#include "forcing_component.hpp"
#include "halocarbon_component.hpp"
#include "core.hpp"
#include "dependency_finder.hpp"
#include "h_util.hpp"
#include "avisitor.hpp"

namespace Hector {

/* These next two arrays and the map that connects them are a
 * workaround for the problems created by storing the halocarbon
 * forcings in the halocarbon components.  Because the halocarbon
 * components don't know about the base year adjustments, they can't
 * provide the forcings relative to the base year, which is what
 * outside callers will generally want.  Internally, however, we still
 * need to be able to get the raw forcings from the halocarbon
 * components, so we can't just change everything to point at the
 * forcing component (which would return the base year adjusted value).
 *
 * The solution we adopted was to create a second set of capabilities
 * to refer to the adjusted values, and we let the forcing component
 * intercept those.  However, the forcing values themselves are stored
 * under the names used for the unadjusted values, so we have to have
 * a name translation table so that we can find the data that the
 * message is asking for.  In the end, the whole process winds up
 * being a little ugly, but it gets the job done.
 */

const char *ForcingComponent::adjusted_halo_forcings[N_HALO_FORCINGS] = {
    D_RFADJ_CF4,
    D_RFADJ_C2F6,
    D_RFADJ_HFC23,
    D_RFADJ_HFC32,
    D_RFADJ_HFC4310,
    D_RFADJ_HFC125,
    D_RFADJ_HFC134a,
    D_RFADJ_HFC143a,
    D_RFADJ_HFC227ea,
    D_RFADJ_HFC245fa,
    D_RFADJ_SF6,
    D_RFADJ_CFC11,
    D_RFADJ_CFC12,
    D_RFADJ_CFC113,
    D_RFADJ_CFC114,
    D_RFADJ_CFC115,
    D_RFADJ_CCl4,
    D_RFADJ_CH3CCl3,
    D_RFADJ_HCF22,
    D_RFADJ_HCF141b,
    D_RFADJ_HCF142b,
    D_RFADJ_halon1211,
    D_RFADJ_halon1301,
    D_RFADJ_halon2402,
    D_RFADJ_CH3Cl,
    D_RFADJ_CH3Br
};

const char *ForcingComponent::halo_forcing_names[N_HALO_FORCINGS] = {
    D_RF_CF4,
    D_RF_C2F6,
    D_RF_HFC23,
    D_RF_HFC32,
    D_RF_HFC4310,
    D_RF_HFC125,
    D_RF_HFC134a,
    D_RF_HFC143a,
    D_RF_HFC227ea,
    D_RF_HFC245fa,
    D_RF_SF6,
    D_RF_CFC11,
    D_RF_CFC12,
    D_RF_CFC113,
    D_RF_CFC114,
    D_RF_CFC115,
    D_RF_CCl4,
    D_RF_CH3CCl3,
    D_RF_HCF22,
    D_RF_HCF141b,
    D_RF_HCF142b,
    D_RF_halon1211,
    D_RF_halon1301,
    D_RF_halon2402,
    D_RF_CH3Cl,
    D_RF_CH3Br
};

std::map<std::string, std::string> ForcingComponent::forcing_name_map;

using namespace std;

//------------------------------------------------------------------------------
/*! \brief Constructor
 */
ForcingComponent::ForcingComponent() {
}

//------------------------------------------------------------------------------
/*! \brief Destructor
 */
ForcingComponent::~ForcingComponent() {
}

//------------------------------------------------------------------------------
// documentation is inherited
string ForcingComponent::getComponentName() const {
    const string name = FORCING_COMPONENT_NAME;
    return name;
}

//------------------------------------------------------------------------------
// documentation is inherited
void ForcingComponent::init( Core* coreptr ) {

    logger.open( getComponentName(), false, coreptr->getGlobalLogger().getEchoToFile(), coreptr->getGlobalLogger().getMinLogLevel() );
    H_LOG( logger, Logger::DEBUG ) << "hello " << getComponentName() << std::endl;

    core = coreptr;

    baseyear = 0.0;
    currentYear = 0.0;

    Ftot_constrain.allowInterp( true );
    Ftot_constrain.name = D_RF_TOTAL;

    // Register the data we can provide
    core->registerCapability( D_RF_TOTAL, getComponentName() );
    core->registerCapability( D_RF_BASEYEAR, getComponentName() );
    core->registerCapability( D_RF_CO2, getComponentName());
    core->registerCapability( D_RF_CH4, getComponentName());
    core->registerCapability( D_RF_N2O, getComponentName());
    core->registerCapability( D_RF_H2O, getComponentName());
    core->registerCapability( D_RF_O3, getComponentName());
    core->registerCapability( D_RF_BC, getComponentName());
    core->registerCapability( D_RF_OC, getComponentName());
    core->registerCapability( D_RF_SO2d, getComponentName());
    core->registerCapability( D_RF_SO2i, getComponentName());
    core->registerCapability( D_RF_SO2, getComponentName());
    core->registerCapability( D_RF_VOL, getComponentName());
    for(int i=0; i<N_HALO_FORCINGS; ++i) {
        core->registerCapability(adjusted_halo_forcings[i], getComponentName());
        forcing_name_map[adjusted_halo_forcings[i]] = halo_forcing_names[i];
    }

    // Register our dependencies

    core->registerDependency( D_ATMOSPHERIC_CH4, getComponentName() );
    core->registerDependency( D_ATMOSPHERIC_CO2, getComponentName() );
    core->registerDependency( D_ATMOSPHERIC_O3, getComponentName() );
    core->registerDependency( D_EMISSIONS_BC, getComponentName() );
    core->registerDependency( D_EMISSIONS_OC, getComponentName() );
    core->registerDependency( D_NATURAL_SO2, getComponentName() );
    core->registerDependency( D_ATMOSPHERIC_N2O, getComponentName() );

    core->registerDependency( D_RF_CF4, getComponentName() );
    core->registerDependency( D_RF_C2F6, getComponentName() );
    core->registerDependency( D_RF_HFC23, getComponentName() );
    core->registerDependency( D_RF_HFC32, getComponentName() );
    core->registerDependency( D_RF_HFC4310, getComponentName() );
    core->registerDependency( D_RF_HFC125, getComponentName() );
    core->registerDependency( D_RF_HFC134a, getComponentName() );
    core->registerDependency( D_RF_HFC143a, getComponentName() );
    core->registerDependency( D_RF_HFC227ea, getComponentName() );
    core->registerDependency( D_RF_HFC245fa, getComponentName() );
    core->registerDependency( D_RF_SF6, getComponentName() );
    core->registerDependency( D_RF_CFC11, getComponentName() );
    core->registerDependency( D_RF_CFC12, getComponentName() );
    core->registerDependency( D_RF_CFC113, getComponentName() );
    core->registerDependency( D_RF_CFC114, getComponentName() );
    core->registerDependency( D_RF_CFC115, getComponentName() );
    core->registerDependency( D_RF_CCl4, getComponentName() );
    core->registerDependency( D_RF_CH3CCl3, getComponentName() );
    core->registerDependency( D_RF_HCF22, getComponentName() );
    core->registerDependency( D_RF_HCF141b, getComponentName() );
    core->registerDependency( D_RF_HCF142b, getComponentName() );
    core->registerDependency( D_RF_halon1211, getComponentName() );
    core->registerDependency( D_RF_halon1301, getComponentName() );
    core->registerDependency( D_RF_halon2402, getComponentName() );
    core->registerDependency( D_RF_CH3Br, getComponentName() );
    core->registerDependency( D_RF_CH3Cl, getComponentName() );
    core->registerDependency( D_RF_T_ALBEDO, getComponentName() );
}

//------------------------------------------------------------------------------
// documentation is inherited
unitval ForcingComponent::sendMessage( const std::string& message,
                                      const std::string& datum,
                                      const message_data info ) throw ( h_exception )
{
    unitval returnval;

    if( message==M_GETDATA ) {          //! Caller is requesting data
        return getData( datum, info.date );

    } else if( message==M_SETDATA ) {   //! Caller is requesting to set data
        //TODO: call setData below
        //TODO: change core so that parsing is routed through sendMessage
        //TODO: make setData private

    } else {                        //! We don't handle any other messages
        H_THROW( "Caller sent unknown message: "+message );
    }

    return returnval;
}

//------------------------------------------------------------------------------
// documentation is inherited
void ForcingComponent::setData( const string& varName,
                                const message_data& data ) throw ( h_exception )
{
    H_LOG( logger, Logger::DEBUG ) << "Setting " << varName << "[" << data.date << "]=" << data.value_str << std::endl;

    try {
        if( varName == D_RF_BASEYEAR ) {
            H_ASSERT( data.date == Core::undefinedIndex(), "date not allowed" );
            baseyear = data.getUnitval(U_UNDEFINED);
        } else if( varName == D_FTOT_CONSTRAIN ) {
            H_ASSERT( data.date != Core::undefinedIndex(), "date required" );
            Ftot_constrain.set(data.date, data.getUnitval(U_W_M2));
        } else {
            H_LOG( logger, Logger::DEBUG ) << "Unknown variable " << varName << std::endl;
            H_THROW( "Unknown variable name while parsing "+ getComponentName() + ": "
                    + varName );
        }
    } catch( h_exception& parseException ) {
        H_RETHROW( parseException, "Could not parse var: "+varName );
    }
}

//------------------------------------------------------------------------------
// documentation is inherited
void ForcingComponent::prepareToRun() throw ( h_exception ) {

    H_LOG( logger, Logger::DEBUG ) << "prepareToRun " << std::endl;

    if( baseyear==0.0 )
        baseyear = core->getStartDate() + 1;        // default, if not supplied by user
    H_LOG( logger, Logger::DEBUG ) << "Base year for reporting is " << baseyear << std::endl;

    H_ASSERT( baseyear > core->getStartDate(), "Base year must be >= model start date" );

    if( Ftot_constrain.size() ) {
        Logger& glog = core->getGlobalLogger();
        H_LOG( glog, Logger::WARNING ) << "Total forcing will be overwritten by user-supplied values!" << std::endl;
    }

    baseyear_forcings.clear();
}

//------------------------------------------------------------------------------
// documentation is inherited
void ForcingComponent::run( const double runToDate ) throw ( h_exception ) {

    // Calculate instantaneous radiative forcing for any & all agents
    // As each is computed, push it into 'forcings' map for Ftot calculation.
	// Note that forcings have to be mutually exclusive, there are no subtotals for different species.
    H_LOG( logger, Logger::DEBUG ) << "-----------------------------" << std::endl;
    currentYear = runToDate;

    if( runToDate < baseyear ) {
        H_LOG( logger, Logger::DEBUG ) << "not yet at baseyear" << std::endl;
    } else {
        forcings_t forcings;

        // ---------- CO2 ----------
        // Instantaneous radiative forcings for CO2, CH4, and N2O from http://www.esrl.noaa.gov/gmd/aggi/
        // These are in turn from IPCC (2001)

        // This is identical to that of MAGICC; see Meinshausen et al. (2011)
        unitval Ca = core->sendMessage( M_GETDATA, D_ATMOSPHERIC_CO2 );
        if( runToDate==baseyear )
            C0 = Ca;
        forcings[D_RF_CO2 ].set( 5.35 * log( Ca/C0 ), U_W_M2 );

        // ---------- Terrestrial albedo ----------
        if( core->checkCapability( D_RF_T_ALBEDO ) ) {
            forcings[ D_RF_T_ALBEDO ] = core->sendMessage( M_GETDATA, D_RF_T_ALBEDO, message_data( runToDate ) );
        }

        // ---------- N2O and CH4 ----------
        // Equations from Joos et al., 2001
        if( core->checkCapability( D_ATMOSPHERIC_CH4 ) && core->checkCapability( D_ATMOSPHERIC_N2O ) ) {

#define f(M,N) 0.47 * log( 1 + 2.01 * 1e-5 * pow( M * N, 0.75 ) + 5.31 * 1e-15 * M * pow( M * N, 1.52 ) )
            double Ma = core->sendMessage( M_GETDATA, D_ATMOSPHERIC_CH4, message_data( runToDate ) ).value( U_PPBV_CH4 );
            double M0 = core->sendMessage( M_GETDATA, D_PREINDUSTRIAL_CH4 ).value( U_PPBV_CH4 );
            double Na = core->sendMessage( M_GETDATA, D_ATMOSPHERIC_N2O, message_data( runToDate ) ).value( U_PPBV_N2O );
            double N0 = core->sendMessage( M_GETDATA, D_PREINDUSTRIAL_N2O ).value( U_PPBV_N2O );

            double fch4 =  0.036 * ( sqrt( Ma ) - sqrt( M0 ) ) - ( f( Ma, N0 ) - f( M0, N0 ) );
            forcings[D_RF_CH4].set( fch4, U_W_M2 );

            double fn2o =  0.12 * ( sqrt( Na ) - sqrt( N0 ) ) - ( f( M0, Na ) - f( M0, N0 ) );
            forcings[D_RF_N2O].set( fn2o, U_W_M2 );

            // ---------- Stratospheric H2O from CH4 oxidation ----------
            // From Tanaka et al, 2007, but using Joos et al., 2001 value of 0.05
            const double fh2o = 0.05 * ( 0.036 * ( sqrt( Ma ) - sqrt( M0 ) ) );
            forcings[D_RF_H2O].set( fh2o, U_W_M2 );
        }

        // ---------- Troposheric Ozone ----------
        if( core->checkCapability( D_ATMOSPHERIC_O3 ) ) {
            //from Tanaka et al, 2007
            const double ozone = core->sendMessage( M_GETDATA, D_ATMOSPHERIC_O3, message_data( runToDate ) ).value( U_DU_O3 );
            const double fo3 = 0.042 * ozone;
            forcings[D_RF_O3].set( fo3, U_W_M2 );
        }

        // ---------- Halocarbons ----------
        // TODO: Would like to just 'know' all the halocarbon instances out there
        boost::array<string, 26> halos = {
            {
                D_RF_CF4,
                D_RF_C2F6,
                D_RF_HFC23,
                D_RF_HFC32,
                D_RF_HFC4310,
                D_RF_HFC125,
                D_RF_HFC134a,
                D_RF_HFC143a,
                D_RF_HFC227ea,
                D_RF_HFC245fa,
                D_RF_SF6,
                D_RF_CFC11,
                D_RF_CFC12,
                D_RF_CFC113,
                D_RF_CFC114,
                D_RF_CFC115,
                D_RF_CCl4,
                D_RF_CH3CCl3,
                D_RF_HCF22,
                D_RF_HCF141b,
                D_RF_HCF142b,
                D_RF_halon1211,
                D_RF_halon1301,
                D_RF_halon2402,
                D_RF_CH3Cl,
                D_RF_CH3Br
            }
        };

        // Halocarbons can be disabled individually via the input file, so we run through all possible ones
         for (unsigned hc=0; hc<halos.size(); ++hc) {
            if( core->checkCapability( halos[hc] ) ) {
                // Forcing values are actually computed by the halocarbon itself
                forcings[ halos[hc] ] = core->sendMessage( M_GETDATA, halos[hc], message_data( runToDate ) );
                }
        }

        // ---------- Black carbon ----------
        if( core->checkCapability( D_EMISSIONS_BC ) ) {
            double fbc = 0.0743 * core->sendMessage( M_GETDATA, D_EMISSIONS_BC, message_data( runToDate ) ).value( U_TG );
            forcings[D_RF_BC].set( fbc, U_W_M2 );
            // includes both indirect and direct forcings from Bond et al 2013, Journal of Geophysical Research Atmo (table C1 - Central)
        }

        // ---------- Organic carbon ----------
        if( core->checkCapability( D_EMISSIONS_OC ) ) {
            double foc = -0.0128 * core->sendMessage( M_GETDATA, D_EMISSIONS_OC, message_data( runToDate ) ).value( U_TG );
            forcings[D_RF_OC].set( foc, U_W_M2 );
            // includes both indirect and direct forcings from Bond et al 2013, Journal of Geophysical Research Atmo (table C1 - Central).
            // The fossil fuel and biomass are weighted (-4.5) then added to the snow and clouds for a total of -12.8 (personal communication Steve Smith, PNNL)
        }

        // ---------- Sulphate Aerosols ----------
        if( core->checkCapability( D_NATURAL_SO2 ) && core->checkCapability( D_EMISSIONS_SO2 ) ) {

            unitval S0 = core->sendMessage( M_GETDATA, D_2000_SO2 );
            unitval SN = core->sendMessage( M_GETDATA, D_NATURAL_SO2 );

            // Includes only direct forcings from Forster et al 2007 (IPCC)
            // Equations from Joos et al., 2001
            H_ASSERT( S0.value( U_GG_S ) >0, "S0 is 0" );
            unitval emission = core->sendMessage( M_GETDATA, D_EMISSIONS_SO2, message_data( runToDate ) );
            double fso2d = -0.35 * emission/S0;
            forcings[D_RF_SO2d].set( fso2d, U_W_M2 );
            // includes only direct forcings from Forster etal 2007 (IPCC)

            // Indirect aerosol effect via changes in cloud properties
            const double a = -0.6 * ( log( ( SN.value( U_GG_S ) + emission.value( U_GG_S ) ) / SN.value( U_GG_S ) ) ); // -.6
            const double b =  pow ( log ( ( SN.value( U_GG_S ) + S0.value( U_GG_S ) ) / SN.value( U_GG_S ) ), -1 );
            double fso2i = a * b;
            forcings[D_RF_SO2i].set( fso2i, U_W_M2 );
        }

        if( core->checkCapability( D_VOLCANIC_SO2 ) ) {
            // Volcanic forcings
            forcings[D_RF_VOL] = core->sendMessage( M_GETDATA, D_VOLCANIC_SO2, message_data( runToDate ) );
        }

        // ---------- Total ----------
        unitval Ftot( 0.0, U_W_M2 );  // W/m2
        for( forcingsIterator it = forcings.begin(); it != forcings.end(); ++it ) {
            Ftot = Ftot + ( *it ).second;
            H_LOG( logger, Logger::DEBUG ) << "forcing " << ( *it).first << " in " << runToDate << " is " << ( *it ).second << std::endl;
        }

        // If the user has supplied total forcing data, use that
        if( Ftot_constrain.size() && runToDate <= Ftot_constrain.lastdate() ) {
            H_LOG( logger, Logger::WARNING ) << "** Overwriting total forcing with user-supplied value" << std::endl;
            forcings[ D_RF_TOTAL ] = Ftot_constrain.get( runToDate );
        } else {
            forcings[ D_RF_TOTAL ] = Ftot;
        }
        H_LOG( logger, Logger::DEBUG ) << "forcing total is " << forcings[ D_RF_TOTAL ] << std::endl;

        //---------- Change to relative forcing ----------
        // Note that the code below assumes model is always consistently run from base-year forward.
        // Results will not be consistent if parameters are changed but base-year is not re-run.

       // At this point, we've computed all absolute forcings. If base year, save those values
        if( runToDate==baseyear ) {
            H_LOG( logger, Logger::DEBUG ) << "** At base year! Storing current forcing values" << std::endl;
            baseyear_forcings = forcings;
        }

        // Subtract base year forcing values from forcings, i.e. make them relative to base year
        for( forcingsIterator it = forcings.begin(); it != forcings.end(); ++it ) {
            forcings[ ( *it ).first ] = ( *it ).second - baseyear_forcings[ ( *it ).first ];
        }

        // Store the forcings that we have calculated
        forcings_ts.set(runToDate, forcings);
    }
}

//------------------------------------------------------------------------------
// documentation is inherited
unitval ForcingComponent::getData( const std::string& varName,
                                  const double date ) throw ( h_exception ) {

    unitval returnval;
    double getdate = date;             // This is why I hate declaring PBV args as const!

    if(getdate == Core::undefinedIndex()) {
        // If no date specified, provide the current date
        getdate = currentYear;
    }

    if(getdate < baseyear) {
        // Forcing component hasn't run yet, so there is no data to get.
        returnval.set(0.0, U_W_M2);
        return returnval;
    }

    H_LOG(logger, Logger::DEBUG) << "getData request, time= "
                                 << getdate
                                 << "  baseyear = "
                                 << baseyear
                                 << std::endl;

    forcings_t forcings(forcings_ts.get(getdate));

    if( varName == D_RF_BASEYEAR ) {
        returnval.set( baseyear, U_UNITLESS );
    } else if (varName == D_RF_SO2) {
        // total SO2 forcing
        std::map<std::string, unitval>::const_iterator forcing_SO2d = forcings.find( D_RF_SO2d );
        std::map<std::string, unitval>::const_iterator forcing_SO2i = forcings.find( D_RF_SO2i );
        if ( forcing_SO2d != forcings.end() ) {
            if ( forcing_SO2i != forcings.end() ) {
                returnval = forcing_SO2d->second + forcing_SO2i->second;
            } else {
                returnval = forcing_SO2d->second;
            }
        } else {
            if ( forcing_SO2i != forcings.end() ) {
                returnval = forcing_SO2i->second;
            } else {
                returnval.set( 0.0, U_W_M2 );
            }
        }
    } else {
        std::string forcing_name;
        auto forcit = forcing_name_map.find(varName);
        if(forcit != forcing_name_map.end()) {
            forcing_name = forcing_name_map[varName];
        }
        else {
            forcing_name = varName;
        }
        std::map<std::string, unitval>::const_iterator forcing = forcings.find(forcing_name);
        if ( forcing != forcings.end() ) {
            // from the forcing map
            returnval = forcing->second;
        } else {
            if (currentYear < baseyear) {
                returnval.set( 0.0, U_W_M2 );
            } else {
                H_THROW( "Caller is requesting unknown variable: " + varName );
            }
        }
    }

    return returnval;
}

void ForcingComponent::reset(double time) throw(h_exception)
{
    // Set the current year to the reset year, and drop outputs after the reset year.
    currentYear = time;
    forcings_ts.truncate(time);
    H_LOG(logger, Logger::NOTICE)
        << getComponentName() << " reset to time= " << time << "\n";
}

//------------------------------------------------------------------------------
// documentation is inherited
void ForcingComponent::shutDown() {
	H_LOG( logger, Logger::DEBUG ) << "goodbye " << getComponentName() << std::endl;
    logger.close();
}

//------------------------------------------------------------------------------
// documentation is inherited
void ForcingComponent::accept( AVisitor* visitor ) {
    visitor->visit( this );
}

}


/*  $Id$
 *
 *  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <list>
// uncomment to disable assert() calls
// #define NDEBUG
#include <cassert>

#include <boost/python.hpp>
#include <boost/foreach.hpp>

#include "point.h"
#include "triangle.h"
#include "millingcutter.h"
#include "numeric.h"
#include "octree.h"
#include "ocode.h"

//#define DEBUG_BUILD_OCT

namespace ocl
{

//**************** LinOCT ********************/

LinOCT::LinOCT() {
    clist.clear();
}

int LinOCT::size() const {
    return clist.size();
}

void LinOCT::append(Ocode& code) {
    clist.push_back( code );
}

void LinOCT::delete_at(std::list<Ocode>::iterator& it) {
    clist.erase( it );
}

void LinOCT::expand_at(std::list<Ocode>::iterator& itr) {
    // note: there was a valid-index check here
    // consider checking that itr is valid?
       // if ( itr->expandable()) {  // was: clist[idx].expandable()
            std::list<Ocode> newnodes = itr->expand(); // the new nodes  clist[idx]
            BOOST_FOREACH( Ocode o, newnodes) {
                clist.insert(itr, o);
            }

            // delete old node
            std::list<Ocode>::iterator temp;
            temp = itr;
            itr--; // jump out of the way from erase()
            clist.erase(temp);
        /*} else {
            std::cout << "LinOCT::expand_at() cannot expand " << *itr << "!\n";
        }*/
    return;
}

/// initialize octree and expand min_expans times
void LinOCT::init(int min_expand)
{
    // assume the list is empty.
    if (size() > 0) {
        std::cout << "cannot call LinOCT::init() on non-empty tree! \n";
        assert(0);
    }

    Ocode o = Ocode(); // create an onode, initally all "8"
    o.init();
    append(o);

    for (int m=0; m<min_expand ; m++) { // go through the list min_expand times

        std::list<Ocode>::iterator itr;
        std::list<Ocode>::iterator current_end;
        itr = clist.begin();
        current_end = clist.end();
        for(int n=0; n<size() ; n++) {
            if ( itr->expandable() ) { // if expandable
                //std::cout << " init() expanding " << *itr << "\n";
                expand_at(itr); // expand the node
                //std::cout << " after expand itr= " << *itr << "\n";
                n=n+7; // jump forward, since we have inserted new nodes
                itr++; // after expand(), itr points to the last expanded node
                       // so jump forward to expand next node
                if ( itr == clist.end() )
                    itr--; // unless at the end of list
            }
        }
        // std::cout << " LinOCT:init() m=" << m << " N=" << size() << "\n";
    }
    return;
}

/// build LinOCT octree from input volume OCTVolume
void LinOCT::build(OCTVolume* vol)
{   // loop through the whole list:
    // - deleting white nodes
    // - expanding grey nodes if possible
    // - skipping over black nodes (only these remain when done)
    // std::cout << size() << " nodes before build()\n";
    std::list<Ocode>::iterator it;
    std::list<Ocode>::iterator temp;
    it = clist.begin();
    int calc_calls = 0;
    while ( it != clist.end()  ) {
        if (  ! (vol->isInsideBBo( *it )) ) { // nodes outside bounding-box can be deleted
            temp = it;
            if ( it != clist.begin() )
                it--; // jump back out of the way from erase()
            else
                it++;
            clist.erase(temp);
        }
        else {  // this ocode contains the bounding-box
            it->calcScore( vol ); // expensive call...
            ++calc_calls;
            if ( (it->score == 9) ) { // black node
                it++; // node is black, so leave it in the list, and move forward
            } else if ( (it->score == 0) && (it->deg > 5) ) {
                temp = it;
                if ( it != clist.begin() )
                    it--; // jump out of the way from erase()
                else
                    it++;
                clist.erase(temp); // white node, delete.
            }
            else { // grey node, expand if possible, otherwise delete
                if ( it->expandable() ) {
                    temp = it;
                    bool first=false;
                    if (it == clist.begin()) {
                        first = true;
                    } else {
                        temp--;
                    }
                    expand_at(it); // iterator moves to last expanded node.
                    if (first) // so need to reset iterator to first expanded node
                        it = clist.begin();
                    else
                        it = temp;
                } else { // grey non-expandable nodes are removed
                    temp = it;
                    if ( it != clist.begin() )
                        it--; // jump out of the way from erase()
                    else
                        it++;
                    clist.erase(temp);
                }
            }
        }
    }
    std::cout << " LinOCT::build() " << calc_calls << " calcScore() calls \n";
    // std::cout << size() << " nodes after build()\n";
}

// NOTE: condense() seems to run very slowly
// >60s run time on length=138000 list.
void LinOCT::condense() {
    // NOTE: list needs to be sorted before we come here.
    // NOTE: consider using std::list<> unique() to remove duplicates
    int n=0;
    int n_duplicates=0;
    int n_contained=0;
    int n_collapse=0;
    std::list<Ocode>::iterator itr;
    std::list<Ocode>::iterator next;
    itr = clist.begin();

    while ( n < (size()-1) ) {
        next=itr;
        next++;
        if ( (*itr) == (*next) ) { //( clist[n] == clist[n+1] ) { // remove duplicates

            // FIXME delete_at(n);
            n_duplicates++;

            // deleting a duplicate creates an opportunity to collapse
            // so need to jump back by 7 steps to check for collapse
            int jump=7;
            if (n<8)
                jump=n;
            n-=jump;
        }
        else if ( itr->containedIn( *next ) ) {
            // remove nodes contained in the following node
            // FIXME delete_at(n);
            if (n>0)
                n--; // jump back to check for more contained nodes
            n_contained++;
        }
        // condense nodes if all eight sub-quadrants are present
        else if ( can_collapse_at(n) ) { // can collapse the octet
            //std::cout << "collapsing at " << n << "\n";
            int deg = itr->degree();

            // construct parent node of sub-octants
            Ocode o; // parent node, all digits default to "8"
            for (int m=0;m<(deg-1); m++) {
                o.code[m] =  itr->code[m]; // match code up to deg-1
            }

            //std::cout << "before collapse at " << n <<" code:" << clist[n] << "\n";
            // add parent

            // FIXME append_at(o, n);

            //std::cout << "parent insert at " << n <<" code:" << clist[n] << "\n";
            n++; // jump forward and delete the redundant sub-octants

            for (int m=0;m<8;m++) {
                //std::cout << " deleting at " << n<< " : " << clist[n] << "\n";
                // FIXME delete_at(n);
            }

            n--; // jump back to the new parent
            int jump=7;
            if (n<8)
                jump=n;
            n-=jump;
                    // jump backward and see if the collapse has created
                     // an opportunity for more collapsing
                     // collapsable nodes can be as far back as 7 steps
            n_collapse++;
        }
        else {
            n++; // move forward in list
            itr++;
        }
    }
    if ( (n_duplicates>0) || (n_contained>0) || (n_collapse>0)) {
        std::cout << "n_duplicates="<<n_duplicates<<"\n";
        std::cout << "n_contained="<<n_contained<<"\n";
        std::cout << "n_collapse="<<n_collapse<<"\n";
    } else {
        std::cout << "condense(): nothing to do!\n";
    }
    return;
}

/// return true if eight consequtive
/// nodes beginning at idx can be collapsed
bool LinOCT::can_collapse_at(int idx) {
    std::list<Ocode>::iterator it;
    it=clist.begin();
    for (int n=0;n<idx;n++)
        it++;

    if ( (size()-idx) < 8 ) // at least 8 nodes must remain
        return false;

    int deg = it->degree();
    // check for consequtive numbers 0-7 at position deg
    Ocode o;
    //std::cout << " checking "<< idx << " to " << idx+7 << " deg=" << deg << "\n";
    for (int n=0; n < 8 ; n++) {
        o = *it; // clist[idx+n];
        //std::cout << "n=" << n << " Ocode= "<< o <<" code=" << (int)o.code[deg-1] << "\n";

        if ( (o.code[deg-1] != n) || (o.degree() != deg) ) {// code must match 0-7
            //std::cout << " no match\n";
            return false;
        }
        it++;
    }
    return true;
}

/// remove o from this
void LinOCT::diff( LinOCT& o )
{
    std::list<Ocode>::iterator itr1;
    std::list<Ocode>::iterator temp;
    std::list<Ocode>::iterator itr2;
    std::vector<Ocode> Q12;

    itr1=clist.begin();
    itr2=o.clist.begin();
    Ocode Hold12;
    Hold12.null();

    while ( (itr1 != clist.end() ) && ( itr2 != o.clist.end() )   ) {
        if  ( *itr1 == *itr2 ) {
            // remove from 1
            temp = itr1;
            itr1++;
            clist.erase(temp);
            itr2++;
        }
        else if ( itr1->containedIn( *itr2 ) ) {
            temp = itr1;
            itr1++;
            clist.erase(temp);
        }
        else if ( itr2->containedIn( *itr1 ) ) { // case 2
            expand_at(itr1);
            // need to jump back 7 steps
            for (int m=0;m<7;m++)
                itr1--;
        }
        else if ( *itr1 < *itr2 ) {  // case 3
            itr1++;
        }
        else { // case 4:  o2 < o1
            itr2++;
        }
    } // end while-loop

    return;
}

/// compute difference, i.e.
/// remove nodes in other from this
LinOCT LinOCT::operation(int type, LinOCT& o)
{
    // traverse through both lists
    int idx1 = 0;
    int idx2 = 0;
    std::list<Ocode>::iterator itr1;
    std::list<Ocode>::iterator itr2;
    itr1=clist.begin();
    itr2=o.clist.begin();

    std::vector<Ocode> intersection;
    std::vector<Ocode> sum; // a.k.a. union
    std::vector<Ocode> diff12;
    std::vector<Ocode> diff21;
    std::list<Ocode> Q21;
    std::list<Ocode> Q12;
    Ocode Hold21;
    Ocode Hold12;
    Hold21.null();
    Hold12.null();

    while ( (idx1<size()) && (idx2<o.size())   ) {

        // case 0
        if  ( *itr1 == *itr2 ) { //(clist[idx1] == o.clist[idx2]) { // identical nodes
            intersection.push_back( *itr1 ); //clist[idx1] );
            sum.push_back( *itr1 ); //clist[idx1] );
            idx1++;
            idx2++;
            itr1++;
            itr2++;
        }

        else if ( itr1->containedIn( *itr2 ) ) { //clist[idx1].containedIn( o.clist[idx2]  )  ) {  // case 1
            intersection.push_back( *itr1 ); //clist[idx1] ); // idx1 contained is in both o1 and o2
            if ( Hold21.isNull() )
                Hold21 = *itr2; // o.clist[idx2];   // remember this node for later processing
            Q21.push_back( *itr1 ); //clist[idx1] ); // these need to be removed from Hold21 later
            idx1++;
            itr1++;
        }

        else if ( itr2->containedIn( *itr1 ) ) { //o.clist[idx2].containedIn( clist[idx1] ) ) { // case 2
            intersection.push_back( *itr2 ); //o.clist[idx2] ); // o2[idx2] is in both o1 and o2
            if ( Hold12.isNull() )
                Hold12 = *itr1; //clist[idx1];        // store for later processing
            Q12.push_back( *itr2 ); //o.clist[idx2] );  // remove these later from Hold12
            idx2++;
            itr2++;
        }

        else if ( *itr1 < *itr2 ) { // clist[idx1] < o.clist[idx2] ) { // case 3
            // add o1 element to union
            sum.push_back( *itr1 ); //clist[idx1] );

            // process the difference queues, if any
            if ( Hold12 == *itr1 ) { //clist[idx1] )  { //compute difference o1-o2  Hold12 == clist[idx1]
                do_diff( Hold12, Q12, diff12 ); // function for calculating difference
                Hold12.null();
            }
            else
                diff12.push_back( *itr1 ); //clist[idx1] );  // no matching node in o2, so o1 belongs to diff
            idx1++;
            itr1++;
        }
        else { // case 4:  o2 < o1
            if ( !( *itr2 < *itr1 ) ) { //o.clist[idx2] < clist[idx1]) ) {
                std::cout << " case 4 o2 < o1 not true!\n";
                // std::cout << "o2=" << *itr2 << "number=" << itr2->number() <<  "\n";
                // std::cout << "o1=" << *itr1 << "number=" << itr1->number() << "\n";
                assert(0);
            }

            // add o2 element to union
            sum.push_back( *itr2 ); //o.clist[idx2] );

            if ( Hold21 == *itr2 ) { //o.clist[idx2] ) { // Hold21 == o.clist[idx2]
                do_diff( Hold21, Q21, diff21);
                Hold21.null();
            }
            else
                diff21.push_back( *itr2 ); //o.clist[idx2] ); // o2 belongs to diff21
            idx2++;
            itr2++;
        }

    } // end of while-loop through elements

    // now process remaining elements, i.e. case where o1 is longer than o2 or vice versa
    if (idx1 < size()) {// process rest of o1
        int idx3 = idx1;
        std::list<Ocode>::iterator itr3;
        itr3 = itr1;
        if ( Hold12 == *itr1 ) { //clist[idx1] ) {
            do_diff( Hold12, Q12, diff12);
            Hold12.null();
            idx3++;
            itr3++;
        }
        //for (int i=idx3; i<size(); i++)
        for ( ; itr3 != clist.end() ; itr3++ )
            diff12.push_back( *itr3 );
            //diff12.push_back( clist[i] ); // o1 elements not in o2 are in diff12

        //union calc here
        //for (int i=idx1; i<size();i++)
        for ( ; itr1 != clist.end(); itr1++)
            sum.push_back( *itr1 );

    }
    else { // process rest of o2
        int idx3=idx2;
        std::list<Ocode>::iterator itr3;
        itr3 = itr2;

        if (Hold21 == *itr2 ) {
            do_diff(Hold21, Q21, diff21);
            Hold21.null();
            idx3++;
            itr3++;
        }
        for (; itr3 != o.clist.end() ; itr3++) {
            diff21.push_back( *itr3 ); // o2 elements to diff21
        }

        // union calc here
        for (; itr2 != o.clist.end(); itr2++)
            sum.push_back( *itr2 );

    }

    /*
    std::cout << " diff12= " << diff12.size() << "\n";
    std::cout << " diff21= " << diff21.size() << "\n";
    std::cout << " inters= " << intersection.size() << "\n";
    */

    LinOCT result;

    if (type==1) {
        BOOST_FOREACH( Ocode o, diff12 ) {
            result.append(o);
        }
    }
    else if (type==2) {
        BOOST_FOREACH( Ocode o, diff21 ) {
            result.append(o);
        }
    } else if (type==3) {
        BOOST_FOREACH( Ocode o, intersection) {
            result.append(o);
        }
    } else if (type==4) {
        BOOST_FOREACH( Ocode o, sum) {
            result.append(o);
        }
        result.condense();
    }

    return result;
}

// computes difference H - Q
// where H is a node that is expanded
// and Q is a queue of nodes to be subtracted from H
// the result is appended to D
void LinOCT::do_diff(Ocode& H, std::list<Ocode>& Q, std::vector<Ocode>& D)
{
    // H - an expandable node
    // Q - queue of nodes contained in H
    // D - difference queue for H-Q results

    // Q2 contains expanded node
    std::list<Ocode> Q2;

    if ( !H.expandable()) {
        /*
        std::cout << " do_diff node H not expandable...\n";
        std::cout << " H=" << H << "\n";
        std::cout << " Q=\n";
        BOOST_FOREACH( Ocode o, Q) {
            std::cout << o << "\n";
        }*/
        Q2.push_back(H);
    } else {
        Q2 = H.expand();
    }

    /*
    std::cout << " H.expand() on H=" << H <<" :\n";
    BOOST_FOREACH( Ocode o, Q2) {
        std::cout << o << "\n";
    }*/

    while ( !Q2.empty() ) { // go through the expanded nodes

        if ( !Q.empty() ) {
            Ocode n = Q.back();
            Ocode n2 = Q2.back();
            if ( n == n2 ) {// matching elements
                Q2.pop_back(); //erase(Q2.begin());  // nothing to put into diff
                Q.pop_back(); // erase(Q.begin());
            } else if ( n.containedIn( n2 ) ) {// need to expand further
                // expand n2 and add to front of Q2
                std::list<Ocode> subocts = n2.expand();
                Q2.pop_back(); // delete parent
                BOOST_FOREACH( Ocode o, subocts) {
                    Q2.push_back(o); // insert new children
                }
            } else {
                // no match in Q, so push node to diff
                D.push_back( n2 );
                Q2.pop_back();
            }
        }
        else { // Q is empty
            D.push_back( Q2.back() ); // no match in Q, so push node to diff
            Q2.pop_back();
        }

    }// end while
    //Q.clear();

    //std::cout << " after do_diff Q.size() = " << Q.size() << "\n";
    return;
}

/// add nodes of two trees together into one
void LinOCT::sum(LinOCT& other) {
    // join the two lists, sort, and condense
    BOOST_FOREACH( Ocode o, other.clist ) {
        append( o );
    }
    sort();
    condense();
    return;
}

/// sort list of ocodes
void LinOCT::sort() {
    clist.sort();
}

/// return list of triangles to python
/// the triangles correspond to every node/cube of the octree
boost::python::list LinOCT::get_triangles()
{
    boost::python::list tlist;
    BOOST_FOREACH( Ocode o, clist) {
        std::vector<Point> p(8);
        for (int m=0;m<8;++m)
            p[m]=o.corner(m);
        // these 12 triangles cover the 6 sides of the cube
        tlist.append(Triangle(p[0],p[1],p[2]));
        tlist.append(Triangle(p[1],p[2],p[6]));
        tlist.append(Triangle(p[3],p[4],p[5]));
        tlist.append(Triangle(p[4],p[5],p[7]));
        tlist.append(Triangle(p[0],p[2],p[3]));
        tlist.append(Triangle(p[3],p[4],p[2]));
        tlist.append(Triangle(p[1],p[5],p[6]));
        tlist.append(Triangle(p[6],p[7],p[5]));
        tlist.append(Triangle(p[2],p[4],p[6]));
        tlist.append(Triangle(p[6],p[7],p[4]));
        tlist.append(Triangle(p[0],p[1],p[3]));
        tlist.append(Triangle(p[3],p[5],p[1]));
    }
    return tlist;
}

boost::python::list LinOCT::get_nodes() {
    boost::python::list nodelist;
    BOOST_FOREACH( Ocode o, clist) {
            nodelist.append(o);
    }
    return nodelist;
}

/// string repr
std::ostream& operator<<(std::ostream &stream, const LinOCT &l) {
    stream << "LinOCT: N="<< l.size() ;
    return stream;
}

/// print the whole list
void LinOCT::printList() {
    BOOST_FOREACH( Ocode o, clist ) {
        std::cout << " " << o << "\n";
    }
}

/// string repr
std::string LinOCT::str() const {
    std::ostringstream o;
    o << *this;
    return o.str();
}

} // end namespace
// end of file octree.cpp