#include <ros/ros.h>
#include <fstream>	//流操作，读写文件
#include <string>
#include <string.h>
#include <std_msgs/String.h>
#include <sensor_msgs/NavSatFix.h>
#include <fcntl.h>	//打开文件
#include <unistd.h>	//读取文件

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <cmath>

// #include <sensor_msgs/NavSatFix.h>

#include <stdlib.h>
#include <vector>
#include <boost/regex.hpp>
#include <boost/algorithm/string/regex.hpp>

#include <typeinfo>

#include <stdlib.h>
#include <iostream>
#include <time.h>
using namespace std;

// #define D_R 0.017453292519943295;
#define R  6370996.81 //地球半径

const double PI=3.14159265358979323846264338328;

const float zoom = 1.0;

int gps_flag = 0;

string frame_id = "/my_frame";
double origin_lon=0, origin_lat=0;	//起点经纬度坐标
int GPS_id = 0;

ros::Publisher GPS_marker_pub;	//实时GPS marker点
visualization_msgs::Marker GPS_marker, GPS_text_marker, GPS_direction_marker, GPS_pose_marker;

ros::Subscriber gps_sub;	//订阅gps话题

double lon, lat, last_lon, last_lat;	//存储经纬度数据

// mygps::myGPS gps_info,gps_info_old;
double get_degree(double lon2,double lat2,double lon1,double lat1);
double get_distance(double lon1,double lat1,double lon2,double lat2);
// double lon, lat, last_lon, last_lat;	//存储经纬度数据

void gpsCallback(const sensor_msgs::NavSatFix& gps)
{
	setlocale(LC_ALL,"");

	if (gps_flag == 0){

		gps_flag = 1;
		ROS_INFO("设置原点坐标---");
		origin_lon = gps.longitude;
		origin_lat = gps.latitude;
		last_lat = gps.latitude;
		last_lon = gps.longitude;
		printf("last lon %lf, last lat %lf \n", last_lon, last_lat);
		string file_path;
		file_path = getenv("HOME") + string("/gps_ws/src/mygps/info/ggm");
		ofstream writefile;
		writefile.open(file_path, ios::app);

		writefile << "------------------" << endl;
		writefile.close();
		return ;
	}
	GPS_id++;
	if(GPS_id < 2)
		return;
	printf(" lon %lf,  lat %lf \n", last_lat, last_lon);

	printf(" lon %lf,  lat %lf \n", lon, lat);

	string file_path;
	file_path = getenv("HOME") + string("/gps_ws/src/mygps/info/ggm");
	ofstream writefile;
	writefile.open(file_path, ios::app);

	writefile << to_string(gps.longitude )<< "," << to_string(gps.latitude) << endl;
	writefile.close();

	last_lat = lat;
	last_lon = lon;
	lon = gps.longitude;
	lat = gps.latitude;
	ROS_INFO("目标点的经纬度是： %lf , %lf", lon, lat);
	ROS_INFO("当前点距离原点：%lf",get_distance(lon, lat, origin_lon, origin_lat));
	// ROS_INFO("进入gpsCallback---");

	//更新GPS_marker坐标
	double x = zoom*get_distance(lon, lat, origin_lon, origin_lat)*cos(get_degree(lon, lat, origin_lon, origin_lat));
	double y = zoom*get_distance(lon, lat, origin_lon, origin_lat)*sin(get_degree(lon, lat, origin_lon, origin_lat));

	GPS_marker.header.stamp = ros::Time::now();
	GPS_marker.id = GPS_id;
	GPS_marker.action = visualization_msgs::Marker::ADD;
	GPS_marker.pose.position.x = x;
	GPS_marker.pose.position.y = y;
	GPS_marker.pose.position.z = 0.4;
	GPS_marker.color.a = 1.0;
	GPS_marker_pub.publish(GPS_marker);

	//---将上一个位置的Marker变成半透明----start---
	// lon=last_lon;
	// lat=last_lat;
	printf("last lon %lf, last lat %lf \n", last_lon, last_lat);
	x = zoom*get_distance(last_lon, last_lat, origin_lon, origin_lat)*cos(get_degree(last_lon, last_lat, origin_lon, origin_lat));
	y = zoom*get_distance(last_lon, last_lat, origin_lon, origin_lat)*sin(get_degree(last_lon, last_lat, origin_lon, origin_lat));

	GPS_marker.header.stamp = ros::Time::now();
	GPS_marker.id = GPS_id - 1;
	GPS_marker.action = visualization_msgs::Marker::ADD;
	GPS_marker.pose.position.x = x;
	GPS_marker.pose.position.y = y;
	GPS_marker.pose.position.z = 0.4;
	GPS_marker.color.a = 0.5;
	printf("+++++++++");
	GPS_marker_pub.publish(GPS_marker);
}

// 目标点经纬度 当前点经纬度
double get_degree(double lon2,double lat2,double lon1,double lat1)
{
	// 角度转弧度
	lat1=lat1*PI/180;
	lon1=lon1*PI/180;

	lat2=lat2*PI/180;
	lon2=lon2*PI/180;

	double a = sin(lon2-lon1) * cos(lat2);
	double b = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(lon2-lon1);

	double result = atan2(a,b);	//  R_D;

	return -result;
}

double get_distance(double lon1,double lat1,double lon2,double lat2)
// double get_distance(double lon2,double lat2,double lon1,double lat1)
{
	double a_lat = lat1 * PI / 180;
	double a_lng = lon1 * PI / 180;
	double b_lng = lon2 * PI / 180;
	double b_lat = lat2 * PI / 180;

	double distance = R * acos(sin(a_lat) * sin(b_lat) + cos(a_lat) * cos(b_lat) * cos(b_lng - a_lng));
	// printf("距离：%f\n",distance);
	return distance;
}

int main( int argc, char** argv )
{
	setlocale(LC_ALL,"");

	ros::init(argc, argv, "view");  //节点
	ros::NodeHandle n;
	ros::Rate r(1);
	gps_sub = n.subscribe("/gps/fix",10,gpsCallback);	//订阅gps话题

//---Marker相关 start-------------
	//---全局路径Marker相关 start-------------
	// ros::Publisher path_point_pub = n.advertise<visualization_msgs::MarkerArray>("path_point",20);
		// 全局路径标记点
	// ros::Publisher path_text_pub = n.advertise<visualization_msgs::MarkerArray>("textArray",20);  //----
		// 全局路径标记点文字提示
	// ros::Publisher path_line_strip_pub = n.advertise<visualization_msgs::Marker>("path_line_strip", 10);
		//全局路径折线段
	// visualization_msgs::MarkerArray path_point;
	// visualization_msgs::MarkerArray path_text;
	// visualization_msgs::Marker path_line_strip;
	//---全局路径Marker相关 end------------

	//---坐标系发布 start--- 红色-x 绿色-y 蓝色-z
	ros::Publisher pose_pub = n.advertise<visualization_msgs::Marker>("pose", 20); //话题名称
	//---坐标系发布 end--- 红色-x 绿色-y 蓝色-z

	//--- GPS坐标实时发布 start-----
	GPS_marker_pub = n.advertise<visualization_msgs::Marker>("GPS_marker", 10);
	// ros::spin();
		//---GPS_marker初始化---start---
	GPS_marker.type = visualization_msgs::Marker::SPHERE;
	GPS_marker.header.frame_id = frame_id;
	GPS_marker.ns = "GPS_space";
	GPS_marker.scale.x = 0.5 *zoom;
	GPS_marker.scale.y = 0.5 *zoom;
	GPS_marker.scale.z = 0.5 *zoom;
	GPS_marker.color.r = 255/255.0;
	GPS_marker.color.g = 165/255.0;
	GPS_marker.color.b = 0/255.0;
	GPS_marker.color.a = 1.0;
	GPS_marker.lifetime = ros::Duration();
	GPS_marker.pose.orientation.x = 0.0;
	GPS_marker.pose.orientation.y = 0.0;
	GPS_marker.pose.orientation.z = 0.0;
	GPS_marker.pose.orientation.w = 1.0;
		//---GPS_marker初始化---end------
		/*
		//--- GPS_text_marker 初始化--- start ---
	GPS_text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
	GPS_text_marker.header.frame_id = frame_id;
	GPS_text_marker.ns = "GPS_text_space";
	GPS_text_marker.pose.orientation.x = 0.0;
	GPS_text_marker.pose.orientation.y = 0.0;
	GPS_text_marker.pose.orientation.z = 0.0;
	GPS_text_marker.pose.orientation.w = 1.0;
	GPS_text_marker.scale.z = 0.1;
	GPS_text_marker.color.r = 1.0;
	GPS_text_marker.color.g = 1.0;
	GPS_text_marker.color.b = 1.0;
	GPS_text_marker.color.a = 1.0;
	GPS_text_marker.lifetime = ros::Duration();
		//--- GPS_text_marker 初始化---	end ---
		//----GPS_direction_marker 初始化-- start-----
	GPS_direction_marker.type = visualization_msgs::Marker::ARROW;
	GPS_direction_marker.header.frame_id = frame_id;
	GPS_direction_marker.ns = "GPS_direction_space";
	GPS_direction_marker.pose.orientation.w = 1.0;
	GPS_direction_marker.color.r = 192/255.0;
	GPS_direction_marker.color.g = 255/255.0;
	GPS_direction_marker.color.b = 62/255.0;
	GPS_direction_marker.color.a = 1.0;
	GPS_direction_marker.scale.x=0.2 *zoom;
	GPS_direction_marker.scale.y=0.5*zoom;
	GPS_direction_marker.scale.z=2*zoom;
	GPS_direction_marker.lifetime = ros::Duration();
		//----GPS_direction_marker 初始化-- start-----

		//----GPS_pose_marker 初始化-- start-----
	GPS_pose_marker.type = visualization_msgs::Marker::ARROW;
	GPS_pose_marker.header.frame_id = frame_id;
	GPS_pose_marker.ns = "GPS_pose_space";
	GPS_pose_marker.pose.orientation.w = 1.0;
	GPS_pose_marker.color.b = 1.0;
	GPS_pose_marker.color.a = 1.0;
	GPS_pose_marker.scale.x=0.1*zoom;
	GPS_pose_marker.scale.y=0.5*zoom;
	GPS_pose_marker.scale.z=2*zoom;
	GPS_pose_marker.lifetime = ros::Duration();
		//----GPS_pose_marker 初始化-- end-----

	//--- GPS坐标实时发布 end-----

	//-----全局路径折线段 初始化 start ---
	path_line_strip.header.frame_id = frame_id;
	path_line_strip.header.stamp = ros::Time::now();
	path_line_strip.ns = "path_line_space";
	path_line_strip.action = visualization_msgs::Marker::ADD;
	path_line_strip.type = visualization_msgs::Marker::LINE_STRIP;
	path_line_strip.pose.orientation.w = 1.0;
	path_line_strip.id = 1;
	path_line_strip.scale.x = 0.1;	//设置线条粗细
	path_line_strip.color.r = 208/255.0;
	path_line_strip.color.g = 32/255.0;
	path_line_strip.color.b = 144/255.0;
	path_line_strip.color.a = 1.0;
	//-----全局路径折线段 end ---
		*/
//----Marker相关 end-------------

//--- 文件读取与参数初始化 start ----
/*
	string file_path;
	file_path = getenv("HOME") + string("/gps_ws/src/mygps/info/path");	//getenv("HOME") 获取用户目录
	// file_path = getenv("HOME") + string("/gps_ws/src/mygps/info/gcj02file.txt");	//getenv("HOME") 获取用户目录
	ifstream readfile;

	char data[1000];	//存储文本数据
	string datastr;
	int datalen;	//获取长度，用于剔除时间戳和跳出循环
	double lon, lat;	//存储经纬度数据

	vector <string> lon_lat_list, lon_lat;

	int time_i = 0;
*/
	//------获取方位角数据测试--start---------
	/*
	^N
				*
				30
				*
	W <- *****105****106,29****107**** -> E
				*
				28
				*
	*/
	if(1){
		printf("东北方向%f\n",get_degree(107,30,106,29)/(PI / 180));
		printf("东南方向%f\n",get_degree(107,28,106,29)/(PI / 180));
		printf("西南方向%f\n",get_degree(106,28,106,29)/(PI / 180));
		printf("西北方向%f\n",get_degree(106,30,106,29)/(PI / 180));
	}
	//------获取方位角数据测试--end----------
	//--- 文件读取与参数初始化 end ----

	//-----------------处理整个高德地图数据的经纬度 start------------------

	// int load_flag=1;//全局路径加载标志
	while(ros::ok()){

		// 等待GPS提供起点位置
		if (gps_flag ==0 )
		{
			ROS_INFO("等待GPS提供起点位置--");
			ros::spinOnce();
			r.sleep();
			continue;
		}
		// if (GPS_id == 3 && gps_info.plan_flag){
		// 	origin_lon = gps_info.WGS84_lon;
		// 	origin_lat = gps_info.WGS84_lat;
		// 	ROS_INFO("设置原点完成");
		// }

			//----rviz显示坐标系箭头 start ---
		for (int i = 0; i < 3; i++)
		{
			// printf("画箭头\n");

			visualization_msgs::Marker pose_marker;
			pose_marker.header.frame_id = frame_id;

			pose_marker.ns = "pose_space";

			pose_marker.type = visualization_msgs::Marker::ARROW;
			pose_marker.action = visualization_msgs::Marker::ADD;

			pose_marker.header.stamp = ros::Time::now();
			pose_marker.id = i;
			pose_marker.pose.orientation.w = 1.0;
			// pose_marker.

			geometry_msgs::Point start_p, end_p;
			start_p.x = 0;
			start_p.y = 0;
			start_p.z = 0;
			pose_marker.points.push_back(start_p);

			end_p.x = 0;
			end_p.y = 0;
			end_p.z = 0;
			if (i==0){
				end_p.x = zoom * 5;
				pose_marker.color.r=1.0;
				// printf("-\n");
			}else if(i==1){
				end_p.y = zoom * 5;
				pose_marker.color.g=1.0;
				// printf("--\n");
			}else if(i==2){
				end_p.z = zoom * 5;
				pose_marker.color.b=1.0;
				// printf("---\n");
			}
			pose_marker.color.a=1.0;
			pose_marker.points.push_back(end_p);

			pose_marker.scale.x=1 * zoom/10;
			pose_marker.scale.y=2 * zoom/10;
			pose_marker.scale.z=1 * zoom/10;

			pose_marker.lifetime = ros::Duration();
			pose_pub.publish(pose_marker);
		}
			//----rviz显示坐标系箭头 end ---

		/* if(load_flag == 1)
		{
			readfile.open(file_path.c_str());	//读取文件
			if(readfile.bad()){
				ROS_INFO("读取文件失败");
				return -1;
			}else{
				printf("正在运行\n");
				// while (!readfile.eof())	//判断文件是否到结尾
			}
			int marker_id=0;

			int tmp_i = 0;
			int tmp_j = 0;
			while (!readfile.eof())	//判断文件是否到结尾
			{
				readfile >> data;
				tmp_i++;
				// if(tmp_i%37 == 0){
				if(1){

				tmp_j++;
				cout << "这是 j " << tmp_j << endl;
				printf("%d \n", tmp_i);
				printf("读取新一行\n");
				// readfile >> data;

				cout << "--data: " << data << endl;

				datastr=string(data);	//字符数组转换为字符串
				if(data[0]=='-' && data[1]=='-')
					break;

				boost::split_regex(lon_lat,datastr,boost::regex(","));

				lon = stod(lon_lat[0].c_str());
				lat = stod(lon_lat[1].c_str());

				// 数据处理结束 end------------------------
				printf("分割字符串 lon : %f lat :%f \n 原点：%f %f \n", lon, lat, origin_lon, origin_lat);
				double x = get_distance(lon, lat, origin_lon, origin_lat)*cos(get_degree(lon, lat, origin_lon, origin_lat));
				double y = get_distance(lon, lat, origin_lon, origin_lat)*sin(get_degree(lon, lat, origin_lon, origin_lat));
				printf("%f, %f \n",x, y);

				//---Marker相关 start-------------

				//---全局路径Marker相关 start-------------
				visualization_msgs::Marker path_marker;
				path_marker.header.frame_id = frame_id;
				path_marker.header.stamp = ros::Time::now();

				path_marker.ns = "path_space";
				marker_id++;
				path_marker.id = marker_id;
				// printf("----%d-----\n",path_marker.id);
				path_marker.type = visualization_msgs::Marker::SPHERE;
				path_marker.action = visualization_msgs::Marker::ADD;

				path_marker.pose.position.x = x * zoom;
				path_marker.pose.position.y = y * zoom;
				path_marker.pose.position.z = 0;
				path_marker.pose.orientation.x = 0.0;
				path_marker.pose.orientation.y = 0.0;
				path_marker.pose.orientation.z = 0.0;
				path_marker.pose.orientation.w = 1.0;

				path_marker.scale.x = 0.3;
				path_marker.scale.y = 0.3;
				path_marker.scale.z = 0.30;

				path_marker.color.r = 0.0f;
				path_marker.color.g = 1.0f;
				path_marker.color.b = 0.0f;
				path_marker.color.a = 1.0;

				path_marker.lifetime = ros::Duration();
				if(load_flag == 1)
					path_point.markers.push_back(path_marker);
					//同一个id的Marker重复push会报错“Adding marker 'path_space/1' multiple times. ”
					// 多次添加标记“path_space/1”。

				// path_point_pub.publish(path_point);
					//---全局路径Marker相关 end-------------

					//---Marker文本显示 start-------------
				visualization_msgs::Marker path_text_marker;
				path_text_marker.header.frame_id = frame_id;
				path_text_marker.header.stamp = path_marker.header.stamp;

				path_text_marker.ns = "text_space";
				path_text_marker.id = marker_id;

				path_text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
				path_text_marker.action = visualization_msgs::Marker::ADD;

				path_text_marker.pose.position.x = path_marker.pose.position.x;
				path_text_marker.pose.position.y = path_marker.pose.position.y;
				path_text_marker.pose.position.z = path_marker.scale.z;
				path_text_marker.pose.orientation.x = 0.0;
				path_text_marker.pose.orientation.y = 0.0;
				path_text_marker.pose.orientation.z = 0.0;
				path_text_marker.pose.orientation.w = 1.0;

				path_text_marker.scale.z = 0.30;	//控制字体大小

				path_text_marker.color.r = 0.0f;
				path_text_marker.color.g = 1.0f;
				path_text_marker.color.b = 1.0f;
				path_text_marker.color.a = 1.0;

				ostringstream str;
				str << marker_id;	//写入文本内容
				path_text_marker.text=str.str();

				path_text_marker.lifetime = ros::Duration();
				if(load_flag == 1)
							path_text.markers.push_back(path_text_marker);
				//---Marker文本显示 end-------------

				//-----全局路径折线段 start ---
				geometry_msgs::Point p;
				p.x = path_marker.pose.position.x;
				p.y = path_marker.pose.position.y;
				p.z = path_marker.pose.position.z;

				path_line_strip.points.push_back(p);
				//-----全局路径折线段 end ---
				//---Marker相关 end-------------
			}
			}
			// path_text_pub.publish(path_text);
			// path_point_pub.publish(path_point);
			// path_line_strip_pub.publish(path_line_strip);
			printf("文本读取完毕\n");

			// printf("文本读取完毕");
			load_flag++; //加载完成
			readfile.close();
			// 	break;

		// }*/
		// path_text_pub.publish(path_text);
		// path_point_pub.publish(path_point);
		// path_line_strip_pub.publish(path_line_strip);
		//------------------------------------
		ros::spinOnce();
		r.sleep();
	}
	// ros::spin();
	return 0;
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/map.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/product.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/maybe.hpp>
using namespace boost::hana;

template <int i>
constexpr auto key = int_<i>;

template <int i>
constexpr auto value = int_<i + 100>;

template <int i, int j>
BOOST_HANA_CONSTEXPR_LAMBDA auto p = detail::minimal::product<>(key<i>, value<j>);

BOOST_HANA_CONSTEXPR_LAMBDA auto is = [](auto x) {
    return [=](auto y) { return equal(x, y); };
};

BOOST_HANA_CONSTANT_ASSERT(find(map(), is(key<1>)) == nothing);

BOOST_HANA_CONSTANT_ASSERT(find(map(p<1, 1>), is(key<1>)) == just(value<1>));
BOOST_HANA_CONSTANT_ASSERT(find(map(p<1, 1>), is(key<2>)) == nothing);

BOOST_HANA_CONSTANT_ASSERT(find(map(p<1, 1>, p<2, 2>), is(key<1>)) == just(value<1>));
BOOST_HANA_CONSTANT_ASSERT(find(map(p<1, 1>, p<2, 2>), is(key<2>)) == just(value<2>));
BOOST_HANA_CONSTANT_ASSERT(find(map(p<1, 1>, p<2, 2>), is(key<3>)) == nothing);

int main() { (void)is; }


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#include <graphblas/graphblas.hpp>

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE vxm_test_suite

#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************

namespace
{
    std::vector<std::vector<double> > m3x3_dense = {{12, 0, 7},
                                                    {1,  0, 0},
                                                    {3,  6, 9}};

    std::vector<std::vector<double> > m4x3_dense = {{5, 6, 4},
                                                    {0, 7, 5},
                                                    {1, 0, 0},
                                                    {2, 0, 1}};
    std::vector<double> u3_dense = {1, 1, 0};
    std::vector<double> u4_dense = {0, 0, 1, 1};

    std::vector<double> ans3_dense = {13, 0, 7};
    std::vector<double> ans4_dense = { 3, 0, 1};

    static std::vector<double> v4_ones = {1, 1, 1, 1};
    static std::vector<double> v3_ones = {1, 1, 1};
}

//****************************************************************************
// Tests without mask
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_bad_dimensions)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);

    grb::Vector<double, grb::SparseTag> w4(4);
    grb::Vector<double, grb::SparseTag> w3(3);

    // incompatible input dimensions
    BOOST_CHECK_THROW(
        (grb::vxm(w3,
                  grb::NoMask(),
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(), u3, mB)),
        grb::DimensionException);

    // incompatible output matrix dimensions
    BOOST_CHECK_THROW(
        (grb::vxm(w4,
                  grb::NoMask(),
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(), u3, mA)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);
    grb::Vector<double> u3(u3_dense, 0.);
    grb::Vector<double> u4(u4_dense, 0.);
    grb::Vector<double> result(3);
    grb::Vector<double> ansA(ans3_dense, 0.);
    grb::Vector<double> ansB(ans4_dense, 0.);

    grb::vxm(result,
             grb::NoMask(),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(), u3, mA);
    BOOST_CHECK_EQUAL(result, ansA);

    grb::vxm(result,
             grb::NoMask(),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(), u4, mB);
    BOOST_CHECK_EQUAL(result, ansB);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_stored_zero_result)
{
    // Build some matrices.
    std::vector<std::vector<int> > A = {{1, 1, 0, 0},
                                        {1, 2, 2, 0},
                                        {0, 2, 3, 3},
                                        {0, 0, 3, 4}};
    std::vector<int> u4_dense =  { 1,-1, 0, 0};
    grb::Matrix<int, grb::DirectedMatrixTag> mA(A, 0);
    grb::Vector<int> u(u4_dense, 0);
    grb::Vector<int> result(4);

    // use a different sentinel value so that stored zeros are preserved.
    int const NIL(666);
    std::vector<int> ans = {  0,  -1, -2, NIL};
    grb::Vector<int> answer(ans, NIL);

    grb::vxm(result,
             grb::NoMask(),
             grb::NoAccumulate(), // Second<int>(),
             grb::ArithmeticSemiring<int>(), u, mA);
    BOOST_CHECK_EQUAL(result, answer);
    BOOST_CHECK_EQUAL(result.nvals(), 3);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_a_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);
    grb::Matrix<double, grb::DirectedMatrixTag> mB(m4x3_dense, 0.);
    grb::Vector<double> u3(u3_dense, 0.);
    grb::Vector<double> u4(u4_dense, 0.);
    grb::Vector<double> result3(3);
    grb::Vector<double> result4(4);

    std::vector<double> ansAT_dense = {12, 1, 9};
    std::vector<double> ansBT_dense = {11, 7, 1, 2};

    grb::Vector<double> ansA(ansAT_dense, 0.);
    grb::Vector<double> ansB(ansBT_dense, 0.);

    grb::vxm(result3,
             grb::NoMask(),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(),
             u3, transpose(mA));
    BOOST_CHECK_EQUAL(result3, ansA);

    BOOST_CHECK_THROW(
        (grb::vxm(result4,
                  grb::NoMask(),
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(),
                  u4, transpose(mB))),
        grb::DimensionException);

    grb::vxm(result4,
             grb::NoMask(),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(),
             u3, transpose(mB));
    BOOST_CHECK_EQUAL(result4, ansB);
}

//****************************************************************************
// Tests using a mask with REPLACE
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_masked_replace_bad_dimensions)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m4(u4_dense, 0.);
    grb::Vector<double, grb::SparseTag> w3(3);

    // incompatible mask matrix dimensions
    BOOST_CHECK_THROW(
        (grb::vxm(w3,
                  m4,
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(), u3, mA,
                  grb::REPLACE)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_masked_replace_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m3(u3_dense, 0.);
    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 1, 0};
        grb::Vector<double> answer(ans, 0.);

        grb::vxm(result,
                 m3,
                 grb::Second<double>(),
                 grb::ArithmeticSemiring<double>(), u3, mA,
                 grb::REPLACE);

        BOOST_CHECK_EQUAL(result.nvals(), 2);
        BOOST_CHECK_EQUAL(result, answer);
    }

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 0, 0};
        grb::Vector<double> answer(ans, 0.);

        grb::vxm(result,
                 m3,
                 grb::NoAccumulate(),
                 grb::ArithmeticSemiring<double>(), u3, mA,
                 grb::REPLACE);

        BOOST_CHECK_EQUAL(result.nvals(), 1);
        BOOST_CHECK_EQUAL(result, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_masked_replace_reg_stored_zero)
{
    // tests a computed and stored zero
    // tests a stored zero in the mask

    // Build some matrices.
    std::vector<std::vector<int> > A_dense = {{1, 1, 0, 0},
                                              {1, 2, 2, 0},
                                              {0, 2, 3, 3},
                                              {0, 0, 3, 4}};
    std::vector<int> u4_dense =  { 1,-1, 1, 0};
    std::vector<int> mask_dense =  { 1, 1, 1, 0};
    std::vector<int> ones_dense =  { 1, 1, 1, 1};
    grb::Matrix<int, grb::DirectedMatrixTag> A(A_dense, 0);
    grb::Vector<int> u(u4_dense, 0);
    grb::Vector<int> mask(mask_dense, 0);
    grb::Vector<int> result(ones_dense, 0);

    BOOST_CHECK_EQUAL(mask.nvals(), 3);  // [ 1 1 1 - ]
    mask.setElement(3, 0);
    BOOST_CHECK_EQUAL(mask.nvals(), 4);  // [ 1 1 1 0 ]

    int const NIL(666);
    std::vector<int> ans = { 0, 1, 1,  NIL};
    grb::Vector<int> answer(ans, NIL);

    grb::vxm(result,
             mask,
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(), u, A,
             grb::REPLACE);
    BOOST_CHECK_EQUAL(result.nvals(), 3);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_masked_replace_a_transpose)
{
    // Build some matrices.
    std::vector<std::vector<int> > A_dense = {{1, 1, 0, 0},
                                              {1, 2, 2, 0},
                                              {0, 2, 3, 3},
                                              {0, 0, 3, 4},
                                              {1, 1, 1, 1}};
    std::vector<int> u4_dense =  { 1,-1, 1, 0};
    std::vector<int> mask_dense =  { 1, 0, 1, 0, 1};
    std::vector<int> ones_dense =  { 1, 1, 1, 1, 1};
    grb::Matrix<int, grb::DirectedMatrixTag> A(A_dense, 0);
    grb::Vector<int> u(u4_dense, 0);
    grb::Vector<int> mask(mask_dense, 0);
    grb::Vector<int> result(ones_dense, 0);

    int const NIL(666);
    std::vector<int> ans = { 0, NIL, 1,  NIL, 1};
    grb::Vector<int> answer(ans, NIL);

    grb::vxm(result,
             mask,
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(), u, transpose(A),
             grb::REPLACE);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
// Tests using a mask with MERGE semantics
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_masked_bad_dimensions)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m4(u4_dense, 0.);
    grb::Vector<double, grb::SparseTag> w3(v3_ones, 0.);

    // incompatible mask matrix dimensions
    BOOST_CHECK_THROW(
        (grb::vxm(w3,
                  m4,
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(), u3, mA)),
        grb::DimensionException);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_masked_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m3(u3_dense, 0.); // [1 1 -]
    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 1, 1};
        grb::Vector<double> answer(ans, 0.);

        grb::vxm(result,
                 m3,
                 grb::Second<double>(),
                 grb::ArithmeticSemiring<double>(), u3, mA);

        BOOST_CHECK_EQUAL(result.nvals(), 3);
        BOOST_CHECK_EQUAL(result, answer);
    }

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 0, 1};
        grb::Vector<double> answer(ans, 0.);

        grb::vxm(result,
                 m3,
                 grb::NoAccumulate(),
                 grb::ArithmeticSemiring<double>(), u3, mA);

        BOOST_CHECK_EQUAL(result.nvals(), 2);
        BOOST_CHECK_EQUAL(result, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_masked_reg_stored_zero)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m3(u3_dense); // [1 1 0]
    BOOST_CHECK_EQUAL(m3.nvals(), 3);

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 1, 1};
        grb::Vector<double> answer(ans, 0.);

        grb::vxm(result,
                 m3,
                 grb::Second<double>(),
                 grb::ArithmeticSemiring<double>(), u3, mA);

        BOOST_CHECK_EQUAL(result.nvals(), 3);
        BOOST_CHECK_EQUAL(result, answer);
    }

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 0, 1};
        grb::Vector<double> answer(ans, 0.);

        grb::vxm(result,
                 m3,
                 grb::NoAccumulate(),
                 grb::ArithmeticSemiring<double>(), u3, mA);

        BOOST_CHECK_EQUAL(result.nvals(), 2);
        BOOST_CHECK_EQUAL(result, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_masked_a_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.); // [1 1 -]
    grb::Vector<double, grb::SparseTag> m3(u3_dense, 0.); // [1 1 -]
    grb::Vector<double> result(v3_ones, 0.);

    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    std::vector<double> ans = {12, 1, 1};
    grb::Vector<double> answer(ans, 0.);

    grb::vxm(result,
             m3,
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(), u3, transpose(mA));

    BOOST_CHECK_EQUAL(result.nvals(), 3);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
// Tests using a complemented mask with REPLACE semantics
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_scmp_masked_replace_bad_dimensions)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m4(u4_dense, 0.);
    grb::Vector<double, grb::SparseTag> w3(3);

    // incompatible mask matrix dimensions
    BOOST_CHECK_THROW(
        (grb::vxm(w3,
                  grb::complement(m4),
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(), u3, mA,
                  grb::REPLACE)),
        grb::DimensionException);
}
//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_scmp_masked_replace_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);

    std::vector<double> scmp_mask_dense = {0., 0., 1.};
    grb::Vector<double, grb::SparseTag> m3(scmp_mask_dense, 0.);
    BOOST_CHECK_EQUAL(m3.nvals(), 1);

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 1, 0};
        grb::Vector<double> answer(ans, 0.);

        grb::vxm(result,
                 grb::complement(m3),
                 grb::Second<double>(),
                 grb::ArithmeticSemiring<double>(), u3, mA,
                 grb::REPLACE);

        BOOST_CHECK_EQUAL(result.nvals(), 2);
        BOOST_CHECK_EQUAL(result, answer);
    }

    {
        grb::Vector<double> result(v3_ones, 0.);

        std::vector<double> ans = {13, 0, 0};
        grb::Vector<double> answer(ans, 0.);

        grb::vxm(result,
                 grb::complement(m3),
                 grb::NoAccumulate(),
                 grb::ArithmeticSemiring<double>(), u3, mA,
                 grb::REPLACE);

        BOOST_CHECK_EQUAL(result.nvals(), 1);
        BOOST_CHECK_EQUAL(result, answer);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_scmp_masked_replace_reg_stored_zero)
{
    // tests a computed and stored zero
    // tests a stored zero in the mask
    // Build some matrices.
    std::vector<std::vector<int> > A_dense = {{1, 1, 0, 0},
                                              {1, 2, 2, 0},
                                              {0, 2, 3, 3},
                                              {0, 0, 3, 4}};
    std::vector<int> u4_dense =  { 1,-1, 1, 0};
    std::vector<int> scmp_mask_dense =  { 0, 0, 0, 1 };
    std::vector<int> ones_dense =  { 1, 1, 1, 1};
    grb::Matrix<int, grb::DirectedMatrixTag> A(A_dense, 0);
    grb::Vector<int> u(u4_dense, 0);
    grb::Vector<int> mask(scmp_mask_dense); //, 0);
    grb::Vector<int> result(ones_dense, 0);

    BOOST_CHECK_EQUAL(mask.nvals(), 4);  // [ 0 0 0 1 ]

    int const NIL(666);
    std::vector<int> ans = { 0, 1, 1,  NIL};
    grb::Vector<int> answer(ans, NIL);

    grb::vxm(result,
             grb::complement(mask),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(), u, A,
             grb::REPLACE);
    BOOST_CHECK_EQUAL(result.nvals(), 3);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_scmp_masked_replace_a_transpose)
{
    // Build some matrices.
    std::vector<std::vector<int> > A_dense = {{1, 1, 0, 0},
                                              {1, 2, 2, 0},
                                              {0, 2, 3, 3},
                                              {0, 0, 3, 4},
                                              {1, 1, 1, 1}};
    std::vector<int> u4_dense =  { 1,-1, 1, 0};
    std::vector<int> mask_dense =  { 0, 1, 0, 1, 0};
    std::vector<int> ones_dense =  { 1, 1, 1, 1, 1};
    grb::Matrix<int, grb::DirectedMatrixTag> A(A_dense, 0);
    grb::Vector<int> u(u4_dense, 0);
    grb::Vector<int> mask(mask_dense, 0);
    grb::Vector<int> result(ones_dense, 0);

    int const NIL(666);
    std::vector<int> ans = { 0, NIL, 1,  NIL, 1};
    grb::Vector<int> answer(ans, NIL);

    grb::vxm(result,
             grb::complement(mask),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(), u, transpose(A),
             grb::REPLACE);

    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
// Tests using a complemented mask (with merge semantics)
//****************************************************************************

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_scmp_masked_bad_dimensions)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m4(u4_dense, 0.);
    grb::Vector<double, grb::SparseTag> w3(v3_ones, 0.);

    // incompatible mask matrix dimensions
    BOOST_CHECK_THROW(
        (grb::vxm(w3,
                  grb::complement(m4),
                  grb::NoAccumulate(),
                  grb::ArithmeticSemiring<double>(), u3, mA)),
        grb::DimensionException);
}
//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_scmp_masked_reg)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m3(u3_dense, 0.); // [1 1 -]
    grb::Vector<double> result(v3_ones, 0.);

    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    std::vector<double> ans = {1, 1, 7}; // if accum is NULL then {1, 0, 7};
    grb::Vector<double> answer(ans, 0.);

    grb::vxm(result,
             grb::complement(m3),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(), u3, mA);

    BOOST_CHECK_EQUAL(result.nvals(), 3); //2);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_scmp_masked_reg_stored_zero)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.);
    grb::Vector<double, grb::SparseTag> m3(u3_dense); // [1 1 0]
    grb::Vector<double> result(v3_ones, 0.);

    BOOST_CHECK_EQUAL(m3.nvals(), 3);

    std::vector<double> ans = {1, 1, 7}; // if accum is NULL then {1, 0, 7};
    grb::Vector<double> answer(ans, 0.);

    grb::vxm(result,
             grb::complement(m3),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(), u3, mA);

    BOOST_CHECK_EQUAL(result.nvals(), 3); //2);
    BOOST_CHECK_EQUAL(result, answer);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(test_vxm_scmp_masked_a_transpose)
{
    grb::Matrix<double, grb::DirectedMatrixTag> mA(m3x3_dense, 0.);

    grb::Vector<double, grb::SparseTag> u3(u3_dense, 0.); // [1 1 -]
    grb::Vector<double, grb::SparseTag> m3(u3_dense, 0.); // [1 1 -]
    grb::Vector<double> result(v3_ones, 0.);

    BOOST_CHECK_EQUAL(m3.nvals(), 2);

    std::vector<double> ans = {1, 1, 9};
    grb::Vector<double> answer(ans, 0.);

    grb::vxm(result,
             grb::complement(m3),
             grb::NoAccumulate(),
             grb::ArithmeticSemiring<double>(), u3, transpose(mA));

    BOOST_CHECK_EQUAL(result.nvals(), 3);
    BOOST_CHECK_EQUAL(result, answer);
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (c) 2013-2015 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ALLSOL_AFFINE_HPP
#define ALLSOL_AFFINE_HPP

#include <iostream>
#include <list>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/matrix-inversion.hpp>
#include <kv/affine.hpp>
#include <kv/lp.hpp>

#ifndef USE_TRIM
#define USE_TRIM 1
#endif

#ifndef USE_SUMABS
#define USE_SUMABS 0
#endif

#ifndef USE_LP
#define USE_LP 0
#endif

#ifndef USE_FI
#define USE_FI 1
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

namespace allsol_affine_sub {

// return index of I_i which has maximum width

template <class T> int search_maxwidth (const ub::vector< interval<T> >& I) {
	int s = I.size();
	int i, mi;
	T m, tmp;

	m = 0.;
	for (i=0; i<s; i++) {
		tmp = width(I(i));
		if (tmp > m) {
			m = tmp; mi = i;
		}
	}

	return mi;
}

// return max width(I_i) / width(J_i)

template <class T> T widthratio_max (const ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J) {
	int s = I.size();
	int i;
	T tmp, r;

	r = 0.;

	for (i=0; i<s; i++) {
		tmp = width(I(i)) / width(J(i));
		if (tmp > r) r = tmp;
	}

	return r;
}

// return min width(I_i) / width(J_i)

template <class T> T widthratio_min (const ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J) {
	int s = I.size();
	int i;
	T tmp, r;

	r = std::numeric_limits<T>::max();

	for (i=0; i<s; i++) {
		tmp = width(I(i)) / width(J(i));
		if (tmp < r) r = tmp;
	}

	return r;
}

} // namespace allsol_affine_sub

// find all solution of f in I

template <class T, class F> std::list< ub::vector< interval<T> > >
allsol_affine(F f, const ub::vector< interval<T> >& I, int verbose=1)
{
	std::list< ub::vector < interval<T> > > targets;
	targets.push_back(I);
	return allsol_list_affine(f, targets, verbose);
}

// find all solution of f in targets (list of intervals)

template <class T, class F> std::list< ub::vector< interval<T> > >
allsol_list_affine(F f, std::list< ub::vector< interval<T> > > targets, int verbose=1)
{
	int s = (targets.front()).size();
	ub::vector< interval<T> > I, fc, fi, C, CK, K, mvf, I1, I2;
	ub::matrix< interval<T> > fdi, M;
	ub::matrix<T> L, R, E;
	std::list< ub::vector< interval<T> > > solutions, solutions_big;
	typename std::list< ub::vector< interval<T> > >::iterator p, p2;
	int i, j, k, mi;
	T tmp;
	bool r, M_calculated, flag, flag2;
	int count_ne_test = 0;
	int count_ex_test = 0;
	int count_unknown = targets.size();
	int count_ne = 0;
	int count_ex = 0;
	ub::vector< affine<T> > ax, ay, ak;
	ub::vector< interval<T> > IR;
	T trim_tmp;
	interval<T> trim_I;
	affine<T> sum_abs;
	ub::vector< ub::vector<T> > ay2;
	interval<T> I_tmp;
	T lp_tmp;
	ub::vector< interval<T> > objfunc_I;
	ub::vector<T> objfunc;
	std::list< ub::vector<T> > constraints;
	ub::vector<T> c_tmp;
	int ep_size, lp_size;
	bool lp_flag;
	T lp_return;

	E = ub::identity_matrix<T>(s);
	L.resize(s,s);

	while (!targets.empty()) {
		if (verbose >= 2) {
			std::cout << "ne_test: " << count_ne_test << ", ex_test: " << count_ex_test << ", unknown: " << count_unknown << ", ne: " << count_ne << ", ex: " << count_ex << "    \r" << std::flush;
		}

		I = targets.front();
		targets.pop_front();
		count_unknown--;

		// non-existence test

		count_ne_test++;

#if USE_FI == 1
		try {
			fi = f(I);
		}
		catch (std::domain_error& e) {
			goto label;
		}

		if (!zero_in(fi)) {
			count_ne++;
			continue;
		}
#endif

		affine<T>::maxnum() = 0;
		ax = I;
		try {
			ay = f(ax);
		}
		catch (std::domain_error& e) {
			goto label;
		}

		fi = to_interval(ay);
		if (!zero_in(fi)) {
			count_ne++;
			continue;
		}

#if USE_SUMABS == 1
		sum_abs = 0.;
		for (i=0; i<s; i++) {
			sum_abs += abs(ay(i));
		}
		if (to_interval(sum_abs).lower() > 0.) {
			count_ne++;
			continue;
		}
#endif

#if USE_LP == 1
#if USE_LP_FAST == 1
		ep_size = 2 * s;
		lp_size = 1 + ep_size * 2;
		ay2.resize(s + ep_size);
		for (i=0; i<s; i++) {
			ay2(i).resize(lp_size);
			for (j=s + 1; j < lp_size; j++) {
				ay2(i)(j) = 0.;
			}
			I_tmp = ay(i).get_mid();
			for (j=1; j <= s; j++) {
				tmp = ay(i).get_coef(j);
				ay2(i)(j) = tmp;
				I_tmp -= tmp;
			}
			lp_tmp = ay(i).get_err();
			rop<T>::begin();
			for (j=s+1; j <= affine<T>::maxnum(); j++) {
				tmp = ay(i).get_coef(j);
				if (tmp < 0.) tmp = -tmp;
				lp_tmp = rop<T>::add_up(lp_tmp, tmp);
			}
			rop<T>::end();
			I_tmp -= lp_tmp;
			rop<T>::begin();
			lp_tmp = rop<T>::add_up(lp_tmp, rad(I_tmp));
			rop<T>::end();
			ay2(i)(s + 1 + i) = lp_tmp;
			ay2(i)(0) = I_tmp.lower();
			if (ay2(i)(0) > 0.) {
				for (j=0; j<lp_size; j++) {
					ay2(i)(j) = -ay2(i)(j);
				}
			}
		}
#else
		ep_size = affine<T>::maxnum() + s;
		lp_size = 1 + ep_size * 2;
		ay2.resize(s + ep_size);
		for (i=0; i<s; i++) {
			ay2(i).resize(lp_size);
			for (j=affine<T>::maxnum() + 1; j < lp_size; j++) {
				ay2(i)(j) = 0.;
			}
			I_tmp = ay(i).get_mid();
			for (j=1; j <= affine<T>::maxnum(); j++) {
				tmp = ay(i).get_coef(j);
				ay2(i)(j) = tmp;
				I_tmp -= tmp;
			}
			lp_tmp = ay(i).get_err();
			I_tmp -= lp_tmp;
			rop<T>::begin();
			lp_tmp = rop<T>::add_up(lp_tmp, rad(I_tmp));
			rop<T>::end();
			ay2(i)(affine<T>::maxnum() + 1 + i) = lp_tmp;
			ay2(i)(0) = I_tmp.lower();
			if (ay2(i)(0) > 0.) {
				for (j=0; j<lp_size; j++) {
					ay2(i)(j) = -ay2(i)(j);
				}
			}
		}
#endif
		for (i=0; i<ep_size; i++) {
			ay2(s + i).resize(lp_size);
			ay2(s + i)(0) = -2.;
			for (j=1; j<lp_size; j++) {
				ay2(s + i)(j) = 0.;
			}
			ay2(s + i)(1 + i) = 1.;
			ay2(s + i)(ep_size + 1 + i) = 1.;
		}
		objfunc_I.resize(lp_size);
		for (j=0; j<lp_size; j++) objfunc_I(j) = 0.;
		for (i=0; i<s + ep_size; i++) {
			for (j=0; j<lp_size; j++) {
				objfunc_I(j) -= ay2(i)(j);
			}
		}
		objfunc.resize(lp_size);
		for (j=0; j<lp_size; j++) {
			objfunc(j) = objfunc_I(j).lower();
		}

		lp_flag = true;
		constraints.clear();
		for (i=0; i<s + ep_size; i++) {
			c_tmp.resize(lp_size);
			for (j=0; j<lp_size; j++) {
				c_tmp(j) = ay2(i)(j);
			}
			/*
			if (c_tmp(0) >= 0.) {
				lp_flag = false;
			}
			*/
			constraints.push_back(c_tmp);
		}

		if (lp_flag) {
			lp_return = lp_minimize_verified(objfunc, constraints, -1);
			if (lp_return > 0.) {
				count_ne++;
				continue;
			}
		}

#endif

#if USE_TRIM == 1
		IR = I;
		flag = false;
		flag2 = false;
		for (i=0; i<s; i++) {
			trim_tmp = rad(ay(i));
			for (j=0; j<s; j++) {
				tmp = ay(i).get_coef(j+1);
				if (tmp == 0.) continue;
				if (tmp < 0.) tmp = -tmp;
				rop<T>::begin();
				tmp = rop<T>::sub_up(trim_tmp, tmp);
				rop<T>::end();
				trim_I = ax(j).get_mid() - (ay(i).get_mid() + tmp * interval<T>(-1., 1.)) * ax(j).get_coef(j+1) / ay(i).get_coef(j+1);
				if (overlap(IR(j), trim_I)) {
					if (!subset(IR(j), trim_I)) {
						IR(j) = intersect(IR(j), trim_I);
						flag2 = true;
					}
				} else {
					flag = true;
					break;
				}
			}
			if (flag == true) break;
		}

		if (flag == true) {
			count_ne++;
			continue;
		}
#endif

		// existence test

		for (i=0; i<s; i++) {
			for (j=0; j<s; j++) {
				L(i, j) = ay(i).get_coef(j+1) / ax(j).get_coef(j+1);
			}
		}

		count_ex_test++;

		r = invert(L, R);
		if (!r) goto label;

		ak = ax - prod(R, ay);
		K = to_interval(ak);

		if (!overlap(K, I)) {
			count_ne++;
			continue;
		}

		if (proper_subset(K, I)) {
			// check whether the solution is already found or not
			flag = true;
			p = solutions.begin();
			p2 = solutions_big.begin();
			while (p != solutions.end()) {
				if (overlap(K, *p)) {
					if (subset(K, *p2)||subset(*p, I)) {
						flag = false;
						break;
					}
					while (true) {
						affine<T>::maxnum() = 0;
						ax = K;
						ay = f(ax);
						#if 0
						for (i=0; i<s; i++) {
							for (j=0; j<s; j++) {
								L(i, j) = ay(i).get_coef(j+1) / ax(j).get_coef(j+1);
							}
						}
						invert(L, R);
						#endif
						ak = ax - prod(R, ay);
						I1 = to_interval(ak);
						K = intersect(K, I1);
						if (subset(K, *p2)) {
							flag2 = true;
							break;
						}
						if (!overlap(K, *p)) {
							flag2 = false;
							break;
						}
					}
					if (flag2 == true) {
						flag = false;
						break;
					} else {
						/* never reach? */
						std::cout << "two overlap intervals includes different solutions";
					}
				}
				p++;
				p2++;
			}
			if (flag) { // new solution found
				if (verbose >= 1) std::cout << I << "(ex)\n";
				solutions_big.push_back(I);
				// iterative refinement
				while (1) {
					affine<T>::maxnum() = 0;
					ax = K;
					ay = f(ax);
					#if 0
					for (i=0; i<s; i++) {
						for (j=0; j<s; j++) {
							L(i, j) = ay(i).get_coef(j+1) / ax(j).get_coef(j+1);
						}
					}
					r = invert(L, R);
					#endif
					ak = ax - prod(R, ay);
					I1 = to_interval(ak);
					I1 = intersect(K, I1);
					tmp = allsol_affine_sub::widthratio_min(I1, K);
					K = I1;
					if (tmp > 0.9) break;
				}
				solutions.push_back(K);
				count_ex++;
				if (verbose >= 1) std::cout << K << "(ex:improved)\n";
			}
			continue;
		}

		// check the case that solution may exist near boundary.
		// If so, use K as next interval
		if (allsol_affine_sub::widthratio_max(K, I) < 0.9) {
			targets.push_back(K);
			count_unknown++;
			continue;
		}

#if USE_TRIM == 1
		if (!overlap(IR, K)) {
			count_ne++;
			continue;
		} else {
			I = intersect(IR, K);
		}
#else
		I = intersect(I, K);
#endif

		label:

		// divide interval

		mi = allsol_affine_sub::search_maxwidth(I);

		tmp = mid(I(mi));
		if (tmp == I(mi).lower() || tmp == I(mi).upper()) {
			std::cout << "too small interval (may be multiple root?):\n" << I << "\n";
			continue;
		}

		I1 = I; I2 = I;
		I1(mi).assign(I1(mi).lower(), tmp);
		I2(mi).assign(tmp, I2(mi).upper());
		targets.push_back(I1);
		targets.push_back(I2);
		count_unknown += 2;
	}

	if (verbose >= 1) {
			std::cout << "ne_test: " << count_ne_test << ", ex_test: " << count_ex_test << ", ne: " << count_ne << ", ex: " << count_ex << "    \n";
	}

	return solutions;
}

} // namespace kv

#endif // ALLSOL_AFFINE_HPP


// (C) Copyright Seyedsepehr Madani, 2021.
// Distributed under MIT license, available at
// https://opensource.org/licenses/MIT.

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Main

#include <stdexcept>
#include <boost/test/unit_test.hpp>

#include "../src/FibLFSR.hpp"

#define CHEATOFORME(X) std::cout << (X) << std::endl

// Testing a 16-bit LFSR with initial seed "1011011000110110" and tap positions
// 10, 12, 13
BOOST_AUTO_TEST_CASE(binarySeedTest) {
  FibLFSR l1("1011011000110110", {10, 12, 13});
  BOOST_REQUIRE(l1.step() == 0);
  BOOST_REQUIRE(l1.step() == 0);
  BOOST_REQUIRE(l1.step() == 0);
  BOOST_REQUIRE(l1.step() == 1);
  BOOST_REQUIRE(l1.step() == 1);
  BOOST_REQUIRE(l1.step() == 0);
  BOOST_REQUIRE(l1.step() == 0);
  BOOST_REQUIRE(l1.step() == 1);

  FibLFSR l2("1011011000110110", {10, 12, 13});
  BOOST_REQUIRE(l2.generate(9) == 51);
}

// Testing an [64-bit] LFSR with initial keyphrase "5NotoriousBlueberries"
BOOST_AUTO_TEST_CASE(keyphraseTest) {
  FibLFSR l1("5NotoriousBlueberries");

  BOOST_REQUIRE(l1.step() == 0);
  BOOST_REQUIRE(l1.step() == 0);
  BOOST_REQUIRE(l1.step() == 1);
  BOOST_REQUIRE(l1.step() == 0);
  BOOST_REQUIRE(l1.step() == 0);
  BOOST_REQUIRE(l1.step() == 0);
  BOOST_REQUIRE(l1.step() == 0);
  BOOST_REQUIRE(l1.step() == 1);
  BOOST_REQUIRE(l1.step() == 1);

  FibLFSR l2("5NotoriousBlueberries");
  BOOST_REQUIRE(l2.generate(16) == 8613);
}

// test exception-safety of the FibLFSR moduole
BOOST_AUTO_TEST_CASE(exceptionSafety) {
  BOOST_REQUIRE_THROW(FibLFSR l1("", {}), std::invalid_argument);
  BOOST_REQUIRE_THROW(FibLFSR l1("100100110", {5, 10}), std::invalid_argument);
  BOOST_REQUIRE_THROW(FibLFSR l1("31646484796", {2, 5}), std::invalid_argument);
  BOOST_REQUIRE_THROW(FibLFSR l1(""), std::invalid_argument);
}


#pragma once

#ifndef _EIGENMATRIXIO_H_
#define _EIGENMATRIXIO_H_

// Libary includes
#include <Eigen/Dense>
#include <cereal/cereal.hpp>
#include <cereal/access.hpp>
#include <cereal/types/vector.hpp>

namespace cereal
{
	template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
	void save(Archive& archive, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> const & matrix)
	{
		std::vector<std::vector<double>> vector(matrix.rows(), std::vector<double>(matrix.cols()));
		for (int i = 0; i < matrix.rows(); i++)
		{
			for (int j = 0; j < matrix.cols(); j++)
			{
				vector[i][j] = matrix(i, j);
			}
		}
		archive(make_nvp("asVector", vector));
	}

	template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
	void load(Archive& archive, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix)
	{
		std::vector<std::vector<double>> vector;
		archive(make_nvp("asVector", vector));
		if (vector.size() > 0)
		{
			matrix.resize(vector.size(), vector[0].size());
			for (int i = 0; i < matrix.rows(); i++)
			{
				for (int j = 0; j < matrix.cols(); j++)
				{
					matrix(i, j) = vector[i][j];
				}
			}
		}
	}
}

#endif


/*
 * testCSP.cpp
 * @brief develop code for CSP solver
 * @date Feb 5, 2012
 * @author Frank Dellaert
 */

#include <gtsam_unstable/discrete/CSP.h>
#include <gtsam_unstable/discrete/Domain.h>
#include <boost/assign/std/map.hpp>
using boost::assign::insert;
#include <CppUnitLite/TestHarness.h>
#include <iostream>
#include <fstream>

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST_UNSAFE( BinaryAllDif, allInOne)
{
	// Create keys and ordering
	size_t nrColors = 2;
//	DiscreteKey ID("Idaho", nrColors), UT("Utah", nrColors), AZ("Arizona", nrColors);
	DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors);

	// Check construction and conversion
	BinaryAllDiff c1(ID, UT);
	DecisionTreeFactor f1(ID & UT, "0 1 1 0");
	EXPECT(assert_equal(f1,c1.toDecisionTreeFactor()));

	// Check construction and conversion
	BinaryAllDiff c2(UT, AZ);
	DecisionTreeFactor f2(UT & AZ, "0 1 1 0");
	EXPECT(assert_equal(f2,c2.toDecisionTreeFactor()));

	DecisionTreeFactor f3 = f1*f2;
	EXPECT(assert_equal(f3,c1*f2));
	EXPECT(assert_equal(f3,c2*f1));
}

/* ************************************************************************* */
TEST_UNSAFE( CSP, allInOne)
{
	// Create keys and ordering
	size_t nrColors = 2;
	DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors);

	// Create the CSP
	CSP csp;
	csp.addAllDiff(ID,UT);
	csp.addAllDiff(UT,AZ);

	// Check an invalid combination, with ID==UT==AZ all same color
	DiscreteFactor::Values invalid;
	invalid[ID.first] = 0;
	invalid[UT.first] = 0;
	invalid[AZ.first] = 0;
	EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9);

	// Check a valid combination
	DiscreteFactor::Values valid;
	valid[ID.first] = 0;
	valid[UT.first] = 1;
	valid[AZ.first] = 0;
	EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9);

	// Just for fun, create the product and check it
	DecisionTreeFactor product = csp.product();
	// product.dot("product");
	DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0");
	EXPECT(assert_equal(expectedProduct,product));

	// Solve
	CSP::sharedValues mpe = csp.optimalAssignment();
	CSP::Values expected;
	insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1);
	EXPECT(assert_equal(expected,*mpe));
	EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9);
}

/* ************************************************************************* */
TEST_UNSAFE( CSP, WesternUS)
{
	// Create keys
	size_t nrColors = 4;
	DiscreteKey
	// Create ordering according to example in ND-CSP.lyx
	WA(0, nrColors), OR(3, nrColors), CA(1, nrColors),NV(2, nrColors),
	ID(8, nrColors), UT(9, nrColors), AZ(10, nrColors),
	MT(4, nrColors), WY(5, nrColors), CO(7, nrColors), NM(6, nrColors);

	// Create the CSP
	CSP csp;
	csp.addAllDiff(WA,ID);
	csp.addAllDiff(WA,OR);
	csp.addAllDiff(OR,ID);
	csp.addAllDiff(OR,CA);
	csp.addAllDiff(OR,NV);
	csp.addAllDiff(CA,NV);
	csp.addAllDiff(CA,AZ);
	csp.addAllDiff(ID,MT);
	csp.addAllDiff(ID,WY);
	csp.addAllDiff(ID,UT);
	csp.addAllDiff(ID,NV);
	csp.addAllDiff(NV,UT);
	csp.addAllDiff(NV,AZ);
	csp.addAllDiff(UT,WY);
	csp.addAllDiff(UT,CO);
	csp.addAllDiff(UT,NM);
	csp.addAllDiff(UT,AZ);
	csp.addAllDiff(AZ,CO);
	csp.addAllDiff(AZ,NM);
	csp.addAllDiff(MT,WY);
	csp.addAllDiff(WY,CO);
	csp.addAllDiff(CO,NM);

	// Solve
	CSP::sharedValues mpe = csp.optimalAssignment();
	// GTSAM_PRINT(*mpe);
	CSP::Values expected;
	insert(expected)
	(WA.first,1)(CA.first,1)(NV.first,3)(OR.first,0)
	(MT.first,1)(WY.first,0)(NM.first,3)(CO.first,2)
	(ID.first,2)(UT.first,1)(AZ.first,0);
	EXPECT(assert_equal(expected,*mpe));
	EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9);

	// Write out the dual graph for hmetis
#ifdef DUAL
	VariableIndex index(csp);
	index.print("index");
	ofstream os("/Users/dellaert/src/hmetis-1.5-osx-i686/US-West-dual.txt");
  index.outputMetisFormat(os);
#endif
}

/* ************************************************************************* */
TEST_UNSAFE( CSP, AllDiff)
{
	// Create keys and ordering
	size_t nrColors = 3;
	DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors);

	// Create the CSP
	CSP csp;
	vector<DiscreteKey> dkeys;
	dkeys += ID,UT,AZ;
	csp.addAllDiff(dkeys);
	csp.addSingleValue(AZ,2);
//	GTSAM_PRINT(csp);

	// Check construction and conversion
	SingleValue s(AZ,2);
	DecisionTreeFactor f1(AZ,"0 0 1");
	EXPECT(assert_equal(f1,s.toDecisionTreeFactor()));

	// Check construction and conversion
	AllDiff alldiff(dkeys);
	DecisionTreeFactor actual = alldiff.toDecisionTreeFactor();
//	GTSAM_PRINT(actual);
//	actual.dot("actual");
	DecisionTreeFactor f2(ID & AZ & UT,
			"0 0 0  0 0 1  0 1 0   0 0 1  0 0 0  1 0 0   0 1 0  1 0 0  0 0 0");
	EXPECT(assert_equal(f2,actual));

	// Check an invalid combination, with ID==UT==AZ all same color
	DiscreteFactor::Values invalid;
	invalid[ID.first] = 0;
	invalid[UT.first] = 1;
	invalid[AZ.first] = 0;
	EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9);

	// Check a valid combination
	DiscreteFactor::Values valid;
	valid[ID.first] = 0;
	valid[UT.first] = 1;
	valid[AZ.first] = 2;
	EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9);

	// Solve
	CSP::sharedValues mpe = csp.optimalAssignment();
	CSP::Values expected;
	insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2);
	EXPECT(assert_equal(expected,*mpe));
	EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9);

	// Arc-consistency
	vector<Domain> domains;
	domains += Domain(ID), Domain(AZ), Domain(UT);
	SingleValue singleValue(AZ,2);
	EXPECT(singleValue.ensureArcConsistency(1,domains));
	EXPECT(alldiff.ensureArcConsistency(0,domains));
	EXPECT(!alldiff.ensureArcConsistency(1,domains));
	EXPECT(alldiff.ensureArcConsistency(2,domains));
	LONGS_EQUAL(2,domains[0].nrValues());
	LONGS_EQUAL(1,domains[1].nrValues());
	LONGS_EQUAL(2,domains[2].nrValues());

	// Parial application, version 1
	DiscreteFactor::Values known;
	known[AZ.first] = 2;
	DiscreteFactor::shared_ptr reduced1 = alldiff.partiallyApply(known);
	DecisionTreeFactor f3(ID & UT, "0 1 1  1 0 1  1 1 0");
	EXPECT(assert_equal(f3,reduced1->toDecisionTreeFactor()));
	DiscreteFactor::shared_ptr reduced2 = singleValue.partiallyApply(known);
	DecisionTreeFactor f4(AZ, "0 0 1");
	EXPECT(assert_equal(f4,reduced2->toDecisionTreeFactor()));

	// Parial application, version 2
	DiscreteFactor::shared_ptr reduced3 = alldiff.partiallyApply(domains);
	EXPECT(assert_equal(f3,reduced3->toDecisionTreeFactor()));
	DiscreteFactor::shared_ptr reduced4 = singleValue.partiallyApply(domains);
	EXPECT(assert_equal(f4,reduced4->toDecisionTreeFactor()));

	// full arc-consistency test
	csp.runArcConsistency(nrColors);
}

/* ************************************************************************* */
int main() {
	TestResult tr;
	return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/minexponent.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check minexponent behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::minexponent;
  using boost::simd::Minexponent;

  STF_TYPE_IS(decltype(Minexponent<T>()), T);
  STF_EQUAL(Minexponent<T>(), T(0));
  STF_EQUAL(minexponent( as(T{}) ),T(0));
}

STF_CASE_TPL( "Check minexponent behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::minexponent;
  using boost::simd::Minexponent;

  STF_TYPE_IS(decltype(Minexponent<T>()), int64_t);
  STF_IEEE_EQUAL(Minexponent<T>(), -1022);
  STF_IEEE_EQUAL(minexponent( as(T{}) ), -1022);
}

STF_CASE_TPL( "Check minexponent behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::minexponent;
  using boost::simd::Minexponent;

  STF_TYPE_IS(decltype(Minexponent<T>()), int32_t);
  STF_IEEE_EQUAL(Minexponent<T>(), -126);
  STF_IEEE_EQUAL(minexponent( as(T{}) ), -126);
}


#include <fiberchain/protocol/asset.hpp>
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>

/*

The bounds on asset serialization are as follows:

index : field
0     : decimals
1..6  : symbol
   7  : \0
*/

namespace fiberchain { namespace protocol {
      typedef boost::multiprecision::int128_t  int128_t;

      uint8_t asset::decimals()const
      {
         auto a = (const char*)&symbol;
         uint8_t result = uint8_t( a[0] );
         FC_ASSERT( result < 15 );
         return result;
      }

      void asset::set_decimals(uint8_t d)
      {
         FC_ASSERT( d < 15 );
         auto a = (char*)&symbol;
         a[0] = d;
      }

      void asset::change_fraction_size(uint8_t size)
      {
         share_type old_precision = precision();
         set_decimals(size);
         share_type new_precision = precision();

         if(new_precision > old_precision) {
            share_type multiply_rate = new_precision / old_precision;
            amount *= multiply_rate;
         } else {
            share_type divide_rate = old_precision / new_precision;
            amount /= divide_rate;
         }
      }

      std::string asset::symbol_name()const
      {
         auto a = (const char*)&symbol;
         FC_ASSERT( a[7] == 0 );
         return &a[1];
      }

      int64_t asset::precision()const
      {
         static int64_t table[] = {
                           1, 10, 100, 1000, 10000,
                           100000, 1000000, 10000000, 100000000ll,
                           1000000000ll, 10000000000ll,
                           100000000000ll, 1000000000000ll,
                           10000000000000ll, 100000000000000ll
                         };
         uint8_t d = decimals();
         return table[ d ];
      }

      string asset::to_string()const
      {
         int64_t prec = precision();
         string result = fc::to_string(amount.value / prec);
         if( prec > 1 )
         {
            auto fract = amount.value % prec;
            // prec is a power of ten, so for example when working with
            // 7.005 we have fract = 5, prec = 1000.  So prec+fract=1005
            // has the correct number of zeros and we can simply trim the
            // leading 1.
            result += "." + fc::to_string(prec + fract).erase(0,1);
         }
         return result + " " + symbol_name();
      }

      asset asset::from_string( const string& from )
      {
         try
         {
            string s = fc::trim( from );
            auto space_pos = s.find( " " );
            auto dot_pos = s.find( "." );

            FC_ASSERT( space_pos != std::string::npos );

            asset result;
            result.symbol = uint64_t(0);
            auto sy = (char*)&result.symbol;

            if( dot_pos != std::string::npos )
            {
               FC_ASSERT( space_pos > dot_pos );

               auto intpart = s.substr( 0, dot_pos );
               auto fractpart = "1" + s.substr( dot_pos + 1, space_pos - dot_pos - 1 );
               result.set_decimals( fractpart.size() - 1 );

               result.amount = fc::to_int64( intpart );
               result.amount.value *= result.precision();
               result.amount.value += fc::to_int64( fractpart );
               result.amount.value -= result.precision();
            }
            else
            {
               auto intpart = s.substr( 0, space_pos );
               result.amount = fc::to_int64( intpart );
               result.set_decimals( 0 );
            }
            auto symbol = s.substr( space_pos + 1 );
            size_t symbol_size = symbol.size();

            if( symbol_size > 0 )
            {
               FC_ASSERT( symbol_size <= 6 );
               memcpy( sy+1, symbol.c_str(), symbol_size );
            }

            return result;
         }
         FC_CAPTURE_AND_RETHROW( (from) )
      }

      bool operator == ( const price& a, const price& b )
      {
         if( std::tie( a.base.symbol, a.quote.symbol ) != std::tie( b.base.symbol, b.quote.symbol ) )
             return false;

         const auto amult = uint128_t( b.quote.amount.value ) * a.base.amount.value;
         const auto bmult = uint128_t( a.quote.amount.value ) * b.base.amount.value;

         return amult == bmult;
      }

      bool operator < ( const price& a, const price& b )
      {
         if( a.base.symbol < b.base.symbol ) return true;
         if( a.base.symbol > b.base.symbol ) return false;
         if( a.quote.symbol < b.quote.symbol ) return true;
         if( a.quote.symbol > b.quote.symbol ) return false;

         const auto amult = uint128_t( b.quote.amount.value ) * a.base.amount.value;
         const auto bmult = uint128_t( a.quote.amount.value ) * b.base.amount.value;

         return amult < bmult;
      }

      bool operator <= ( const price& a, const price& b )
      {
         return (a == b) || (a < b);
      }

      bool operator != ( const price& a, const price& b )
      {
         return !(a == b);
      }

      bool operator > ( const price& a, const price& b )
      {
         return !(a <= b);
      }

      bool operator >= ( const price& a, const price& b )
      {
         return !(a < b);
      }

      asset operator * ( const asset& a, const price& b )
      {
         if( a.symbol_name() == b.base.symbol_name() )
         {
            FC_ASSERT( b.base.amount.value > 0 );
            uint128_t result = (uint128_t(a.amount.value) * b.quote.amount.value)/b.base.amount.value;
            FC_ASSERT( result.hi == 0 );
            return asset( result.to_uint64(), b.quote.symbol );
         }
         else if( a.symbol_name() == b.quote.symbol_name() )
         {
            FC_ASSERT( b.quote.amount.value > 0 );
            uint128_t result = (uint128_t(a.amount.value) * b.base.amount.value)/b.quote.amount.value;
            FC_ASSERT( result.hi == 0 );
            return asset( result.to_uint64(), b.base.symbol );
         }
         FC_THROW_EXCEPTION( fc::assert_exception, "invalid asset * price", ("asset",a)("price",b) );
      }

      price operator / ( const asset& base, const asset& quote )
      { try {
         FC_ASSERT( base.symbol_name() != quote.symbol_name() );
         return price{ base, quote };
      } FC_CAPTURE_AND_RETHROW( (base)(quote) ) }

      price price::max( asset_symbol_type base, asset_symbol_type quote ) { return asset( share_type(FIBERCHAIN_MAX_SHARE_SUPPLY), base ) / asset( share_type(1), quote); }
      price price::min( asset_symbol_type base, asset_symbol_type quote ) { return asset( 1, base ) / asset( FIBERCHAIN_MAX_SHARE_SUPPLY, quote); }

      bool price::is_null() const { return *this == price(); }

      void price::validate() const
      { try {
         FC_ASSERT( base.amount > share_type(0) );
         FC_ASSERT( quote.amount > share_type(0) );
         FC_ASSERT( base.symbol_name() != quote.symbol_name() );
      } FC_CAPTURE_AND_RETHROW( (base)(quote) ) }

} } // fiberchain::protocol


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MODULES_GEOMETRY_LINE_HPP_
#define MODULES_GEOMETRY_LINE_HPP_

#include <vector>
#include <cmath>
#include <limits>

#include <boost/tuple/tuple.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>

#include "modules/geometry/commons.hpp"
#include "modules/geometry/angle.hpp"

namespace modules {
namespace geometry {

//! templated line class with a boost polygon as a member function
template <typename T>
class Line_t : public Shape<bg::model::linestring<T>, T> {
 public:
  Line_t() : Shape<bg::model::linestring<T>, T>(Pose(0, 0, 0), std::vector<T>(), 0) {}

  virtual Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> toArray() const;

  virtual Shape<bg::model::linestring<T>, T> *Clone() const;

  //! @todo improvement: do not recompute full s but only add one point (but we would need to store the line length for that!)
  bool add_point(const T &p) {
    return Shape<bg::model::linestring<T>, T>::add_point(p) && recompute_s();
  }

  auto length() { return bg::length(Shape<bg::model::linestring<T>, T>::obj_);}

  int size() { return Shape<bg::model::linestring<T>, T>::obj_.size(); }

  void append_linestring(const Line_t &ls) {
    // TODO(@fortiss): wrong
    bg::append(Shape<bg::model::linestring<T>, T>::obj_, ls.obj_);
    recompute_s();
  }

  void ConcatenateLinestring(const Line_t &other_line) {
    using boost::geometry::append;
    // Get first and last point
    auto last_point_this = Shape<bg::model::linestring<T>, T>::obj_[Shape<bg::model::linestring<T>, T>::obj_.size() - 1];
    auto first_point_other = other_line.obj_[0];
    auto last_point_other = other_line.obj_[other_line.obj_.size() - 1];
    float distance_last_last = distance(last_point_this, last_point_other);
    float distance_last_first = distance(last_point_this, first_point_other);

    //std::cout << distance_last_last << " - " << distance_last_first << std::endl;
    if (distance_last_last> distance_last_first) {
      append_linestring(other_line);
    } else {
      //std::cout << "REVERSING" << std::endl;
      Line_t new_line = other_line;
      boost::geometry::reverse(new_line.obj_);
      append_linestring(new_line);
    }

  }

  typedef typename std::vector<T>::iterator point_iterator;
  typedef typename std::vector<T>::const_iterator const_point_iterator;
  point_iterator begin() { return Shape<bg::model::linestring<T>, T>::obj_.begin(); }
  const_point_iterator  begin() const { Shape<bg::model::linestring<T>, T>::obj_.begin(); }
  point_iterator end() { return Shape<bg::model::linestring<T>, T>::obj_.end(); }
  const_point_iterator end() const { return Shape<bg::model::linestring<T>, T>::obj_.end(); }

  std::vector<float> s_;  //! local coordinates 0..[total distance] along the lines

  //! @todo free function, s_ private?
  bool recompute_s() {
    s_.clear();
    // edge case no points
    if (Shape<bg::model::linestring<T>, T>::obj_.empty()) {
      return true;

    } else if (Shape<bg::model::linestring<T>, T>::obj_.size() == 1) {  // edge case one point
      s_.push_back(0.0);
      return true;
    } else {  // nominal case
      // compute distance from last point to next point and store these on vector s_ (avoid additional tmp vector)
      T last_pt = Shape<bg::model::linestring<T>, T>::obj_.front();
      s_.reserve(Shape<bg::model::linestring<T>, T>::obj_.size());
      double distance_until_now = 0.0;
      for (const T &next_pt : Shape<bg::model::linestring<T>, T>::obj_) {
        distance_until_now += bg::distance(next_pt, last_pt);
        s_.push_back(distance_until_now);
        last_pt = next_pt;
      }
      // // divide by total distance --> s \in 0..1
      // const double &last_s = s_.back();
      // for (int i = 0; i < s_.size(); ++i) {  // normalized 0..1
      //   s_[i] = s_[i] / last_s;
      // }
      return true;
    }
  }
};

//! for better usage simple float defines
using LinePoint = Point2d;
using Line = Line_t<LinePoint>;

template <>
inline Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> Line::toArray() const {
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> mat(obj_.size(), 2);
  for (uint32_t i = 0; i < obj_.size(); i++) {
    mat.row(i) << bg::get<0>(obj_[i]), bg::get<1>(obj_[i]);
  }
  return mat;
}

template <typename T>
inline Shape<bg::model::linestring<T>, T> *Line_t<T>::Clone() const {
  return new Line_t<T>(*this);
}

inline float distance(const Line &line, const Point2d &p) {
  return bg::distance(line.obj_, p);
}

inline float distance(const Line &line, const Line &line2) {
  return bg::distance(line.obj_, line2.obj_);
}

template <typename T>
inline T length(const Line &line) {
  return bg::length<T>(line.obj_);
}

inline int get_segment_end_idx(Line l, float s) {
  std::vector<float>::iterator up = std::upper_bound(l.s_.begin(), l.s_.end(), s);
  int retval = up - l.s_.begin();
  return retval;
}

inline bool check_s_for_segment_intersection(Line l, float s) {
  int start_it = get_segment_end_idx(l, s);
  std::vector<float>::iterator low = std::lower_bound(l.s_.begin(), l.s_.end(), s);
  int start_it_low = low - l.s_.begin();
  return start_it != start_it_low;
}

inline Point2d get_point_at_s(Line l, float s) {
  const size_t &length = l.obj_.size();
  if (length <= 1) {  // this is an error Line consist of 0 or 1 element
    return Point2d(0, 0);
  } else if (s <= 0.0) {  // edge case begin
    return l.obj_.at(0);
  } else if (s >= l.s_.back()) {  // edge case end
    return l.obj_.at(length - 1);
  } else {  // nominal case
    int segment_end_idx = get_segment_end_idx(l, s);
    int segment_begin_idx = segment_end_idx - 1;

    float s_on_segment = (s - l.s_.at(segment_begin_idx)) / (l.s_.at(segment_end_idx) - l.s_.at(segment_begin_idx));
    float interp_pt_x = bg::get<0>(l.obj_.at(segment_begin_idx))
                        + s_on_segment * (bg::get<0>(l.obj_.at(segment_end_idx))
                        - bg::get<0>(l.obj_.at(segment_begin_idx)));
    float interp_pt_y = bg::get<1>(l.obj_.at(segment_begin_idx))
                        + s_on_segment * (bg::get<1>(l.obj_.at(segment_end_idx))
                        - bg::get<1>(l.obj_.at(segment_begin_idx)));
    return Point2d(interp_pt_x, interp_pt_y);
  }
}

inline float get_tangent_angle_at_s(Line l, float s) {
  if (s >= l.s_.back()) {
    Point2d p1 = l.obj_.at(l.obj_.size()-2);
    Point2d p2 = l.obj_.at(l.obj_.size()-1);
    return atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1));
  } else if (s <= 0.0) {
    Point2d p1 = l.obj_.at(0);
    Point2d p2 = l.obj_.at(1);
    return atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1));
  } else {  // not start or end
    int end_segment_it = get_segment_end_idx(l, s);
    if (check_s_for_segment_intersection(l, s)) {  // check if s is at intersection, if true then calculate the intermediate angle
    Point2d p1 = l.obj_.at(end_segment_it-2);
    Point2d p2 = l.obj_.at(end_segment_it-1);
    Point2d p3 = l.obj_.at(end_segment_it);
    float sin_mean = 0.5 * (sin(atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1)))
                      + sin(atan2(bg::get<1>(p3) - bg::get<1>(p2), bg::get<0>(p3) - bg::get<0>(p2))));
    float cos_mean = 0.5 * (cos(atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1)))
                      + cos(atan2(bg::get<1>(p3) - bg::get<1>(p2), bg::get<0>(p3) - bg::get<0>(p2))));
    return atan2(sin_mean, cos_mean);
    } else {  // every s not start, end or intersection
      Point2d p1 = l.obj_.at(end_segment_it-1);
      Point2d p2 = l.obj_.at(end_segment_it);
      return atan2(bg::get<1>(p2) - bg::get<1>(p1), bg::get<0>(p2) - bg::get<0>(p1));
    }
  }
}

inline Point2d get_normal_at_s(Line l, float s) {
  float tangent = get_tangent_angle_at_s(l, s);
  Point2d t(cos(tangent+asin(1)), sin(tangent+asin(1)));  // rotate unit vector anti-clockwise with angle = tangent by 1/2 pi
  return t;
}

inline boost::tuple<Point2d, double, uint> get_nearest_point_and_s(Line l, const Point2d &p) {  // get_nearest_point
  // edge cases: empty or one-point line
  if (l.obj_.empty()) {
    return boost::make_tuple(Point2d(0, 0), 0.0, 0);
  } else if (l.obj_.size() == 1) {
    return boost::make_tuple(l.obj_.at(0), 0.0, 0);
  }

  // nominal case:
  // check distance to each line segment and find closest segment
  double min_dist = boost::numeric::bounds<float>::highest();
  int min_segment_idx = 0;
  for (uint line_idx = 0; line_idx < l.obj_.size() - 1; ++line_idx) {
    bg::model::linestring<Point2d> current_segment;
    bg::append(current_segment, l.obj_.at(line_idx));
    bg::append(current_segment, l.obj_.at(line_idx + 1));
    double d = bg::comparable_distance(current_segment, p);
    if (d < min_dist) {
      min_dist = d;
      min_segment_idx = line_idx;
    }
  }

  // compute closest point on segment and interpolation factor lambda
  // point: p, line a--b, closest point on line: s, tangent factor: lambda, normal factor: delta
  // equation by matlab:
  // syms a1 a2 b1 b2 s1 s2 p1 p2 lambda delta
  // gl3x = (a1 + lambda*(b1 - a1) == s1);
  // gl3y = (a2 + lambda*(b2 - a2) == s2);
  // gl4x = (s1 + delta*(-b2 + a2) == p1);
  // gl4y = (s2 + delta*(b1 - a1) == p2);
  // sol = solve([gl3x, gl3y, gl4x, gl4y], [s1, s2, lambda, delta], 'ReturnConditions', true);
  const double a1 = bg::get<0>(l.obj_.at(min_segment_idx));
  const double a2 = bg::get<1>(l.obj_.at(min_segment_idx));
  const double b1 = bg::get<0>(l.obj_.at(min_segment_idx + 1));
  const double b2 = bg::get<1>(l.obj_.at(min_segment_idx + 1));
  const double p1 = bg::get<0>(p);
  const double p2 = bg::get<1>(p);

  const double lambda = -(a1 * b1 + a2 * b2 + a1 * p1 + a2 * p2 - b1 * p1 - b2 * p2 - a1 * a1 - a2 * a2)
                        / (a1 * a1 - 2 * a1 * b1 + a2 * a2 - 2 * a2 * b2 + b1 * b1 + b2 * b2);

  // calculate interpolated s value
  double s;
  // double dist;  // unused
  Point2d retval;

  if (lambda < 0) {  // extrapolation front
    s = l.s_.at(min_segment_idx);
    retval = Point2d(a1, a2);
    // debug
    // dist = sqrt(pow(p1 - a1, 2) + pow(p2 - a2, 2));
  } else if (lambda > 1) {  // extrapolation end
    s = l.s_.at(min_segment_idx + 1);
    retval = Point2d(b1, b2);
    // debug
    // dist = sqrt(pow(p1 - b1, 2) + pow(p2 - b2, 2));
  } else {  // real interpolation
    // std::cout << "size s_: " << l.s_.size() << "object size: " << l.obj_.size() << std::endl;
    s = (1 - lambda) * l.s_.at(min_segment_idx) + lambda * l.s_.at(min_segment_idx + 1);

    const double s1 = (p1 * a1 * a1 - a1 * a2 * b2 + p2 * a1 * a2
                    - 2 * p1 * a1 * b1 + a1 * b2 * b2 - p2 * a1 * b2
                    + a2 * a2 * b1 - a2 * b1 * b2 - p2 * a2 * b1 + p1 * b1 * b1 + p2 * b1 * b2)
                    / (a1 * a1 - 2 * a1 * b1 + a2 * a2 - 2 * a2 * b2 + b1 * b1 + b2 * b2);
    const double s2 = (a1 * a1 * b2 - a1 * a2 * b1 + p1 * a1 * a2
                    - a1 * b1 * b2 - p1 * a1 * b2 + p2 * a2 * a2
                    + a2 * b1 * b1 - p1 * a2 * b1 - 2 * p2 * a2 * b2 + p1 * b1 * b2 + p2 * b2 * b2)
                    / (a1 * a1 - 2 * a1 * b1 + a2 * a2 - 2 * a2 * b2 + b1 * b1 + b2 * b2);

    // debug
    // dist = sqrt(pow(p1 - s1, 2) + pow(p2 - s2, 2));
    retval = Point2d(s1, s2);
  }

  // debug
  // const double dist_boost = bg::distance(l.obj_, p);

  // return
  return boost::make_tuple(retval, s, min_segment_idx);
}
inline Point2d get_nearest_point(Line l, const Point2d &p) {
  return boost::get<0>(get_nearest_point_and_s(l, p));
}
inline float get_nearest_s(Line l, const Point2d &p) {
  return boost::get<1>(get_nearest_point_and_s(l, p));
}
inline uint FindNearestIdx(Line l, const Point2d &p) {
  return boost::get<2>(get_nearest_point_and_s(l, p));
}
//! Point - Line collision checker using boost::intersection
inline bool Collide(const Line &l, const LinePoint &p) {
  std::vector<LinePoint> shape_intersect;
  bg::intersection(l.obj_, p, shape_intersect);
  return !shape_intersect.empty();
}

//! Line - Point collision checker
inline bool Collide(const LinePoint &p, const Line &l) {
  return Collide(l, p);
}

//! Line - Line collision checker using boost::intersection
inline bool Collide(const Line &l1, const Line &l2) {
  std::vector<bg::model::linestring<LinePoint>> shape_intersect;
  bg::intersection(l1.obj_, l2.obj_, shape_intersect);
  return !shape_intersect.empty();
}

// An oriented point can have a linestring (the nearest point on it) on the left or right side, left side < 0, right side > 0
inline double signed_distance(const Line &line, const Point2d &p, const float& orientation) {
  auto closest_point = get_nearest_point(line, p);
  auto direction_vector = closest_point - p;

  double diff = signed_angle_diff(orientation , atan2(bg::get<1>(direction_vector), bg::get<0>(direction_vector)));
  double sign = (diff > 0) ? 1 : ((diff < 0) ? -1 : 0);

  return bg::distance(line.obj_, p)*sign;
}

inline Line ComputeCenterLine(const Line& outer_line_,
                                  const Line& inner_line_) {
  Line center_line_;
  Line line_more_points = outer_line_;
  Line line_less_points = inner_line_;
  if ( inner_line_.obj_.size() > outer_line_.obj_.size() ) {
    line_more_points = inner_line_;
    line_less_points = outer_line_;
  }
  for ( Point2d& point_loop : line_more_points.obj_ ) {
    Point2d nearest_point_other = geometry::get_nearest_point(line_less_points,
                                                              point_loop);
    geometry::Point2d middle_point = (point_loop + nearest_point_other) / 2;
    center_line_.add_point(middle_point);
  }
  return center_line_;
}

}  // namespace geometry
}  // namespace modules
#endif  // MODULES_GEOMETRY_LINE_HPP_


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

/*! \file mcgaussian1dnonstandardswaptionengine.hpp
    \brief Monte Carlo engine for bermudan (non standard) swaptions
           in a gaussian 1d model
*/

#ifndef quantlib_mc_gaussian1d_nonstandardswaption_engine_hpp
#define quantlib_mc_gaussian1d_nonstandardswaption_engine_hpp

#include <ql/pricingengines/genericmodelengine.hpp>
#include <ql/pricingengines/mclongstaffschwartzengine.hpp>
#include <ql/experimental/models/longstaffschwartzproxypathpricer.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/instruments/nonstandardswaption.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

/*! All fixed coupons with start date greater or equal to the
    respective option expiry are considered to be part of the
    exercise into right.

    \warning Cash settled swaptions are not supported
*/

template <class RNG = PseudoRandom, class S = Statistics>
class McGaussian1dNonstandardSwaptionEngine
    : public MCLongstaffSchwartzEngine<
          GenericModelEngine<Gaussian1dModel, NonstandardSwaption::arguments,
                             NonstandardSwaption::results>,
          SingleVariate, RNG, S> {

  public:
    typedef MCLongstaffSchwartzEngine<
        GenericModelEngine<Gaussian1dModel, NonstandardSwaption::arguments,
                           NonstandardSwaption::results>,
        SingleVariate, RNG, S> base_class;

    /*! proxy function */
    struct LsFunction : NonstandardSwaption::Proxy::ProxyFunction {
        Real operator()(const Real state) const {
            Real tmp = 0.0;
            if (state > cutoff) {
                for (Size i = 0; i < coeffItm.size(); ++i)
                    tmp += coeffItm[i] * v[i](state);
            } else {
                for (Size i = 0; i < coeffOtm.size(); ++i)
                    tmp += coeffOtm[i] * v[i](state);
            }
            return std::max(tmp, 0.0); // continuation value is always positive
        }
        Array coeffItm, coeffOtm;
        Real cutoff;
        std::vector<boost::function1<Real, Real> > v;
    };

    McGaussian1dNonstandardSwaptionEngine(
        const boost::shared_ptr<Gaussian1dModel> &model,
        const Handle<Quote>
            &oas, // continuously compounded w.r.t. yts daycounter
        const Handle<YieldTermStructure> &discountCurve,
        Size timeSteps, Size timeStepsPerYear, bool brownianBridge,
        bool antitheticVariate, Size requiredSamples, Real requiredTolerance,
        Size maxSamples, BigNatural seed, Size nCalibrationSamples,
        bool generateProxy);

    void calculate() const;
    void reset() const;

  protected:
    boost::shared_ptr<LongstaffSchwartzPathPricer<Path> > lsmPathPricer() const;

  private:
    boost::shared_ptr<Gaussian1dModel> model_;
    Handle<Quote> oas_;
    Handle<YieldTermStructure> discountCurve_;
    bool generateProxy_;
    mutable boost::shared_ptr<NonstandardSwaption::Proxy> proxy_;
};

//! factory

template <class RNG = PseudoRandom, class S = Statistics>
class MakeMcGaussian1dNonstandardSwaptionEngine {
  public:
    MakeMcGaussian1dNonstandardSwaptionEngine(
        const boost::shared_ptr<Gaussian1dModel> &model);
    MakeMcGaussian1dNonstandardSwaptionEngine &withSteps(Size steps);
    MakeMcGaussian1dNonstandardSwaptionEngine &withStepsPerYear(Size steps);
    MakeMcGaussian1dNonstandardSwaptionEngine &
    withBrownianBridge(bool b = true);
    MakeMcGaussian1dNonstandardSwaptionEngine &
    withAntitheticVariate(bool b = true);
    MakeMcGaussian1dNonstandardSwaptionEngine &withSamples(Size samples);
    MakeMcGaussian1dNonstandardSwaptionEngine &
    withAbsoluteTolerance(Real tolerance);
    MakeMcGaussian1dNonstandardSwaptionEngine &withMaxSamples(Size samples);
    MakeMcGaussian1dNonstandardSwaptionEngine &withSeed(BigNatural seed);
    MakeMcGaussian1dNonstandardSwaptionEngine &
    withCalibrationSamples(Size calibrationSamples);
    MakeMcGaussian1dNonstandardSwaptionEngine &
    withOas(const Handle<Quote> &oas);
    MakeMcGaussian1dNonstandardSwaptionEngine &
    withDiscount(const Handle<YieldTermStructure> &discount);
    MakeMcGaussian1dNonstandardSwaptionEngine &withProxy(bool b = true);
    // conversion to pricing engine
    operator boost::shared_ptr<PricingEngine>() const;

  private:
    boost::shared_ptr<Gaussian1dModel> model_;
    Handle<Quote> oas_;
    Handle<YieldTermStructure> discount_;
    bool brownianBridge_, antithetic_;
    Size steps_, stepsPerYear_, samples_, calibrationSamples_, maxSamples_;
    Real tolerance_;
    BigNatural seed_;
    bool generateProxy_;
};

//! Path Pricer

class Gaussian1dNonstandardSwaptionPathPricer
    : public EarlyExercisePathPricer<Path> {
  public:
    Gaussian1dNonstandardSwaptionPathPricer(
        const boost::shared_ptr<Gaussian1dModel> &model,
        const NonstandardSwaption::arguments *arguments,
        const Handle<YieldTermStructure> &discount, const Handle<Quote> &oas);
    Real operator()(const Path &path, Size t) const;
    Real state(const Path &path, Size t) const;
    std::vector<boost::function1<Real, Real> > basisSystem() const;
    std::vector<boost::function1<Real, Real> > basisSystem2() const;

  private:
    void initExerciseIndices(const Path &path) const;
    const boost::shared_ptr<Gaussian1dModel> model_;
    const NonstandardSwaption::arguments *arguments_;
    const Handle<YieldTermStructure> discount_;
    const Handle<Quote> oas_;
    std::vector<boost::function1<Real, Real> > basis_, basis2_;
    mutable std::vector<Size> exerciseIdx_;
    Size minIdxAlive_;
};

// implementation

template <class RNG, class S>
McGaussian1dNonstandardSwaptionEngine<RNG, S>::
    McGaussian1dNonstandardSwaptionEngine(
        const boost::shared_ptr<Gaussian1dModel> &model,
        const Handle<Quote>
            &oas, // continuously compounded w.r.t. yts daycounter
        const Handle<YieldTermStructure> &discountCurve,
        Size timeSteps, Size timeStepsPerYear, bool brownianBridge,
        bool antitheticVariate, Size requiredSamples, Real requiredTolerance,
        Size maxSamples, BigNatural seed, Size nCalibrationSamples,
        bool generateProxy)
    : MCLongstaffSchwartzEngine<
          GenericModelEngine<Gaussian1dModel, NonstandardSwaption::arguments,
                             NonstandardSwaption::results>,
          SingleVariate, RNG, S>(
          model->stateProcess(), timeSteps, timeStepsPerYear, brownianBridge,
          antitheticVariate, false, requiredSamples, requiredTolerance,
          maxSamples, seed, nCalibrationSamples),
      model_(model), oas_(oas), discountCurve_(discountCurve),
      generateProxy_(generateProxy) {

    if (!oas_.empty())
        this->registerWith(oas_);
    if (!discountCurve_.empty())
        this->registerWith(discountCurve_);
}

template <class RNG, class S>
void McGaussian1dNonstandardSwaptionEngine<RNG, S>::calculate() const {
// a lazy object is not thread safe, neither is the caching
// in gsrprocess. therefore we trigger computations here such
// that neither lazy object recalculation nor write access
// during caching occurs in the parallized loop in MonteCarloModel
#ifdef _OPENMP
    boost::shared_ptr<LongstaffSchwartzPathPricer<Path> > tmp =
        this->lsmPathPricer();
    tmp->calibrate();
    model_->numeraire(1.0);
    tmp->operator()(this->pathGenerator()->next().value);
#endif
    // continue with usual calculations
    base_class::calculate();
    // transform the deflated values into plain ones
    this->results_.value *= model_->numeraire(0.0, 0.0, discountCurve_);
    this->results_.errorEstimate *= model_->numeraire(0.0, 0.0, discountCurve_);
    if (generateProxy_) {
        // generate proxy
        this->proxy_ = boost::make_shared<NonstandardSwaption::Proxy>();
        Date today = Settings::instance().evaluationDate();
        // original evaluation date
        // open expiry dates
        std::vector<Date>::const_iterator start =
            std::upper_bound(this->arguments_.exercise->dates().begin(),
                             this->arguments_.exercise->dates().end(), today);
        for (std::vector<Date>::const_iterator i = start;
             i != this->arguments_.exercise->dates().end(); ++i) {
            this->proxy_->expiryDates.push_back(*i);
        }
        // model
        this->proxy_->model = model_;
        // discount curve (may be empty)
        this->proxy_->discount = discountCurve_;
        // oas (may be empty)
        this->proxy_->oas = oas_;
        // regression functions for values
        for (int i = 0; i < static_cast<int>(this->proxy_->expiryDates.size());
             ++i) {
            boost::shared_ptr<LsFunction> lsTmp =
                boost::make_shared<LsFunction>();
            boost::shared_ptr<LongstaffSchwartzProxyPathPricer> pathPricer =
                boost::dynamic_pointer_cast<LongstaffSchwartzProxyPathPricer>(
                    this->pathPricer_);
            lsTmp->coeffItm = pathPricer->coefficientsItm()[i];
            lsTmp->coeffOtm = pathPricer->coefficientsOtm()[i];
            lsTmp->cutoff = pathPricer->cutoff();
            lsTmp->v = pathPricer->basisSystem();
            this->proxy_->regression.push_back(lsTmp);
        }
        this->results_.proxy = this->proxy_;
    }
}

template <class RNG, class S>
void McGaussian1dNonstandardSwaptionEngine<RNG, S>::reset() const {
    base_class::reset();
    this->proxy_ = boost::shared_ptr<NonstandardSwaption::Proxy>();
}

template <class RNG, class S>
boost::shared_ptr<LongstaffSchwartzPathPricer<Path> >
McGaussian1dNonstandardSwaptionEngine<RNG, S>::lsmPathPricer() const {

    // reduced grid which only contains the exercise times and 0
    std::vector<Real> exerciseTimes;
    exerciseTimes.push_back(0.0);
    for (Size i = 0; i < this->arguments_.exercise->dates().size(); ++i) {
        Time tmp =
            model_->stateProcess()->time(this->arguments_.exercise->date(i));
        if (tmp > 0.0) {
            exerciseTimes.push_back(tmp);
        }
    }
    TimeGrid exerciseGrid =
        TimeGrid(exerciseTimes.begin(), exerciseTimes.end());
    boost::shared_ptr<Gaussian1dNonstandardSwaptionPathPricer>
        earlyExercisePricer =
            boost::make_shared<Gaussian1dNonstandardSwaptionPathPricer>(
                model_, &this->arguments_, discountCurve_, oas_);
    // we work with deflated npvs produced in the early exercise pricer
    // so we pass a dummyCurve, which produces discount factors of 1.0 always
    boost::shared_ptr<YieldTermStructure> dummyCurve =
        boost::make_shared<FlatForward>(0, NullCalendar(), 0.0,
                                        Actual365Fixed());
    if (generateProxy_) {
        return boost::make_shared<LongstaffSchwartzProxyPathPricer>(
            exerciseGrid, earlyExercisePricer, dummyCurve);
    } else {
        return boost::make_shared<LongstaffSchwartzPathPricer<Path> >(
            exerciseGrid, earlyExercisePricer, dummyCurve);
    }
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::
    MakeMcGaussian1dNonstandardSwaptionEngine(
        const boost::shared_ptr<Gaussian1dModel> &model)
    : model_(model), oas_(Handle<Quote>()),
      discount_(Handle<YieldTermStructure>()), brownianBridge_(false),
      antithetic_(false), steps_(Null<Size>()), stepsPerYear_(Null<Size>()),
      samples_(Null<Size>()), calibrationSamples_(Null<Size>()),
      maxSamples_(Null<Size>()), tolerance_(Null<Real>()), seed_(0),
      generateProxy_(false) {}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withSteps(Size steps) {
    steps_ = steps;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withStepsPerYear(
    Size steps) {
    stepsPerYear_ = steps;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withBrownianBridge(
    bool brownianBridge) {
    brownianBridge_ = brownianBridge;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withAntitheticVariate(
    bool b) {
    antithetic_ = b;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withSamples(Size samples) {
    QL_REQUIRE(tolerance_ == Null<Real>(), "tolerance already set");
    samples_ = samples;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withCalibrationSamples(
    Size samples) {
    calibrationSamples_ = samples;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withAbsoluteTolerance(
    Real tolerance) {
    QL_REQUIRE(samples_ == Null<Size>(), "number of samples already set");
    QL_REQUIRE(RNG::allowsErrorEstimate, "chosen random generator policy "
                                         "does not allow an error estimate");
    tolerance_ = tolerance;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withMaxSamples(
    Size samples) {
    maxSamples_ = samples;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withSeed(BigNatural seed) {
    seed_ = seed;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withOas(
    const Handle<Quote> &oas) {
    oas_ = oas;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withDiscount(
    const Handle<YieldTermStructure> &discount) {
    discount_ = discount;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S> &
MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::withProxy(bool b) {
    generateProxy_ = b;
    return *this;
}

template <class RNG, class S>
inline MakeMcGaussian1dNonstandardSwaptionEngine<RNG, S>::
operator boost::shared_ptr<PricingEngine>() const {
    QL_REQUIRE(steps_ != Null<Size>() || stepsPerYear_ != Null<Size>(),
               "number of steps not given");
    QL_REQUIRE(steps_ == Null<Size>() || stepsPerYear_ == Null<Size>(),
               "number of steps overspecified");
    return boost::shared_ptr<PricingEngine>(
        new McGaussian1dNonstandardSwaptionEngine<RNG, S>(
            model_, oas_, discount_, steps_, stepsPerYear_, brownianBridge_,
            antithetic_, samples_, tolerance_, maxSamples_, seed_,
            calibrationSamples_, generateProxy_));
}

} // namespace QuantLib

#endif