// COL216 Minor
// Sayam Sethi 2019CS10399

#include <bits/stdc++.h>
#include <boost/tokenizer.hpp>

using namespace std;

// struct to store the registers and the functions to be executed
struct MIPS_Architecture
{
	// "static" vars
	static const int MAX = (1 << 20), ROWS = (1 << 10);
	int row_access_delay, col_access_delay;
	unordered_map<string, function<int(MIPS_Architecture &, string, string, string)>> instructions;
	unordered_map<string, int> registerMap, address;
	vector<vector<string>> commands;
	// "dynamic" vars
	int registers[32], registersBuffer[32], PCcurr, PCnext, delay, currBuffer, clockCycles, rowBufferUpdates;
	vector<vector<int>> data;
	vector<int> commandCount;
	queue<pair<pair<bool, int>, array<int, 5>>> DRAM_Buffer;

	// constructor to initialise the instruction set
	MIPS_Architecture(ifstream &file, int row_delay, int col_delay)
	{
		row_access_delay = row_delay, col_access_delay = col_delay;

		instructions = {{"add", &MIPS_Architecture::add}, {"sub", &MIPS_Architecture::sub}, {"mul", &MIPS_Architecture::mul}, {"beq", &MIPS_Architecture::beq}, {"bne", &MIPS_Architecture::bne}, {"slt", &MIPS_Architecture::slt}, {"j", &MIPS_Architecture::j}, {"lw", &MIPS_Architecture::lw}, {"sw", &MIPS_Architecture::sw}, {"addi", &MIPS_Architecture::addi}};

		for (int i = 0; i < 32; ++i)
			registerMap["$" + to_string(i)] = i;
		registerMap["$zero"] = 0;
		registerMap["$at"] = 1;
		registerMap["$v0"] = 2;
		registerMap["$v1"] = 3;
		for (int i = 0; i < 4; ++i)
			registerMap["$a" + to_string(i)] = i + 4;
		for (int i = 0; i < 8; ++i)
			registerMap["$t" + to_string(i)] = i + 8, registerMap["$s" + to_string(i)] = i + 16;
		registerMap["$t8"] = 24;
		registerMap["$t9"] = 25;
		registerMap["$k0"] = 26;
		registerMap["$k1"] = 27;
		registerMap["$gp"] = 28;
		registerMap["$sp"] = 29;
		registerMap["$s8"] = 30;
		registerMap["$ra"] = 31;

		constructCommands(file);
	}

	// perform add immediate operation
	int addi(string r1, string r2, string num)
	{
		if (!checkRegisters({r1, r2}) || registerMap[r1] == 0)
			return 1;
		try
		{
			if (registersBuffer[registerMap[r1]] || registersBuffer[registerMap[r2]])
				return -1;
			registers[registerMap[r1]] = registers[registerMap[r2]] + stoi(num);
			PCnext = PCcurr + 1;
			return 0;
		}
		catch (exception &e)
		{
			return 4;
		}
	}

	// perform add operation
	int add(string r1, string r2, string r3)
	{
		return op(r1, r2, r3, [&](int a, int b) { return a + b; });
	}

	// perform subtraction operation
	int sub(string r1, string r2, string r3)
	{
		return op(r1, r2, r3, [&](int a, int b) { return a - b; });
	}

	// perform multiplication operation
	int mul(string r1, string r2, string r3)
	{
		return op(r1, r2, r3, [&](int a, int b) { return a * b; });
	}

	// perform the operation
	int op(string r1, string r2, string r3, function<int(int, int)> operation)
	{
		if (!checkRegisters({r1, r2, r3}) || registerMap[r1] == 0)
			return 1;
		if (registersBuffer[registerMap[r1]] || registersBuffer[registerMap[r2]] || registersBuffer[registerMap[r3]])
			return -1;
		registers[registerMap[r1]] = operation(registers[registerMap[r2]], registers[registerMap[r3]]);
		PCnext = PCcurr + 1;
		return 0;
	}

	// perform the beq operation
	int beq(string r1, string r2, string label)
	{
		return bOP(r1, r2, label, [](int a, int b) { return a == b; });
	}

	// perform the bne operation
	int bne(string r1, string r2, string label)
	{
		return bOP(r1, r2, label, [](int a, int b) { return a != b; });
	}

	// implements beq and bne by taking the comparator
	int bOP(string r1, string r2, string label, function<bool(int, int)> comp)
	{
		if (!checkLabel(label))
			return 4;
		if (address.find(label) == address.end() || address[label] == -1)
			return 2;
		if (!checkRegisters({r1, r2}))
			return 1;
		if (registersBuffer[registerMap[r1]] || registersBuffer[registerMap[r2]])
			return -1;
		PCnext = comp(registers[registerMap[r1]], registers[registerMap[r2]]) ? address[label] : PCcurr + 1;
		return 0;
	}

	// implements slt operation
	int slt(string r1, string r2, string r3)
	{
		if (!checkRegisters({r1, r2, r3}) || registerMap[r1] == 0)
			return 1;
		if (registersBuffer[registerMap[r1]] || registersBuffer[registerMap[r2]] || registersBuffer[registerMap[r3]])
			return -1;
		registers[registerMap[r1]] = registers[registerMap[r2]] < registers[registerMap[r3]];
		PCnext = PCcurr + 1;
		return 0;
	}

	// perform the jump operation
	int j(string label, string unused1 = "", string unused2 = "")
	{
		if (!checkLabel(label))
			return 4;
		if (address.find(label) == address.end() || address[label] == -1)
			return 2;
		PCnext = address[label];
		return 0;
	}

	// perform load word operation
	int lw(string r, string location, string unused1 = "")
	{
		if (!checkRegister(r) || registerMap[r] == 0)
			return 1;
		int address = locateAddress(location);
		if (address < 0)
			return abs(address);
		bufferUpdate(address / ROWS);
		DRAM_Buffer.push({{1, PCcurr}, {delay, registerMap[r], address / ROWS, (address % ROWS) / 4, -1}});
		++registersBuffer[registerMap[r]];
		PCnext = PCcurr + 1;
		return 0;
	}

	// perform store word operation
	int sw(string r, string location, string unused1 = "")
	{
		if (!checkRegister(r))
			return 1;
		int address = locateAddress(location);
		if (address < 0)
			return abs(address);
		if (registersBuffer[registerMap[r]])
			return -1;
		bufferUpdate(address / ROWS);
		DRAM_Buffer.push({{0, PCcurr}, {delay, registers[registerMap[r]], address / ROWS, (address % ROWS) / 4, -1}});
		++rowBufferUpdates;
		PCnext = PCcurr + 1;
		return 0;
	}

	// implement buffer update
	void bufferUpdate(int row)
	{
		if (row == -1)
			delay = (currBuffer != -1) * row_access_delay, rowBufferUpdates += (currBuffer != -1);
		else if (currBuffer == -1)
			delay = row_access_delay + col_access_delay, ++rowBufferUpdates;
		else if (currBuffer != row)
			delay = 2 * row_access_delay + col_access_delay, ++rowBufferUpdates;
		else
			delay = col_access_delay;
		currBuffer = row;
	}

	// parse the address and return the actual address or error
	int locateAddress(string location)
	{
		if (location.back() == ')')
		{
			try
			{
				int lparen = location.find('('), offset = stoi(lparen == 0 ? "0" : location.substr(0, lparen));
				string reg = location.substr(lparen + 1);
				reg.pop_back();
				if (!checkRegister(reg))
					return -3;
				int address = registers[registerMap[reg]] + offset;
				if (address % 4 || address < int(4 * commands.size()) || address >= MAX)
					return -3;
				return address;
			}
			catch (exception &e)
			{
				return -4;
			}
		}
		try
		{
			int address = stoi(location);
			if (address % 4 || address < int(4 * commands.size()) || address >= MAX)
				return -3;
			return address;
		}
		catch (exception &e)
		{
			return -4;
		}
	}

	// checks if label is valid
	inline bool checkLabel(string str)
	{
		return str.size() > 0 && isalpha(str[0]) && all_of(++str.begin(), str.end(), [](char c) { return (bool)isalnum(c); }) && instructions.find(str) == instructions.end();
	}

	// checks if the register is a valid one
	inline bool checkRegister(string r)
	{
		return registerMap.find(r) != registerMap.end();
	}

	// checks if all of the registers are valid or not
	bool checkRegisters(vector<string> regs)
	{
		return all_of(regs.begin(), regs.end(), [&](string r) { return checkRegister(r); });
	}

	/*
		handle all exit codes:
		0: correct execution
		1: register provided is incorrect
		2: invalid label
		3: unaligned or invalid address
		4: syntax error
		5: commands exceed memory limit
	*/
	void handleExit(int code)
	{
		switch (code)
		{
		case 1:
			cerr << "Invalid register provided or syntax error in providing register\n";
			break;
		case 2:
			cerr << "Label used not defined or defined too many times\n";
			break;
		case 3:
			cerr << "Unaligned or invalid memory address specified\n";
			break;
		case 4:
			cerr << "Syntax error encountered\n";
			break;
		case 5:
			cerr << "Memory limit exceeded\n";
			break;
		default:
			break;
		}
		if (code != 0)
		{
			cerr << "Error encountered at:\n";
			for (auto &s : commands[PCcurr])
				cerr << s << ' ';
			cerr << '\n';
		}

		cout << "Exit code: " << code << '\n';

		cout << "\nThe Row Buffer was updated " << rowBufferUpdates << " times.\n";
		cout << "\nFollowing are the non-zero data values:\n";
		for (int i = 0; i < ROWS; ++i)
			for (int j = 0; j < ROWS / 4; ++j)
				if (data[i][j] != 0)
					cout << (ROWS * i + 4 * j) << '-' << (ROWS * i + 4 * j) + 3 << hex << ": " << data[i][j] << '\n'
						 << dec;
		cout << "\nTotal number of cycles: " << clockCycles << " + " << delay << " (cycles taken for code execution + final writeback delay)\n";
		cout << "\nCount of instructions executed:\n";
		for (int i = 0; i < (int)commands.size(); ++i)
		{
			cout << commandCount[i] << " times:\t";
			for (auto &s : commands[i])
				cout << s << ' ';
			cout << '\n';
		}
	}

	// parse the command assuming correctly formatted MIPS instruction (or label)
	void parseCommand(string line)
	{
		// strip until before the comment begins
		line = line.substr(0, line.find('#'));
		vector<string> command;
		boost::tokenizer<boost::char_separator<char>> tokens(line, boost::char_separator<char>(", \t"));
		for (auto &s : tokens)
			command.push_back(s);
		// empty line or a comment only line
		if (command.empty())
			return;
		else if (command.size() == 1)
		{
			string label = command[0].back() == ':' ? command[0].substr(0, command[0].size() - 1) : "?";
			if (address.find(label) == address.end())
				address[label] = commands.size();
			else
				address[label] = -1;
			command.clear();
		}
		else if (command[0].back() == ':')
		{
			string label = command[0].substr(0, command[0].size() - 1);
			if (address.find(label) == address.end())
				address[label] = commands.size();
			else
				address[label] = -1;
			command = vector<string>(command.begin() + 1, command.end());
		}
		else if (command[0].find(':') != string::npos)
		{
			int idx = command[0].find(':');
			string label = command[0].substr(0, idx);
			if (address.find(label) == address.end())
				address[label] = commands.size();
			else
				address[label] = -1;
			command[0] = command[0].substr(idx + 1);
		}
		else if (command[1][0] == ':')
		{
			if (address.find(command[0]) == address.end())
				address[command[0]] = commands.size();
			else
				address[command[0]] = -1;
			command[1] = command[1].substr(1);
			if (command[1] == "")
				command.erase(command.begin(), command.begin() + 2);
			else
				command.erase(command.begin(), command.begin() + 1);
		}
		if (command.empty())
			return;
		if (command.size() > 4)
			for (int i = 4; i < (int)command.size(); ++i)
				command[3] += " " + command[i];
		command.resize(4);
		commands.push_back(command);
	}

	// construct the commands vector from the input file
	void constructCommands(ifstream &file)
	{
		string line;
		while (getline(file, line))
			parseCommand(line);
		file.close();
	}

	// execute the commands sequentially
	void executeCommands()
	{
		if (commands.size() >= MAX / 4)
		{
			handleExit(5);
			return;
		}

		initVars();
		cout << "Cycle info:\n";
		while (PCcurr < commands.size())
		{
			delay = 0;
			vector<string> &command = commands[PCcurr];
			if (instructions.find(command[0]) == instructions.end())
			{
				handleExit(4);
				return;
			}
			int ret = instructions[command[0]](*this, command[1], command[2], command[3]);
			if (ret > 0)
			{
				handleExit(ret);
				return;
			}
			while (ret != 0)
			{
				updateRegisterBuffer();
				ret = instructions[command[0]](*this, command[1], command[2], command[3]);
			}
			++clockCycles;
			++commandCount[PCcurr];
			PCcurr = PCnext;
			if (!DRAM_Buffer.empty())
			{
				if (DRAM_Buffer.front().second[4] == -1)
					DRAM_Buffer.front().second[4] = clockCycles + 1;
				else if (--DRAM_Buffer.front().second[0] == 0)
					updateRegisterBuffer();
			}
			printCycleExecution(command);
		}
		while (!DRAM_Buffer.empty())
			updateRegisterBuffer();
		bufferUpdate(-1);
		handleExit(0);
	}

	// wait and finish DRAM communication
	void updateRegisterBuffer()
	{
		assert(!DRAM_Buffer.empty());
		auto &top = DRAM_Buffer.front();
		DRAM_Buffer.pop();
		auto &sec = top.second;
		clockCycles += sec[0];
		if (top.first.first)
		{
			registers[sec[1]] = data[sec[2]][sec[3]];
			--registersBuffer[sec[1]];
		}
		else
			data[sec[2]][sec[3]] = sec[1];
		if (!DRAM_Buffer.empty())
			DRAM_Buffer.front().second[4] = clockCycles + 1;
		printDRAMCompletion(top.first.second, sec[4], clockCycles);
	}

	// prints the cycle info of DRAM delay
	void printDRAMCompletion(int PCaddr, int begin, int end)
	{
		cout << begin << '-' << end << " (DRAM call executed): ";
		for (auto s : commands[PCaddr])
			cout << s << ' ';
		cout << "\n\n";
	}

	// print cycle info
	void printCycleExecution(vector<string> &command)
	{
		if (delay == 0)
		{
			cout << clockCycles << ": ";
		}
		else
			cout << clockCycles << " (DRAM call queued): ";
		for (auto &s : command)
			cout << s << ' ';
		cout << '\n';
		printRegisters();
		cout << "\n";
	}

	// print the register data in hexadecimal
	void printRegisters()
	{
		cout << hex;
		for (int i = 0; i < 32; ++i)
			cout << registers[i] << ' ';
		cout << dec << "\n";
	}

	// initialize variables before executing commands
	void initVars()
	{
		clockCycles = 0, PCcurr = 0, rowBufferUpdates = 0, currBuffer = -1;
		fill_n(registers, 32, 0);
		fill_n(registersBuffer, 32, 0);
		data = vector<vector<int>>(ROWS, vector<int>(ROWS >> 2, 0));
		commandCount.clear();
		commandCount.assign(commands.size(), 0);
		while (!DRAM_Buffer.empty())
			DRAM_Buffer.pop();
	}
};

int main(int argc, char *argv[])
{
	if (argc != 4)
	{
		cerr << "Required arguments: file_name ROW_ACCESS_DELAY COL_ACCESS_DELAY\n";
		return 0;
	}
	ifstream file(argv[1]);
	MIPS_Architecture *mips;
	if (file.is_open())
		try
		{
			mips = new MIPS_Architecture(file, stoi(argv[2]), stoi(argv[3]));
		}
		catch (exception &e)
		{
			cerr << "Required arguments: file_name ROW_ACCESS_DELAY COL_ACCESS_DELAY\n";
			return 0;
		}
	else
	{
		cerr << "File could not be opened. Terminating...\n";
		return 0;
	}

	mips->executeCommands();
	return 0;
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program 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.
//
// This program 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 this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph.hh"
#include "graph_filtering.hh"
#include "graph_properties.hh"
#include "graph_selectors.hh"
#include "graph_util.hh"

#include "random.hh"

#include <boost/python.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct do_maximal_vertex_set
{
    template <class Graph, class VertexIndex, class VertexSet,
              class RNG>
    void operator()(const Graph& g, VertexIndex vertex_index, VertexSet mvs,
                    bool high_deg, RNG& rng) const
    {
        typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

        VertexSet marked(vertex_index, num_vertices(g));
        vector<vertex_t> vlist;
        double max_deg = 0, tmp_max_deg = 0;
        typename graph_traits<Graph>::vertex_iterator v, v_end;
        for (tie(v, v_end) = vertices(g); v != v_end; ++v)
        {
            vlist.push_back(*v);
            mvs[*v] = marked[*v] = false;
            max_deg = max(out_degree(*v, g), max_deg);
        }

        vector<vertex_t> selected, tmp;
        tmp.reserve(vlist.size());
        selected.reserve(vlist.size());
        while (!vlist.empty())
        {
            selected.clear();
            tmp.clear();
            tmp_max_deg = 0;

            parallel_loop
                (vlist,
                 [&](size_t, auto v)
                 {
                     marked[v] = false;
                     bool include = true;
                     for (auto u : adjacent_vertices_range(v, g))
                     {
                         if (mvs[u])
                         {
                             include = false;
                             break;
                         }
                     }
                     if (!include)
                         return;

                     include = false;
                     if (out_degree(v, g) > 0)
                     {
                         double p, r;
                         if (high_deg)
                             p = out_degree(v, g) / max_deg;
                         else
                             p = 1. / (2 * out_degree(v, g));

                         uniform_real_distribution<> sample(0, 1);
                         auto& rng_ = rng; // workaround clang
                         #pragma omp critical
                         {
                             r = sample(rng_);
                         }
                         if (r < p)
                             include = true;
                     }
                     else
                     {
                         include = true;
                     }

                     if (include)
                     {
                         marked[v] = true;
                         auto& selected_ = selected; // workaround clang
                         #pragma omp critical (selected)
                         {
                             selected_.push_back(v);
                         }
                     }
                     else
                     {
                         auto& tmp_ = tmp;                 // workaround clang
                         auto& tmp_max_deg_ = tmp_max_deg;
                         #pragma omp critical (tmp)
                         {
                             tmp_.push_back(v);
                             tmp_max_deg_ = max(tmp_max_deg_, out_degree(v, g));
                         }
                     }
                 });

            parallel_loop
                (selected,
                 [&](size_t, auto v)
                 {
                     bool include = true;
                     for (auto u : adjacent_vertices_range(v, g))
                     {
                         if (u == v)  //skip self-loops
                             continue;
                         if (mvs[u])
                         {
                             include = false;
                             break;
                         }

                         if (marked[u])
                         {
                             bool inc = ((high_deg && (out_degree(v, g) >
                                                       out_degree(u, g))) ||
                                         (!high_deg && (out_degree(v, g) <
                                                   out_degree(u, g))));
                             if (out_degree(v, g) == out_degree(u, g))
                                 inc = v < u;
                             include = include && inc;
                         }
                     }

                     if (include)
                     {
                         mvs[v] = true;
                     }
                     else
                     {
                         auto& tmp_ = tmp;                 // workaround clang
                         auto& tmp_max_deg_ = tmp_max_deg;
                         #pragma omp critical (tmp)
                         {
                             tmp_.push_back(v);
                             tmp_max_deg_ = max(tmp_max_deg_, out_degree(v, g));
                         }
                     }
                     marked[v] = false;
                 });

            vlist = tmp;
            max_deg = tmp_max_deg;
        }
    }
};

void maximal_vertex_set(GraphInterface& gi, boost::any mvs, bool high_deg,
                        rng_t& rng)
{
    run_action<>()
        (gi, std::bind(do_maximal_vertex_set(), std::placeholders::_1, gi.get_vertex_index(),
                       std::placeholders::_2, high_deg, std::ref(rng)),
         writable_vertex_scalar_properties())(mvs);
}

void export_maximal_vertex_set()
{
    python::def("maximal_vertex_set", &maximal_vertex_set);
}


/*
 * Copyright (c) 2020 Intel Corporation
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/**
 * @file planner.hpp
 *
 * Maintainer: Yu Yan <yu.yan@intel.com>
 *
 */

#pragma once

#include <RTmotion/global.hpp>
#include <Eigen/Core>
#include <RTmotion/algorithm/offline_scurve_planner.hpp>
#include <RTmotion/algorithm/online_scurve_planner.hpp>
#include <RTmotion/algorithm/ruckig_planner.hpp>
#include <chrono>

using RTmotion::MC_ERROR_CODE;

namespace trajectory_processing
{
typedef enum { OFFLine = 0, ONLine = 1, Ruckig = 2} PlannerType;

class AxisPlanner
{
public:
  AxisPlanner(double f)
  : ruckig_planner_(f)
  {
    start_time_ = 0.0;
    type_ = Ruckig;
    scurve_planner_ = nullptr;
  }

  virtual ~AxisPlanner()
  {
    scurve_planner_ = nullptr;
  }

  void setCondition(double start_pos, double end_pos, double start_vel,
                    double end_vel, double vel_max, double acc_max,
                    double jerk_max);

  MC_ERROR_CODE planTrajectory();

  Eigen::Vector4d getTrajectoryPoint(double t);

  MC_ERROR_CODE onReplan();

  MC_ERROR_CODE onExecution(double t, double* pos_cmd, double* vel_cmd);

  ScurveCondition& getScurveCondition();

  ScurveProfile& getScurveProfile();

  void setStartTime(double t);

private:
  ScurvePlanner* scurve_planner_;
  ScurvePlannerOnLine online_planner_;
  ScurvePlannerOffLine offline_planner_;
  RuckigPlanner ruckig_planner_;
  double start_time_;
  PlannerType type_;
};

}  // namespace trajectory_processing


#include <QtCore/qglobal.h>

#include "Messages_interface.h"
#include "Scene_surface_mesh_item.h"
#include "Scene_points_with_normal_item.h"
#include <CGAL/Three/Scene_interface.h>
#include <CGAL/Three/Three.h>
#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>

#include <CGAL/Timer.h>
#include <CGAL/Random.h>
#include <CGAL/Side_of_triangle_mesh.h>
#include "ui_Point_inside_polyhedron_widget.h"

#include <QAction>
#include <QMainWindow>
#include <QApplication>
#include <QDockWidget>
#include <QInputDialog>

#include <vector>
#include <algorithm>

#include <CGAL/boost/iterator/transform_iterator.hpp>
#include <boost/optional/optional.hpp>

using namespace CGAL::Three;

typedef CGAL::Exact_predicates_inexact_constructions_kernel Epic_kernel;

class Polyhedron_demo_point_inside_polyhedron_plugin :
  public QObject,
  public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")

public:
  bool applicable(QAction*) const
  {
    for(CGAL::Three::Scene_interface::Item_id i = 0, end = scene->numberOfEntries();
        i < end; ++i)
    {
      if( qobject_cast<Scene_surface_mesh_item*>(scene->item(i)) != NULL)
        return true;
    }

    //if the loop ends without returning true, return false
    return false;
  }
  void print_message(QString message) { CGAL::Three::Three::information(message); }
  QList<QAction*> actions() const { return QList<QAction*>() << actionPointInsidePolyhedron; }

  void init(QMainWindow* mainWindow, CGAL::Three::Scene_interface* scene_interface, Messages_interface* m)
  {
    mw = mainWindow;
    scene = scene_interface;
    messages = m;

    actionPointInsidePolyhedron = new QAction(tr("Point Inside Polyhedron"), mw);
    actionPointInsidePolyhedron->setProperty("subMenuName", "Polygon Mesh Processing");
    connect(actionPointInsidePolyhedron, SIGNAL(triggered()), this, SLOT(point_inside_polyhedron_action()));

    dock_widget = new QDockWidget("Point Inside Polyhedron", mw);
    dock_widget->setVisible(false);
    ui_widget.setupUi(dock_widget);

    addDockWidget(dock_widget);

    connect(ui_widget.Select_button,  SIGNAL(clicked()), this, SLOT(on_Select_button()));
    connect(ui_widget.Sample_random_points_from_bbox,  SIGNAL(clicked()), this, SLOT(on_Sample_random_points_from_bbox()));

  }

  virtual void closure()
  {
    dock_widget->hide();
  }

public Q_SLOTS:
  void point_inside_polyhedron_action() {
    dock_widget->show();
    dock_widget->raise();
  }

  void on_Select_button()
  {
    bool inside = ui_widget.Inside_check_box->isChecked();
    bool on_boundary = ui_widget.On_boundary_check_box->isChecked();
    bool outside = ui_widget.Outside_check_box->isChecked();

    if(!(inside || on_boundary || outside)) {
      print_message("Error: please check at least one parameter check box.");
      return;
    }
    QApplication::setOverrideCursor(Qt::WaitCursor);
    // place all selected polyhedron and point items to vectors below
    std::vector<const SMesh*> smeshs;

    typedef CGAL::Side_of_triangle_mesh<SMesh, Kernel> Point_inside_smesh;
        // it does not support copy-construction so let's use pointers
    std::vector<Point_inside_smesh*>inside_smesh_testers;

    std::vector<Point_set*> point_sets;
    Q_FOREACH(CGAL::Three::Scene_interface::Item_id id, scene->selectionIndices()) {
      Scene_surface_mesh_item* sm_item = qobject_cast<Scene_surface_mesh_item*>(scene->item(id));
      if (sm_item){
        inside_smesh_testers.push_back(new Point_inside_smesh(*(sm_item->polyhedron())));
         smeshs.push_back(sm_item->polyhedron());
      }

      Scene_points_with_normal_item* point_item = qobject_cast<Scene_points_with_normal_item*>(scene->item(id));
      if(point_item) { point_sets.push_back(point_item->point_set()); }
    }

    // there should be at least one selected polyhedron and point item
    if(inside_smesh_testers.empty()) { print_message("Error: there is no selected polyhedron item(s)."); }
    if(point_sets.empty()) {
    if(!generated_points.empty())
      point_sets.push_back(generated_points.last()->point_set());
    else
      print_message("Error: there is no selected point set item(s).");
    }
    if((inside_smesh_testers.empty()) || point_sets.empty()) { QApplication::restoreOverrideCursor(); return; }

    // deselect all points
    for(std::vector<Point_set*>::iterator point_set_it = point_sets.begin();
      point_set_it != point_sets.end(); ++point_set_it) {
      (*point_set_it)->unselect_all();
    }

    CGAL::Timer timer; timer.start();

    print_message(
      QString("Constructing with %1 items is done in %2 sec.").arg(inside_smesh_testers.size()).arg(timer.time()));
    timer.reset();

    std::size_t nb_selected = 0;
    for (Point_set* point_set : point_sets)
      point_set->set_first_selected
        (std::partition
         (point_set->begin(), point_set->end(),
          [&](const Point_set::Index& idx) -> bool
          {
            for (const Point_inside_smesh* tester : inside_smesh_testers)
            {
              CGAL::Bounded_side res = (*tester)(point_set->point(idx));
              if ( (inside      && res == CGAL::ON_BOUNDED_SIDE) ||
                   (on_boundary && res == CGAL::ON_BOUNDARY)     ||
                   (outside     && res == CGAL::ON_UNBOUNDED_SIDE) )
              {
                ++ nb_selected;
                return false;
              }
            }
            return true;
          }));

    print_message(QString("%1 points are selected. All Done!").arg(nb_selected));

    // delete testers
  for (std::size_t i = 0; i < inside_smesh_testers.size(); ++i)
      delete inside_smesh_testers[i];

    bool found = false;
    // for repaint
    Q_FOREACH(CGAL::Three::Scene_interface::Item_id id, scene->selectionIndices()) {
      Scene_points_with_normal_item* point_item = qobject_cast<Scene_points_with_normal_item*>(scene->item(id));
      if(point_item) {
        found = true;
        point_item->invalidateOpenGLBuffers();
        scene->itemChanged(point_item);
      }
    }
    if(!found && !generated_points.empty()) {
      generated_points.last()->invalidateOpenGLBuffers();
      scene->itemChanged(generated_points.last());
    }
    QApplication::restoreOverrideCursor();
  }

  void on_Sample_random_points_from_bbox() {

    // calculate bbox of selected polyhedron items
    boost::optional<CGAL::Three::Scene_interface::Bbox> bbox
      = boost::make_optional(false, CGAL::Three::Scene_interface::Bbox());
    // Workaround a bug in g++-4.8.3:
    //   http://stackoverflow.com/a/21755207/1728537
    // Using boost::make_optional to copy-initialize 'bbox' hides the
    //   warning about '*bbox' not being initialized.
    // -- Laurent Rineau, 2014/10/30

    Q_FOREACH(CGAL::Three::Scene_interface::Item_id id, scene->selectionIndices()) {
      Scene_surface_mesh_item* sm_item = qobject_cast<Scene_surface_mesh_item*>(scene->item(id));
      if(sm_item) {
        if(!bbox) {
          bbox = sm_item->bbox();
        }
        else {
          *bbox = *bbox + sm_item->bbox();
        }
      }
    }

    if(!bbox) {
      print_message("Error: there is no selected polyhedron item(s).");
      return;
    }

    // take number of points param
    bool ok;
    const int nb_points =
      QInputDialog::getInt(mw, tr("Number of Points"),
      tr("Number of Points:"),
      100000, // default value
      1, // min
      (int)1.e9, // max
      10, // step for the spinbox
      &ok);

    if(!ok) { return; }
    QApplication::setOverrideCursor(Qt::WaitCursor);
    QApplication::processEvents();
    // sample random points and constuct item
    Scene_points_with_normal_item* point_item = new Scene_points_with_normal_item();
    point_item->setName(QString("sample-%1").arg(nb_points));
    CGAL::Random rg(1340818006);

    double grid_dx = bbox->xmax() - bbox->xmin();
    double grid_dy = bbox->ymax() - bbox->ymin();
    double grid_dz = bbox->zmax() - bbox->zmin();

    for(int i=0; i < nb_points; i++){
      point_item->point_set()->insert(
      Epic_kernel::Point_3(bbox->xmin ()+ rg.get_double()* grid_dx,
        bbox->ymin() + rg.get_double()* grid_dy,
        bbox->zmin() + rg.get_double()* grid_dz)
      );
    }

    scene->addItem(point_item);
    scene->itemChanged(point_item);
    generated_points.append(point_item);
    connect(point_item, SIGNAL(destroyed(QObject*)),
            this, SLOT(resetGeneratedPoints(QObject*)));
    QApplication::restoreOverrideCursor();
  }
private Q_SLOTS:
  void resetGeneratedPoints(QObject* o)
  {
    Q_FOREACH(Scene_points_with_normal_item* item , generated_points)
    if(item == o)
    {
      generated_points.removeAll(item);
    }
  }
private:
  Messages_interface* messages;
  QAction* actionPointInsidePolyhedron;
  QList<Scene_points_with_normal_item*> generated_points;
  QDockWidget* dock_widget;
  Ui::Point_inside_polyhedron ui_widget;

}; // end Polyhedron_demo_point_inside_polyhedron_plugin

#include "Point_inside_polyhedron_plugin.moc"


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Steffen Urban, urbste@gmail.com

#include <Eigen/Core>
#include <Eigen/LU>
#include <algorithm>
#include <ceres/rotation.h>
#include <glog/logging.h>

#include "theia/matching/feature_correspondence.h"
#include "theia/math/util.h"
#include "theia/sfm/bundle_adjustment/bundle_adjustment.h"
#include "theia/sfm/camera/camera.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/sfm/reconstruction.h"
#include "theia/util/random.h"
#include "gtest/gtest.h"

namespace theia {

namespace {
RandomNumberGenerator rng(52);

Camera RandomCamera() {
  Camera camera;
  camera.SetPosition(rng.RandVector3d());
  camera.SetOrientationFromAngleAxis(0.2 * rng.RandVector3d());
  camera.SetImageSize(1000, 1000);
  camera.SetFocalLength(500);
  camera.SetPrincipalPoint(500, 500);
  return camera;
}

void TestOptimizeView(const int kNumPoints, const double kPixelNoise) {
  // Set up random cameras.
  Camera camera1 = RandomCamera();
  Reconstruction reconstruction;
  ViewId vid = reconstruction.AddView("0", 0, 0.0);
  reconstruction.MutableView(vid)->MutableCamera()->DeepCopy(camera1);
  reconstruction.MutableView(vid)->SetEstimated(true);
  // Set up random points.
  for (int i = 0; i < kNumPoints; i++) {
    Eigen::Vector3d point(rng.RandDouble(-5.0, 5.0), rng.RandDouble(-5.0, 5.0),
                          rng.RandDouble(4.0, 10.0));
    TrackId tid = reconstruction.AddTrack();
    reconstruction.MutableTrack(tid)->SetPoint(point.homogeneous());
    reconstruction.MutableTrack(tid)->SetEstimated(true);

    Eigen::Vector2d pixel;
    double depth = reconstruction.View(vid)->Camera().ProjectPoint(
        point.homogeneous(), &pixel);
    if (kPixelNoise > 0.0) {
        AddNoiseToProjection(kPixelNoise, &rng, &pixel);
    }
    if (depth > 0.0) {
      reconstruction.AddObservation(vid, tid, Feature(pixel));
    }
  }

  BundleAdjustmentOptions opts;
  opts.verbose = true;
  BundleAdjustmentSummary sum = BundleAdjustView(opts, vid, &reconstruction);
  std::cout << "Success: " << sum.success << "\n";
  std::cout << "Final squared reprojection error: " << 2.0 * sum.final_cost
            << "\n";
  if (kPixelNoise == 0.0) {
      EXPECT_TRUE(2.0 * sum.final_cost / reconstruction.View(vid)->NumFeatures() < 1e-15);
  } else {
    EXPECT_TRUE(2.0 * sum.final_cost / reconstruction.View(vid)->NumFeatures() < kPixelNoise);
  }
}

} // namespace

TEST(OptimizeView, NoNoise) {
  static const double kPixelNoise = 0.0;
  static const int kNumPoints = 100;
  TestOptimizeView(kNumPoints, kPixelNoise);
}

TEST(OptimizeView, Noise) {
  static const double kPixelNoise = 0.1;
  static const int kNumPoints = 100;
  TestOptimizeView(kNumPoints, kPixelNoise);
}

} // namespace theia


/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: Erik Nelson            ( eanelson@eecs.berkeley.edu )
 *          David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

#include "pose_estimator_2d3d.h"

#include <ceres/ceres.h>
#include <Eigen/SVD>
#include <Eigen/QR>
#include <glog/logging.h>

#include "normalization.h"
#include "../optimization/cost_functors.h"

DEFINE_double(max_reprojection_error, 100.0,
              "Maximum tolerable reprojection error for a single 2D<-->3D "
              "point. This error is squared pixel distance. This is not exact, "
              "since it does not account for camera radial distortion, so this "
              "tolerance should be a little higher than the maximum admissible "
              "reprojection error.");

namespace bsfm {

PoseEstimator2D3D::PoseEstimator2D3D()
    : T_(Matrix3d::Identity()), U_(Matrix4d::Identity()) {}

PoseEstimator2D3D::~PoseEstimator2D3D() {}

bool PoseEstimator2D3D::Initialize(const FeatureList& points_2d,
                                   const Point3DList& points_3d,
                                   const CameraIntrinsics& intrinsics) {
  if (points_2d.size() != points_3d.size()) {
    VLOG(1) << "Inputs 'points_2d' and 'points_3d' do not contain the "
               "same number of elements.";
    return false;
  }

  // Copy camera intrinsics.
  intrinsics_ = intrinsics;

  // Normalize the 3D points.
  T_ = ComputeNormalization(points_2d);
  U_ = ComputeNormalization(points_3d);

  // Store the input set of points and a normalized copy.
  points_2d_ = points_2d;
  normalized_points_2d_.clear();
  for (size_t ii = 0; ii < points_2d.size(); ++ii) {
    // Get (u, v) image-space point and normalize.
    double u = points_2d[ii].u_;
    double v = points_2d[ii].v_;
    u = T_(0, 0) * u + T_(0, 2);
    v = T_(1, 1) * v + T_(1, 2);

    normalized_points_2d_.push_back(Feature(u, v));
  }

  points_3d_ = points_3d;
  normalized_points_3d_.clear();
  for (size_t ii = 0; ii < points_3d.size(); ++ii) {
    // Get (x, y, z) world-space point and normalize.
    double x = points_3d[ii].X();
    double y = points_3d[ii].Y();
    double z = points_3d[ii].Z();
    x = U_(0, 0) * x + U_(0, 3);
    y = U_(1, 1) * y + U_(1, 3);
    z = U_(2, 2) * z + U_(2, 3);

    normalized_points_3d_.push_back(Point3D(x, y, z));
  }

  return true;
}

bool PoseEstimator2D3D::Solve(Pose& camera_pose) {
  // Get an initial projection matrix.
  Matrix34d P;
  if (!ComputeInitialSolution(P)) {
    VLOG(1) << "Failed to compute an initial solution for P.";
    return false;
  }

  // Is the initial projection good? If not, we shouldn't try to optimize it.
  if (!TolerableReprojectionError(P)) {
    VLOG(1) << "Initial solution is too bad to optimize.";
    return false;
  }

  // Get the camera pose from the computed projection matrix.
  if (!ExtractPose(P, camera_pose)) {
    VLOG(1) << "Computed rotation is non-invertible.";
    return false;
  }

  // Refine P with non-linear optimization.
  if (!OptimizePose(camera_pose)) {
    VLOG(1) << "Failed to optimize camera pose. Continuing using the initial solution.";
  }

  return true;
}

bool PoseEstimator2D3D::ComputeInitialSolution(
    Matrix34d& initial_solution) const {
  // Use Eq. 7.2 from H&Z: Multiple-View Geometry to determine an
  // over-determined solution for the camera projection matrix P.

  // First build the A matrix, which is 2n x 12.
  MatrixXd A;
  A.resize(normalized_points_2d_.size() * 2, 12);
  for (size_t ii = 0; ii < normalized_points_2d_.size(); ii++) {
    // Get (u, v) image-space point.
    const double u = normalized_points_2d_[ii].u_;
    const double v = normalized_points_2d_[ii].v_;

    // Get (x, y, z) world-space point.
    const double x = normalized_points_3d_[ii].X();
    const double y = normalized_points_3d_[ii].Y();
    const double z = normalized_points_3d_[ii].Z();

    // First 4 columns, top row of block.
    A(2*ii+0, 0) = 0;
    A(2*ii+0, 1) = 0;
    A(2*ii+0, 2) = 0;
    A(2*ii+0, 3) = 0;

    // Second 4 columns, top row of block.
    A(2*ii+0, 4) = -x;
    A(2*ii+0, 5) = -y;
    A(2*ii+0, 6) = -z;
    A(2*ii+0, 7) = -1;

    // Third 4 columns, top row of block.
    A(2*ii+0, 8) = v*x;
    A(2*ii+0, 9) = v*y;
    A(2*ii+0, 10) = v*z;
    A(2*ii+0, 11) = v;

    // First 4 columns, bottom row of block.
    A(2*ii+1, 0) = x;
    A(2*ii+1, 1) = y;
    A(2*ii+1, 2) = z;
    A(2*ii+1, 3) = 1;

    // Second 4 columns, bottom row of block.
    A(2*ii+1, 4) = 0;
    A(2*ii+1, 5) = 0;
    A(2*ii+1, 6) = 0;
    A(2*ii+1, 7) = 0;

    // Third 4 columns, bottom row of block.
    A(2*ii+1, 8) = -u*x;
    A(2*ii+1, 9) = -u*y;
    A(2*ii+1, 10) = -u*z;
    A(2*ii+1, 11) = -u;
  }

  // Get svd(A). Save some time and compute a thin U. We still need a full V.
  Eigen::JacobiSVD<Eigen::MatrixXd> svd;
  svd.compute(A, Eigen::ComputeThinU | Eigen::ComputeFullV);
  if (!svd.computeV()) {
    VLOG(1) << "Failed to compute a singular value decomposition of A matrix.";
    return false;
  }

  // Get the projection matrix elements from the SVD decomposition.
  const VectorXd P_vec = svd.matrixV().col(11).normalized();

  // Reshape the vector into the initial solution.
  initial_solution.row(0) = P_vec.topRows(4).transpose();
  initial_solution.row(1) = P_vec.middleRows(4, 4).transpose();
  initial_solution.row(2) = P_vec.bottomRows(4).transpose();

  return true;
}

bool PoseEstimator2D3D::ExtractPose(const Matrix34d& P, Pose& pose) const {
  // Un-normalize the projection matrix.
  Matrix34d P_unnormalized = T_.inverse() * P * U_;

  // Extract camera extrinsics matrix.
  Matrix34d Rt = intrinsics_.Kinv() * P_unnormalized;

  // [R|t] is only determined up to scale. To get this scale, note that det(R)
  // must equal 1. Also note that for an nxn matrix, c^n*det(R) = det(cR).
  // Use this property to scale our matrix.
  double det = Rt.block(0, 0, 3, 3).determinant();
  if (std::abs(det) < 1e-16) {
    VLOG(1) << "Computed rotation has a determinant of 0.";
    return false;
  }

  // Make sure the determinant is positive.
  if (det < 0.0) {
    det *= -1.0;
    Rt *= -1.0;
  }

  // Normalize the rotation and translation.
  Rt *= std::pow(1.0 / det, 1.0 / 3.0);

  // Make sure the rotation is valid by projecting onto SO3. Compute a QR
  // decomposition of R and set R back to the orthogonal 'Q' matrix. Make sure
  // the signs of all the columns in the projection match the original.
  const Matrix3d R = Rt.block(0, 0, 3, 3);
  Eigen::HouseholderQR<Matrix3d> qr;
  qr.compute(R);

  const Matrix3d upper_triangle = qr.matrixQR().triangularView<Eigen::Upper>();
  Matrix3d signs(Matrix3d::Zero());
  signs(0, 0) = upper_triangle(0, 0) > 0.0 ? 1.0 : -1.0;
  signs(1, 1) = upper_triangle(1, 1) > 0.0 ? 1.0 : -1.0;
  signs(2, 2) = upper_triangle(2, 2) > 0.0 ? 1.0 : -1.0;
  Rt.block(0, 0, 3, 3) = qr.householderQ() * signs;

  // Initialize the output pose from the rotation and translation blocks.
  pose = Pose(Rt);
  return true;
}

bool PoseEstimator2D3D::OptimizePose(Pose& pose) const {
  // Create the non-linear least squares problem and cost function.
  ceres::Problem problem;

  // Create storage containers for optimization variables.
  // Need to convert camera projection matrix P into intrinsics
  // and axis-angle + translation.
  Vector3d rotation = pose.AxisAngle();
  Vector3d translation = -pose.Rotation().transpose() * pose.Translation();

  // Get static camera intrinsics for evaluating cost function.
  Matrix3d K = intrinsics_.K();

  // Add a term to the cost function for each 2D<-->3D correspondence.
  // Do all computations in un-normalized coordinates.
  for (size_t ii = 0; ii < points_2d_.size(); ++ii) {
    problem.AddResidualBlock(
        GeometricError::Create(points_2d_[ii], points_3d_[ii], K),
        NULL, /* squared loss */
        rotation.data(),
        translation.data());
  }

  // Solve the non-linear least squares problem to get the projection matrix.
  ceres::Solver::Summary summary;
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.function_tolerance = 1e-16;
  options.gradient_tolerance = 1e-16;
  options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
  ceres::Solve(options, &problem, &summary);

  // Store the solved variable back in 'pose'.
  if (summary.IsSolutionUsable()) {
    pose.FromAxisAngle(rotation);
    pose.SetTranslation(-pose.Rotation() * translation);
  }

  return summary.IsSolutionUsable();
}

bool PoseEstimator2D3D::TolerableReprojectionError(const Matrix34d& P) const {

  // Compute reprojection error for each 2D<-->3D match.
  double error = 0.0;
  for (size_t ii = 0; ii < normalized_points_3d_.size(); ++ii) {
    Vector3d x;
    Vector4d X;
    x << normalized_points_2d_[ii].u_,
         normalized_points_2d_[ii].v_,
         1.0;
    X << normalized_points_3d_[ii].X(),
         normalized_points_3d_[ii].Y(),
         normalized_points_3d_[ii].Z(),
         1.0;

    const Vector3d reprojected_x = P*X;
    const double error_u = x(0) - reprojected_x(0) / reprojected_x(2);
    const double error_v = x(1) - reprojected_x(1) / reprojected_x(2);
    error += error_u*error_u + error_v*error_v;
  }

  // Return whether the average error is below the threshold.
  return error / normalized_points_3d_.size() <= FLAGS_max_reprojection_error;
}

}  //\namespace bsfm


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNITS_MEASUREMENT_HPP
#define BOOST_UNITS_MEASUREMENT_HPP

#include <cmath>
#include <cstdlib>
#include <iomanip>
#include <iostream>

#include <boost/io/ios_state.hpp>
#include <boost/units/static_rational.hpp>

namespace boost {

namespace units {

namespace sqr_namespace /**/ {

template<class Y>
Y sqr(Y val)
{ return val*val; }

} // namespace

using sqr_namespace::sqr;

template<class Y>
class measurement
{
    public:
        typedef measurement<Y>                  this_type;
        typedef Y                               value_type;

        measurement(const value_type& val = value_type(),
                    const value_type& err = value_type()) :
            value_(val),
            uncertainty_(std::abs(err))
        { }

        measurement(const this_type& source) :
            value_(source.value_),
            uncertainty_(source.uncertainty_)
        { }

        //~measurement() { }

        this_type& operator=(const this_type& source)
        {
            if (this == &source) return *this;

            value_ = source.value_;
            uncertainty_ = source.uncertainty_;

            return *this;
        }

        operator value_type() const    { return value_; }

        value_type value() const       { return value_; }
        value_type uncertainty() const { return uncertainty_; }
        value_type lower_bound() const { return value_-uncertainty_; }
        value_type upper_bound() const { return value_+uncertainty_; }

        this_type& operator+=(const value_type& val)
        {
            value_ += val;
            return *this;
        }

        this_type& operator-=(const value_type& val)
        {
            value_ -= val;
            return *this;
        }

        this_type& operator*=(const value_type& val)
        {
            value_ *= val;
            uncertainty_ *= val;
            return *this;
        }

        this_type& operator/=(const value_type& val)
        {
            value_ /= val;
            uncertainty_ /= val;
            return *this;
        }

        this_type& operator+=(const this_type& /*source*/);
        this_type& operator-=(const this_type& /*source*/);
        this_type& operator*=(const this_type& /*source*/);
        this_type& operator/=(const this_type& /*source*/);

    private:
        value_type          value_,
                            uncertainty_;
};

}

}

#if BOOST_UNITS_HAS_BOOST_TYPEOF

BOOST_TYPEOF_REGISTER_TEMPLATE(boost::units::measurement, 1)

#endif

namespace boost {

namespace units {

template<class Y>
inline
measurement<Y>&
measurement<Y>::operator+=(const this_type& source)
{
    uncertainty_ = std::sqrt(sqr(uncertainty_)+sqr(source.uncertainty_));
    value_ += source.value_;

    return *this;
}

template<class Y>
inline
measurement<Y>&
measurement<Y>::operator-=(const this_type& source)
{
    uncertainty_ = std::sqrt(sqr(uncertainty_)+sqr(source.uncertainty_));
    value_ -= source.value_;

    return *this;
}

template<class Y>
inline
measurement<Y>&
measurement<Y>::operator*=(const this_type& source)
{
    uncertainty_ = (value_*source.value_)*
              std::sqrt(sqr(uncertainty_/value_)+
                        sqr(source.uncertainty_/source.value_));
    value_ *= source.value_;

    return *this;
}

template<class Y>
inline
measurement<Y>&
measurement<Y>::operator/=(const this_type& source)
{
    uncertainty_ = (value_/source.value_)*
              std::sqrt(sqr(uncertainty_/value_)+
                        sqr(source.uncertainty_/source.value_));
    value_ /= source.value_;

    return *this;
}

// value_type op measurement
template<class Y>
inline
measurement<Y>
operator+(Y lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs,Y(0))+=rhs);
}

template<class Y>
inline
measurement<Y>
operator-(Y lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs,Y(0))-=rhs);
}

template<class Y>
inline
measurement<Y>
operator*(Y lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs,Y(0))*=rhs);
}

template<class Y>
inline
measurement<Y>
operator/(Y lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs,Y(0))/=rhs);
}

// measurement op value_type
template<class Y>
inline
measurement<Y>
operator+(const measurement<Y>& lhs,Y rhs)
{
    return (measurement<Y>(lhs)+=measurement<Y>(rhs,Y(0)));
}

template<class Y>
inline
measurement<Y>
operator-(const measurement<Y>& lhs,Y rhs)
{
    return (measurement<Y>(lhs)-=measurement<Y>(rhs,Y(0)));
}

template<class Y>
inline
measurement<Y>
operator*(const measurement<Y>& lhs,Y rhs)
{
    return (measurement<Y>(lhs)*=measurement<Y>(rhs,Y(0)));
}

template<class Y>
inline
measurement<Y>
operator/(const measurement<Y>& lhs,Y rhs)
{
    return (measurement<Y>(lhs)/=measurement<Y>(rhs,Y(0)));
}

// measurement op measurement
template<class Y>
inline
measurement<Y>
operator+(const measurement<Y>& lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs)+=rhs);
}

template<class Y>
inline
measurement<Y>
operator-(const measurement<Y>& lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs)-=rhs);
}

template<class Y>
inline
measurement<Y>
operator*(const measurement<Y>& lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs)*=rhs);
}

template<class Y>
inline
measurement<Y>
operator/(const measurement<Y>& lhs,const measurement<Y>& rhs)
{
    return (measurement<Y>(lhs)/=rhs);
}

/// specialize power typeof helper
template<class Y,long N,long D>
struct power_typeof_helper<measurement<Y>,static_rational<N,D> >
{
    typedef measurement<
        typename power_typeof_helper<Y,static_rational<N,D> >::type
    > type;

    static type value(const measurement<Y>& x)
    {
        const static_rational<N,D>  rat;

        const Y m = Y(rat.numerator())/Y(rat.denominator()),
                newval = std::pow(x.value(),m),
                err = newval*std::sqrt(std::pow(m*x.uncertainty()/x.value(),2));

        return type(newval,err);
    }
};

/// specialize root typeof helper
template<class Y,long N,long D>
struct root_typeof_helper<measurement<Y>,static_rational<N,D> >
{
    typedef measurement<
        typename root_typeof_helper<Y,static_rational<N,D> >::type
    > type;

    static type value(const measurement<Y>& x)
    {
        const static_rational<N,D>  rat;

        const Y m = Y(rat.denominator())/Y(rat.numerator()),
                newval = std::pow(x.value(),m),
                err = newval*std::sqrt(std::pow(m*x.uncertainty()/x.value(),2));

        return type(newval,err);
    }
};

// stream output
template<class Y>
inline
std::ostream& operator<<(std::ostream& os,const measurement<Y>& val)
{
    boost::io::ios_precision_saver precision_saver(os);
    boost::io::ios_flags_saver flags_saver(os);

    os << val.value() << "(+/-" << val.uncertainty() << ")";

    return os;
}

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_MEASUREMENT_HPP


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_TWOTO31_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_TWOTO31_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4310) // cast truncate constants
#endif

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Twoto31 generic tag

     Represents the Twoto31 constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Twoto31, double, (1 << 31)
                                , 0x4f000000, 0x41e0000000000000LL
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Twoto31, Site> dispatching_Twoto31(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Twoto31, Site>();
   }
   template<class... Args>
   struct impl_Twoto31;
  }
  /*!
    Generates 2 to the power 31 (\f$2^{31}\f$)

    @par Semantic:

    @code
    T r = Twoto31<T>();
    @endcode

    is similar to:

    @code
    T r = pow(2, 31);
    @endcode
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Twoto31, Twoto31)
} }

#include <boost/simd/constant/common.hpp>

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#endif


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

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

using namespace std;

int main()
{
    mtl::dense_vector<double> vec(5,0.0);
    mtl::dense_vector<double>& refVec = vec;

    typedef mtl::tag::iter::all iall;
    typedef mtl::traits::range_generator<iall, mtl::dense_vector<double> >::type Iter;

    for (Iter iter(mtl::begin<iall>(refVec)), iend(mtl::end<iall>(refVec)); iter != iend; ++iter)
	cout << *iter << "\n";

    return 0;
}


// Copyright (c) 2013
// Nathanael Fillmore (University of Wisconsin-Madison)
// nathanae@cs.wisc.edu
//
// This file is part of REF-EVAL.
//
// REF-EVAL 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.
//
// REF-EVAL 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 REF-EVAL.  If not, see <http://www.gnu.org/licenses/>.

#define BOOST_TEST_MODULE test_pairset
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include "pairset.hh"

typedef boost::mpl::list<brute_force_pairset, big_matrix_pairset, smart_pairset> test_types;

size_t choose_2(size_t n) { return n*(n-1)/2; }

static size_t no_exceptions[0] = {};

BOOST_AUTO_TEST_CASE_TEMPLATE(simple, PairsetType, test_types)
{
  PairsetType ps(5);
  BOOST_CHECK_EQUAL(ps.size(), 0ul);

  size_t one_exception[1] = {1};
  ps.add_square_with_exceptions(1, 1, one_exception, one_exception+1);
  BOOST_CHECK_EQUAL(ps.size(), 0ul);

  ps.add_square_with_exceptions(1, 1, no_exceptions, no_exceptions);
  BOOST_CHECK_EQUAL(ps.size(), 1ul);

  ps.add_square_with_exceptions(1, 1, one_exception, one_exception+1);
  BOOST_CHECK_EQUAL(ps.size(), 1ul);

  ps.add_square_with_exceptions(0, 4, no_exceptions, no_exceptions);
  BOOST_CHECK_EQUAL(ps.size(), choose_2(5+1));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(crossovers, PairsetType, test_types)
{
  PairsetType ps(100);
  BOOST_CHECK_EQUAL(ps.size(), 0ul);

  ps.add_square_with_exceptions(0, 50-1, no_exceptions, no_exceptions);
  BOOST_CHECK_EQUAL(ps.size(), choose_2(50+1));

  ps.add_square_with_exceptions(60, 100-1, no_exceptions, no_exceptions);
  BOOST_CHECK_EQUAL(ps.size(), choose_2(50+1) + choose_2(40+1));

  ps.add_square_with_exceptions(40, 65-1, no_exceptions, no_exceptions);
  BOOST_CHECK_EQUAL(ps.size(), choose_2(50+1) + choose_2(40+1) + choose_2(25+1) - choose_2(10+1) - choose_2(5+1));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(multiple_exceptions, PairsetType, test_types)
{
  PairsetType ps(5);
  BOOST_CHECK_EQUAL(ps.size(), 0ul);

  size_t two_exceptions[2] = {1,3};
  ps.add_square_with_exceptions(0, 4, two_exceptions, two_exceptions+2);
  BOOST_CHECK_EQUAL(ps.size(), choose_2(5-2+1));

  size_t one_exception[1] = {1};
  ps.add_square_with_exceptions(1, 4, one_exception, one_exception+1);
  BOOST_CHECK_EQUAL(ps.size(), choose_2(5-2+1) + 3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(random_squares, PairsetType, test_types)
{
  size_t num_tests = 100;
  size_t max_num_squares = 50;
  size_t max_num_exceptions = 25;
  size_t top = 100;

  boost::random::random_device rng;
  boost::random::uniform_int_distribution<> lo_hi_dist(0, top - 1);
  boost::random::uniform_int_distribution<> num_squares_dist(0, max_num_squares);
  boost::random::uniform_int_distribution<> num_exceptions_dist(0, max_num_exceptions);

  for (size_t test = 0; test < num_tests; ++test) {

    PairsetType ps(top);
    BOOST_CHECK_EQUAL(ps.size(), 0ul);

    brute_force_pairset bf(top);
    BOOST_CHECK_EQUAL(bf.size(), 0ul);

    size_t num_squares = num_squares_dist(rng);
    for (size_t sq = 0; sq < num_squares; ++sq) {
      size_t lo = lo_hi_dist(rng);
      size_t hi = lo_hi_dist(rng);
      if (lo >= hi)
        std::swap(lo, hi);

      size_t num_exceptions = num_exceptions_dist(rng);
      boost::random::uniform_int_distribution<> exceptions_dist(lo, hi);
      std::set<size_t> exceptions;
      for (size_t i = 0; i < num_exceptions; ++i)
        exceptions.insert(exceptions_dist(rng));

      ps.add_square_with_exceptions(lo, hi, exceptions.begin(), exceptions.end());
      bf.add_square_with_exceptions(lo, hi, exceptions.begin(), exceptions.end());

      BOOST_CHECK_EQUAL(ps.size(), bf.size());
    }

  }
}


/* -*-c++-*--------------------------------------------------------------------
 * 2017 Bernd Pfrommer bernd.pfrommer@gmail.com
 *      Kartik Mohta
 */

#include "multicam_calibration/camera_extrinsics.h"
#include <boost/range/irange.hpp>

namespace multicam_calibration {

  CameraExtrinsics zeros() {
    return (CameraExtrinsics::Zero());
  }

  CameraExtrinsics identity() {
    return (CameraExtrinsics::Identity());
  }
  bool isNonZero(const CameraExtrinsics &T) {
    return ((T(0,0) + T(0,1) + T(0,2) + T(0,3) +
             T(1,0) + T(1,1) + T(1,2) + T(1,3) +
             T(2,0) + T(2,1) + T(2,2) + T(2,3) +
             T(3,0) + T(3,1) + T(3,2) + T(3,3)) != 0);
  }
}


#include "ScriptVM.h"
#include <cstring>
#include <sstream>
#include <stdexcept>
#include <boost/type_traits.hpp>

template <typename T>
inline const T &qMin1(const T &a, const T &b) { return (a < b) ? a : b; }
template <typename T>
inline const T &qMax1(const T &a, const T &b) { return (a < b) ? b : a; }
template <typename T>
inline T qAbs1(const T &t) { return t >= 0 ? t : -t; }

template <typename T>
inline bool qFuzzyIsNull1(T p) { return p == 0; }

template <>
inline bool qFuzzyIsNull1<double>(double d) {return qAbs1(d) <= 0.000000000001; }

template <>
inline bool qFuzzyIsNull1<float>(float f) { return qAbs1(f) <= 0.00001f; }

#define CONDITIONAL_OP(name, cond)\
template <typename resT, typename T>\
inline void name(resT& ,const T& , const T& ,\
				 typename boost::disable_if<boost::cond<T> >::type*  = 0) { ;  }\
template <typename resT, typename T>\
inline void name(resT& res, const T& op1, const T& op2,\
				 typename boost::enable_if<boost::cond<T> >::type*  = 0)

CONDITIONAL_OP(PLUS_F, is_arithmetic)   { res = op1 + op2; }
CONDITIONAL_OP(MINUS_F, is_arithmetic)  { res = op1 - op2; }
CONDITIONAL_OP(MUL_F, is_arithmetic)    { res = op1 * op2; }
CONDITIONAL_OP(DIV_F, is_arithmetic)    { res = uint64_t(op1 / op2); }
CONDITIONAL_OP(DIVR_F, is_arithmetic)   { res = op1 / op2; }
CONDITIONAL_OP(MOD_F, is_integral)      { res = op1 % op2; }
CONDITIONAL_OP(ANDLOG_F, is_arithmetic) { res = op1 && op2; }
CONDITIONAL_OP(ANDBIN_F, is_integral)   { res = op1 & op2; }
CONDITIONAL_OP(ORLOG_F, is_arithmetic)  { res = op1 || op2; }
CONDITIONAL_OP(ORBIN_F, is_integral)    { res = op1 | op2; }
CONDITIONAL_OP(XORLOG_F, is_integral)   { res = bool(op1) ^ bool(op2); }
CONDITIONAL_OP(XORBIN_F, is_integral)   { res = op1 ^  op2; }
CONDITIONAL_OP(SHL_F, is_integral)      { res = op1 << op2; }
CONDITIONAL_OP(SHR_F, is_integral)      { res = op1 >> op2; }

CONDITIONAL_OP(MIN__F, is_arithmetic)   { res = op1 < op2 ? op1 : op2; }
CONDITIONAL_OP(MAX__F, is_arithmetic)   { res = op1 > op2 ? op1 : op2; }

CONDITIONAL_OP(LT_F, is_arithmetic)     { res = op1 < op2; }
CONDITIONAL_OP(GT_F, is_arithmetic)     { res = op1 > op2; }
CONDITIONAL_OP(LE_F, is_arithmetic)     { res = op1 <= op2; }
CONDITIONAL_OP(GE_F, is_arithmetic)     { res = op1 >= op2; }

CONDITIONAL_OP(INVERSE_F, is_integral)  { res = ~op1; (void)op2; }

template <typename T>
inline void CMP_F(bool &res, const T& p1,const T& p2){ res = (p1 == p2); }

template <>
inline void CMP_F<double>(bool &res,const double& p1, const double& p2)
{ res = (qAbs1(p1 - p2) <= 0.000000000001 * qMin1(qAbs1(p1), qAbs1(p2))); }
template <>
inline void CMP_F<float>(bool &res,const float& p1, const float& p2)
{ res = (qAbs1(p1 - p2) <= 0.00001f * qMin1(qAbs1(p1), qAbs1(p2))); }

inline void PLUS_F(std::string &res,const std::string& p1,const std::string& p2)
{ res = p1 + p2; } // concatenation.
inline void DIV_F(bool &res,const bool& ,const bool& )
{ res = false; }
inline void DIVR_F(bool &res,const bool& ,const bool& )
{ res = false; }
inline void MOD_F(bool &res,const bool& ,const bool& )
{ res = false; }
inline void SHL_F(bool &res,const bool& ,const bool& )
{ res = false; }
inline void SHR_F(bool &res,const bool& ,const bool& )
{ res = false; }

#define CASE_BIN(name, nameF) \
	 case BytecodeVM::name:{\
		   nameF(res , t1.getValue<T>(), t2.getValue<T>());\
		   Vres.setValue(res, ScriptVariant::T_AUTO); \
		};break

#define CASE_BIN_L(name, nameF) \
	 case BytecodeVM::name:{\
		   nameF(bres , t1.getValue<T>(), t2.getValue<T>());\
		   Vres.setValue(bres, ScriptVariant::T_bool); \
		};break

template <class T>
void makeBinaryOperation(BytecodeVM::BinOp op, ScriptVariant& Vres, const ScriptVariant& t1, const ScriptVariant& t2)
{
	T res = T();
	bool bres=false;

	switch(op)
	{
		CASE_BIN(PLUS, PLUS_F);
		CASE_BIN(MINUS, MINUS_F);
		CASE_BIN(MUL, MUL_F);
		CASE_BIN(DIV, DIV_F);
		CASE_BIN(DIVR, DIVR_F);
		CASE_BIN(MOD, MOD_F);

	  //  CASE_BIN_L(AND, ANDLOG_F);
		CASE_BIN_L(ANDLOG, ANDLOG_F);
	   // CASE_BIN_L(OR, ORLOG_F);
		CASE_BIN_L(ORLOG, ORLOG_F);
	   // CASE_BIN_L(XOR, XORLOG_F);

		case BytecodeVM::EQ:
			CMP_F(bres, t1.getValue<T>() , t2.getValue<T>());
			Vres.setValue(bres, ScriptVariant::T_bool);
			break;
		case BytecodeVM::NE:
			CMP_F(bres, t1.getValue<T>() , t2.getValue<T>());
			Vres.setValue(!bres, ScriptVariant::T_bool);
			break;
		CASE_BIN_L(LT, LT_F);
		CASE_BIN_L(GT, GT_F);
		CASE_BIN_L(LE, LE_F);
		CASE_BIN_L(GE, GE_F);

		CASE_BIN(ANDBIN, ANDBIN_F);
		CASE_BIN(ORBIN, ORBIN_F);
		CASE_BIN(XORBIN, XORBIN_F);
		CASE_BIN(SHL, SHL_F);
		CASE_BIN(SHR, SHR_F);

		default: Vres.setValue(res, ScriptVariant::T_AUTO); break;

	}

}

template <class T>
void makeUnaryOperation(BytecodeVM::UnOp op, ScriptVariant& t1)
{
	ScriptVariant tmp;
	switch(op)
	{
		case BytecodeVM::UPLUS:
			 tmp.setValue(+t1.getValue<T>(), ScriptVariant::T_AUTO);
			 t1 = tmp;
			 break;
		case BytecodeVM::UMINUS:
			 tmp.setValue(-t1.getValue<T>(), ScriptVariant::T_AUTO);
			 t1 = tmp;
			 break ;
		case BytecodeVM::UNOT:
			 tmp.setValue(!t1.getValue<T>(), ScriptVariant::T_AUTO);
			 t1 = tmp;
			 break ;

		case BytecodeVM::UINC:
			 t1.setValue(t1.getValue<T>() + 1);
			 break;
		case BytecodeVM::UDEC:
			 t1.setValue(t1.getValue<T>() - 1);
			 break;
		case BytecodeVM::UINV:{
			 T val;
			 INVERSE_F(val, t1.getValue<T>(), t1.getValue<T>());
			 t1.setValue(val);
			 } break;
		default: ; break;
	}

}

template <>
void makeUnaryOperation<bool>(BytecodeVM::UnOp op, ScriptVariant& t1)
{
	ScriptVariant ret;
	switch(op)
	{
		case BytecodeVM::UNOT:
		ret.setValue(! t1.getValue<bool>(), ScriptVariant::T_AUTO);
		t1 = ret;
		break;
		default: ; break;
	}

}

template <class T>
inline bool makeCmpOperation(ScriptVariant& t1, ScriptVariant& t2)
{
	bool res;
	CMP_F(res, t1.getValue<T>(), t2.getValue<T>());
	return res;
}

bool makeCmpOperationWrap(ScriptVariant& t1, ScriptVariant& t2)
{
	ScriptVariant::Types optype = ScriptVariant::Types(t1.getReferenced(0)->_Type);
	switch(optype){
		case ScriptVariant::T_bool:       return makeCmpOperation<bool       >(t1, t2); break;
		case ScriptVariant::T_float32:    return makeCmpOperation<float      >(t1, t2); break;
		case ScriptVariant::T_float64:    return makeCmpOperation<double     >(t1, t2); break;
		case ScriptVariant::T_int8_t:     return makeCmpOperation<int8_t     >(t1, t2); break;
		case ScriptVariant::T_uint8_t:    return makeCmpOperation<uint8_t    >(t1, t2); break;
		case ScriptVariant::T_int16_t:    return makeCmpOperation<int16_t    >(t1, t2); break;
		case ScriptVariant::T_uint16_t:   return makeCmpOperation<uint16_t   >(t1, t2); break;
		case ScriptVariant::T_int32_t:    return makeCmpOperation<int32_t    >(t1, t2); break;
		case ScriptVariant::T_uint32_t:   return makeCmpOperation<uint32_t   >(t1, t2); break;
		case ScriptVariant::T_int64_t:    return makeCmpOperation<int64_t    >(t1, t2); break;
		case ScriptVariant::T_uint64_t:   return makeCmpOperation<uint64_t   >(t1, t2); break;
		case ScriptVariant::T_string:     return makeCmpOperation<std::string>(t1, t2); break;
		default:
			return false;
	}

}

void ScriptVM::termOperation(BytecodeVM::BinOp op, ScriptVariant::Types optype, BytecodeVM::BINOP_flags /*flags*/)
{
	ScriptVariant& t1= sTop(1);
	ScriptVariant& t2= sTop(0);
	ScriptVariant res;
	switch(optype) {
	   case ScriptVariant::T_bool:        makeBinaryOperation<bool       >(op, res, t1, t2); break;
	   case ScriptVariant::T_float32:     makeBinaryOperation<float      >(op, res, t1, t2); break;
	   case ScriptVariant::T_float64:     makeBinaryOperation<double     >(op, res, t1, t2); break;
	   case ScriptVariant::T_int8_t:      makeBinaryOperation<int8_t     >(op, res, t1, t2); break;
	   case ScriptVariant::T_uint8_t:     makeBinaryOperation<uint8_t    >(op, res, t1, t2); break;
	   case ScriptVariant::T_int16_t:     makeBinaryOperation<int16_t    >(op, res, t1, t2); break;
	   case ScriptVariant::T_uint16_t:    makeBinaryOperation<uint16_t   >(op, res, t1, t2); break;
	   case ScriptVariant::T_int32_t:     makeBinaryOperation<int32_t    >(op, res, t1, t2); break;
	   case ScriptVariant::T_uint32_t:    makeBinaryOperation<uint32_t   >(op, res, t1, t2); break;
	   case ScriptVariant::T_int64_t:     makeBinaryOperation<int64_t    >(op, res, t1, t2); break;
	   case ScriptVariant::T_uint64_t:    makeBinaryOperation<uint64_t   >(op, res, t1, t2); break;
	   case ScriptVariant::T_string:      makeBinaryOperation<std::string>(op, res, t1, t2); break;
	   default: res._Type = ScriptVariant::T_UNDEFINED;
	}

	if (_debugFlags & dOperations)
		(*_debugout) << "BINOP t=" << optype << " " << t1.getString() << " " << BytecodeVM::binopStr[op] << " " << t2.getString() << " = " << res.getString() << "\n";
	sPops(2);
	sPush(res);

}

void ScriptVM::movs(BytecodeVM::MOVS_flags flags, int size)
{

	int baseOffset =0;
	const bool isLeftRef  = (flags & BytecodeVM::mLeftIsRef);
	const bool isRightRef = (flags & BytecodeVM::mRightIsRef);
	const bool isAddress  = (flags & BytecodeVM::mAddress);
	baseOffset +=isRightRef ? 1 : size;
	const int rightOffset = baseOffset;
	baseOffset +=isLeftRef  ? 1 : size;
	ScriptVariant &leftPointer  = sList(baseOffset, 0);
	ScriptVariant &rightPointer = sList(rightOffset, 0);
	std::string v;
	const bool debugOperations =(_debugFlags & dOperations) && _debugout ;
	for (int i=0; i<size;i++){
		ScriptVariant &leftVal  = isLeftRef ?  (* (leftPointer .getReferenced(i, 1))) : sList(baseOffset, i);
		ScriptVariant &rightVal = isRightRef ? (* (rightPointer.getReferenced(i, 1))) : sList(rightOffset, i);
		if (debugOperations){
			v = leftVal.getString(true);
		}

		if (isAddress) {
			leftVal.setOpValueAddress(rightVal);
		}else {
			leftVal.setOpValue(rightVal);
		}
		if (debugOperations){
			(*_debugout) << "MOVS " << v << " := " << rightVal.getString(true) << "\n";
		}

	}
	sPops(baseOffset);

}

void ScriptVM::cmps(BytecodeVM::CMPS_flags flags, int size)
{

	ScriptVariant res;
	int baseOffset =0;
	bool isLeftRef  = (flags & BytecodeVM::cLeftIsRef);
	bool isRightRef = (flags & BytecodeVM::cRightIsRef);
	baseOffset +=isRightRef ? 1 : size;
	int rightOffset = baseOffset;
	baseOffset +=isLeftRef  ? 1 : size;
	ScriptVariant &leftPointer  = sList(baseOffset, 0);
	ScriptVariant &rightPointer = sList(rightOffset, 0);

	bool t = true;
	for (int i=0; i<size;i++){
		ScriptVariant &leftVal  = isLeftRef ?  (* (leftPointer .getReferenced(i))) : sList(baseOffset, i);
		ScriptVariant &rightVal = isRightRef ? (* (rightPointer.getReferenced(i))) : sList(rightOffset, i);
		t = makeCmpOperationWrap(leftVal, rightVal);
		if (!t) break;
	}
	if (flags & BytecodeVM::cNot){
		t = !t;
	}
	res.setValue(t, ScriptVariant::T_bool);
	sPops(baseOffset);
	sPush(res);
}

void ScriptVM::unaryOperation(BytecodeVM::UnOp op, ScriptVariant::Types optype)
{
	ScriptVariant& t1= sTop(0);
	ScriptVariant dbg = t1;
	switch(optype){
		case ScriptVariant::T_bool:       makeUnaryOperation<bool       >(op, t1); break;
		case ScriptVariant::T_float32:    makeUnaryOperation<float      >(op, t1); break;
		case ScriptVariant::T_float64:    makeUnaryOperation<double     >(op, t1); break;
		case ScriptVariant::T_int8_t:     makeUnaryOperation<int8_t     >(op, t1); break;
		case ScriptVariant::T_uint8_t:    makeUnaryOperation<uint8_t    >(op, t1); break;
		case ScriptVariant::T_int16_t:    makeUnaryOperation<int16_t    >(op, t1); break;
		case ScriptVariant::T_uint16_t:   makeUnaryOperation<uint16_t   >(op, t1); break;
		case ScriptVariant::T_int32_t:    makeUnaryOperation<int32_t    >(op, t1); break;
		case ScriptVariant::T_uint32_t:   makeUnaryOperation<uint32_t   >(op, t1); break;
		case ScriptVariant::T_int64_t:    makeUnaryOperation<int64_t    >(op, t1); break;
		case ScriptVariant::T_uint64_t:   makeUnaryOperation<uint64_t   >(op, t1); break;
	   // case ScriptVariant::T_string:     makeUnaryOperation<std::string>(op, t1); break;
		default: t1._Type = ScriptVariant::T_UNDEFINED;
	}
	if (_debugFlags & dOperations)
		(*_debugout) << "UNNOP t=" << optype<< " " << BytecodeVM::unopStr[op] << " " << dbg.getValue<double>()  <<  " = " << t1.getValue<double>() << "\n";

}
#define IF_FUN_2(name, namef) \
	else if (op == BytecodeVM::name)\
		for (size_t i=1; i< args.size();i++){\
		   namef(r , r, args[i]->getValue<T>());\
		}
#define IF_FUN(name) IF_FUN_2(name, name##_F)

template <class T>
void multOper(BytecodeVM::BinOp op, std::vector<ScriptVariant*>& args, ScriptVariant& res)
{
	T r = args[0]->getValue<T>();
	if (op == BytecodeVM::PLUS)
		for (size_t i=1; i< args.size();i++){
		   PLUS_F(r, r, args[i]->getValue<T>());
		}
	IF_FUN(MUL)
	IF_FUN(ANDLOG)
	IF_FUN(ANDBIN)
	IF_FUN(ORLOG)
	IF_FUN(ORBIN)
	IF_FUN_2(XOR, XORLOG_F)
	IF_FUN(XORBIN)
	IF_FUN(MIN_)
	IF_FUN(MAX_)
	else if (op == BytecodeVM::MUX){
		size_t index =   args[0]->getValue<uint32_t>();
		if (index < args.size() - 1){
			r = args[index + 1]->getValue<T>();
		}
	}
	res.setValue(r, ScriptVariant::T_AUTO);
}

void ScriptVM::multOperation(BytecodeVM::BinOp op, ScriptVariant::Types optype, int count)
{
	if (!count) return;
	std::vector<ScriptVariant*> args;
	for (int i=0;i<count;i++){
		args.push_back(&(sList(count, i)));
	}
	ScriptVariant res;
	switch(optype){
		case ScriptVariant::T_bool:       multOper<bool       >(op, args, res); break;
		case ScriptVariant::T_float32:    multOper<float      >(op, args, res); break;
		case ScriptVariant::T_float64:    multOper<double     >(op, args, res); break;
		case ScriptVariant::T_int8_t:     multOper<int8_t     >(op, args, res); break;
		case ScriptVariant::T_uint8_t:    multOper<uint8_t    >(op, args, res); break;
		case ScriptVariant::T_int16_t:    multOper<int16_t    >(op, args, res); break;
		case ScriptVariant::T_uint16_t:   multOper<uint16_t   >(op, args, res); break;
		case ScriptVariant::T_int32_t:    multOper<int32_t    >(op, args, res); break;
		case ScriptVariant::T_uint32_t:   multOper<uint32_t   >(op, args, res); break;
		case ScriptVariant::T_int64_t:    multOper<int64_t    >(op, args, res); break;
		case ScriptVariant::T_uint64_t:   multOper<uint64_t   >(op, args, res); break;
		case ScriptVariant::T_string:     multOper<std::string>(op, args, res); break;
		default: res._Type = ScriptVariant::T_UNDEFINED;
	}
	//printStack();
	sPops(count);
	sPush(res);

	if (_debugFlags & dOperations)
		(*_debugout) << "MULTOP t=" << optype << " " <<  BytecodeVM::binopStr[op] << " [" << count << "] = " << res.getValue<double>() << "\n";

}


#pragma once

#include <polyfem/ProblemWithSolution.hpp>

#include <vector>
#include <Eigen/Dense>

namespace polyfem
{
	class LinearProblem: public ProblemWithSolution
	{
	public:
		LinearProblem(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt) const override;

		bool is_scalar() const override { return true; }
	};

	class QuadraticProblem : public ProblemWithSolution
	{
	public:
		QuadraticProblem(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt) const override;

		bool is_scalar() const override { return true; }
	};

	class CubicProblem : public ProblemWithSolution
	{
	public:
		CubicProblem(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt) const override;

		bool is_scalar() const override { return true; }
	};

	class SineProblem : public ProblemWithSolution
	{
	public:
		SineProblem(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt) const override;

		bool is_scalar() const override { return true; }
	};

	class ZeroBCProblem: public ProblemWithSolution
	{
	public:
		ZeroBCProblem(const std::string &name);

		VectorNd eval_fun(const VectorNd &pt) const override;
		AutodiffGradPt eval_fun(const AutodiffGradPt &pt) const override;
		AutodiffHessianPt eval_fun(const AutodiffHessianPt &pt) const override;

		bool is_scalar() const override { return true; }
	};

	class MinSurfProblem: public Problem
	{
	public:
		MinSurfProblem(const std::string &name);

		void rhs(const std::string &formulation, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return false; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;

		bool is_scalar() const override { return true; }
		bool has_exact_sol() const override { return false; }
	};

	class TimeDependentProblem: public Problem
	{
	public:
		TimeDependentProblem(const std::string &name);

		void rhs(const std::string &formulation, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return false; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts,const double t, Eigen::MatrixXd &val) const override;
		void initial_solution(const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return true; }
		bool is_time_dependent() const override { return true; }
	};
}


/*

Copyright (c) 2015, Project OSRM contributors
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "douglas_peucker.hpp"

#include "../data_structures/segment_information.hpp"

#include <boost/assert.hpp>
#include <osrm/coordinate.hpp>

#include <cmath>
#include <algorithm>
#include <iterator>

namespace
{
struct CoordinatePairCalculator
{
    CoordinatePairCalculator() = delete;
    CoordinatePairCalculator(const FixedPointCoordinate &coordinate_a,
                             const FixedPointCoordinate &coordinate_b)
    {
        // initialize distance calculator with two fixed coordinates a, b
        const float RAD = 0.017453292519943295769236907684886f;
        first_lat = (coordinate_a.lat / COORDINATE_PRECISION) * RAD;
        first_lon = (coordinate_a.lon / COORDINATE_PRECISION) * RAD;
        second_lat = (coordinate_b.lat / COORDINATE_PRECISION) * RAD;
        second_lon = (coordinate_b.lon / COORDINATE_PRECISION) * RAD;
    }

    int operator()(FixedPointCoordinate &other) const
    {
        // set third coordinate c
        const float RAD = 0.017453292519943295769236907684886f;
        const float earth_radius = 6372797.560856f;
        const float float_lat1 = (other.lat / COORDINATE_PRECISION) * RAD;
        const float float_lon1 = (other.lon / COORDINATE_PRECISION) * RAD;

        // compute distance (a,c)
        const float x_value_1 = (first_lon - float_lon1) * cos((float_lat1 + first_lat) / 2.f);
        const float y_value_1 = first_lat - float_lat1;
        const float dist1 = std::hypot(x_value_1, y_value_1) * earth_radius;

        // compute distance (b,c)
        const float x_value_2 = (second_lon - float_lon1) * cos((float_lat1 + second_lat) / 2.f);
        const float y_value_2 = second_lat - float_lat1;
        const float dist2 = std::hypot(x_value_2, y_value_2) * earth_radius;

        // return the minimum
        return static_cast<int>(std::min(dist1, dist2));
    }

    float first_lat;
    float first_lon;
    float second_lat;
    float second_lon;
};
}

void DouglasPeucker::Run(std::vector<SegmentInformation> &input_geometry, const unsigned zoom_level)
{
    Run(std::begin(input_geometry), std::end(input_geometry), zoom_level);
}

void DouglasPeucker::Run(RandomAccessIt begin, RandomAccessIt end, const unsigned zoom_level)
{
    const auto size = std::distance(begin, end);
    if (size < 2)
    {
        return;
    }

    begin->necessary = true;
    std::prev(end)->necessary = true;

    {
        BOOST_ASSERT_MSG(zoom_level < DOUGLAS_PEUCKER_THRESHOLDS.size(), "unsupported zoom level");
        auto left_border = begin;
        auto right_border = std::next(begin);
        // Sweep over array and identify those ranges that need to be checked
        do
        {
            // traverse list until new border element found
            if (right_border->necessary)
            {
                // sanity checks
                BOOST_ASSERT(left_border->necessary);
                BOOST_ASSERT(right_border->necessary);
                recursion_stack.emplace(left_border, right_border);
                left_border = right_border;
            }
            ++right_border;
        } while (right_border != end);
    }

    // mark locations as 'necessary' by divide-and-conquer
    while (!recursion_stack.empty())
    {
        // pop next element
        const GeometryRange pair = recursion_stack.top();
        recursion_stack.pop();
        // sanity checks
        BOOST_ASSERT_MSG(pair.first->necessary, "left border must be necessary");
        BOOST_ASSERT_MSG(pair.second->necessary, "right border must be necessary");
        BOOST_ASSERT_MSG(std::distance(pair.second, end) > 0, "right border outside of geometry");
        BOOST_ASSERT_MSG(std::distance(pair.first, pair.second) >= 0,
                         "left border on the wrong side");

        int max_int_distance = 0;
        auto farthest_entry_it = pair.second;
        const CoordinatePairCalculator dist_calc(pair.first->location, pair.second->location);

        // sweep over range to find the maximum
        for (auto it = std::next(pair.first); it != pair.second; ++it)
        {
            const int distance = dist_calc(it->location);
            // found new feasible maximum?
            if (distance > max_int_distance && distance > DOUGLAS_PEUCKER_THRESHOLDS[zoom_level])
            {
                farthest_entry_it = it;
                max_int_distance = distance;
            }
        }

        // check if maximum violates a zoom level dependent threshold
        if (max_int_distance > DOUGLAS_PEUCKER_THRESHOLDS[zoom_level])
        {
            //  mark idx as necessary
            farthest_entry_it->necessary = true;
            if (1 < std::distance(pair.first, farthest_entry_it))
            {
                recursion_stack.emplace(pair.first, farthest_entry_it);
            }
            if (1 < std::distance(farthest_entry_it, pair.second))
            {
                recursion_stack.emplace(farthest_entry_it, pair.second);
            }
        }
    }
}


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2018.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Timo Sachsenberg $
// $Authors: Eva Lange $
// --------------------------------------------------------------------------

#include <OpenMS/FILTERING/SMOOTHING/SavitzkyGolayFilter.h>
#include <OpenMS/MATH/MISC/MathFunctions.h>

#include <Eigen/Core>
#include <Eigen/SVD>

namespace OpenMS
{
  SavitzkyGolayFilter::SavitzkyGolayFilter() :
    ProgressLogger(),
    DefaultParamHandler("SavitzkyGolayFilter"),
    coeffs_()
  {
    defaults_.setValue("frame_length", 11, "The number of subsequent data points used for smoothing.\nThis number has to be uneven. If it is not, 1 will be added.");
    defaults_.setValue("polynomial_order", 4, "Order or the polynomial that is fitted.");

    defaultsToParam_();
  }

  SavitzkyGolayFilter::~SavitzkyGolayFilter()
  {
  }

  void SavitzkyGolayFilter::updateMembers_()
  {
    frame_size_ = (UInt)param_.getValue("frame_length");
    order_ = (UInt)param_.getValue("polynomial_order");

    //recalculate coefficients
    if (!Math::isOdd(frame_size_))
    {
      frame_size_ += 1;
    }
    if (frame_size_ <= order_)
    {
      throw Exception::InvalidValue(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "The degree of the polynomial has to be less than the frame length.", String(order_));
    }
    coeffs_.resize(frame_size_ * (frame_size_ / 2 + 1));

    for (int nl = 0; nl <= (int) (frame_size_ / 2); ++nl)
    {
      int nr = frame_size_ - 1 - nl;

      // compute a Vandermonde matrix whose columns are powers of the vector [-nL,...,nR]
      Eigen::MatrixXd A (frame_size_, order_ + 1);
      for (int i = -nl; i <= nr; i++)
      {
        for (int j = 0; j <= static_cast<int>(order_); j++)
        {
          A(i + nl, j) = std::pow((float)i, j); // pow(int, int) is not defined
        }
      }

      // compute the singular-value decomposition of A
      Eigen::JacobiSVD<Eigen::MatrixXd> svd (A, Eigen::ComputeThinU | Eigen::ComputeThinV);

      Eigen::VectorXd B (order_ + 1);
      for (UInt i = 0; i <= order_; ++i)
      {
        B(i) = svd.matrixV()(0, i) / svd.singularValues()(i);
      }

      // compute B*transpose(U)*b, where b is the unit vector b=[1 0 ... 0]
      for (UInt i = 0; i < frame_size_; ++i)
      {
        coeffs_[(nl + 1) * frame_size_ - i - 1] = 0;
        for (UInt j = 0; j <= order_; ++j)
        {
          coeffs_[(nl + 1) * frame_size_ - i - 1] += B(j) * svd.matrixU()(i, j);
        }
      }
    }
  }
}


// easylazy
//
// Copyright iorate 2019.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <boost/core/lightweight_test.hpp>
#include "../easylazy.hpp"

using namespace easylazy;

int main() {
    // Postfix Expressions
    BOOST_TEST((list<int_>{23, 42}[1_d].get()) == 42);
    BOOST_TEST((EASYLAZY_FUNCTION(int_ x, int_ y) { return x + y; })(3_d, 4_d).get() == 7);
    BOOST_TEST(double_(10_d).get() == 10.0);

    // Unary Expressions
    BOOST_TEST((+4_d).get() == 4);
    BOOST_TEST((-2.5_lf).get() == -2.5);
    BOOST_TEST((!bool_(false)).get() == true);
    BOOST_TEST((!12_d).get() == false);
    BOOST_TEST((~0_d).get() == -1);

    // Explicit Type Conversion
    BOOST_TEST((float_)1_d == 1.0_f);

    // Multiplicative Operators
    BOOST_TEST((1.5_lf * 4_d).get() == 6.0);
    BOOST_TEST((19_d / 4_d).get() == 4);
    BOOST_TEST((19_d % 4_d).get() == 3);

    // Additive Operators
    BOOST_TEST((3_d + 4.5_lf).get() == 7.5);
    BOOST_TEST(('9'_c - '3'_c).get() == 6);

    // Shift Operators
    BOOST_TEST((18_d << 2_d).get() == 72);
    BOOST_TEST((18_d >> 2_d).get() == 4);

    // Relational Operators
    BOOST_TEST((1.5_f < 3.5_f).get() == true);
    BOOST_TEST((1.5_f < 1.5_f).get() == false);
    BOOST_TEST((35_d > 15_d).get() == true);
    BOOST_TEST((35_d > 35_d).get() == false);
    BOOST_TEST((1.5_f <= 1.5_f).get() == true);
    BOOST_TEST((1.5_f <= 0.5_f).get() == false);
    BOOST_TEST((15_d >= 15_d).get() == true);
    BOOST_TEST((5_d >= 15_d).get() == false);

    // Equality Operators
    BOOST_TEST((10_d == 10_d).get() == true);
    BOOST_TEST((8.0_f == 1.5_f).get() == false);
    BOOST_TEST((80_d != 15_d).get() == true);
    BOOST_TEST((1.0_f != 1.0_f).get() == false);

    // Bitwise Operators
    BOOST_TEST((12_d & 10_d).get() == 8);
    BOOST_TEST((12_d ^ 10_d).get() == 6);
    BOOST_TEST((12_d | 10_d).get() == 14);

    // Logical Operators
    BOOST_TEST((bool_(true) && bool_(true)).get() == true);
    BOOST_TEST((bool_(true) && 0_d).get() == false);
    BOOST_TEST((bool_(false) || bool_(true)).get() == true);
    BOOST_TEST((bool_(false) || 0_d).get() == false);

    // Implicit Type Conversion
    int_ n = ' '_c;
    BOOST_TEST(n.get() == 0x20);
    if ('\0'_c) {
        BOOST_ERROR("not reached");
    }

    return boost::report_errors();
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/kinematics/skeleton/skeleton_types.h>
#include <OpenTissue/kinematics/inverse/inverse.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

typedef float																                             real_type;
typedef OpenTissue::math::default_math_types		  			                 math_types;
typedef OpenTissue::skeleton::DefaultBoneTraits<math_types>              bone_traits;
typedef OpenTissue::kinematics::inverse::BoneTraits< bone_traits >       ik_bone_traits;
typedef OpenTissue::skeleton::Types<math_types,ik_bone_traits>           skeleton_types;
typedef skeleton_types::skeleton_type                                    skeleton_type;
typedef OpenTissue::kinematics::inverse::NonlinearSolver<skeleton_type>  solver_type;

BOOST_AUTO_TEST_SUITE(opentissue_kinematics_inverse_nonlinear_solver);

BOOST_AUTO_TEST_CASE(test_cases)
{
  skeleton_type skeleton;
  skeleton_type::bone_type * b0 = skeleton.create_bone();
  skeleton_type::bone_type * b1 = skeleton.create_bone(b0);
  skeleton_type::bone_type * b2 = skeleton.create_bone(b0);
  skeleton_type::bone_type * b3 = skeleton.create_bone(b2);
  solver_type solver = OpenTissue::kinematics::inverse::make_solver( skeleton );
  BOOST_CHECK( solver.skeleton() == &skeleton );
  BOOST_CHECK( solver.size() == 2u );
  solver_type::chain_iterator chain = solver.chain_begin();
  solver_type::chain_iterator end = solver.chain_end();
  BOOST_CHECK( chain != end);
  solver_type::chain_iterator c0 = chain; ++chain;
  BOOST_CHECK( chain != end);
  solver_type::chain_iterator c1 = chain; ++chain;
  BOOST_CHECK( c0 != c1);
  BOOST_CHECK( chain == end);
  BOOST_CHECK(c0->get_end_effector() == b1);
  BOOST_CHECK(c1->get_end_effector() == b3);
  skeleton_type::bone_type * b4 = skeleton.create_bone(b0);
  solver_type::chain_iterator c2 =  OpenTissue::kinematics::inverse::add_chain(solver,*b0,*b4);
  BOOST_CHECK( solver.size() == 3u );
  BOOST_CHECK( c0 != c1);
  BOOST_CHECK( c0 != c2);
  BOOST_CHECK( c1 != c2);
  BOOST_CHECK(c0->get_end_effector() == b1);
  BOOST_CHECK(c1->get_end_effector() == b3);
  BOOST_CHECK(c2->get_end_effector() == b4);
  solver.remove_chain(c2);
  BOOST_CHECK( solver.size() == 2u );
  BOOST_CHECK( c0 != c1);
  BOOST_CHECK(c0->get_end_effector() == b1);
  BOOST_CHECK(c1->get_end_effector() == b3);
  BOOST_CHECK_NO_THROW( solver.solve() );

  BOOST_CHECK_NO_THROW( solver.solve(0,0) );
  BOOST_CHECK_NO_THROW( solver.solve( &solver_type::default_BFGS_settings() ) );
  BOOST_CHECK_NO_THROW( solver.solve( &solver_type::default_SD_settings() ) );
  BOOST_CHECK_NO_THROW( solver.solve( &solver_type::default_SDF_settings() ) );

  solver_type::Settings settings = solver_type::default_BFGS_settings();
  solver_type::Output output;
  BOOST_CHECK_NO_THROW( solver.solve( &settings, &output ) );
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef EIGEN_CSV_HPP
#define EIGEN_CSV_HPP

#include <Eigen/Dense>

#include <cfloat>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <string>
#include <vector>

namespace eigen_csv {

    template <typename T> void write(const Eigen::DenseBase<T>& A,
            const std::string& filename) {

        using namespace std;

        ofstream file;

        file.open(filename);

        file << setprecision(DBL_DIG);

        for (int i = 0; i < A.rows(); i++) {
            for (int j = 0; j < A.cols(); j++)
                file << A(i,j) << ',';
            file << endl;
        }

        file.close();

    }

    template <typename T> void write(const Eigen::DenseBase<T>& A,
            const std::vector<std::string>& header,
            const std::string& filename) {

        using namespace std;

        ofstream file;

        file.open(filename);

        for (unsigned int i = 0; i < header.size(); i++)
            file << header[i] << ',';
        file << endl;

        file << setprecision(DBL_DIG);

        for (int i = 0; i < A.rows(); i++) {
            for (int j = 0; j < A.cols(); j++)
                file << A(i,j) << ',';
            file << endl;
        }

        file.close();

    }

    template <typename T> void read(const std::string& filename,
            bool header, bool resize, Eigen::DenseBase<T>& A) {

        using namespace std;

        ifstream file;

        file.open(filename);

        string line, cell;

        if (header) {
            getline(file, line);
        }

        vector<vector<double>> vals;

        while(getline(file, line)) {

            stringstream str(line);

            vector<double> valr;

            while(getline(str, cell, ','))
                valr.push_back(stod(cell));

            vals.push_back(valr);

        }

        int rows, cols;

        if (resize) {
            rows = vals.size();
            cols = vals[0].size();
            T& B = (T&) A;
            B.resize(rows, cols);
        } else {
            rows = A.rows();
            cols = A.cols();
        }

        for (int i = 0; i < rows; i++)
            for (int j = 0; j < cols; j++)
                A(i,j) = vals[i][j];

        file.close();

    }

}

#endif