#include <vector>
#include <string>
#include <Eigen/Core>
#include <cmath>
#include "ndt_map/ndt_map.h"
#include <fstream>
#include "ndt_map/ndt_cell.h"
#include "ros/ros.h"
class map_converter {
	std::string ndt_map_name;
	std::string occ_map_name;
	double ndt_resolution;
	double occ_resolution;
	double lik_tr;
	double floor_height;
	double floor_deviation;
	double robot_height;
	int width, height;
	perception_oru::NDTMap *ndtMap;
	perception_oru::LazyGrid *mapGrid;
	bool floor_only;
public:
	map_converter(ros::NodeHandle private_nh){
		private_nh.param<std::string>("ndt_map_name", ndt_map_name, "map.jff");
		private_nh.param<double>("ndt_resolution", ndt_resolution, 0.4);
		private_nh.param<std::string>("occ_map_name", occ_map_name, "occ");
		private_nh.param<double>("occ_resolution", occ_resolution, 0.1);
		private_nh.param<double>("floor_height", floor_height, -0.3);
		private_nh.param<double>("floor_deviation", floor_deviation, 0.15);
		private_nh.param<double>("lik_tr", lik_tr, 0.1);
		private_nh.param<double>("robot_height", robot_height, 1.9);
		private_nh.param<bool>("floor_only", floor_only, false);
		LoadMap();
		BuildMap();
		// SaveMap();
		// saveYAMLFile();
	}

private:
	int LoadMap(){
		FILE * jffin;

		jffin = fopen(ndt_map_name.c_str(), "r+b");
		mapGrid = new perception_oru::LazyGrid(ndt_resolution);
		ndtMap = new perception_oru::NDTMap(mapGrid);
		if(ndtMap->loadFromJFF(jffin) < 0)
			return -1;
		return 0;
	}

	double getAverageLevel(std::vector<perception_oru::NDTCell*> floors){
		double aver = 0;

		for(size_t i = 0; i < floors.size(); i++)
			aver += floors[i]->getMean()[2];
		return aver / double(floors.size());
	}

	void BuildMap(){

		std::vector<perception_oru::NDTCell*> ndts = ndtMap->getAllCells();
		std::vector<perception_oru::NDTCell*> floors;
		std::vector<float> av_level;
		double sx, sy, sz;
		ndtMap->getCellSizeInMeters(sx, sy, sz);
		for(size_t i = 0; i < ndts.size(); i++){
			//if(ndts[i]->getClass() == lslgeneric::NDTCell::HORIZONTAL){ // commented 14.03
				//bool found = false;
				//for(size_t j = 0; j < floors.size(); j++){
					//if(av_level[j] - floor_deviation<ndts[i]->getMean()[2] && av_level[j] + floor_deviation>ndts[i]->getMean()[2]){
					//if(floor_height - floor_deviation<ndts[i]->getMean()[2] && floor_height + floor_deviation>ndts[i]->getMean()[2]){
					double ccx, ccy, ccz;
					ndts[i]->getCenter(ccx, ccy, ccz);
					if(floor_height - floor_deviation<ccz && floor_height + floor_deviation>ccz){
						//found = true;
						floors.push_back(ndts[i]);
						//av_level[j] = getAverageLevel(floors[j]);
						//break;
					}
				//}
				// if(!found && ndts[i]->getMean()[2]<floor_deviation+floor_height && ndts[i]->getMean()[2]>floor_height-floor_deviation ){
				// 	av_level.push_back(ndts[i]->getMean()[2]);
				// 	std::vector<lslgeneric::NDTCell*> temp;
				// 	temp.push_back(ndts[i]);
				// 	floors.push_back(temp);
				// }

			//}// commented 14.03
		}
		double xs, ys, zs;
		double xc, yc, zc;
		double minx, miny, minz, maxx, maxy, maxz;
		ndtMap->getCentroid(xc, yc, zc);
		ndtMap->getGridSizeInMeters(xs, ys, zs);
		minx = xc - xs / 2.0;
		miny = yc - ys / 2.0;
		maxx = xc + xs / 2.0;
		maxy = yc + ys / 2.0;
		height = abs(ceil(xs / occ_resolution));
		width = abs(ceil(ys / occ_resolution));
		std::vector<std::vector<int> > maps;
		maps.resize(width, std::vector<int>(height));

		//for(size_t j = 0; j < floors.size(); j++){
			for(size_t k = 0; k < floors.size(); k++){
				double ccx, ccy, ccz;
				floors[k]->getCenter(ccx, ccy, ccz);
				for(double lx = ccx - sx/2.0 + occ_resolution / 2.0; lx <= ccx + sx/2.0 - occ_resolution / 2.0; lx += occ_resolution){
					for(double ly = ccy - sy/2.0 + occ_resolution / 2.0; ly <= ccy + sy/2.0 - occ_resolution / 2.0; ly += occ_resolution){
						double lccz = ccz;
						int X = floor((lx+sx/2.0)/occ_resolution+0.5) + height/2.0;// round((lx - (occ_resolution - xs) / 2.0) / occ_resolution);
						int Y = floor((ly+sy/2.0)/occ_resolution+0.5) + width/2.0;//round((ly - (occ_resolution - ys) / 2.0) / occ_resolution);
						pcl::PointXYZ p1(lx, ly, ccz - sz / 2.0);
						pcl::PointXYZ p2(lx, ly, ccz + sz / 2.0);
						Eigen::Vector3d out;
						// ROS_INFO_STREAM(X<<" "<<Y);
						// ROS_INFO_STREAM(p1<<" "<<p2<<" "<<floors[j][k]->computeMaximumLikelihoodAlongLine(p1, p2, out)<<" "<<X<<" "<<Y);

						//if(floors[k]->hasGaussian_){//computeMaximumLikelihoodAlongLine(p1, p2, out) > lik_tr){
							bool traversable = true;
							if(!floor_only){
								while(lccz + 0.5 * sz - floor_height < robot_height){
									lccz += sz;

									perception_oru::NDTCell *cell;
									pcl::PointXYZ refPoint(lx, ly, lccz);

									if(lccz + 0.5 * sz - floor_height < robot_height){
										p1.z = lccz - sz / 2.0;
										p2.z = lccz + sz / 2.0;
									}
									if(lccz + 0.5 * sz - floor_height >= robot_height){
										p1.z = lccz - sz / 2.0;
										p2.z = floor_height + robot_height;
									}
									//std::cout << av_level[j] << " " << p1 << " " << p2 << std::endl;
									if(ndtMap->getCellAtPoint(refPoint, cell)){
										if(cell->hasGaussian_&&cell->getOccupancyRescaled()>0.9){

											if(cell->computeMaximumLikelihoodAlongLine(p1, p2, out) > lik_tr){
												traversable = false;
												//std::cout << cell->getCenter() << " break occ" << std::endl;
												break;
											}
										}
									}else{
										traversable = false;
										//std::cout << "break null" << std::endl;
										break;
									}

								}
							}
							if(traversable){
								maps[Y][X] = 1;
								//std::cout << "done" << std::endl;
							}
						}
					//}

				}
			}
			std::cout << "saving map" << std::endl;
			std::string png_map_file = occ_map_name;
			png_map_file += std::to_string(floor_height);
			png_map_file += ".pgm";
			std::ofstream map_file(png_map_file.c_str());
			if(map_file.is_open()){
				map_file << "P2\n";
				map_file << width << " " << height << std::endl;
				map_file << "1\n";
				// 12.10.15 to fit the inverted map
				//for(int w=0;w<width;w++){
				//  for(int h=height-1;h>=0;h--){
				////////////////////////////////////////////
				for(int w = width - 1; w >= 0; w--){
					for(int h = 0; h < height; h++)
						map_file << maps[w][h] << " ";
					map_file << std::endl;
				}
				map_file.close();
			}else std::cout << "Unable to open file";
		//}
	}
// void BuildMap(){
//  double xs, ys, zs;
//  double xc, yc, zc;
//  double minx, miny, minz, maxx, maxy, maxz;
//
//  ndtMap->getCentroid(xc, yc, zc);
//  ndtMap->getGridSizeInMeters(xs, ys, zs);
//  minx = xc - xs / 2.0;
//  miny = yc - ys / 2.0;
//  maxx = xc + xs / 2.0;
//  maxy = yc + ys / 2.0;
//  height = round(xs / occ_resolution);
//  width = round(ys / occ_resolution);
//  std::cout << height << " " << width << std::endl;
//  int h = 0;
//  map.resize(width, std::vector<int>(height));
//  for(double current_x = -xs / 2 + occ_resolution; current_x < xs - occ_resolution; current_x += occ_resolution){
//   int w = 0;
//   for(double current_y = -ys / 2 + occ_resolution; current_y < ys - occ_resolution; current_y += occ_resolution){
//
//
//    lslgeneric::NDTCell *check_cell;
//    pcl::PointXYZ p;
//    p.x = current_x;
//    p.y = current_y;
//    p.z = 0.0;
//    if(ndtMap->getCellAtPoint(p, check_cell)){
//     //std::cout<<"init"<<std::endl;
//     if(check_cell->getOccupancy() < 0.0)
//      map[w][h] = 1;
//     else if(check_cell->getOccupancy() > 0.0){
//      //std::cout<<"lik "<<check_cell->getLikelihood(p)<<std::endl;
//      if(check_cell->getLikelihood(p) < lik_tr)
//       // Eigen::Vector3d m=check_cell->getMean();
//       // Eigen::Vector3d e=check_cell->getEvals();
//       // double d=sqrt((m[0]-current_x)*(m[0]-current_x)+(m[1]-current_y)*(m[1]-current_y));
//       // double de=sqrt(e[0]*e[0]+e[1]*e[1]);
//       // if(d<=de)
//       map[w][h] = 1;
//     }
//    }
//
//
//
//    w++;
//   }
//   h++;
//  }
// }
//
// void SaveMap(){
//  std::cout << "saving map" << std::endl;
//  std::string png_map_file = occ_map_name;
//  png_map_file += ".png";
//  std::ofstream map_file(png_map_file.c_str());
//  if(map_file.is_open()){
//   map_file << "P2\n";
//   map_file << width << " " << height << std::endl;
//   map_file << "1\n";
//   // 12.10.15 to fit the inverted map
//   //for(int w=0;w<width;w++){
//   //  for(int h=height-1;h>=0;h--){
//   ////////////////////////////////////////////
//   for(int w = width - 1; w >= 0; w--){
//    for(int h = 0; h < height; h++)
//     map_file << map[w][h] << " ";
//    map_file << std::endl;
//   }
//   map_file.close();
//  }else std::cout << "Unable to open file";
// }
//
// void saveYAMLFile(){
//  double xs, ys, zs;
//  double xc, yc, zc;
//  double cenx, ceny;
//
//  ndtMap->getCentroid(xc, yc, zc);
//  ndtMap->getGridSizeInMeters(xs, ys, zs);
//  cenx = xc - xs / 2.0;
//  ceny = yc - ys / 2.0;
//  std::cout << "saving yaml file" << std::endl;
//  std::string yaml_map_file = occ_map_name;
//  yaml_map_file += ".yaml";
//  std::ofstream map_file(yaml_map_file.c_str());
//  if(map_file.is_open()){
//   map_file << "image: " << occ_map_name << ".png\n";
//   map_file << "resolution: " << occ_resolution << std::endl;
//   map_file << "origin: [" << cenx << ", " << ceny << ", 0.0]\n";
//   map_file << "occupied_thresh: 0.5\n";
//   map_file << "free_thresh: 0.5\n";
//   map_file << "negate: 0\n";
//   std::cout << "YAML FILE SAVED, PLEAS EDIT PATH TO MAP!" << std::endl;
//   map_file.close();
//  }else std::cout << "Unable to open file";
// }
};

int main(int argc, char **argv){
	ROS_INFO("Starting node");
	ros::init(argc, argv, "off_line_velodyne_test");
	ros::NodeHandle nh;
	ros::NodeHandle parameters("~");
	map_converter mc(parameters);
}


/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <ored/portfolio/commodityoption.hpp>

#include <boost/make_shared.hpp>

#include <ql/errors.hpp>
#include <ql/exercise.hpp>
#include <ql/instruments/compositeinstrument.hpp>
#include <ql/instruments/vanillaoption.hpp>
#include <qle/indexes/commodityindex.hpp>

#include <ored/portfolio/enginefactory.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/to_string.hpp>

using namespace std;
using namespace QuantLib;
using QuantExt::CommodityFuturesIndex;
using QuantExt::PriceTermStructure;

namespace ore {
namespace data {

CommodityOption::CommodityOption() : VanillaOptionTrade(AssetClass::COM) { tradeType_ = "CommodityOption"; }

CommodityOption::CommodityOption(const Envelope& env, const OptionData& optionData, const string& commodityName,
                                 const string& currency, Real strike, Real quantity,
                                 const boost::optional<bool>& isFuturePrice, const Date& futureExpiryDate)
    : VanillaOptionTrade(env, AssetClass::COM, optionData, commodityName, currency, strike, quantity),
      isFuturePrice_(isFuturePrice), futureExpiryDate_(futureExpiryDate) {
    tradeType_ = "CommodityOption";
}

void CommodityOption::build(const boost::shared_ptr<EngineFactory>& engineFactory) {

    // Checks
    QL_REQUIRE(quantity_ > 0, "Commodity option requires a positive quatity");
    QL_REQUIRE(strike_ > 0, "Commodity option requires a positive strike");

    // Populate the index_ in case the option is automatic exercise.
    // Intentionally use null calendar because we will ask for index value on the expiry date without adjustment.
    const boost::shared_ptr<Market>& market = engineFactory->market();
    index_ = *market->commodityIndex(assetName_, engineFactory->configuration(MarketContext::pricing));
    if (!isFuturePrice_ || *isFuturePrice_) {

        // Assume future price if isFuturePrice_ is not explicitly set or if it is and true.

        auto index = *market->commodityIndex(assetName_, engineFactory->configuration(MarketContext::pricing));

        // If we are given an explicit future contract expiry date, use it, otherwise use option's expiry.
        Date expiryDate;
        if (futureExpiryDate_ != Date()) {
            expiryDate = futureExpiryDate_;
        } else {
            // Get the expiry date of the option. This is the expiry date of the commodity future index.
            const vector<string>& expiryDates = option_.exerciseDates();
            QL_REQUIRE(expiryDates.size() == 1,
                       "Expected exactly one expiry date for CommodityOption but got " << expiryDates.size() << ".");
            expiryDate = parseDate(expiryDates[0]);
        }

        // Clone the index with the relevant expiry date.
        index_ = index->clone(expiryDate);

        // Set the VanillaOptionTrade forwardDate_ if the index is a CommodityFuturesIndex - we possibly still have a
        // CommoditySpotIndex at this point so check. Also, will only work for European exercise.
        auto et = parseExerciseType(option_.style());
        if (et == Exercise::European && boost::dynamic_pointer_cast<CommodityFuturesIndex>(index_)) {
            forwardDate_ = expiryDate;
        }

    }

    VanillaOptionTrade::build(engineFactory);

    // LOG the volatility if the trade expiry date is in the future.
    if (expiryDate_ > Settings::instance().evaluationDate()) {
        DLOG("Implied vol for " << tradeType_ << " on " << assetName_ << " with expiry " << expiryDate_
                                << " and strike " << strike_ << " is "
                                << market->commodityVolatility(assetName_)->blackVol(expiryDate_, strike_));
    }

    additionalData_["quantity"] = quantity_;
    additionalData_["strike"] = strike_;
    additionalData_["strikeCurrency"] = currency_;
}

std::map<AssetClass, std::set<std::string>>
CommodityOption::underlyingIndices(const boost::shared_ptr<ReferenceDataManager>& referenceDataManager) const {
    return {{AssetClass::COM, std::set<std::string>({assetName_})}};
}

void CommodityOption::fromXML(XMLNode* node) {

    Trade::fromXML(node);

    XMLNode* commodityNode = XMLUtils::getChildNode(node, "CommodityOptionData");
    QL_REQUIRE(commodityNode, "A commodity option needs a 'CommodityOptionData' node");

    option_.fromXML(XMLUtils::getChildNode(commodityNode, "OptionData"));

    assetName_ = XMLUtils::getChildValue(commodityNode, "Name", true);
    currency_ = XMLUtils::getChildValue(commodityNode, "Currency", true);
    strike_ = XMLUtils::getChildValueAsDouble(commodityNode, "Strike", true);
    quantity_ = XMLUtils::getChildValueAsDouble(commodityNode, "Quantity", true);

    isFuturePrice_ = boost::none;
    if (XMLNode* n = XMLUtils::getChildNode(commodityNode, "IsFuturePrice"))
        isFuturePrice_ = parseBool(XMLUtils::getNodeValue(n));

    futureExpiryDate_ = Date();
    if (XMLNode* n = XMLUtils::getChildNode(commodityNode, "FutureExpiryDate"))
        futureExpiryDate_ = parseDate(XMLUtils::getNodeValue(n));
}

XMLNode* CommodityOption::toXML(XMLDocument& doc) {

    XMLNode* node = Trade::toXML(doc);

    XMLNode* eqNode = doc.allocNode("CommodityOptionData");
    XMLUtils::appendNode(node, eqNode);

    XMLUtils::appendNode(eqNode, option_.toXML(doc));

    XMLUtils::addChild(doc, eqNode, "Name", assetName_);
    XMLUtils::addChild(doc, eqNode, "Currency", currency_);
    XMLUtils::addChild(doc, eqNode, "Strike", strike_);
    XMLUtils::addChild(doc, eqNode, "Quantity", quantity_);

    if (isFuturePrice_)
        XMLUtils::addChild(doc, eqNode, "IsFuturePrice", *isFuturePrice_);

    if (futureExpiryDate_ != Date())
        XMLUtils::addChild(doc, eqNode, "FutureExpiryDate", to_string(futureExpiryDate_));

    return node;
}

} // namespace data
} // namespace ore


#include "SnapSemantics.h"

// Add semantics to Interior room polyhedra
void set_semantic_InteriorLoD4(Polyhedron& polyhe) {
	std::transform( polyhe.facets_begin(), polyhe.facets_end(),polyhe.planes_begin(),Plane_Newel_equation());
	for (Polyhedron::Facet_iterator fIt = polyhe.facets_begin(); fIt != polyhe.facets_end(); ++fIt) {	// Iterate over faces
		Kernel::FT z = fIt->plane().orthogonal_vector().z();
		if		(z <= -HORIZONTAL_ANGLE_RANGE)	fIt->semanticBLA = "FloorSurface";
		else if (z >=  HORIZONTAL_ANGLE_RANGE)	fIt->semanticBLA = "CeilingSurface";
		else									fIt->semanticBLA = "InteriorWallSurface";
}	}

// Assign either ceiling, floor or the default semantic based on input booleans and normal vector
void assignCeilVloor(std::set<Polyhedron::Facet_handle>& fhSet, bool canBeUp, bool canBeDown) {
	pointVector facetPoints;
	Vector_3 ortVec;
	for (std::set<Polyhedron::Facet_handle>::iterator sfIt=fhSet.begin();sfIt!=fhSet.end();++sfIt) {
		if (!canBeUp && !canBeDown) {
			(*sfIt)->semanticBLA = DEFAULT_HOR_SEMANTIC; continue;
		}

		facetPoints = comp_facetPoints(*sfIt);
		CGAL::normal_vector_newell_3(facetPoints.begin(),facetPoints.end(),ortVec);
		if (!normalizeVector(ortVec)) continue;
		if (canBeDown && ortVec.z() <= -HORIZONTAL_ANGLE_RANGE )	(*sfIt)->semanticBLA = DEFAULT_DOWN_SEMANTIC;
		else if (canBeUp && ortVec.z() >= HORIZONTAL_ANGLE_RANGE )	(*sfIt)->semanticBLA = DEFAULT_UP_SEMANTIC;
		else														(*sfIt)->semanticBLA = DEFAULT_HOR_SEMANTIC;
	}
}

// Search whether a facet is touching a certain semantic directly or indirectly through the facets in the list
bool indirectlyTouchingFindSem(std::string findSem,std::set<Polyhedron::Facet_handle>& fhSet) {
	for (std::set<Polyhedron::Facet_handle>::iterator sfIt=fhSet.begin();sfIt!=fhSet.end();++sfIt) {
		Polyhedron::Facet::Halfedge_around_facet_circulator itHDS = (*sfIt)->facet_begin();
		do	if (itHDS->opposite()->facet()->semanticBLA == findSem)	return true;
		while (++itHDS != (*sfIt)->facet_begin());
	}
	return false;
}

// Creates a List of all connected facets based on sem
void connectedSemFacets(Polyhedron::Facet_handle fh, std::string sem,bool allExceptThisSem, std::set<Polyhedron::Facet_handle>& fhSet, bool checkCoPlanar) {
	if (sem=="-1") sem = fh->semanticBLA;														// Set semantic to input facet sem
	fhSet.insert(fh);																			// Add to list

	Polyhedron::Facet::Halfedge_around_facet_circulator itHDS = fh->facet_begin();				// Loop over the edges
	do {
		if ((itHDS->opposite()->facet()->semanticBLA==sem ^ allExceptThisSem) &&
				(!checkCoPlanar ||  is_NewelCoplanar(itHDS))){										// If part of sem(s) we're looking for (and coplanar if needed)
			if (fhSet.find(itHDS->opposite()->facet())==fhSet.end())							// If not yet in set
				connectedSemFacets(itHDS->opposite()->facet(),sem,allExceptThisSem,fhSet,checkCoPlanar);// Recursive search
		}
	} while (++itHDS != fh->facet_begin());
}

// Depricated
// Creates a list of coplanar facets with equal semantics
// Returns the combined area (Assumes area has been set)
double coplanarSem(Polyhedron::Facet_handle fh, std::set<Polyhedron::Facet_handle>& fhSet) {
	fhSet.insert(fh);																			// Add to list
	double summedArea = fh->area;																// Get area, assumes precalculated
	Polyhedron::Facet::Halfedge_around_facet_circulator itHDS = fh->facet_begin();
	do {
		if (is_coplanar(itHDS,true)) {															// Check coplanearity and equal semantics
			if (fhSet.find(itHDS->opposite()->facet())==fhSet.end())							// If new to list
				summedArea += coplanarSem(itHDS->opposite()->facet(),fhSet);					// Recursive search and sum
		}
	} while (++itHDS != fh->facet_begin());
	return summedArea;
}

// Remove small triangles with different semantics
void remove_singularSemantics(Polyhedron& exteriorPolyhe) {
	double SMALL_TRIANGLE_LIMIT = 0.1;
	Polyhedron::Facet_iterator exfIt;						// Iterate over exterior faces

	while (true) {
		bool newSemFound = false;
		for (exfIt = exteriorPolyhe.facets_begin(); exfIt != exteriorPolyhe.facets_end(); ++exfIt) {
			if (exfIt->area < SMALL_TRIANGLE_LIMIT) {
				std::map<std::string,double> semCountList;
				std::map<std::string,double>::iterator semCount;
				Polyhedron::Facet::Halfedge_around_facet_circulator itHDS = exfIt->facet_begin();

				double maxArea = 0;
				std::string newSem;

					do {
						if (is_coplanar(itHDS,false)) {
							std::string otherSem = itHDS->opposite()->facet()->semanticBLA;

							// Compute sum of area

							if (is_coplanar(itHDS->opposite()->next(),true) ||
								is_coplanar(itHDS->opposite()->prev(),true)) {
								semCount = semCountList.find(otherSem);
								if (semCount == semCountList.end())
									semCountList[otherSem] = 1; // Might not be needed
								else
									++(semCountList[otherSem]);
							}
						}
					} while (++itHDS != exfIt->facet_begin());

					for (semCount = semCountList.begin();semCount!=semCountList.end();++semCount) {
						if (semCount->second > maxArea) {
							maxArea = semCount->second;
							newSem = semCount->first;
						} else if (semCount->second == maxArea &&
							((exfIt->semanticBLA=="Anything"&&semCount->first!="Anything")||semCount->first==exfIt->semanticBLA))
							newSem = semCount->first;
					}

				if (maxArea > 0 && newSem!=exfIt->semanticBLA) {
					exfIt->semanticBLA = newSem;
					newSemFound = true;
					break;										// Dafuq? is this correct?
				}
			}
		}
		if (!newSemFound) break;								// Really?
	}
}
void apply_semanticRequirements(Polyhedron& exteriorPolyhe) {
	std::map<std::string,std::vector<bool>> semanticNormals;
	semanticNormals["Roof"] = std::vector<bool>(3,true);
	semanticNormals["Roof"][2] = false;		// down
	semanticNormals["Ground"] = std::vector<bool>(3,false);
	semanticNormals["Ground"][2] = true;
	semanticNormals["Ceiling"] = std::vector<bool>(3,false);
	semanticNormals["Ceiling"][2] = true;
	semanticNormals["Floor"] = std::vector<bool>(3,false);
	semanticNormals["Floor"][0] = true;
	semanticNormals["Wall"] = std::vector<bool>(3,true);
	semanticNormals["Window"] = std::vector<bool>(3,true);
	semanticNormals["Door"] = std::vector<bool>(3,true);
	semanticNormals["Closure"] = std::vector<bool>(3,true);
	semanticNormals[TO_DIST_SEMANTIC] = std::vector<bool>(3,true);
	//semanticNormals["Anything"] = std::vector<bool>(3,true);

	std::map<std::string,int> semanticEnum;
	semanticEnum["Roof"]		= 1;
	semanticEnum["Window"]		= 2;
	semanticEnum["Door"]		= 3;
	semanticEnum["Wall"]		= 4;
	semanticEnum["Ground"]		= 5;
	semanticEnum["Ceiling"]		= 6;
	semanticEnum["Floor"]		= 7;
	semanticEnum["Closure"]		= 8;

	semanticEnum["Anything"]	= 9;
	//semanticEnum["Install"]		= 10;

	Vector_3 ortVec;

	Polyhedron::Facet_iterator exfIt;						// Iterate over exterior faces
	for (exfIt = exteriorPolyhe.facets_begin(); exfIt != exteriorPolyhe.facets_end(); ++exfIt) {

		int minSem = 99;

		for (std::vector<std::string>::iterator svIt=exfIt->equidistSems.begin();svIt!=exfIt->equidistSems.end();++svIt) {				// Add equidist semantics
			std::map<std::string,int>::iterator seIt = semanticEnum.find(*svIt);
			if (seIt!=semanticEnum.end()) {				// ......
				if (seIt->second<minSem) {
					minSem = seIt->second;
					exfIt->semanticBLA = *svIt;
				}
			}else {										// This seems wrong...
				exfIt->semanticBLA = *svIt;
				break;
			}
		}

		pointVector facetPoints = comp_facetPoints(exfIt);
		CGAL::normal_vector_newell_3(facetPoints.begin(),facetPoints.end(),ortVec); // Calculate normal vector, ortVec set to zero in newell
		if (!normalizeVector(ortVec)) continue;

		std::map<std::string,std::vector<bool>>::iterator  snIt = semanticNormals.find(exfIt->semanticBLA);
		if (snIt!=semanticNormals.end()) {
			if		(!snIt->second[0] && ortVec.z() > HORIZONTAL_ANGLE_RANGE)
				exfIt->semanticBLA = DEFAULT_UP_SEMANTIC;
			else if (!snIt->second[1] && ortVec.z() <= HORIZONTAL_ANGLE_RANGE && ortVec.z() >= -HORIZONTAL_ANGLE_RANGE)
				exfIt->semanticBLA = DEFAULT_HOR_SEMANTIC;
			else if (!snIt->second[2] && ortVec.z() <= -HORIZONTAL_ANGLE_RANGE)
				exfIt->semanticBLA = DEFAULT_DOWN_SEMANTIC;
		} else {
			if		(ortVec.z() > HORIZONTAL_ANGLE_RANGE)
				exfIt->semanticBLA = DEFAULT_UP_SEMANTIC;
			else if (ortVec.z() <= HORIZONTAL_ANGLE_RANGE && ortVec.z() >= -HORIZONTAL_ANGLE_RANGE)
				exfIt->semanticBLA = DEFAULT_HOR_SEMANTIC;
			else if (ortVec.z() <= -HORIZONTAL_ANGLE_RANGE)
				exfIt->semanticBLA = DEFAULT_DOWN_SEMANTIC;
		}
	}
	// Set semantics of facets created due to closing (too far from original geometry)
	for (exfIt = exteriorPolyhe.facets_begin(); exfIt != exteriorPolyhe.facets_end(); ++exfIt) {
		if (exfIt->semanticBLA==TO_DIST_SEMANTIC) {
			std::set<Polyhedron::Facet_handle> fhSet;
			connectedSemFacets(exfIt,TO_DIST_SEMANTIC,false,fhSet);
			bool canBeUp	= indirectlyTouchingFindSem(DEFAULT_UP_SEMANTIC,fhSet);
			bool canBeDown	= indirectlyTouchingFindSem(DEFAULT_DOWN_SEMANTIC,fhSet);
			assignCeilVloor(fhSet,canBeUp,canBeDown);
		}
	}
}

struct distSemFace {
    Kernel::FT dist;
    std::string sem;
	Polyhedron::Facet_const_handle fh;
};

struct by_dist {
    bool operator()(distSemFace const &a, distSemFace const &b) {
        return a.dist < b.dist;
    }
};
#include <boost/algorithm/string.hpp>
//std::vector<person> people;
//std::sort(people.begin(), people.end(), by_age());

void set_semantic_AABB_C2V(Polyhedron& exteriorPolyhe,PolVector& polyVec) {

	if (exteriorPolyhe.is_pure_triangle()) {
		std::transform( exteriorPolyhe.facets_begin(), exteriorPolyhe.facets_end(),exteriorPolyhe.planes_begin(),Plane_equation());
		std::vector<std::string>	semList;
		std::vector<std::shared_ptr<AAbbTree>>		treeList;

		// Build Trees. One for each semantic
		for(PolVector::iterator pvIt = polyVec.begin();pvIt!=polyVec.end();++pvIt) {// Get AABB trees of all semantics
			if (pvIt->is_pure_triangle()) {
				std::string semP = pvIt->facets_begin()->semanticBLA;
				std::vector<std::string>::iterator  strIt = std::find(semList.begin(), semList.end(),semP);
				if (strIt==semList.end()) {											// If new sematic
					semList.push_back(semP);										// Add sem
					std::shared_ptr<AAbbTree> tree = std::make_shared<AAbbTree>(pvIt->facets_begin(),pvIt->facets_end());		// Create tree
					tree->accelerate_distance_queries();							// accelerate
					treeList.push_back(tree);										// Add tree
				} else																					// If not new
					treeList[strIt-semList.begin()]->insert(pvIt->facets_begin(),pvIt->facets_end());	// Append to tree
			} else std::cerr << "ERROR: Not pure triangle (set_semantic_AABB2C2V)" << std::endl;
		}

		// For each facet calculate the least distance to each tree
		std::string semListStr = boost::algorithm::join((semList), " ");
		int percCount = 1;
		Polyhedron::Facet_iterator exfIt;						// Iterate over exterior faces
		for (exfIt = exteriorPolyhe.facets_begin(); exfIt != exteriorPolyhe.facets_end(); ++exfIt,++percCount) {

			std::cout << "\r"<<semListStr<<". ("<<100*percCount/exteriorPolyhe.size_of_facets()<<"%)";

			Vector_3 orthVec = exfIt->plane().orthogonal_vector();
			normalizeVector(orthVec);	//if (!normalizeVector(ortVec)) continue;

			std::vector<distSemFace> dsfList(semList.size());
			Point_3 centerPoint = comp_facetCentroid(exfIt);						// Compute centroid
			std::vector<Kernel::FT> leastSemDistances;
			for (int intIt=0;intIt<(int)treeList.size();++intIt) {					// Loop over all trees
				AAbbTree::Point_and_primitive_id pp = treeList[intIt]->closest_point_and_primitive(centerPoint);
				dsfList[intIt].dist	= CGAL::squared_distance(centerPoint,pp.first);	// Store distance semantic and facet for each tree
				dsfList[intIt].sem	= semList[intIt];
				dsfList[intIt].fh	= pp.second;
			}

			std::sort(dsfList.begin(),dsfList.end(),by_dist());

			exfIt->leastSqDistance = dsfList[0].dist;					// least sqrt distance
			if (exfIt->isMinkFacet = dsfList[0].dist > SEMANTIC_DISTANCE_THRESHOLD) {
				exfIt->semanticBLA = TO_DIST_SEMANTIC;						// Default semantic if too distant
				continue;
			} else
				exfIt->semanticBLA = dsfList[0].sem;					// Semantics of closest

			Vector_3 faceNormal;
			Kernel::FT faceSqArea;
			double minAngle = 10;
			Kernel::FT maxArea= 0;

			for (std::vector<distSemFace>::iterator slIt = dsfList.begin();slIt != dsfList.end();++slIt)// HANDLE ANYTHING AS LESS IMPORTANT
				if (slIt->dist < dsfList[0].dist+OVERLAP_DIST_THRESHOLD) {	// Check if Equidistant
					pointVector facetPoints = comp_facetPoints(exfIt);
					CGAL::normal_vector_newell_3(facetPoints.begin(),facetPoints.end(),faceNormal); // Calculate normal vector, ortVec set to zero in newell
					double angle = comp_angle(orthVec,faceNormal);
					if (angle!=-1 && angle < minAngle+OVERLAP_ANGLE_THRESHOLD) {
						if (minAngle >= angle+OVERLAP_ANGLE_THRESHOLD)		exfIt->equidistSems.clear();
						if (angle < minAngle)								minAngle = angle;
						faceSqArea = comp_facetSquaredArea(facetPoints);
						if (faceSqArea>maxArea-OVERLAP_AREA_THRESHOLD) {
							if (maxArea<=faceSqArea-OVERLAP_AREA_THRESHOLD)	exfIt->equidistSems.clear();
							if (faceSqArea>maxArea)							maxArea = faceSqArea;
							exfIt->equidistSems.push_back(slIt->sem);				// Add equidist semantics
						}
					}
				}
		}
		std::cout << "\r"<<semListStr<<". (100%)" << std::endl;
	}else std::cerr << "ERROR: Not pure triangle (set_semantic_AABB2C2V)" << std::endl;
}


/* ----------------------------------------------------------------------- *//**
 *
 * @file multilogistic.cpp
 *
 * @brief Multinomial Logistic-Regression functions
 *
 * We implement the iteratively-reweighted-least-squares method.
 *
 *//* ----------------------------------------------------------------------- */

#include <dbconnector/dbconnector.hpp>

#include <modules/shared/HandleTraits.hpp>
#include <modules/prob/boost.hpp>
#include <boost/math/distributions.hpp>
#include <modules/prob/student.hpp>
#include "multilogistic.hpp"

#include <vector>
#include <fstream>

namespace madlib {

// Use Eigen
using namespace dbal::eigen_integration;

namespace modules {

// Import names from other MADlib modules
using dbal::NoSolutionFoundException;

namespace regress {

/**
 * @brief Logistic function
 */
inline double sigma(double x) {
    return 1. / (1. + std::exp(-x));
}

/**
 * @brief Inter- and intra-iteration state for iteratively-reweighted-least-
 *        squares method for logistic regression
 *
 * TransitionState encapsualtes the transition state during the
 * logistic-regression aggregate function. To the database, the state is
 * exposed as a single DOUBLE PRECISION array, to the C++ code it is a proper
 * object containing scalars, a vector, and a matrix.
 *
 * Note: We assume that the DOUBLE PRECISION array is initialized by the
 * database with length at least 4, and all elemenets are 0.
 */
template <class Handle>
class MLogRegrIRLSTransitionState {

    // By §14.5.3/9: "Friend declarations shall not declare partial
    // specializations." We do access protected members in operator+=().
    template <class OtherHandle>
    friend class MLogRegrIRLSTransitionState;

public:

    MLogRegrIRLSTransitionState(const AnyType &inArray)
        : mStorage(inArray.getAs<Handle>()) {
        rebind(static_cast<uint16_t>(mStorage[0]),static_cast<uint16_t>(mStorage[1]));
    }

    /**
     * @brief Convert to backend representation
     *
     * We define this function so that we can use State in the
     * argument list and as a return type.
     */
    inline operator AnyType() const {
        return mStorage;
    }

    /**
     * @brief Initialize the iteratively-reweighted-least-squares state.
     *
     * This function is only called for the first iteration, for the first row.
     */
    inline void initialize(
        const Allocator &inAllocator,
        uint16_t inWidthOfX,
        uint16_t inNumCategories, uint16_t inRefCategory) {

        size_t state_size = arraySize(inWidthOfX, inNumCategories);
        // GPDB limits the single array size to be 1GB, which means that the size
        // of a double array cannot be large than 134217727 because
        // (134217727 * 8) / (1024 * 1024) = 1023. And solve
        // state_size = x^2 + 2^x + 6 <= 134217727 will give x <= 11584.
        if(state_size > 134217727)
            throw std::domain_error(
                "The product of number of independent variables and number of "
                 "categories cannot be larger than 11584.");

        mStorage = inAllocator.allocateArray<double, dbal::AggregateContext,
            dbal::DoZero, dbal::ThrowBadAlloc>(state_size);
        rebind(inWidthOfX, inNumCategories);
        widthOfX = inWidthOfX;
        numCategories = inNumCategories;
        ref_category = inRefCategory;
    }

    /**
     * @brief We need to support assigning the previous state
     */
    template <class OtherHandle> MLogRegrIRLSTransitionState &operator=(
        const MLogRegrIRLSTransitionState<OtherHandle> &inOtherState) {

        for (size_t i = 0; i < mStorage.size(); i++)
            mStorage[i] = inOtherState.mStorage[i];
        return *this;
    }

    /**
     * @brief Merge with another State object by copying the intra-iteration
     *     fields
     */
    template <class OtherHandle> MLogRegrIRLSTransitionState &operator+=(
        const MLogRegrIRLSTransitionState<OtherHandle> &inOtherState) {
        if (mStorage.size() != inOtherState.mStorage.size() ||
            widthOfX != inOtherState.widthOfX)
            throw std::logic_error("Internal error: Incompatible transition "
                "states");

        numRows += inOtherState.numRows;
        gradient += inOtherState.gradient;
        X_transp_AX += inOtherState.X_transp_AX;
        logLikelihood += inOtherState.logLikelihood;
        return *this;
    }

    /**
     * @brief Reset the inter-iteration fields.
     */
    inline void reset() {
        numRows = 0;
        gradient.fill(0);
        X_transp_AX.fill(0);
        logLikelihood = 0;
    }

private:
    static inline uint32_t arraySize(const uint16_t inWidthOfX,
        const uint16_t inNumCategories) {
        return 6 + inWidthOfX * inWidthOfX * inNumCategories * inNumCategories
                                 + 2 * inWidthOfX * inNumCategories;
    }

    void rebind(uint16_t inWidthOfX = 0, uint16_t inNumCategories = 0) {
        widthOfX.rebind(&mStorage[0]);
        numCategories.rebind(&mStorage[1]);
        conditionNo.rebind(&mStorage[2]);

        coef.rebind(&mStorage[3], inWidthOfX*inNumCategories);

        numRows.rebind(&mStorage[3 + inWidthOfX*inNumCategories]);

        gradient.rebind(&mStorage[4 + inWidthOfX*inNumCategories],inWidthOfX*inNumCategories);
        X_transp_AX.rebind(&mStorage[4 + 2 * inWidthOfX*inNumCategories],
            inNumCategories*inWidthOfX, inWidthOfX*inNumCategories);
        logLikelihood.rebind(&mStorage[4 +
             inNumCategories*inNumCategories*inWidthOfX*inWidthOfX
             + 2 * inWidthOfX*inNumCategories]);
        ref_category.rebind(&mStorage[5 +
             inNumCategories*inNumCategories*inWidthOfX*inWidthOfX
             + 2 * inWidthOfX*inNumCategories]);
    }

    Handle mStorage;

public:
    typename HandleTraits<Handle>::ReferenceToUInt16 widthOfX;
    typename HandleTraits<Handle>::ReferenceToUInt16 numCategories;
    typename HandleTraits<Handle>::ReferenceToDouble conditionNo;

    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap coef;
    typename HandleTraits<Handle>::ReferenceToUInt64 numRows;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap gradient;
    typename HandleTraits<Handle>::MatrixTransparentHandleMap X_transp_AX;
    typename HandleTraits<Handle>::ReferenceToDouble logLikelihood;
    typename HandleTraits<Handle>::ReferenceToUInt16 ref_category;
};

/**
 * @brief Inter- and intra-iteration state for robust variance calculations
 *
 * TransitionState encapsualtes the transition state during the
 * logistic-regression robust variance calculation. To the database, the state is
 * exposed as a single DOUBLE PRECISION array, to the C++ code it is a proper
 * object containing scalars, a vector, and a matrix.
 *
 * Note: We assume that the DOUBLE PRECISION array is initialized by the
 * database with length at least 4, and all elemenets are 0.
 */
template <class Handle>
class MLogRegrRobustTransitionState {

    // By §14.5.3/9: "Friend declarations shall not declare partial
    // specializations." We do access protected members in operator+=().
    template <class OtherHandle>
    friend class MLogRegrRobustTransitionState;

public:
    MLogRegrRobustTransitionState(const AnyType &inArray)
        : mStorage(inArray.getAs<Handle>()) {
        rebind(static_cast<uint16_t>(mStorage[0]),static_cast<uint16_t>(mStorage[1]));
    }

    /**
     * @brief Convert to backend representation
     *
     * We define this function so that we can use State in the
     * argument list and as a return type.
     */
    inline operator AnyType() const {
        return mStorage;
    }

    /**
     * @brief Initialize the iteratively-reweighted-least-squares state.
     *
     * This function is only called for the first iteration, for the first row.
     */
    inline void initialize(const Allocator &inAllocator,
        uint16_t inWidthOfX, uint16_t inNumCategories, uint16_t inRefCategory) {
        mStorage = inAllocator.allocateArray<double, dbal::AggregateContext,
            dbal::DoZero, dbal::ThrowBadAlloc>(arraySize(inWidthOfX, inNumCategories));
        rebind(inWidthOfX, inNumCategories);
        widthOfX = inWidthOfX;
        numCategories = inNumCategories;
        ref_category = inRefCategory;
    }

    /**
     * @brief We need to support assigning the previous state
     */
    template <class OtherHandle> MLogRegrRobustTransitionState &operator=(
        const MLogRegrRobustTransitionState<OtherHandle> &inOtherState) {
        for (size_t i = 0; i < mStorage.size(); i++)
            mStorage[i] = inOtherState.mStorage[i];
        return *this;
    }

    /**
     * @brief Merge with another State object by copying the intra-iteration
     *     fields
     */
    template <class OtherHandle> MLogRegrRobustTransitionState &operator+=(
        const MLogRegrRobustTransitionState<OtherHandle> &inOtherState) {
        if (mStorage.size() != inOtherState.mStorage.size() ||
            widthOfX != inOtherState.widthOfX)
            throw std::logic_error("Internal error: Incompatible transition "
                "states");

        numRows += inOtherState.numRows;
        X_transp_AX += inOtherState.X_transp_AX;
        meat += inOtherState.meat;
        return *this;
    }

    /**
     * @brief Reset the inter-iteration fields.
     */
    inline void reset() {
        numRows = 0;
        meat.fill(0);
        X_transp_AX.fill(0);
    }

private:
    static inline uint32_t arraySize(const uint16_t inWidthOfX,
        const uint16_t inNumCategories) {
        return 4 + 2*inWidthOfX * inWidthOfX * inNumCategories * inNumCategories
                                 + inWidthOfX * inNumCategories;
    }

    /**
     * @brief Rebind to a new storage array
     *
     * @param inWidthOfX             The number of independent variables.
     * @param inNumCategories The number of categories of the dependant var

     * Array layout (iteration refers to one aggregate-function call):
     * Inter-iteration components (updated in final function):
     * - 0: widthOfX (number of independant variables)
     * - 1: numCategories (number of categories)
     * - 2: ref_category
     * - 3: coef (vector of coefficients)
     *
     * Intra-iteration components (updated in transition step):
     * - 3 + widthOfX*numCategories: numRows (number of rows already processed in this iteration)
     * - 4 + widthOfX * inNumCategories: X_transp_AX (X^T A X).
     * - 4 + widthOfX^2*numCategories^2: meat  (The meat matrix)
     */
    void rebind(uint16_t inWidthOfX = 0, uint16_t inNumCategories = 0) {
        widthOfX.rebind(&mStorage[0]);
        numCategories.rebind(&mStorage[1]);
        ref_category.rebind(&mStorage[2]);
        coef.rebind(&mStorage[3], inWidthOfX * inNumCategories);
        numRows.rebind(&mStorage[3 + inWidthOfX * inNumCategories]);
        X_transp_AX.rebind(&mStorage[4 + inWidthOfX * inNumCategories],
            inNumCategories * inWidthOfX, inWidthOfX * inNumCategories);
        meat.rebind(&mStorage[4 +
             inNumCategories * inNumCategories * inWidthOfX * inWidthOfX
             + inWidthOfX * inNumCategories], inWidthOfX * inNumCategories, inWidthOfX * inNumCategories);
    }

    Handle mStorage;

public:
    typename HandleTraits<Handle>::ReferenceToUInt16 widthOfX;
    typename HandleTraits<Handle>::ReferenceToUInt16 numCategories;
    typename HandleTraits<Handle>::ReferenceToUInt16 ref_category;

    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap coef;
    typename HandleTraits<Handle>::ReferenceToUInt64 numRows;
    typename HandleTraits<Handle>::MatrixTransparentHandleMap X_transp_AX;
    typename HandleTraits<Handle>::MatrixTransparentHandleMap meat;
};

/**
 * @brief IRLS Transition
 * @param args
 *
 * Arguments (Matched with PSQL wrapped)
 * - 0: Current State
 * - 1: y value (Integer)
 * - 2: numCategories (Integer)
 * - 3: ref_category (Integer)
 * - 4: X value (Column Vector)
 * - 5: Previous State

 */
AnyType
__mlogregr_irls_step_transition::run(AnyType &args) {
    MLogRegrIRLSTransitionState<MutableArrayHandle<double> > state = args[0];

    if (args[1].isNull() || args[2].isNull() || args[3].isNull() ||
            args[4].isNull()) {
        return args[0];
    }

    // Get x as a vector of double
    MappedColumnVector x;
    try{
        // an exception is raised in the backend if args[2] contains nulls
        MappedColumnVector xx = args[4].getAs<MappedColumnVector>();
        // x is a const reference, we can only rebind to change its pointer
        x.rebind(xx.memoryHandle(), xx.size());
    } catch (const ArrayWithNullException &e) {
        return args[0];
    }

    // Get the category & numCategories as integer
    int32_t category = args[1].getAs<int32_t>();
    // Number of categories after pivoting (we pivot around the first category)
    int32_t numCategories = (args[2].getAs<int32_t>() - 1);
    int32_t ref_category = args[3].getAs<int32_t>();

    // The following check was added with MADLIB-138.
    if (!x.is_finite())
            throw std::domain_error("Design matrix is not finite.");

    if (state.numRows == 0) {
        if (x.size() > std::numeric_limits<uint16_t>::max())
                throw std::domain_error("Number of independent variables cannot be "
                        "larger than 65535.");

        if (numCategories < 1)
                throw std::domain_error("Number of cateogires must be at least 2");

        // Init the state (requires x.size() and category.size())
        state.initialize(*this,
            static_cast<uint16_t>(x.size()) ,
            static_cast<uint16_t>(numCategories),
            static_cast<uint16_t>(ref_category));

        if (!args[5].isNull()) {
                MLogRegrIRLSTransitionState<ArrayHandle<double> >
                        previousState = args[5];
                state = previousState;
                state.reset();
        }
    }

    /*
     * This check should be done for each iteration. Only checking the first
     * run is not enough.
     */
    if (category > numCategories || category < 0)
        throw std::domain_error("Invalid category. Categories must be integer "
            "values between 0 and (number of categories - 1).");

    if (ref_category > numCategories || ref_category < 0)
        throw std::domain_error("Invalid reference category. Reference category must be integer "
            "value between 0 and (number of categories - 1).");

    // Now do the transition step
    state.numRows++;
    /*     Get y: Convert to 1/0 boolean vector
            Example: Category 4 : 0 0 0 1 0 0
            Storing it in this forms helps us get a nice closed form expression
    */

    //To pivot around the specified reference category
    ColumnVector y(numCategories);
    y.fill(0);
    if (category > ref_category) {
        y(category - 1) = 1;
    } else if (category < ref_category) {
        y(category) = 1;
    }

    /*
    Compute the parameter vector (the 'pi' vector in the documentation)
    for the data point being processed.
    Casting the coefficients into a matrix makes the calculation simple.
    */
    Matrix coef = state.coef;
    coef.resize(numCategories, state.widthOfX);

    //Store the intermediate calculations because we'll reuse them in the LLH
    ColumnVector t1 = x; //t1 is vector of size state.widthOfX
    t1 = coef*x;
    /* Note: The above 2 lines could have been written as:
        ColumnVector t1 = -coef*x;

        but this creates warnings. These warnings are somehow related to the factor
        that x is of an immutable type. The following alternative could resolve
        the warnings (although not necessary the best alternative):
    */

    ColumnVector t2 = t1.array().exp();
    double t3 = 1 + t2.sum();
    ColumnVector pi = t2/t3;

    //The gradient matrix has numCategories rows and widthOfX columns
    Matrix grad = -y*x.transpose() + pi*x.transpose();
    //We cast the gradient into a vector to make the Newton step calculations much easier.
    grad.resize(numCategories*state.widthOfX,1);

    /*
         a is a matrix of size JxJ where J is the number of categories
         a_j1j2 = -pi(j1)*(1-pi(j2))if j1 == j2
         a_j1j2 =  pi(j1)*pi(j2) if j1 != j2
    */
    Matrix a(numCategories,numCategories);
    // Compute the 'a' matrix.
    Matrix piDiag = pi.asDiagonal();
    a = pi * pi.transpose() - piDiag;
    state.gradient.noalias() += grad;

    //Start the Hessian calculations
    Matrix X_transp_AX(numCategories * state.widthOfX, numCategories * state.widthOfX);

    /*
        Again: The following 3 lines could have been written as
        Matrix XXTrans = x * x.transpose();
        but it creates warnings related to the type of x. Here is an easy fix
    */
    Matrix cv_x = x;
    Matrix XXTrans = trans(cv_x);
    XXTrans = cv_x * XXTrans;

    //Eigen doesn't supported outer-products for matrices, so we have to do our own.
    //This operation is also known as a tensor-product.
    for (int i1 = 0; i1 < state.widthOfX; i1++){
         for (int i2 = 0; i2 <state.widthOfX; i2++){
            int rowOffset = numCategories * i1;
            int colOffset = numCategories * i2;

            X_transp_AX.block(rowOffset, colOffset, numCategories,  numCategories) = XXTrans(i1,i2)*a;
        }
    }

    triangularView<Lower>(state.X_transp_AX) += X_transp_AX;

    state.logLikelihood += y.transpose()*t1 - log(t3);

    return state;

}

/**
 * @brief Perform the perliminary aggregation function: Merge transition states
     This merge step is the same as that of logistic regression.
 */
AnyType
__mlogregr_irls_step_merge_states::run(AnyType &args) {
    MLogRegrIRLSTransitionState<MutableArrayHandle<double> > stateLeft = args[0];
    MLogRegrIRLSTransitionState<ArrayHandle<double> > stateRight = args[1];

    // We first handle the trivial case where this function is called with one
    // of the states being the initial state
    if (stateLeft.numRows == 0)
        return stateRight;
    else if (stateRight.numRows == 0)
        return stateLeft;

    // Merge states together and return
    stateLeft += stateRight;
    return stateLeft;
}

/**
 * @brief Perform the logistic-regression final step
 */
AnyType
__mlogregr_irls_step_final::run(AnyType &args) {
    // We request a mutable object.
    // Depending on the backend, this might perform a deep copy.
    MLogRegrIRLSTransitionState<MutableArrayHandle<double> > state = args[0];

    // Aggregates that haven't seen any data just return Null.
    if (state.numRows == 0)
        return Null();

    // See MADLIB-138. At least on certain platforms and with certain versions,
    // LAPACK will run into an infinite loop if pinv() is called for non-finite
    // matrices. We extend the check also to the dependent variables.
    if (!state.X_transp_AX.is_finite() || !state.gradient.is_finite())
        throw NoSolutionFoundException("Over- or underflow in intermediate "
            "calculation. Input data is likely of poor numerical condition.");

    SymmetricPositiveDefiniteEigenDecomposition<Matrix> decomposition(
        -1 * state.X_transp_AX, EigenvaluesOnly, ComputePseudoInverse);

    // Precompute (X^T * A * X)^-1
    Matrix hessianInv = -1 * decomposition.pseudoInverse();

    state.coef.noalias() += hessianInv * state.gradient;

    if(!state.coef.is_finite())
        throw NoSolutionFoundException("Over- or underflow in Newton step, "
            "while updating coefficients. Input data is likely of poor "
            "numerical condition.");

    // We use the intra-iteration field gradient for storing the diagonal
    // of X^T A X, so that we don't have to recompute it in the result function.
    // Likewise, we store the condition number.
    // FIXME: This feels a bit like a hack.
    state.conditionNo = decomposition.conditionNo();
    state.X_transp_AX = -1 * hessianInv;

    return state;
}

/**
 * @brief Compute the diagnostic statistics
 *
 * This function wraps the common parts of mlogregr state into the result.
 * (the result data type is defined in multilogistic.sql_in)
 */
AnyType mLogstateToResult(
    const Allocator &inAllocator,
    MLogRegrIRLSTransitionState<ArrayHandle<double> > state) {

    int ref_category = state.ref_category;
    const HandleMap<const ColumnVector, TransparentHandle<double> > &inCoef = state.coef;
    double logLikelihood = state.logLikelihood;
    uint64_t num_processed = state.numRows;

    // Per the hack at the end of the final function we place the inverse
    // of the X_tranp_AX into the state.X_transp_AX
    const Matrix & X_transp_AX_inverse = state.X_transp_AX;
    const ColumnVector &diagonal_of_hessian = X_transp_AX_inverse.diagonal();

    MutableNativeColumnVector stdErr(
        inAllocator.allocateArray<double>(inCoef.size()));
    MutableNativeColumnVector waldZStats(
        inAllocator.allocateArray<double>(inCoef.size()));
    MutableNativeColumnVector waldPValues(
        inAllocator.allocateArray<double>(inCoef.size()));
    MutableNativeColumnVector oddsRatios(
        inAllocator.allocateArray<double>(inCoef.size()));

    for (Index i = 0; i < inCoef.size(); ++i) {
        stdErr(i) = std::sqrt(diagonal_of_hessian(i));
        waldZStats(i) = inCoef(i) / stdErr(i);
        waldPValues(i) = 2. * prob::cdf( prob::normal(),
            -std::abs(waldZStats(i)));
        oddsRatios(i) = std::exp( inCoef(i) );
    }
    int num_iterations = 0;
    // Return all coefficients, standard errors, etc. in a tuple
    AnyType tuple;
    tuple << ref_category << inCoef << logLikelihood << stdErr
          << waldZStats << waldPValues << oddsRatios
          << static_cast<double>(state.conditionNo) << num_iterations << num_processed;
    return tuple;
}

/**
 * @brief Return the difference in log-likelihood between two states
 */
AnyType
__internal_mlogregr_irls_step_distance::run(AnyType &args) {
    MLogRegrIRLSTransitionState<ArrayHandle<double> > stateLeft = args[0];
    MLogRegrIRLSTransitionState<ArrayHandle<double> > stateRight = args[1];

    return std::abs(stateLeft.logLikelihood - stateRight.logLikelihood);
}

/**
 * @brief Return the coefficients and diagnostic statistics of the state
 */
AnyType
__internal_mlogregr_irls_result::run(AnyType &args) {
    MLogRegrIRLSTransitionState<ArrayHandle<double> > state = args[0];

    return mLogstateToResult(*this, state);
    // state.ref_category, state.coef,
    // state.gradient, state.logLikelihood, state.X_transp_AX(0,0));
}

/**
 * @brief Return the coefficients and diagnostic statistics of the state
 */
AnyType
__internal_mlogregr_summary_results::run(AnyType &args) {
    MLogRegrIRLSTransitionState<ArrayHandle<double> > state = args[0];
    const Matrix & X_transp_AX_inverse = state.X_transp_AX;
    // X_transp_AX is actually it's inverse - this is a hack added at the end of
    // the final function
    AnyType tuple;
    Matrix coef = state.coef;
    coef.resize(state.numCategories, state.widthOfX);
    coef.transposeInPlace();
    tuple << coef << X_transp_AX_inverse;
    return tuple;
}
// ----------------- End of Multinomial Logistic Regression --------------------

// ---------------------------------------------------------------------------
//             Robust Variance Multi-Logistic
// ---------------------------------------------------------------------------

AnyType
mlogregr_robust_step_transition::run(AnyType &args) {
    using std::endl;

	MLogRegrRobustTransitionState<MutableArrayHandle<double> > state = args[0];

   if (args[1].isNull() || args[2].isNull() || args[3].isNull() ||
            args[4].isNull() || args[5].isNull()) {
        return args[0];
    }

    // Get x as a vector of double
    MappedColumnVector x;
    try{
        // an exception is raised in the backend if args[2] contains nulls
        MappedColumnVector xx = args[4].getAs<MappedColumnVector>();
        // x is a const reference, we can only rebind to change its pointer
        x.rebind(xx.memoryHandle(), xx.size());
    } catch (const ArrayWithNullException &e) {
        return args[0];
    }

    // Get the category & numCategories as integer
    int16_t category = static_cast<int16_t>(args[1].getAs<int>());
    // Number of categories after pivoting (We pivot around the first category)
    int16_t numCategories = static_cast<int16_t>(args[2].getAs<int>() - 1);
    int32_t ref_category = args[3].getAs<int32_t>();
	MappedMatrix coefMat = args[5].getAs<MappedMatrix>();

    // The following check was added with MADLIB-138.
    if (!x.is_finite())
            throw std::domain_error("Design matrix is not finite.");

    if (state.numRows == 0) {
        if (x.size() > std::numeric_limits<uint16_t>::max())
                throw std::domain_error("Number of independent variables cannot be "
                        "larger than 65535.");
        if (numCategories < 1)
                throw std::domain_error("Number of cateogires must be at least 2");
        if (category > numCategories)
                throw std::domain_error("You have entered a category > numCategories"
                    "Categories must be of values {0,1... numCategories-1}");

        // Init the state (requires x.size() and category.size())
        state.initialize(*this,
            static_cast<uint16_t>(x.size()) ,
            static_cast<uint16_t>(numCategories),
            static_cast<uint16_t>(ref_category));

        Matrix mat = coefMat;
        mat.transposeInPlace();
        mat.resize(coefMat.size(), 1);
        state.coef = mat;
    }

    // Now do the transition step
    state.numRows++;
    /*
        Get y: Convert to 1/0 boolean vector
        Example: Category 4 : 0 0 0 1 0 0
        Storing it in this forms helps us get a nice closed form expression
    */

    //To pivot around the specified reference category
    ColumnVector y(numCategories);
    y.fill(0);
    if (category > ref_category) {
        y(category - 1) = 1;
    } else if (category < ref_category) {
        y(category) = 1;
    }

    /*
    Compute the parameter vector (the 'pi' vector in the documentation)
    for the data point being processed.
    Casting the coefficients into a matrix makes the calculation simple.
    */

    Matrix coef = state.coef;
    coef.resize(numCategories, state.widthOfX);

    //Store the intermediate calculations because we'll reuse them in the LLH
    ColumnVector t1 = x; //t1 is vector of size state.widthOfX
    t1 = coef*x;
    /*
        Note: The above 2 lines could have been written as:
        ColumnVector t1 = -coef*x;

        but this creates warnings. These warnings are somehow related to the factor
        that x is an immutable type.
    */

    ColumnVector t2 = t1.array().exp();
    double t3 = 1 + t2.sum();
    ColumnVector pi = t2/t3;

	//The gradient matrix has numCategories rows and widthOfX columns
    Matrix grad = -y * x.transpose() + pi * x.transpose();
    //We cast the gradient into a vector to make the math easier.
    grad.resize(numCategories * state.widthOfX, 1);

    Matrix GradGradTranspose;
    GradGradTranspose = grad * grad.transpose();
	state.meat += GradGradTranspose;

    /*
         a is a matrix of size JxJ where J is the number of categories
         a_j1j2 = -pi(j1)*(1-pi(j2))if j1 == j2
         a_j1j2 =  pi(j1)*pi(j2) if j1 != j2
    */
    // Compute the 'a' matrix.
    Matrix a(numCategories,numCategories);
    Matrix piDiag = pi.asDiagonal();
    a = pi * pi.transpose() - piDiag;

    //Start the Hessian calculations
    Matrix X_transp_AX(numCategories * state.widthOfX, numCategories * state.widthOfX);

    /*
        Again: The following 3 lines could have been written as
        Matrix XXTrans = x * x.transpose();
        but it creates warnings related to the type of x. Here is an easy fix
    */
    Matrix cv_x = x;
    Matrix XXTrans = trans(cv_x);
    XXTrans = cv_x * XXTrans;

    //Eigen doesn't supported outer-products for matrices, so we have to do our own.
    //This operation is also known as a tensor-product.
    for (int i1 = 0; i1 < state.widthOfX; i1++){
         for (int i2 = 0; i2 <state.widthOfX; i2++){
            int rowOffset = numCategories * i1;
            int colOffset = numCategories * i2;

            X_transp_AX.block(rowOffset, colOffset, numCategories,  numCategories) = XXTrans(i1, i2) * a;
        }
    }

    triangularView<Lower>(state.X_transp_AX) += X_transp_AX;

    return state;

}

/**
 * @brief Perform the perliminary aggregation function: Merge transition states
     This merge step is the same as that of logistic regression.
 */
AnyType
mlogregr_robust_step_merge_states::run(AnyType &args) {
    MLogRegrRobustTransitionState<MutableArrayHandle<double> > stateLeft = args[0];
    MLogRegrRobustTransitionState<ArrayHandle<double> > stateRight = args[1];

    // We first handle the trivial case where this function is called with one
    // of the states being the initial state
    if (stateLeft.numRows == 0)
        return stateRight;
    else if (stateRight.numRows == 0)
        return stateLeft;

    // Merge states together and return
    stateLeft += stateRight;
    return stateLeft;
}

AnyType MLrobuststateToResult(
    const Allocator &inAllocator,
    int ref_category,
    const HandleMap<const ColumnVector, TransparentHandle<double> >& inCoef,
    const ColumnVector &diagonal_of_varianceMat) {

	MutableNativeColumnVector variance(
        inAllocator.allocateArray<double>(inCoef.size()));
    MutableNativeColumnVector stdErr(
        inAllocator.allocateArray<double>(inCoef.size()));
    MutableNativeColumnVector waldZStats(
        inAllocator.allocateArray<double>(inCoef.size()));
    MutableNativeColumnVector waldPValues(
        inAllocator.allocateArray<double>(inCoef.size()));

    for (Index i = 0; i < inCoef.size(); ++i) {
        variance(i) = diagonal_of_varianceMat(i);
        stdErr(i) = std::sqrt(diagonal_of_varianceMat(i));
        waldZStats(i) = inCoef(i) / stdErr(i);
        waldPValues(i) = 2. * prob::cdf(
            prob::normal(), -std::abs(waldZStats(i)));
    }

    // Return all coefficients, standard errors, etc. in a tuple
    AnyType tuple;
    tuple <<  ref_category << inCoef << stdErr << waldZStats << waldPValues;
    return tuple;
}

/**
 * @brief Perform the logistic-regression final step
 */
AnyType
mlogregr_robust_step_final::run(AnyType &args) {
    // We request a mutable object. Depending on the backend, this might perform
    // a deep copy.
    MLogRegrRobustTransitionState<ArrayHandle<double> > state = args[0];
    // Aggregates that haven't seen any data just return Null.
    if (state.numRows == 0)
        return Null();

    // See MADLIB-138. At least on certain platforms and with certain versions,
    // LAPACK will run into an infinite loop if pinv() is called for non-finite
    // matrices. We extend the check also to the dependent variables.
    if (!state.X_transp_AX.is_finite())
        throw NoSolutionFoundException("Over- or underflow in intermediate "
            "calculation. Input data is likely of poor numerical condition.");

    SymmetricPositiveDefiniteEigenDecomposition<Matrix> decomposition(
        -1 * state.X_transp_AX, EigenvaluesOnly, ComputePseudoInverse);

    // Precompute (X^T * A * X)^-1
    Matrix bread = decomposition.pseudoInverse();
	Matrix varianceMat;
    varianceMat = bread * state.meat * bread;

    if(!state.coef.is_finite())
        throw NoSolutionFoundException("Over- or underflow in Newton step, "
            "while updating coefficients. Input data is likely of poor "
            "numerical condition.");

    return MLrobuststateToResult(*this, state.ref_category, state.coef, varianceMat.diagonal());
}
// ------------------------ End of Robust Variance -----------------------------

AnyType __sub_array::run(AnyType &args) {
    if (args[0].isNull() || args[1].isNull())
        return Null();

    ArrayHandle<double> value = args[0].getAs<ArrayHandle<double> >();
    ArrayHandle<int32> index = args[1].getAs<ArrayHandle<int32> >();

    for (size_t i = 0; i < index.size(); i++) {
        if (index[i] < 1 || index[i] > static_cast<int>(value.size()))
            throw std::domain_error("Invalid indices - out of bound");
    }

    MutableArrayHandle<double> res =
        allocateArray<double, dbal::AggregateContext, dbal::DoZero,
            dbal::ThrowBadAlloc>(index.size());

    for (size_t i = 0; i < index.size(); i++)
        res[i] = value[index[i] - 1];

    return res;
}

// ----------------------------------------------------------------------------
typedef struct __sr_ctx{
    const double * inarray;
    int32_t maxcall;
    int32_t num_feature;
    int32_t num_category;
    int32_t ref_category;
    int32_t curcall;
} sr_ctx;

void *
__mlogregr_format::SRF_init(AnyType &args) {
    sr_ctx *ctx = new sr_ctx;
    ctx->curcall = 0;

    MutableArrayHandle<double> inarray = NULL;
    try{
        inarray = args[0].getAs<MutableArrayHandle<double> >();
    } catch (const ArrayWithNullException &e) {
        ctx->maxcall = 0;
        return ctx;
    }

    int32_t num_feature = args[1].getAs<int32_t>();
    int32_t num_category = args[2].getAs<int32_t>();
    int32_t ref_category = args[3].getAs<int32_t>();

    ctx->inarray = inarray.ptr();
    ctx->maxcall = num_category - 1;
    ctx->num_category = num_category - 1;
    ctx->num_feature = num_feature;
    ctx->ref_category = ref_category;

    if (num_feature * (num_category - 1) !=
            static_cast<int32_t>(inarray.size())) {
        throw std::runtime_error("num_feature * (num_category - 1) != "
                "inarray.size()");
    }

    if (ref_category >= num_category){
        throw std::runtime_error("ref_category >= num_category");
    }

    return ctx;
}

AnyType
__mlogregr_format::SRF_next(void * user_fctx, bool * is_last_call) {
    sr_ctx * ctx = (sr_ctx *) user_fctx;
    if (ctx->curcall >= ctx->maxcall) {
        *is_last_call = true;
        return Null();
    }

    MutableArrayHandle<double> outarray =
        allocateArray<double, dbal::FunctionContext,
            dbal::DoZero, dbal::ThrowBadAlloc>(ctx->num_feature);
    for(int i = 0; i < ctx->num_feature; i++) {
        outarray[i] = ctx->inarray[i * ctx->num_category + ctx->curcall];
    }

    AnyType tuple;
    tuple << (ctx->curcall < ctx->ref_category
            ? ctx->curcall
            : ctx->curcall + 1)
          << outarray;

    ctx->curcall++;

    return tuple;
}
/* ------------------------------------------------------------ */

AnyType mlogregr_predict_prob::run(AnyType &args)
{
    // dimension: N x (L - 1), where L is the number of categories
    MappedMatrix coef = args[0].getAs<MappedMatrix>();

    int ref_category = args[1].getAs<int>();
    MappedColumnVector x;
    try {
        MappedColumnVector xx = args[2].getAs<MappedColumnVector>();
        x.rebind(xx.memoryHandle(), xx.size());
    } catch (const ArrayWithNullException &e) {
        return Null();
    }

    int L = static_cast<int>(coef.cols()) + 1; // number of categories

    ColumnVector res(L);
    int cat_index = 0;
    for (int i = 0; i < L; i++) {
        if (i == ref_category) {
            res(i) = 1;
        } else {
            res(i) = exp(coef.col(cat_index).dot(x));
            cat_index++;
        }
    }
    res /= res.sum();
    return res;
}
/* ------------------------------------------------------------ */

AnyType mlogregr_predict_response::run(AnyType &args)
{
    // dimension: N x (L - 1), where L is the number of categories
    MappedMatrix coef = args[0].getAs<MappedMatrix>();

    int ref_category = args[1].getAs<int>();
    MappedColumnVector x;
    try {
        MappedColumnVector xx = args[2].getAs<MappedColumnVector>();
        x.rebind(xx.memoryHandle(), xx.size());
    } catch (const ArrayWithNullException &e) {
        return Null();
    }

    ColumnVector linear_predictors(x.transpose() * coef);
    Index max_cat = 0;
    linear_predictors.maxCoeff(&max_cat);
    double max_lp = linear_predictors(max_cat);

    if (exp(max_lp) < 1){
        // no category has high enough probability as reference category
        return ref_category;
    } else if (max_cat < ref_category){
        return static_cast<uint>(max_cat);
    }
    else{
        // since ref_category is not present in the coef matrix, index of
        // categories after ref_category is 1 less than the actual index
        return static_cast<uint>(max_cat + 1u);
    }
}

} // namespace regress

} // namespace modules

} // namespace madlib


//
//  expr/atom.hpp
//  solver
//
//  Created by Alexandre Martin on 28/04/2016.
//  Copyright © 2016 scalexm. All rights reserved.
//

#ifndef ATOM_HPP
#define ATOM_HPP

#include <ostream>
#include <boost/variant.hpp>
#include <vector>

namespace expr { namespace atom {
    using variable = int;

    struct fun;

    using term = boost::variant<variable, fun>;

    struct fun {
        char name;
        std::vector<term> args;
    };

    inline bool operator ==(const fun & a, const fun & b) {
        return a.name == b.name && a.args == b.args;
    }

    inline std::ostream & operator <<(std::ostream & stream, const fun & f) {
        stream << f.name;
        if (f.args.size() > 0) {
            stream << '(';
            for (auto i = 0; i < f.args.size(); ++i) {
                if (i != 0)
                    stream << ", ";
                stream << f.args[i];
            }
            stream << ')';
        }
        return stream;
    }

    struct equality {
        term left, right;
    };

    inline bool operator ==(const equality & a, const equality & b) {
        return a.left == b.left && a.right == b.right;
    }

    inline std::ostream & operator <<(std::ostream & stream, const equality & exp) {
        return stream << exp.left << " = " << exp.right;
    }
} }

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "ViewerCore.h"
#include "ViewerData.h"
#include "gl.h"
#include "../quat_to_mat.h"
#include "../snap_to_fixed_up.h"
#include "../look_at.h"
#include "../frustum.h"
#include "../ortho.h"
#include "../massmatrix.h"
#include "../barycenter.h"
#include "../PI.h"
#include <Eigen/Geometry>
#include <iostream>

IGL_INLINE void igl::opengl::ViewerCore::align_camera_center(
  const Eigen::MatrixXd& V,
  const Eigen::MatrixXi& F)
{
  if(V.rows() == 0)
    return;

  get_scale_and_shift_to_fit_mesh(V,F,camera_base_zoom,camera_base_translation);
  // Rather than crash on empty mesh...
  if(V.size() > 0)
  {
    object_scale = (V.colwise().maxCoeff() - V.colwise().minCoeff()).norm();
  }
}

IGL_INLINE void igl::opengl::ViewerCore::get_scale_and_shift_to_fit_mesh(
  const Eigen::MatrixXd& V,
  const Eigen::MatrixXi& F,
  float& zoom,
  Eigen::Vector3f& shift)
{
  if (V.rows() == 0)
    return;

  Eigen::MatrixXd BC;
  if (F.rows() <= 1)
  {
    BC = V;
  } else
  {
    igl::barycenter(V,F,BC);
  }
  return get_scale_and_shift_to_fit_mesh(BC,zoom,shift);
}

IGL_INLINE void igl::opengl::ViewerCore::align_camera_center(
  const Eigen::MatrixXd& V)
{
  if(V.rows() == 0)
    return;

  get_scale_and_shift_to_fit_mesh(V,camera_base_zoom,camera_base_translation);
  // Rather than crash on empty mesh...
  if(V.size() > 0)
  {
    object_scale = (V.colwise().maxCoeff() - V.colwise().minCoeff()).norm();
  }
}

IGL_INLINE void igl::opengl::ViewerCore::get_scale_and_shift_to_fit_mesh(
  const Eigen::MatrixXd& V,
  float& zoom,
  Eigen::Vector3f& shift)
{
  if (V.rows() == 0)
    return;

  auto min_point = V.colwise().minCoeff();
  auto max_point = V.colwise().maxCoeff();
  auto centroid  = (0.5*(min_point + max_point)).eval();
  shift.setConstant(0);
  shift.head(centroid.size()) = -centroid.cast<float>();
  zoom = 2.0 / (max_point-min_point).array().abs().maxCoeff();
}

IGL_INLINE void igl::opengl::ViewerCore::clear_framebuffers()
{
  // The glScissor call ensures we only clear this core's buffers,
  // (in case the user wants different background colors in each viewport.)
  glScissor(viewport(0), viewport(1), viewport(2), viewport(3));
  glEnable(GL_SCISSOR_TEST);
  glClearColor(background_color[0],
               background_color[1],
               background_color[2],
               background_color[3]);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glDisable(GL_SCISSOR_TEST);
}

IGL_INLINE void igl::opengl::ViewerCore::draw(
  Eigen::Matrix4f &worldMat,
  ViewerData& data,
  bool update_matrices)
{
  using namespace std;
  using namespace Eigen;

  if (depth_test)
    glEnable(GL_DEPTH_TEST);
  else
    glDisable(GL_DEPTH_TEST);

  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  /* Bind and potentially refresh mesh/line/point data */
  if (data.dirty)
  {
    data.updateGL(data, data.invert_normals, data.meshgl);
    data.dirty = MeshGL::DIRTY_NONE;
  }
  data.meshgl.bind_mesh();

  // Initialize uniform
  glViewport(viewport(0), viewport(1), viewport(2), viewport(3));

  if(update_matrices)
  {
    view = Eigen::Matrix4f::Identity();
    proj = Eigen::Matrix4f::Identity();
    norm = Eigen::Matrix4f::Identity();

    float width  = viewport(2);
    float height = viewport(3);

    // Set view
    look_at( camera_eye, camera_center, camera_up, view);
    view = view
      * (trackball_angle * Eigen::Scaling(camera_zoom * camera_base_zoom)
      * Eigen::Translation3f(camera_translation + camera_base_translation)).matrix()* worldMat*data.MakeTrans();

    norm = view.inverse().transpose() ;

    // Set projection
    if (orthographic)
    {
      float length = (camera_eye - camera_center).norm();
      float h = tan(camera_view_angle/360.0 * igl::PI) * (length);
      ortho(-h*width/height, h*width/height, -h, h, camera_dnear, camera_dfar,proj);
    }
    else
    {
      float fH = tan(camera_view_angle / 360.0 * igl::PI) * camera_dnear;
      float fW = fH * (double)width/(double)height;
      frustum(-fW, fW, -fH, fH, camera_dnear, camera_dfar,proj);
    }
  }

  // Send transformations to the GPU
  GLint viewi  = glGetUniformLocation(data.meshgl.shader_mesh,"view");
  GLint proji  = glGetUniformLocation(data.meshgl.shader_mesh,"proj");
  GLint normi  = glGetUniformLocation(data.meshgl.shader_mesh,"normal_matrix");
  glUniformMatrix4fv(viewi, 1, GL_FALSE, view.data());
  glUniformMatrix4fv(proji, 1, GL_FALSE, proj.data());
  glUniformMatrix4fv(normi, 1, GL_FALSE, norm.data());

  // Light parameters
  GLint specular_exponenti    = glGetUniformLocation(data.meshgl.shader_mesh,"specular_exponent");
  GLint light_position_eyei = glGetUniformLocation(data.meshgl.shader_mesh,"light_position_eye");
  GLint lighting_factori      = glGetUniformLocation(data.meshgl.shader_mesh,"lighting_factor");
  GLint fixed_colori          = glGetUniformLocation(data.meshgl.shader_mesh,"fixed_color");
  GLint texture_factori       = glGetUniformLocation(data.meshgl.shader_mesh,"texture_factor");

  glUniform1f(specular_exponenti, data.shininess);
  glUniform3fv(light_position_eyei, 1, light_position.data());
  glUniform1f(lighting_factori, lighting_factor); // enables lighting
  glUniform4f(fixed_colori, 0.0, 0.0, 0.0, 0.0);

  if (data.V.rows()>0)
  {
    // Render fill
    if (is_set(data.show_faces))
    {
      // Texture
      glUniform1f(texture_factori, is_set(data.show_texture) ? 1.0f : 0.0f);
      data.meshgl.draw_mesh(true);
      glUniform1f(texture_factori, 0.0f);
    }

    // Render wireframe
    if (is_set(data.show_lines))
    {
      glLineWidth(data.line_width);
      glUniform4f(fixed_colori,
        data.line_color[0],
        data.line_color[1],
        data.line_color[2], 1.0f);
      data.meshgl.draw_mesh(false);
      glUniform4f(fixed_colori, 0.0f, 0.0f, 0.0f, 0.0f);
    }
  }

  if (is_set(data.show_overlay))
  {
    if (is_set(data.show_overlay_depth))
      glEnable(GL_DEPTH_TEST);
    else
      glDisable(GL_DEPTH_TEST);

    if (data.lines.rows() > 0)
    {
      data.meshgl.bind_overlay_lines();
      viewi  = glGetUniformLocation(data.meshgl.shader_overlay_lines,"view");
      proji  = glGetUniformLocation(data.meshgl.shader_overlay_lines,"proj");

      glUniformMatrix4fv(viewi, 1, GL_FALSE, view.data());
      glUniformMatrix4fv(proji, 1, GL_FALSE, proj.data());
      // This must be enabled, otherwise glLineWidth has no effect
      glEnable(GL_LINE_SMOOTH);
      glLineWidth(data.line_width);

      data.meshgl.draw_overlay_lines();
    }

    if (data.points.rows() > 0)
    {
      data.meshgl.bind_overlay_points();
      viewi  = glGetUniformLocation(data.meshgl.shader_overlay_points,"view");
      proji  = glGetUniformLocation(data.meshgl.shader_overlay_points,"proj");

      glUniformMatrix4fv(viewi, 1, GL_FALSE, view.data());
      glUniformMatrix4fv(proji, 1, GL_FALSE, proj.data());
      glPointSize(data.point_size);

      data.meshgl.draw_overlay_points();
    }

    glEnable(GL_DEPTH_TEST);
  }

}

IGL_INLINE void igl::opengl::ViewerCore::UpdateUniforms(Eigen::Matrix4f &worldMat, ViewerData& data, bool update_matrices)
{

}

IGL_INLINE void igl::opengl::ViewerCore::draw_buffer(Eigen::Matrix4f &worldMat, ViewerData& data,
  bool update_matrices,
  Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& R,
  Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& G,
  Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& B,
  Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic>& A)
{
  assert(R.rows() == G.rows() && G.rows() == B.rows() && B.rows() == A.rows());
  assert(R.cols() == G.cols() && G.cols() == B.cols() && B.cols() == A.cols());

  unsigned width = R.rows();
  unsigned height = R.cols();

  // https://learnopengl.com/Advanced-OpenGL/Anti-Aliasing
  unsigned int framebuffer;
  glGenFramebuffers(1, &framebuffer);
  glBindFramebuffer(GL_FRAMEBUFFER, framebuffer);
  // create a multisampled color attachment texture
  unsigned int textureColorBufferMultiSampled;
  glGenTextures(1, &textureColorBufferMultiSampled);
  glBindTexture(GL_TEXTURE_2D_MULTISAMPLE, textureColorBufferMultiSampled);
  glTexImage2DMultisample(GL_TEXTURE_2D_MULTISAMPLE, 4, GL_RGBA, width, height, GL_TRUE);
  glBindTexture(GL_TEXTURE_2D_MULTISAMPLE, 0);
  glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D_MULTISAMPLE, textureColorBufferMultiSampled, 0);
  // create a (also multisampled) renderbuffer object for depth and stencil attachments
  unsigned int rbo;
  glGenRenderbuffers(1, &rbo);
  glBindRenderbuffer(GL_RENDERBUFFER, rbo);
  glRenderbufferStorageMultisample(GL_RENDERBUFFER, 4, GL_DEPTH24_STENCIL8, width, height);
  glBindRenderbuffer(GL_RENDERBUFFER, 0);
  glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_STENCIL_ATTACHMENT, GL_RENDERBUFFER, rbo);
  assert(glCheckFramebufferStatus(GL_FRAMEBUFFER) == GL_FRAMEBUFFER_COMPLETE);
  glBindFramebuffer(GL_FRAMEBUFFER, 0);

  // configure second post-processing framebuffer
  unsigned int intermediateFBO;
  glGenFramebuffers(1, &intermediateFBO);
  glBindFramebuffer(GL_FRAMEBUFFER, intermediateFBO);
  // create a color attachment texture
  unsigned int screenTexture;
  glGenTextures(1, &screenTexture);
  glBindTexture(GL_TEXTURE_2D, screenTexture);
  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
  glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, screenTexture, 0);	// we only need a color buffer
  assert(glCheckFramebufferStatus(GL_FRAMEBUFFER) == GL_FRAMEBUFFER_COMPLETE);
  glBindFramebuffer(GL_FRAMEBUFFER, 0);

  glBindFramebuffer(GL_FRAMEBUFFER, framebuffer);

  // Clear the buffer
  glClearColor(background_color(0), background_color(1), background_color(2), 0.f);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  // Save old viewport
  Eigen::Vector4f viewport_ori = viewport;
  viewport << 0,0,width,height;
  // Draw
  draw(worldMat,data,update_matrices);
  // Restore viewport
  viewport = viewport_ori;

  glBindFramebuffer(GL_READ_FRAMEBUFFER, framebuffer);
  glBindFramebuffer(GL_DRAW_FRAMEBUFFER, intermediateFBO);
  glBlitFramebuffer(0, 0, width, height, 0, 0, width, height, GL_COLOR_BUFFER_BIT, GL_NEAREST);

  glBindFramebuffer(GL_FRAMEBUFFER, intermediateFBO);
  // Copy back in the given Eigen matrices
  GLubyte* pixels = (GLubyte*)calloc(width*height*4,sizeof(GLubyte));
  glReadPixels(0, 0,width, height,GL_RGBA, GL_UNSIGNED_BYTE, pixels);

  // Clean up
  glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0);
  glBindFramebuffer(GL_READ_FRAMEBUFFER, 0);
  glBindFramebuffer(GL_FRAMEBUFFER, 0);
  glDeleteTextures(1, &screenTexture);
  glDeleteTextures(1, &textureColorBufferMultiSampled);
  glDeleteFramebuffers(1, &framebuffer);
  glDeleteFramebuffers(1, &intermediateFBO);
  glDeleteRenderbuffers(1, &rbo);

  int count = 0;
  for (unsigned j=0; j<height; ++j)
  {
    for (unsigned i=0; i<width; ++i)
    {
      R(i,j) = pixels[count*4+0];
      G(i,j) = pixels[count*4+1];
      B(i,j) = pixels[count*4+2];
      A(i,j) = pixels[count*4+3];
      ++count;
    }
  }
  // Clean up
  free(pixels);
}

IGL_INLINE void igl::opengl::ViewerCore::set_rotation_type(
  const igl::opengl::ViewerCore::RotationType & value)
{
  using namespace Eigen;
  using namespace std;
  const RotationType old_rotation_type = rotation_type;
  rotation_type = value;
  if(rotation_type == ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP &&
    old_rotation_type != ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP)
  {
    snap_to_fixed_up(Quaternionf(trackball_angle),trackball_angle);
  }
}

IGL_INLINE void igl::opengl::ViewerCore::set(unsigned int &property_mask, bool value) const
{
  if (!value)
    unset(property_mask);
  else
    property_mask |= id;
}

IGL_INLINE void igl::opengl::ViewerCore::unset(unsigned int &property_mask) const
{
  property_mask &= ~id;
}

IGL_INLINE void igl::opengl::ViewerCore::toggle(unsigned int &property_mask) const
{
  property_mask ^= id;
}

IGL_INLINE bool igl::opengl::ViewerCore::is_set(unsigned int property_mask) const
{
  return (property_mask & id);
}

IGL_INLINE igl::opengl::ViewerCore::ViewerCore()
{
  // Default colors
  background_color << 0.3f, 0.3f, 0.5f, 1.0f;

  // Default lights settings
  light_position << 0.0f, 0.3f, 0.0f;
  lighting_factor = 1.0f; //on

  // Default trackball
  trackball_angle = Eigen::Quaternionf::Identity();
  set_rotation_type(ViewerCore::ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP);

  // Camera parameters
  camera_base_zoom = 1.0f;
  camera_zoom = 1.0f;
  orthographic = false;
  camera_view_angle = 45.0;
  camera_dnear = 1.0;
  camera_dfar = 100.0;
  camera_base_translation << 0, 0, 0;
  camera_translation << 0, 0, 0;
  camera_eye << 0, 0, 5;
  camera_center << 0, 0, 0;
  camera_up << 0, 1, 0;

  depth_test = true;

  is_animating = false;
  animation_max_fps = 30.;

  viewport.setZero();
}

IGL_INLINE void igl::opengl::ViewerCore::init()
{
}

IGL_INLINE void igl::opengl::ViewerCore::shut()
{
}


// Copyright (c) 2014
// Akira Takahashi, Fumiki Fukuda.
// Released under the CC0 1.0 Universal license.

#include <iostream>
#include <cmath> // sqrt
#include <algorithm> // min
#include <utility> // pair
#include <boost/utility/enable_if.hpp>

struct point_category {};

template <class T, class Enable = void>
struct get_geometry_category;

template <class T, class Enable = void>
struct point_traits {
  static double x(const T& p) { return p.x(); }
  static double y(const T& p) { return p.y(); }

  static T subtract(const T& a, const T& b)
    { return T(a.x() - b.x(), a.y() - b.y()); }
};

class point {
  double x_ = 0;
  double y_ = 0;
public:
  point() = default;
  point(double x, double y)
    : x_(x), y_(y) {}

  // それぞれの座標を取得する
  double x() const { return x_; }
  double y() const { return y_; }
};

template <>
struct get_geometry_category<point> {
  typedef point_category type;
};

// 別の計算幾何ライブラリ
namespace geo {

template <class T>
struct is_point_category {
  static const bool value = false;
};

template <class T>
struct point_traits {
  static double getX(const T& p) { return p.getX(); }
  static double getY(const T& p) { return p.getY(); }

  static T subtract(const T& a, const T& b)
    { return T(a.getX() - b.getX(), a.getY() - b.getY()); }
};

class point {
  double x_ = 0;
  double y_ = 0;
public:
  point() = default;
  point(double x, double y)
    : x_(x), y_(y) {}

  // それぞれの座標を取得する
  double getX() const { return x_; }
  double getY() const { return y_; }
};

template <>
struct is_point_category<point> {
  static const bool value = true;
};

template <>
struct point_traits<std::pair<double, double>> {
  typedef std::pair<double, double> point_type;

  static double getX(const point_type& p)
    { return p.first; }

  static double getY(const point_type& p)
    { return p.second; }

  static point_type subtract(
                      const point_type& a,
                      const point_type& b)
  {
    return std::make_pair(
             a.first - b.first,
             a.second - b.second);
  }
};

template <>
struct is_point_category<std::pair<double, double>> {
  static const bool value = true;
};

} // namespace geo

template <class T>
struct get_geometry_category<
         T,
         typename boost::enable_if<
           geo::is_point_category<T>
         >::type
       > {
  typedef point_category type;
};

template <class T>
struct point_traits<
         T,
         typename boost::enable_if<
           geo::is_point_category<T>
         >::type
       > {
  static double x(const T& p) { return geo::point_traits<T>::getX(p); }
  static double y(const T& p) { return geo::point_traits<T>::getY(p); }

  static T subtract(const T& a, const T& b)
    { return geo::point_traits<T>::subtract(a, b); }
};

// 点と点
template <class Point>
double distance_impl(Point a, Point b,
                     point_category, point_category)
{
  typedef point_traits<Point> traits;
  const Point d = traits::subtract(a, b);
  return std::sqrt(traits::x(d) * traits::x(d) +
                   traits::y(d) * traits::y(d));
}

template <class Geometry1, class Geometry2>
double distance(Geometry1 a, Geometry2 b)
{
  return distance_impl(a, b,
           typename get_geometry_category<Geometry1>::type(),
           typename get_geometry_category<Geometry2>::type());
}

int main()
{
  {
    point p1(0.0, 0.0);
    point p2(3.0, 3.0);

    double d = distance(p1, p2);
    std::cout << d << std::endl;
  }
  {
    geo::point p1(0.0, 0.0);
    geo::point p2(3.0, 3.0);

    double d = distance(p1, p2);
    std::cout << d << std::endl;
  }
  {
    std::pair<double, double> p1(0.0, 0.0);
    std::pair<double, double> p2(3.0, 3.0);

    double d = distance(p1, p2);
    std::cout << d << std::endl;
  }
}