#include "image_segmentation.h"

#include "frame_data.h"

#include <string>
#include <stdint.h>
#include <tuple>
#include <iostream>
#include <cmath>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include "opencv2/ximgproc/segmentation.hpp"
#include <opencv2/video/background_segm.hpp>
#include <armadillo>

using namespace cv::ximgproc::segmentation;
using namespace std;

ImagSegm::ImagSegm() {
	// empty
}

void ImagSegm::SetBodySegmentationParam(int BodyThresh, int BodyBlurSigma, int BodyBlurWindow, double BodySigma, int BodyK, int MinBodySize, double BodyLength, vector<tuple<double,double>> Origin) {

	body_thresh = BodyThresh;
	body_blur_sigma = BodyBlurSigma;
	body_blur_window = BodyBlurWindow;
	body_sigma = BodySigma;
	body_K = BodyK;
	min_body_size = MinBodySize;
	body_length = BodyLength;
	origin = Origin;

}

void ImagSegm::SetWingSegmentationParam(int WingThresh, int BodyDilation, double WingSigma, int WingK, int MinWingSize, double WingLength) {

	wing_thresh = WingThresh;
	body_dilation = BodyDilation;
	wing_sigma = WingSigma;
	wing_K = WingK;
	min_wing_size = MinWingSize;
	wing_length = WingLength;

}

void ImagSegm::SegmentFrame(frame_data &frame_in) {

	int N_cam = frame_in.N_cam;

	frame_in.seg_frame.clear();
	frame_in.seg_contours.clear();

	for (int n=0; n<N_cam; n++) {

		//arma::Col<int> img_vec = frame_in.raw_frame[n];
		arma::Col<int> img_vec = ImagSegm::ApplyMask(frame_in.raw_frame[n], n);
		int N_row = get<0>(frame_in.image_size[n]);
		int N_col = get<1>(frame_in.image_size[n]);

		// Get body segment:
		arma::Col<int> body_thresh_img = ImagSegm::BodyThresh(img_vec, N_row, N_col, body_thresh, body_blur_sigma, body_blur_window);
		arma::Col<int> body_seg_img = ImagSegm::GraphSeg(body_thresh_img, N_row, N_col, body_sigma, body_K, min_body_size);
		arma::Mat<double> body_seg_prop = ImagSegm::GetSegmentProperties(body_seg_img, N_row, N_col);
		tuple<arma::Col<double>,arma::Col<int>> body_seg = ImagSegm::SelectBody(body_seg_img, N_row, N_col, body_seg_prop, origin[n]);

		arma::Mat<double> frame_contour = ImagSegm::FindBodyAndWingContours(img_vec, get<1>(body_seg), N_row, N_col);
		frame_in.seg_contours.push_back(frame_contour);

		// Get wing segments:
		arma::Col<int> wing_thresh_img = ImagSegm::WingThresh(img_vec, get<1>(body_seg), N_row, N_col, wing_thresh, body_dilation, get<0>(body_seg));
		arma::Col<int> wing_seg_img = ImagSegm::GraphSeg(wing_thresh_img, N_row, N_col, wing_sigma, wing_K, min_wing_size);
		arma::Mat<double> wing_seg_prop = ImagSegm::GetSegmentProperties(wing_seg_img, N_row, N_col);
		tuple<arma::Mat<double>,arma::Mat<int>> wing_seg = ImagSegm::SelectWing(wing_seg_img, N_row, N_col, wing_seg_prop, get<0>(body_seg));

		arma::Col<int> seg_frame = ImagSegm::GlueSegments(get<1>(body_seg), get<1>(wing_seg), img_vec, N_row, N_col);

		// Load segmented frame into the frames struct:
		frame_in.seg_frame.push_back(seg_frame);

	}

}

void ImagSegm::SetImageMask(frame_data &frame_in, int cam_nr, int seg_nr) {

	arma::Col<int> seg_frame = frame_in.seg_frame[cam_nr];
	int N_row = get<0>(frame_in.image_size[cam_nr]);
	int N_col = get<1>(frame_in.image_size[cam_nr]);

	arma::Col<int> mask_vec;

	cv::Mat seg_img, bin_img;
	seg_img = ImagSegm::Vector2CVMat(seg_frame,N_row,N_col);
	cv::inRange(seg_img,seg_nr,seg_nr,bin_img);
	mask_vec = ImagSegm::CVMat2Vector(bin_img);

	arma::Col<int> mask(N_row*N_col);

	image_masks[cam_nr] = image_masks[cam_nr] % (1-(mask_vec/255));
}

arma::Col<int> ImagSegm::ApplyMask(arma::Col<int> &img_in, int cam_nr) {
	arma::Col<int> masked_img = (255-img_in) % image_masks[cam_nr];
	arma::Col<int> img_out = (255-masked_img);
	return img_out;
}

void ImagSegm::ResetImageMask(frame_data &frame_in) {

	image_masks.clear();

	for (int i=0; i<frame_in.N_cam; i++) {
		int N_row = get<0>(frame_in.image_size[i]);
		int N_col = get<1>(frame_in.image_size[i]);
		arma::Col<int> mask;
		mask.ones(N_row*N_col);
		image_masks.push_back(mask);
	}
}

tuple<arma::Col<double>,arma::Col<int>> ImagSegm::SelectBody(arma::Col<int> &frame_in, int N_row, int N_col, arma::Mat<double> seg_prop, tuple<double,double> origin_loc) {

	int N_seg = frame_in.max()+1;
	bool is_body = true;
	tuple<arma::Col<double>, arma::Col<int>> body_img;

	vector<tuple<int,double>> pos_body_segs;

	if (N_seg == 1) {
		cout << "no body segment found" << endl;
		arma::Col<int> body_img_vec(N_row*N_col);
		body_img_vec.zeros();
		arma::Col<double> body_prop_vec(4);
		body_prop_vec.ones();
		body_img = make_tuple((body_prop_vec*-1.0),body_img_vec);
	}
	else {
		for (int i=0; i<N_seg; i++) {
			is_body = true;

			//if (seg_prop(2,i)<(0.1*pow(body_length,2))) {
			//	is_body = false;
			//}
			//if (seg_prop(2,i)>(0.75*pow(body_length,2))) {
			//	is_body = false;
			//}
			//if (seg_prop(3,i)>0) {
			//	is_body = false;
			//}

			if (is_body == true) {
				pos_body_segs.push_back(make_tuple(i,seg_prop(2,i)));
			}
		}

		if (pos_body_segs.size()==1) {
			// Only one candidate for the body segment.
			cv::Mat seg_img, bin_img;
			seg_img = ImagSegm::Vector2CVMat(frame_in,N_row,N_col);
			cv::inRange(seg_img,get<0>(pos_body_segs[0]),get<0>(pos_body_segs[0]),bin_img);
			arma::Col<int> body_img_vec = ImagSegm::CVMat2Vector(bin_img);
			body_img = make_tuple(seg_prop.col(get<0>(pos_body_segs[0])),body_img_vec);
		}
		else if (pos_body_segs.size()>1) {
			// More than one candidate for the body segment, select the segment that is closest to 0.25*pow(body_length,2)
			arma::Col<double> area_diff(pos_body_segs.size());
			for (int j=0; j<pos_body_segs.size(); j++) {
				area_diff(j) = abs(get<1>(pos_body_segs[j])-0.25*pow(body_length,2));
			}
			int min_ind = area_diff.index_min();
			cv::Mat seg_img, bin_img;
			seg_img = ImagSegm::Vector2CVMat(frame_in,N_row,N_col);
			cv::inRange(seg_img,get<0>(pos_body_segs[min_ind]),get<0>(pos_body_segs[min_ind]),bin_img);
			arma::Col<int> body_img_vec = ImagSegm::CVMat2Vector(bin_img);
			body_img = make_tuple(seg_prop.col(min_ind),body_img_vec);
		}
		else {
			// No candidate for the body segment
			arma::Col<int> body_img_vec(N_row*N_col);
			body_img_vec.zeros();
			arma::Col<double> body_prop_vec(4);
			body_prop_vec.ones();
			body_img = make_tuple((body_prop_vec*-1.0),body_img_vec);
		}
	}

	return body_img;
}

arma::Mat<double> ImagSegm::FindBodyAndWingContours(arma::Col<int> &raw_img_vec, arma::Col<int> &body_img_vec, int N_row, int N_col) {

	cv::Mat img_raw, img_thresh;

	img_raw = ImagSegm::Vector2CVMat(raw_img_vec, N_row, N_col);

	cv::threshold(img_raw, img_thresh, 255-wing_thresh, 255, cv::THRESH_TOZERO_INV);

	vector<vector<cv::Point>> contours_outer;

	cv::findContours(img_thresh, contours_outer, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);

	int N_seg = contours_outer.size();

	arma::Row<int> contour_size(N_seg);

	for (int i=0; i<N_seg; i++) {
		contour_size(i) = contours_outer[i].size();
	}

	int max_ind = arma::index_max(contour_size);

	int N_pts = contour_size(max_ind);

	arma::Mat<double> contour_pts(N_pts,2);

	for (int j=0; j<N_pts; j++) {
		contour_pts(j,0) = contours_outer[max_ind][j].x;
		contour_pts(j,1) = contours_outer[max_ind][j].y;
	}

	return contour_pts;
}

tuple<arma::Mat<double>,arma::Mat<int>> ImagSegm::SelectWing(arma::Col<int> &frame_in, int N_row, int N_col, arma::Mat<double> seg_prop, arma::Col<double> body_prop) {

	int N_seg = frame_in.max()+1;
	bool is_wing = true;
	tuple<arma::Mat<double>,arma::Mat<int>> wing_img;

	vector<int> wing_segs;

	if (N_seg == 1) {
		cout << "no wing segment found" << endl;
		arma::Mat<int> wing_img_vec(N_row*N_col,1);
		wing_img_vec.zeros();
		arma::Mat<double> wing_prop_vec(4,1);
		wing_prop_vec.ones();
		wing_img = make_tuple((wing_prop_vec*-1.0),wing_img_vec);
	}
	else {
		for (int i=0; i<N_seg; i++) {
			is_wing = true;

			double dist_cg = sqrt(pow(seg_prop(0,i)-body_prop(0),2)+pow(seg_prop(1,i)-body_prop(1),2));

			//if (dist_cg>(1.5*wing_length)) {
			//	is_wing = false;
			//}
			//if (seg_prop(2,i)>(2.0*pow(wing_length,2))) {
			//	is_wing = false;
			//}
			//if (seg_prop(3,i)>0) {
			//	is_wing = false;
			//}

			if (is_wing == true) {
				wing_segs.push_back(i);
			}
		}

		if (wing_segs.size()>0) {
			arma::Mat<int> wing_img_vec(N_row*N_col,wing_segs.size());
			arma::Mat<double> wing_prop_vec(4,wing_segs.size());
			for (int j=0; j<wing_segs.size(); j++) {
				cv::Mat seg_img, bin_img;
				seg_img = ImagSegm::Vector2CVMat(frame_in,N_row,N_col);
				cv::inRange(seg_img,wing_segs[j],wing_segs[j],bin_img);
				wing_img_vec.col(j) = ImagSegm::CVMat2Vector(bin_img);
				wing_prop_vec.col(j) = seg_prop.col(wing_segs[j]);
			}
			wing_img = make_tuple(wing_prop_vec,wing_img_vec);
		}
		else {
			cout << "no wing segment found" << endl;
			arma::Mat<int> wing_img_vec(N_row*N_col,1);
			wing_img_vec.zeros();
			arma::Mat<double> wing_prop_vec(4,1);
			wing_prop_vec.ones();
			wing_img = make_tuple((wing_prop_vec*-1.0),wing_img_vec);
		}
	}

	return wing_img;
}

arma::Col<int> ImagSegm::GlueSegments(arma::Col<int> body_seg, arma::Mat<int> wing_seg, arma::Col<int> raw_img_vec, int N_row, int N_col) {

	int seg_dilation = 15;

	// Dilate the body and wing segments, merge the segments which thouch:
	int N_seg = wing_seg.n_cols;
	int body_seg_max = arma::max(body_seg);
	arma::Col<int> img_out;

	if (body_seg_max > 0) {

		cv::Mat img_body, img_contour, img_i;

		img_body = ImagSegm::Vector2CVMat(body_seg, N_row, N_col);

		cv::Mat RawImg, ThreshImg;

		RawImg = ImagSegm::Vector2CVMat(raw_img_vec,N_row,N_col);

		cv::threshold(RawImg, ThreshImg, 255-wing_thresh, 255, cv::THRESH_TOZERO_INV);

		//cv::threshold(255-RawImg, ThreshImg, wing_thresh, 255, cv::THRESH_TOZERO);

		vector<vector<cv::Point>> contours_outer;

		cv::findContours(ThreshImg, contours_outer, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE);

		arma::Col<int> img_c_vec;

		if (contours_outer.size() > 0) {

			arma::Row<double> contours_size(contours_outer.size());

			for (int k=0; k<contours_outer.size(); k++) {
				contours_size(k) = contours_outer[k].size();
			}

			int c_max_ind;

			c_max_ind = arma::index_max(contours_size);

			img_contour = cv::Mat::zeros(ThreshImg.size(), CV_8UC1);

			cv::drawContours(img_contour, contours_outer, c_max_ind, cv::Scalar(255), cv::FILLED);

			arma::Col<int> img_contour_vec = arma::clamp(ImagSegm::CVMat2Vector(img_contour),0,1);

			img_c_vec = img_contour_vec;

		}
		else {
			img_c_vec.zeros(N_row*N_col);
		}

		/*
		cv::Mat RawImg, ThreshImg;

		RawImg = ImagSegm::Vector2CVMat(raw_img_vec,N_row,N_col);

		// Perform thresholding:
		cv::threshold(255-RawImg, img_contour, wing_thresh, 255, cv::THRESH_TOZERO);

		arma::Col<int> img_c_vec = arma::clamp(ImagSegm::CVMat2Vector(img_contour),0,1);
		*/

		if (body_dilation > 0) {
			cv::dilate(img_body, img_body, cv::Mat(), cv::Point(-1, -1), body_dilation, 1, 1);
		}

		arma::Col<int> img_b_vec = img_c_vec%arma::clamp(ImagSegm::CVMat2Vector(img_body),0,1);

		if (N_seg > 0) {

			// Iterate through the segments and dilate by 1 pixel at the time:

			arma::Col<int> wing_seg_sum;

			wing_seg_sum.zeros(N_row*N_col);

			for (int i=0; i<N_seg; i++) {

				arma::Col<int> wing_sec_col = wing_seg.col(i);

				img_i = ImagSegm::Vector2CVMat(wing_sec_col, N_row, N_col);

				cv::dilate(img_i, img_i, cv::Mat(), cv::Point(-1, -1), seg_dilation, 1, 1);

				wing_seg_sum += (img_c_vec%arma::clamp(ImagSegm::CVMat2Vector(img_i),0,1))-img_b_vec;

			}

			arma::Col<int> glue_img_vec = arma::clamp(wing_seg_sum,0,1);

			cv::Mat glued_img = ImagSegm::Vector2CVMat(glue_img_vec, N_row, N_col);

			vector<vector<cv::Point>> contours;

			cv::findContours(glued_img, contours, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE);

			int N_cont = contours.size();

			img_out = img_b_vec;

			int seg_count = 2;

			if (N_cont>0) {
				for (int j=0; j<N_cont; j++) {
					cv::Mat contour_img = cv::Mat::zeros(N_row,N_col, CV_8UC1);
					if (cv::contourArea(contours[j])>min_wing_size) {
						cv::drawContours(contour_img,contours,j,cv::Scalar(seg_count), cv::FILLED);
						img_out += ImagSegm::CVMat2Vector(contour_img);
						seg_count++;
					}
				}
			}

		}
		else {
			img_out = img_b_vec;
		}
	}
	else {
		img_out.zeros(N_col*N_row);
	}
	return img_out;
}

arma::Col<int> ImagSegm::BodyThresh(arma::Col<int> &frame_in, int N_row, int N_col, int body_thresh, int sigma_blur, int blur_window) {

	cv::Mat img, ThreshImg;

	img = ImagSegm::Vector2CVMat(frame_in,N_row,N_col);

	cv::GaussianBlur(img, img, cv::Size(blur_window,blur_window),sigma_blur,sigma_blur);

	cv::threshold(img, ThreshImg, body_thresh, 0, cv::THRESH_TOZERO_INV);

	arma::Col<int> thresh_vec;

	thresh_vec = ImagSegm::CVMat2Vector(ThreshImg);

	return thresh_vec;
}

arma::Col<int> ImagSegm::WingThresh(arma::Col<int> &frame_in, arma::Col<int> &body_frame_in, int N_row, int N_col, int wing_thresh, int body_dilation, arma::Col<double> body_prop) {

	cv::Mat img, img_coords_body, body_img, circle_mask, ThreshImg;

	img = ImagSegm::Vector2CVMat(frame_in,N_row,N_col);

	body_img = cv::Mat::zeros(N_row, N_col, CV_8UC1 );

	body_img = body_img+ImagSegm::Vector2CVMat(body_frame_in,N_row,N_col);

	// Dilate the image a bit:
	if (body_dilation > 0) {
		cv::dilate(body_img, body_img, cv::Mat(), cv::Point(-1, -1), body_dilation, 1, 1);
	}

	// Perform thresholding:
	cv::threshold(255-(img+body_img), ThreshImg, wing_thresh, 255, cv::THRESH_TOZERO);

	arma::Col<int> thresh_vec;

	thresh_vec = ImagSegm::CVMat2Vector(ThreshImg);

	return thresh_vec;
}

arma::Mat<double> ImagSegm::GetSegmentProperties(arma::Col<int> &frame_in, int N_row, int N_col) {

	int N_seg = frame_in.max()+1;

	arma::Mat<double> seg_prop(4,N_seg);

	cv::Mat seg_img = ImagSegm::Vector2CVMat(frame_in, N_row, N_col);

	for (int i=0; i<N_seg; i++) {

		cv::Mat bin_img;

		cv::inRange(seg_img,i,i,bin_img);

		double minVal;
		double maxVal;
		cv::Point minLoc;
		cv::Point maxLoc;

		cv::minMaxLoc(bin_img, &minVal, &maxVal, &minLoc, &maxLoc);

		double cg_x, cg_y, Area, Mx, My, border_sum;

		// Calculate area

		Area = cv::sum(bin_img)[0]/maxVal;

		Mx = 0.0;
		for (int j=0; j<N_row; j++) {
			Mx+=(cv::sum(bin_img.row(j))[0]*j)/maxVal;
		}

		My = 0.0;
		for (int k=0; k<N_col; k++) {
			My+=(cv::sum(bin_img.col(k))[0]*k)/maxVal;
		}

		// Calculate cg

		cg_x = My/Area;
		cg_y = Mx/Area;

		// Check if the segment hits one of the borders
		border_sum = cv::sum(bin_img.row(0))[0]+cv::sum(bin_img.row(N_row-1))[0]+cv::sum(bin_img.col(0))[0]+cv::sum(bin_img.col(N_col-1))[0];

		seg_prop(0,i) = cg_x;
		seg_prop(1,i) = cg_y;
		seg_prop(2,i) = Area;
		seg_prop(3,i) = border_sum;

	}
	return seg_prop;
}

arma::Col<int> ImagSegm::GraphSeg(arma::Col<int> &frame_in, int N_row, int N_col, double Sigma, int K, int minsize) {

	gs->setSigma(Sigma);
	gs->setK(K);
	gs->setMinSize(minsize);

	cv::Mat input_img, output_img;

	input_img = ImagSegm::Vector2CVMat(frame_in,N_row,N_col);

	gs->processImage(input_img,output_img);

	arma::Col<int> segmented_frame;
	segmented_frame = ImagSegm::CVMat2Vector(output_img);

	return segmented_frame;
}

arma::Mat<double> ImagSegm::FindDoubleContours(arma::Mat<double> &img_mat_in, int N_row, int N_col) {

	arma::Col<int> bin_img_vec = arma::conv_to<arma::Col<int>>::from(arma::clamp(img_mat_in.row(2).t(),0,1)*255);

	cv::Mat ThreshImg;

	ThreshImg = ImagSegm::Vector2CVMat(bin_img_vec,N_row,N_col);

	vector<vector<cv::Point>> contours;

	cv::findContours(ThreshImg, contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);

	int N_contours = contours.size();

	int N_pts;

	arma::Mat<double> contour_now;

	arma::Mat<double> contour_temp;

	int contour_count = 1;

	int uv_ind;

	cout << N_contours << endl;

	if (N_contours > 0) {
		for (int i=0; i<N_contours; i++) {
			if (i==0) {
				N_pts = contours[i].size();
				contour_now.zeros(3,N_pts+1);
				for (int j=0; j<=N_pts; j++) {
					if (j<N_pts) {
						uv_ind = contours[i][j].y*N_row+contours[i][j].x;
						contour_now(0,j) = img_mat_in(3,uv_ind);
						contour_now(1,j) = img_mat_in(4,uv_ind);
						contour_now(2,j) = contour_count;
					}
					else {
						contour_now(0,j) = contour_now(0,0);
						contour_now(1,j) = contour_now(1,0);
						contour_now(2,j) = contour_now(2,0);
					}
				}
			}
			else {
				N_pts = contours[i].size();
				contour_temp.zeros(3,N_pts+1);
				for (int j=0; j<=N_pts; j++) {
					if (j<N_pts) {
						uv_ind = contours[i][j].y*N_row+contours[i][j].x;
						contour_temp(0,j) = img_mat_in(3,uv_ind);
						contour_temp(1,j) = img_mat_in(4,uv_ind);
						contour_temp(2,j) = contour_count;
					}
					else {
						contour_temp(0,j) = contour_temp(0,0);
						contour_temp(1,j) = contour_temp(1,0);
						contour_temp(2,j) = contour_temp(2,0);
					}
				}
				arma::join_rows(contour_now,contour_temp);
				contour_count++;
			}
		}
	}
	else {
		contour_now.zeros(3,1);
	}
	return contour_now;
}

arma::Mat<double> ImagSegm::FindContours(arma::Col<int> &img_in, int N_row, int N_col) {

	// Clamp the image between 0 and 255
	arma::Col<int> img_thresh = arma::clamp(img_in,0,1)*255;

	cv::Mat ThreshImg;

	ThreshImg = ImagSegm::Vector2CVMat(img_thresh,N_row,N_col);

	vector<vector<cv::Point>> contours;

	cv::findContours(ThreshImg, contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);

	int N_contours = contours.size();

	int N_pts;

	arma::Mat<double> contour_now;

	arma::Mat<double> contour_temp;

	int contour_count = 1;

	if (N_contours > 0) {
		for (int i=0; i<N_contours; i++) {
			if (i==0) {
				N_pts = contours[i].size();
				contour_now.zeros(3,N_pts+1);
				for (int j=0; j<=N_pts; j++) {
					if (j<N_pts) {
						contour_now(0,j) = contours[i][j].x;
						contour_now(1,j) = contours[i][j].y;
						contour_now(2,j) = contour_count;
					}
					else {
						contour_now(0,j) = contour_now(0,0);
						contour_now(1,j) = contour_now(1,0);
						contour_now(2,j) = contour_now(2,0);
					}
				}
			}
			else {
				if (cv::contourArea(contours[i])>pow(min_wing_size,2)) {
					N_pts = contours[i].size();
					contour_temp.zeros(3,N_pts+1);
					for (int j=0; j<=N_pts; j++) {
						if (j<N_pts) {
							contour_temp(0,j) = contours[i][j].x;
							contour_temp(1,j) = contours[i][j].y;
							contour_temp(2,j) = contour_count;
						}
						else {
							contour_temp(0,j) = contour_temp(0,0);
							contour_temp(1,j) = contour_temp(1,0);
							contour_temp(2,j) = contour_temp(2,0);
						}
					}
					arma::join_rows(contour_now,contour_temp);
					contour_count++;
				}
			}
		}
	}
	else {
		contour_now.zeros(3,1);
	}
	return contour_now;
}

arma::Col<int> ImagSegm::CVMat2Vector(cv::Mat &img) {

	arma::Col<int> img_vec;

	img.convertTo(img, CV_8UC1);
	arma::Mat<uint8_t> img_mat(reinterpret_cast<uint8_t*>(img.data), img.rows, img.cols);
	img_vec = arma::vectorise(arma::conv_to<arma::Mat<int>>::from(img_mat));

	return img_vec;
}

cv::Mat ImagSegm::Vector2CVMat(arma::Col<int> &img_vec, int N_row, int N_col) {

	arma::Mat<int> img_mat;

	img_mat = img_vec;

	img_mat.reshape(N_row,N_col);

	cv::Mat img(img_mat.n_rows, img_mat.n_cols, CV_32SC1, img_mat.memptr());

	img.convertTo(img, CV_8UC1);

	return img;
}

np::ndarray ImagSegm::ReturnSegFrame(frame_data &frame_now, int cam_nr) {

	int N_row = get<0>(frame_now.image_size[cam_nr]);
	int N_col = get<1>(frame_now.image_size[cam_nr]);

	p::tuple shape = p::make_tuple(N_row,N_col);
	np::dtype dtype = np::dtype::get_builtin<int>();
	np::ndarray array_out = np::zeros(shape,dtype);

	for (int i=0; i<N_row; i++) {
		for (int j=0; j<N_col; j++) {
			array_out[i][j] = frame_now.seg_frame[cam_nr](i*N_row+j);
		}
	}

	return array_out;
}

np::ndarray ImagSegm::ReturnOuterContour(frame_data &frame_now, int cam_nr) {

	int N_pts = frame_now.seg_contours[cam_nr].n_rows;

	p::tuple shape = p::make_tuple(N_pts,2);
	np::dtype dtype = np::dtype::get_builtin<int>();
	np::ndarray array_out = np::zeros(shape,dtype);

	for (int i=0; i<N_pts; i++) {
		array_out[i][0] = frame_now.seg_contours[cam_nr](i,0);
		array_out[i][1] = frame_now.seg_contours[cam_nr](i,1);
	}

	return array_out;
}


#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <cmath>
#include "tbb/tbb.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "ray.h"
#define STB_IMAGE_WRITE_IMPLEMENTATION // Do not include this line twice in your project!
#include "stb_image_write.h"
#include "utils.h"
#define TIME_TRACE
#define SPECULAR_CUTOFF 0.7
#define REFLECTION_CUTOFF 0.01
#define DBL_INFINITY (1.0/0.0)
#define SCREEN_WIDTH ((double) (SCREEN_SIZE))
#define SCREEN_HEIGHT ((((double) (SCREEN_RESOLUTION_HEIGHT))/((double) (SCREEN_RESOLUTION_WIDTH))) * SCREEN_SIZE)
#define TRACE_DEPTH 5
#define AMBIENT_COLOUR (colour(32, 32, 32))
#define SPECULAR_POWER 20
#define LIGHT_FALLOFF 500
#define COL_MAX 255
#define SCREEN_RESOLUTION_WIDTH 800
#define SCREEN_RESOLUTION_HEIGHT 800
#define SCREEN_DISTANCE 1
#define SCREEN_SIZE 1.0
#define EYE_POSITION (Vector3d (0, 0, -1))
#define EYE_DIRECTION (Vector3d(0, 0 ,0.5))
#define BACKGROUND_COLOUR (colour(32, 32, 32))
#define SHADOWS_ENABLE

 using namespace Eigen;
 using namespace std;
struct s_screen {
	Vector3d  eye;
	Vector3d  botl;
	Vector3d  topl;
	Vector3d  botr;
	int height;
	int width;
};

void gen_screen(ray eye, s_screen *s) {
	Vector3d  planevec, zx_comp, zy_comp;
	Vector3d  viewdir = eye.dir.normalized();
	Vector3d  centre = eye.origin + SCREEN_DISTANCE*viewdir;
	//Form vector in eye.dir-y plane, then find z-x screen component
	//if eye.dir ~= y then z-x component is x
	if(viewdir(0) < SCREEN_ALIGN_EPSILON && viewdir(0) > -SCREEN_ALIGN_EPSILON &&\
			 viewdir(2) < SCREEN_ALIGN_EPSILON && viewdir(2) > -SCREEN_ALIGN_EPSILON) {
		zx_comp = SCREEN_WIDTH * Vector3d (1, 0, 0);
	} else {
		planevec = Vector3d (viewdir(0), viewdir(1) + 1, viewdir(2));
		zx_comp = SCREEN_WIDTH * planevec.cross(viewdir).normalized();
	}
	//Then the z-y component
	if(viewdir(1) < SCREEN_ALIGN_EPSILON && viewdir(1) > -SCREEN_ALIGN_EPSILON &&\
			 viewdir(2) < SCREEN_ALIGN_EPSILON && viewdir(2) > -SCREEN_ALIGN_EPSILON) {
		zy_comp = SCREEN_HEIGHT * Vector3d (0, 1, 0);
	} else {
		planevec = Vector3d (viewdir(0) + 1, viewdir(1), viewdir(2));
		zy_comp = -SCREEN_HEIGHT * planevec.cross(viewdir).normalized();
	}

	s->botl   = centre - (zx_comp + zy_comp);
	s->topl   = centre + (zy_comp - zx_comp);
	s->botr   = centre + (zx_comp - zy_comp);
	s->eye    = eye.origin;
	s->height = SCREEN_RESOLUTION_HEIGHT;
	s->width  = SCREEN_RESOLUTION_WIDTH;
}

//Find the nearest intersecting shpae and return a pointer to it or NULL on no intersection.
shape* trace_nearest(ray &r, std::vector<shape*> &world, double &distance) {
	double closest_distance = DBL_INFINITY;
	double t;
	shape* closest_shape = NULL;
	std::vector<shape*>::iterator v = world.begin();
	while(v != world.end()) {
		t = (**v).intersect(r);
		if(t != 0.0 && t < closest_distance) {
			closest_distance = t;
			closest_shape = *v;
		}
		v++;
	}
	distance = closest_distance;
	return closest_shape;
}

bool trace_light(Vector3d  pos, std::vector<shape*> &w, light *l, colour *c) {
	ray r;
	double t, distsq, strength;
	r.origin = pos;
	r.dir = l->pos - pos;
	shape* s;
	#ifdef SHADOWS_ENABLE
	//Check for collisions before the light source
	// cout<<"Shadow enabled";
	s = trace_nearest(r, w, t);
	#else
	s = NULL;
	#endif

	if(s != NULL && t < (1.0+INTERSECT_EPSILON)) {
		//Light is blocked (shadow)
		return false;
	} else {
		distsq = r.dir(0)*r.dir(0)+r.dir(1)*r.dir(1)+r.dir(2)*r.dir(2);
		strength = (distsq <= LIGHT_FALLOFF ? 1.0 : LIGHT_FALLOFF/(distsq));
		*c = l->col * strength;
		return true;
	}
}

bool trace(ray &r, std::vector<shape*> &world, std::vector<light*> &lights, int depth, double strength, colour *c) {
	double t, light_cosine;
	Vector3d  hit_position, normal, inv_ray_dir, dir_to_light;
	colour tmpcol, diff_light_col = AMBIENT_COLOUR, spec_light_col = colour(0, 0, 0);
	std::vector<light*>::iterator liter;
	shape *intersect_shape;
	intersect_shape = trace_nearest(r, world, t);
	ray reflection;
	double reflection_cosine;

	if(intersect_shape != NULL) {
		inv_ray_dir = -r.dir;
		liter = lights.begin();
		hit_position = r.origin + t*r.dir;
		normal = intersect_shape->get_normal(hit_position).normalized();
		if(inv_ray_dir.dot(normal) < 0.0) {
			normal = -normal;
		}
		reflection.origin = hit_position;
		reflection.dir    = r.dir - 2*(r.dir.dot(normal))*normal;

		//Go through each light and find its contribution
		while(liter != lights.end()) {
			dir_to_light = ((**liter).pos - hit_position).normalized();
			//Don't look for light begind the object
			light_cosine = dir_to_light.dot(normal);
			if(light_cosine > 0.0) {
				if(trace_light(hit_position, world, *liter, &tmpcol)) {
					diff_light_col = diff_light_col + tmpcol*light_cosine;
					reflection_cosine = reflection.dir.normalized().dot(dir_to_light);
					if(reflection_cosine > SPECULAR_CUTOFF) {
						spec_light_col = spec_light_col + tmpcol*std::pow(reflection_cosine, SPECULAR_POWER);
					}
				}
			}
			liter++;
		}

		colour diffuse_component  = diff_light_col * intersect_shape->col * intersect_shape->surf.diffusion;
		colour specular_component = spec_light_col * intersect_shape->surf.specular;
		*c = diffuse_component + specular_component;

		return true;
	} else {
		*c = BACKGROUND_COLOUR;
		return false;
	}
}

void ray_trace(s_screen &s, std::vector<shape*> &world, std::vector<light*> &lights, string filename) {
	Vector3d  h_step, v_step, pos;

	MatrixXd R = MatrixXd::Zero(800,800); // Store the color
    MatrixXd A = MatrixXd::Zero(800,800);
    MatrixXd G = MatrixXd::Zero(800,800);
    MatrixXd B = MatrixXd::Zero(800,800);

	ray r;
	r.origin = s.eye;
	colour col;
	// int i, j;

	h_step = (s.botr - s.botl)/(s.width-1);
	v_step = (s.topl - s.botl)/(-s.height+1);

	for(int j=0; j<s.height; j++) {
		pos = s.topl + j*v_step;
		for(int i=0; i<s.width; i++) {

			r.dir = pos-s.eye;
			trace(r, world, lights, TRACE_DEPTH, 1.0, &col);

			// cout << "col.blue is " <<col.blue<<","<<col.red<<","<<col.green<<std::endl;
			B(i,j) = fmax(col.blue/255.0,0.0);
			G(i,j) = fmax(col.green/255.0,0.0);
			R(i,j) = fmax(col.red/255.0,0.0);
			A(i,j) = 1.0;
			pos += h_step;
		}
	}

	write_matrix_to_png(R,G,B,A,filename);

}

void animation(int num_scenes)
{	s_screen screen;
	std::vector<shape*> *w;
	std::vector<light*> *l;
	l=partELights();
	w=partEWorld(27);
	char filename[20];
	for(int i=0;i<num_scenes;i++)
	{

	cout << "### Ray tracer ###" << std::endl;

	ray eye = {Vector3d (0, 0, 0.5), Vector3d (0, 0, -1+i)};
	gen_screen(eye, &screen);
	sprintf(filename, "animation%d.png", i);
	ray_trace(screen, *w, *l,filename);
	l->pop_back();
	l->push_back(new light(Vector3d(15, 15, 5+i), colour(255, 255, 255)));

	}
	freeWorld(w);
	freeLights(l);

}

void ray_tracer() {

	// s_screen screen;
	// std::vector<shape*> *w;
	// std::vector<light*> *l;

	// cout << "### Ray tracer ###" << std::endl;

	// ray eye = {EYE_DIRECTION, EYE_POSITION};
	// gen_screen(eye, &screen);

	// //EYE_DIRECTION set to 0,0,-1
	// // w = getWorld();
	// // l = getLights();
	// // w = partAWorld();
	// // l = partALights();

	// //w = partBWorld();
	// // l = partBLights();
	// //FOr part C we use the same lights as B and change the EYE_DIRECTION to incorporate x,y,z and make it perspective
	// //Thus for part C we change EYE_DIRECTION from 0,0,-1 to 2,3,4
	// // w = partCWorld();

	// //For part D we get the data from the Mesh - we use the same lights as partBLights()
	// //This is a big buggy as was not able to get the triange intersect logic work PERFECTLY
	// // Have the paths to both the meshes commented in the Word.cpp getMeshWorld() function
	// // w = getMeshWorld();

	// //for shadows we need to #define SHADOWS_ENABLE from the header of THIS FILE
	// //then shadows get enabled
	// l=partELights();
	// w=partEWorld(27);

	// cout << w->size() << " shapes and " << l->size() << " lights." << std::endl;

	//  ray_trace(screen, *w, *l);

	// freeWorld(w);
	// freeLights(l);

	//EXtra credit  Animation
	animation(20);
}

int main() {

	ray_tracer();

	return 0;
}


#include "Exp.hh"
#include "TypeClasses.hh"
using namespace TypeClasses;

#include <Eigen/Core>
#include "config_vars.h"

#ifdef GNA_CUDA_SUPPORT
#include "cuElementary.hh"
#endif

namespace GNA {
  namespace GNAObjectTemplates {
    /**
     * @brief Constructor.
     */
    template<typename FloatType>
    ExpT<FloatType>::ExpT() {
        this->transformation_("exp")
            .input("points")
    	    .output("result")
    	    .types(new CheckKindT<FloatType>(DataKind::Points), new PassTypeT<FloatType>(0, {0,-1}))
    	    .func(&ExpT<FloatType>::calculate)
    #ifdef GNA_CUDA_SUPPORT
    	    .func("gpu", &ExpT<FloatType>::calc_gpu, DataLocation::Device)
    #endif
          ;
    }

    /**
     * @brief Calculate the value of function.
     */
    template<typename FloatType>
    void ExpT<FloatType>::calculate(FunctionArgs& fargs){
        fargs.rets[0].x = fargs.args[0].x.exp();
    }

#ifdef GNA_CUDA_SUPPORT
    template<typename FloatType>
    void ExpT<FloatType>::calc_gpu(FunctionArgs& fargs) {
            fargs.args.touch();
            auto& gpuargs=fargs.gpu;
            gpuargs->provideSignatureDevice();
            auto** source=gpuargs->args;
            auto** dest  =gpuargs->rets;
            cuexp(source, dest, gpuargs->nargs, fargs.args[0].arr.size());
    }
#endif
  }
}

template class GNA::GNAObjectTemplates::ExpT<double>;
#ifdef PROVIDE_SINGLE_PRECISION
  template class GNA::GNAObjectTemplates::ExpT<float>;
#endif


#include <string>
#include <unordered_set>
#include <boost/functional/hash.hpp>
#include <gtest/gtest.h>

//
// This example demonstrates how to use an unordered_set.
// To combine hashes we use boost::hash_combine.
//

class Person {
public:
  Person(const std::string& name, int age)
  : name_{name}, age_{age} { }

  auto name() const {
    return name_;
  }
  auto age() const {
    return age_;
  }

private:
  std::string name_{};
  int age_{};
};

auto person_eq = [](const Person& lhs, const Person& rhs) {
  return lhs.name() == rhs.name() && lhs.age() == rhs.age();
};

auto person_hash = [](const Person& person) {
  auto seed = size_t{0};
  // See also http://www.boost.org/doc/libs/1_55_0/doc/html/hash/reference.html#boost.hash_combine
  boost::hash_combine(seed, person.name());
  boost::hash_combine(seed, person.age());
  return seed;
};

TEST(UnorderedSets, CheckElementPresence) {
  using Set = std::unordered_set<Person, decltype(person_hash), decltype(person_eq)>;

  auto persons = Set{100, person_hash, person_eq};
  persons.emplace("John", 32);
  persons.emplace("Anne", 45);

  ASSERT_EQ(0, persons.count(Person{"John", 31}));
  ASSERT_EQ(0, persons.count(Person{"Anne", 32}));
  ASSERT_EQ(1, persons.count(Person{"John", 32}));
  ASSERT_EQ(1, persons.count(Person{"Anne", 45}));
}


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

#ifndef BARK_WORLD_OPENDRIVE_PLAN_VIEW_HPP_
#define BARK_WORLD_OPENDRIVE_PLAN_VIEW_HPP_

#include <Eigen/Core>
#include "bark/geometry/geometry.hpp"
#include "bark/world/opendrive/lane.hpp"

namespace bark {
namespace world {
namespace opendrive {

using bark::geometry::Line;
using bark::geometry::Point2d;

class PlanView {
 public:
  PlanView() : length_(0.0) {}
  ~PlanView() {}

  //! setter functions
  bool AddLine(Point2d start_point, double heading, double length,
               double s_inc);

  bool AddSpiral(Point2d start_point, double heading, double length,
                 double curvStart, double curvEnd, double s_inc = 2.0);

  bool AddArc(Point2d start_point, double heading, double length, double curvature,
              double s_inc = 2.0);

  void CalcArcPosition(const double s, double initial_heading, double curvature,
                       double& dx, double& dy);

  //! getter functions
  Line GetReferenceLine() const { return reference_line_; }

  bool ApplyOffsetTransform(double x, double y, double hdg);

  double GetLength() const { return length_; }
  double GetDistance(const Point2d& p) const {
    return boost::geometry::distance(reference_line_.obj_, p);
  }

 private:
  Line reference_line_;  // sequential build up
  double length_;
};

using PlanViewPtr = std::shared_ptr<PlanView>;

}  // namespace opendrive
}  // namespace world
}  // namespace bark

#endif  // BARK_WORLD_OPENDRIVE_PLAN_VIEW_HPP_


// main program of the   - Felipe Figueredo Rocha
#include <iostream>
#include "solver.hpp"
#include <armadillo>

//~ SET(ARMADILLO_DIR /home/felipe/PToolsV201311/armadillo-6.500.5/include)
//~ SET(ARMADILLO_LIB /home/felipe/PToolsV201311/armadillo-6.500.5/libarmadillo.so)

using namespace std;
using namespace arma;

int main(int argc, char **argv){

	Solver *s;

	s = new Solver;
	s->init();

	s->run();

	//~ int n = 3;
	//~ sp_mat A(n,n);
//~
	//~ A(0,0) = 1.0;
	//~ A(1,1) = 2.0;
	//~ A(2,1) = 4.0;
	//~ A(2,2) = 3.0;
	//~
	//~ vec b(n);
	//~ b(0) = 1.0;
	//~ b(1) = 4.0;
	//~ b(2) = 17.0;
	//~
	//~ vec x = spsolve(A, b, "lapack");  // solve one system

	//~ int n = 3;
	//~ mat *A;
	//~ A = new mat(n,n,fill::zeros);
//~
	//~ (*A)(0,0) = 1.0;
	//~ (*A)(1,1) = 2.0;
	//~ (*A)(2,1) = 4.0;
    //~ (*A)(2,2) = 3.0;
	//~
	//~ vec *b;
	//~ b = new vec(n,fill::zeros);
	//~
	//~ (*b)(0) = 1.0;
	//~ (*b)(1) = 4.0;
	//~ (*b)(2) = 17.0;
	//~
	//~ vec *x;
	//~ x = new vec(n,fill::zeros);
	//~
	//~ solve(*x, *A, *b);  // solve one system
	//~
	//~ cout << *x;
}


#include <cfloat>
#include <type_traits>

#include <boost/qvm/all.hpp>

#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>

#include <cppid/gm3d/vector_2d.hpp>

using cppid::gm3d::vector_2d;
using vec2f = vector_2d<float>;

TEST_CASE("float_vector2_test traits")
{
  REQUIRE(std::is_trivial_v<vec2f>);
  REQUIRE(std::is_standard_layout_v<vec2f>);
  REQUIRE(sizeof(vec2f) == (sizeof(float) * 2));

  REQUIRE((
   std::is_same_v<float, typename boost::qvm::vec_traits<vec2f>::scalar_type>));
  REQUIRE(boost::qvm::vec_traits<vec2f>::dim == 2);
}

TEST_CASE("float_vector2_test initialization")
{
  {
    auto vt = vec2f{0};
    REQUIRE(vt.x() == 0.0f);
    REQUIRE(vt.y() == 0.0f);
  }
  {
    auto vt = vec2f{1.5, 2.2};
    REQUIRE(vt.x() == 1.5f);
    REQUIRE(vt.y() == 2.2f);
  }
  {
    auto vt = vec2f{vec2f{1.5, 2.3}};
    REQUIRE(vt.x() == 1.5f);
    REQUIRE(vt.y() == 2.3f);
  }
  {
    auto vt = vec2f{vector_2d<int>{0}};
    REQUIRE(vt.x() == 0.0f);
    REQUIRE(vt.y() == 0.0f);
  }
  {
    auto vt = vec2f{0};
    vt = vector_2d<int>{0};
    REQUIRE(vt.x() == 0.0f);
    REQUIRE(vt.y() == 0.0f);
  }
  {
    auto vd = vector_2d<double>{0};
    vec2f vt = vd;
    CHECK(vt.x() == 0.0f);
    CHECK(vt.y() == 0.0f);
  }
}

TEST_CASE("float_vector2_test comparison")
{
  REQUIRE((vec2f{0} == vec2f{0, 0}));
  REQUIRE((vec2f{3.5} == vec2f{3.5, 3.5}));
  REQUIRE((vec2f{-.5} == vec2f{-.5, -.5}));

  REQUIRE((vec2f{-.5} != vec2f{-.5, -.1}));
}

TEST_CASE("float_vector2_test addition")
{
  {
    auto vt = vec2f{-10.4, 0.4};
    vt += vec2f{0};
    REQUIRE(vt.x() == Approx(-10.4f));
    REQUIRE(vt.y() == Approx(0.4f));
  }
  {
    auto vt = vec2f{10.4, -2.2};
    vt += 3.2;
    REQUIRE(vt.x() == Approx(13.6f));
    REQUIRE(vt.y() == Approx(1.0f));
  }
  {
    auto vt = vec2f{0} + vec2f{2.5, 0.3};
    REQUIRE(vt.x() == Approx(2.5f));
    REQUIRE(vt.y() == Approx(0.3f));
  }
  {
    auto vt = vec2f{-1., 0} + 0.2;
    REQUIRE(vt.x() == Approx(-0.8f));
    REQUIRE(vt.y() == Approx(0.2f));
  }
  {
    auto vt = 0.2 + vec2f{-1., 0};
    REQUIRE(vt.x() == Approx(-0.8f));
    REQUIRE(vt.y() == Approx(0.2f));
  }
}

TEST_CASE("float_vector2_test subtraction")
{
  {
    auto vt = vec2f{0};
    vt -= vec2f(-10.4, 0.4);
    REQUIRE(vt.x() == Approx(10.4f));
    REQUIRE(vt.y() == Approx(-0.4f));
  }
  {
    auto vt = vec2f{1.5, 2.2};
    vt -= 3.2;
    REQUIRE(vt.x() == Approx(-1.7f));
    REQUIRE(vt.y() == Approx(-1.f));
  }
  {
    auto vt = vec2f{1.5, 2.2} - vec2f{0};
    REQUIRE(vt.x() == Approx(1.5f));
    REQUIRE(vt.y() == Approx(2.2f));
  }
  {
    auto vt = vec2f{2.5, -0.3} - 1;
    REQUIRE(vt.x() == Approx(1.5f));
    REQUIRE(vt.y() == Approx(-1.3f));
  }
  {
    auto vt = 1u - vec2f{2.5, -0.3};
    REQUIRE(vt.x() == Approx(-1.5f));
    REQUIRE(vt.y() == Approx(1.3f));
  }
}

TEST_CASE("float_vector2_test multiplication")
{
  {
    auto vt = vec2f(-10.4, 0.4);
    vt *= vec2f{-1};
    REQUIRE(vt.x() == Approx(10.4f));
    REQUIRE(vt.y() == Approx(-0.4f));
  }
  {
    auto vt = vec2f(-10.4, 0.4);
    vt *= -1l;
    REQUIRE(vt.x() == Approx(10.4f));
    REQUIRE(vt.y() == Approx(-0.4f));
  }
  {
    auto vt = vec2f(-10.4, 0.4) * vec2f{-1};
    REQUIRE(vt.x() == Approx(10.4f));
    REQUIRE(vt.y() == Approx(-0.4f));
  }
  {
    auto vt = vec2f(-10.4, 0.4) * -1.0;
    REQUIRE(vt.x() == Approx(10.4f));
    REQUIRE(vt.y() == Approx(-0.4f));
  }
  {
    auto vt = -1 * vec2f(-10.4, 0.4);
    REQUIRE(vt.x() == Approx(10.4f));
    REQUIRE(vt.y() == Approx(-0.4f));
  }
}

TEST_CASE("float_vector2_test division")
{
  {
    auto vt = vec2f(-10.0, 0.5);
    vt /= vec2f{2};
    REQUIRE(vt.x() == Approx(-5.0f));
    REQUIRE(vt.y() == Approx(0.25f));
  }
  {
    auto vt = vec2f(-10.0, 0.5);
    vt /= 2ull;
    REQUIRE(vt.x() == Approx(-5.0f));
    REQUIRE(vt.y() == Approx(0.25f));
  }
  {
    auto vt = vec2f(-10.0, 0.5) / vec2f{2};
    REQUIRE(vt.x() == Approx(-5.0f));
    REQUIRE(vt.y() == Approx(0.25f));
  }
  {
    auto vt = vec2f(-10.0, 0.5) / 2;
    REQUIRE(vt.x() == Approx(-5.0f));
    REQUIRE(vt.y() == Approx(0.25f));
  }
  {
    auto vt = 2 / vec2f(-10.0, 0.5);
    REQUIRE(vt.x() == Approx(-0.2f));
    REQUIRE(vt.y() == Approx(4.0f));
  }
}

TEST_CASE("float_vector2_test reflect")
{
  {
    auto d = vec2f{10, -2};
    auto r = d.reflect({0, 1});
    REQUIRE(r.x() == Approx(10.f));
    REQUIRE(r.y() == Approx(2.0f));
  }
}


/*=============================================================================
    Copyright (c) 2001-2007 Joel de Guzman

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/
#include <iostream>
#include <cmath>
#include <boost/detail/lightweight_test.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_bind.hpp>

using namespace boost::phoenix;
using namespace boost::phoenix::arg_names;
using namespace std;

    struct test
    {
        typedef void result_type;
        void operator()() const
        {
            cout << "Test lazy functions...\n";
        }
    };

    struct sqr
    {
        template <typename Arg>
        struct result
        {
            typedef Arg type;
        };

        template <typename Arg>
        Arg operator()(Arg n) const
        {
            return n * n;
        }
    };

    struct fact
    {
        template <typename Arg>
        struct result
        {
            typedef Arg type;
        };

        template <typename Arg>
        Arg operator()(Arg n) const
        {
            return (n <= 0) ? 1 : n * (*this)(n-1);
        }
    };

    struct power
    {
        template <typename Arg1, typename Arg2>
        struct result
        {
            typedef Arg1 type;
        };

        template <typename Arg1, typename Arg2>
        Arg1 operator()(Arg1 a, Arg2 b) const
        {
            return pow(a, b);
        }
    };

    struct add
    {
        template <typename Arg1, typename Arg2, typename Arg3, typename Arg4>
        struct result
        {
            typedef Arg1 type;
        };

        template <typename Arg1, typename Arg2, typename Arg3, typename Arg4>
        Arg1 operator()(Arg1 a, Arg2 b, Arg3 c, Arg4 d) const
        {
            return a + b + c + d;
        }
    };

int
main()
{
    int i5 = 5;
    double d5 = 5, d3 = 3;

    test()();
    BOOST_TEST(bind(sqr(), arg1)(i5) == (i5*i5));
    BOOST_TEST(bind(fact(), 4)() == 24);
    BOOST_TEST(bind(fact(), arg1)(i5) == 120);
    BOOST_TEST((int)bind(power(), arg1, arg2)(d5, d3) == (int)pow(d5, d3));
    BOOST_TEST((bind(sqr(), arg1) + 5)(i5) == ((i5*i5)+5));
    BOOST_TEST(bind(add(), arg1, arg1, arg1, arg1)(i5) == (5+5+5+5));

    int const ic5 = 5;
    // testing consts
    BOOST_TEST(bind(sqr(), arg1)(ic5) == (ic5*ic5));

    // From Steven Watanabe
    sqr s;
    int x = 2;
    int result = bind(ref(s), _1)(x);
    BOOST_TEST(result == 4);

    return boost::report_errors();
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file normalize_sorts_test.cpp
/// \brief Test for normalizing sorts.

#include "mcrl2/data/data_specification.h"
#include "mcrl2/data/normalize_sorts.h"
#include "mcrl2/data/parse.h"
#include <algorithm>
#include <boost/test/minimal.hpp>
#include <iterator>
#include <set>
#include <vector>

using namespace mcrl2;
using namespace mcrl2::data;

void test_normalize_sorts()
{
  std::string DATASPEC =
    "sort Bit = struct e0 | e1;      \n"
    "     AbsBit = struct arbitrary; \n"
    "                                \n"
    "map  inv: Bit -> Bit;           \n"
    "     h: Bit -> AbsBit;          \n"
    "                                \n"
    "eqn  inv(e0)  =  e1;            \n"
    "     inv(e1)  =  e0;            \n"
    ;

  data_specification dataspec = parse_data_specification(DATASPEC);

  data::function_symbol f;
  f = parse_function_symbol("abseq : AbsBit # AbsBit -> Set(Bool)", DATASPEC);
  dataspec.add_mapping(f);
  f = parse_function_symbol("absinv : AbsBit -> Set(AbsBit)", DATASPEC);
  dataspec.add_mapping(f);

  data_equation_vector equations = dataspec.user_defined_equations();
  data::normalize_sorts(equations, dataspec);
}

// The test below checks whether the calculation of a confluent and terminating rewrite system for types using
// Knuth-Bendix completion is efficient. Aleksi Peltonen showed in the spring of 2018 that Knuth-Bendix completion
// was exponential, causing the example below not to terminate within reasonable time.
void test_apply_knuth_bendix_completion_on_sorts()
{
  std::string DATASPEC =
    "sort A_t = Nat; B_t = Nat; C_t = Nat; D_t = Nat; E_t = Nat; F_t = Nat; G_t = Nat; H_t = Nat; I_t = Nat; J_t=Nat; K_t=Nat; \n"
    "     L_t = Nat; M_t = Nat; N_t = Nat; O_t = Nat;\n"
    "     S_t = struct s( A:A_t, B:B_t, C:C_t, D:D_t, E:E_t, F:F_t, G:G_t, H:H_t, I:I_t, J:J_t, K:K_t, L:L_t, M:M_t, N:N_t, O:O_t); \n";

  data_specification dataspec = parse_data_specification(DATASPEC);

  data_equation_vector equations = dataspec.user_defined_equations();
  data::normalize_sorts(equations, dataspec);
}

int test_main(int argc, char* argv[])
{
  test_normalize_sorts();
  test_apply_knuth_bendix_completion_on_sorts();

  return 0;
}


//
// Created by haohanwang on 1/24/16.
//

#include "ProximalGradientDescent.hpp"
#include "BrentSearch.hpp"

#include <Eigen/Dense>
#include <iostream>
#include <map>

#ifdef BAZEL
#include "Models/AdaMultiLasso.hpp"
#include "Models/GFlasso.h"
#include "Models/LinearRegression.hpp"
#include "Models/Model.hpp"
#include "Models/MultiPopLasso.hpp"
#include "Models/TreeLasso.hpp"
#else
#include "../Models/AdaMultiLasso.hpp"
#include "../Models/GFlasso.h"
#include "../Models/LinearRegression.hpp"
#include "../Models/Model.hpp"
#include "../Models/MultiPopLasso.hpp"
#include "../Models/TreeLasso.hpp"
#endif

using namespace Eigen;
using namespace std;

ProximalGradientDescent::ProximalGradientDescent(const unordered_map<string, string>& opts) {
    try {
        tolerance = stod(opts.at("tolerance"));
    } catch(std::out_of_range& oor) {
        tolerance = default_tolerance;
    }
    try {
        learningRate = stod(opts.at("learning_rate"));
    } catch(std::out_of_range& oor) {
        learningRate = default_learning_rate;
    }
    try {
        learningRate2 = stod(opts.at("learning_rate2"));
    } catch(std::out_of_range& oor) {
        learningRate2 = default_learning_rate2;
    }
    try {
        innerStep1 = stoi(opts.at("innerStep1"));
    } catch(std::out_of_range& oor) {
        innerStep1 = default_inner_step1;
    }
    try {
        innerStep2 = stoi(opts.at("innerStep2"));
    } catch(std::out_of_range& oor) {
        innerStep2 = default_inner_step2;
    }
    prev_residue = numeric_limits<float>::max();
}

ProximalGradientDescent::ProximalGradientDescent() {
    learningRate = default_learning_rate;
    learningRate2 = default_learning_rate2;
    tolerance = default_tolerance;
    prev_residue = numeric_limits<float>::max();
    innerStep1 = default_inner_step1;
    innerStep2 = default_inner_step2;
}

/*void ProximalGradientDescent::stop() {
    shouldStop = true;
}*/

void ProximalGradientDescent::setTolerance(float tol) {
    tolerance = tol;
}

void ProximalGradientDescent::setLearningRate2(float d) {
    learningRate2 = d;
}

void ProximalGradientDescent::setPrevResidule(float d) {
    prev_residue = d;
}

void ProximalGradientDescent::setInnerStep1(long d) {
    innerStep1 = d;
}

void ProximalGradientDescent::setInnerStep2(long d) {
    innerStep2 =d;
}

void ProximalGradientDescent::assertReadyToRun() {
    return;    // there is no data that cannot be inferred
}

void ProximalGradientDescent::setUpRun() {
    mtx.lock();
    isRunning = true;
    progress = 0.01;
    shouldStop = false;
}

void ProximalGradientDescent::finishRun() {
    isRunning = false;
    progress = 1.0;
    mtx.unlock();
}

void ProximalGradientDescent::setLearningRate(float lr) {
    learningRate = lr;
}

void ProximalGradientDescent::run(shared_ptr<Model> model) {
    cerr << "The algorithm for this specific model is not implemented, runs on basic model"<<endl;
    int epoch = 0;
    float residue = model->cost();
    VectorXf grad;
    VectorXf in;
    while (!shouldStop && epoch < maxIteration && residue > tolerance) {
        epoch++;
        progress = float(epoch) / maxIteration;
        grad = model->proximal_derivative();
        in = model->getBeta() - learningRate * grad;
        model->updateBeta(model->proximal_operator(in, learningRate));
        residue = model->cost();
    }
}

void ProximalGradientDescent::run(shared_ptr<Gflasso> model) {
    model->initBeta();
    learningRate = learningRate*2e6;
    int epoch = 0;
    float residue = model->cost();
    float theta = 1;
    float theta_new = 0;
    MatrixXf beta_prev = model->get_beta(); //bx
    MatrixXf beta_curr = model->get_beta(); //bx_new
    MatrixXf beta = model->get_beta();  //bw
    MatrixXf best_beta = model->get_beta();
    MatrixXf in;
    MatrixXf grad;
    float diff = tolerance*2;
    prev_residue= 9999999;
    while (!shouldStop && epoch < maxIteration && diff > tolerance) {
        epoch++;
        progress = float(epoch) / maxIteration;
        theta_new = 2.0/(epoch+3.0);
        grad = model->gradient();
        in = beta - 1/model->getL() * grad;
        beta_curr = model->proximal_operator(in, learningRate);
        beta = beta_curr + (1-theta)/theta * theta_new * (beta_curr-beta_prev);
        beta_prev = beta_curr;
        theta = theta_new;
        model->updateBeta(beta);
        residue = model->cost();
        diff = abs(prev_residue - residue);
        if (residue < prev_residue){
            best_beta = beta;
            prev_residue = residue;
        }
    }
    model->updateBeta(best_beta);
}

void ProximalGradientDescent::run(shared_ptr<LinearRegression> model) {
    int epoch = 0;
    MatrixXf y = model->getY();
    model->setL1_reg(model->getL1_reg()*10);
    long s = y.cols();
    for (long i=0; i<s; i++){
        if (shouldStop) {
            break;
        }
        model->setY(y.col(i));
        model->initBeta();
        float residue = model->cost();
        VectorXf grad;
        VectorXf in;
        epoch = 0;
        while (!shouldStop && epoch < maxIteration && residue > tolerance && !shouldStop) {
            epoch++;
            progress = (float(epoch) + i*maxIteration )/(maxIteration*s);
            grad = model->proximal_derivative();
            in = model->getBeta() - learningRate * grad;
            model->updateBeta(model->proximal_operator(in, learningRate));
            residue = model->cost();
        }
        model->updateBetaAll(model->getBeta());
    }
    model->updateBeta(model->getBetaAll());
}

void ProximalGradientDescent::run(shared_ptr<TreeLasso> model) {
    model->initBeta();
    model->hierarchicalClustering();
    learningRate = learningRate*1e5;
    int epoch = 0;
    float residue = model->cost();
    float theta = 1;
    float theta_new = 0;
    MatrixXf beta_prev = model->getBeta(); //bx
    MatrixXf beta_curr = model->getBeta(); //bx_new
    MatrixXf beta = model->getBeta();  //bw
    MatrixXf best_beta = model->getBeta();
    MatrixXf in;
    MatrixXf grad;
    model->initGradientUpdate();
    float diff = tolerance*2;
    while (!shouldStop && epoch < maxIteration && diff > tolerance) {
        epoch++;
        progress = float(epoch) / maxIteration;

        theta_new = 2.0/(epoch+2);

        grad = model->proximal_derivative();

        in = beta - 1/model->getL() * grad;
        beta_curr = model->proximal_operator(in, learningRate);
        beta = beta_curr + (1-theta)/theta * theta_new * (beta_curr-beta_prev);

        beta_prev = beta_curr;
        theta = theta_new;
        model->updateBeta(beta);
        residue = model->cost();
        if (residue < prev_residue){
            best_beta = beta;
        }
        prev_residue = residue;
        diff = abs(prev_residue - residue);
    }
    model->updateBeta(best_beta);
}

void ProximalGradientDescent::run(shared_ptr<MultiPopLasso> model) {
    MatrixXf X = model->getX();
    MatrixXf y = model->getY();
    int epoch = 0;
    long s = y.cols();
    for (long i=0; i<y.cols(); i++) {
        if (shouldStop) {
            break;
        }
        model->reSetFlag();
        model->setXY(X, y.col(i));
        model->initTraining();
        epoch = 0;
        float residue = model->cost();
        float theta = 1;
        float theta_new = 0;
        MatrixXf beta_prev = model->getFormattedBeta(); //bx
        MatrixXf beta_curr = model->getFormattedBeta(); //bx_new
        MatrixXf beta = model->getFormattedBeta();  //bw
        MatrixXf best_beta = model->getFormattedBeta();
        MatrixXf in;
        MatrixXf grad;
        float diff = tolerance * 2;
        while (!shouldStop && epoch < maxIteration && diff > tolerance) {
            epoch++;
            progress = (float(epoch) + i*maxIteration ) / (maxIteration*s);
            theta_new = 2.0 / (epoch + 2);
            grad = model->proximal_derivative();
            in = beta - 1 / model->getL() * grad;
            beta_curr = model->proximal_operator(in, learningRate);
            beta = beta_curr + (1 - theta) / theta * theta_new * (beta_curr - beta_prev);

            beta_prev = beta_curr;
            theta = theta_new;
            model->updateBeta(beta);
            residue = model->cost();
            if (residue < prev_residue) {
                best_beta = beta;
            }
            diff = abs(prev_residue - residue);
        }
        model->updateBeta(best_beta);
        model->updateBetaAll();
    }
//    model->updateBeta(model->getBetaAll());
}

void ProximalGradientDescent::run(shared_ptr<AdaMultiLasso> model) {
    // this is not just proximal gradient descent, also including iteratively updating beta and w, v
    model->initBeta();
    model->initTraining();
    int epoch = 0;
    float residue = model->cost();
    float theta = 1;
    float theta_new = 0;
    MatrixXf beta_prev = model->getFormattedBeta(); //bx
    MatrixXf beta_curr = model->getFormattedBeta(); //bx_new
    MatrixXf beta = model->getFormattedBeta();  //bw
    MatrixXf best_beta = model->getFormattedBeta();
    MatrixXf beta_prev2 = model->getFormattedBeta();
    MatrixXf in;
    MatrixXf grad;
    float diff = tolerance*2;
    float lr2 = 0;
    long i1 = 0;
    long i2 = 0;
    VectorXf w_update = model->getW();
    VectorXf v_update = model->getV();
    VectorXf w_prev = model->getW();
    VectorXf v_prev = model->getV();
    VectorXf w_grad = model->getW();
    VectorXf v_grad = model->getV();
    while (!shouldStop && epoch < maxIteration && diff > tolerance) {
        i1 = 0;
        i2 = 0;
        epoch++;
        while (!shouldStop && i1 < innerStep1){
            i1 ++ ;
            beta_prev2 = model->getFormattedBeta();
            progress = float(epoch) / maxIteration;
            theta_new = 2.0/(epoch+2);
            grad = model->proximal_derivative();
            in = beta - 1/model->getL() * grad;
            beta_curr = model->proximal_operator(in, learningRate);
            beta = beta_curr + (1-theta)/theta * theta_new * (beta_curr-beta_prev);
            beta_prev = beta_curr;
            theta = theta_new;
            model->updateBeta(beta);
        }
        while (!shouldStop && i2 < innerStep2){
            i2 ++ ;
            lr2 = learningRate2 / sqrt(i2);
            w_prev = model->getW();
            v_prev = model->getV();
            w_grad = model->gradient_w();
            v_grad = model->gradient_v();
            w_update = w_prev - lr2*w_grad;
            v_update = v_prev - lr2*v_grad;
            w_update = model->projection(w_update);
            v_update = model->projection(v_update);
            model->updateW(w_update);
            model->updateV(v_update);
            model->updateTheta_Rho();
        }
        residue = model->cost();
        if (residue < prev_residue){
            best_beta = beta;
        }
        diff = abs(prev_residue - residue);
        //cerr << "epoch: " << epoch << "\tresidue: " << residue << "\ndiff: " << diff << endl;
        prev_residue = residue;
//        cout << "--------"<<endl;
//        cout << beta << endl;
//        cout << "--------"<<endl;
    }
    model->updateBeta(best_beta);
}

bool ProximalGradientDescent::checkVectorConvergence(VectorXf v1, VectorXf v2, float d) {
    float r = (v1 - v2).squaredNorm();
    return (r < d);
}

void ProximalGradientDescent::run(shared_ptr<SparseLMM> model) {
    BrentSearch *brentSearch = new BrentSearch();
    brentSearch->set_delta(0.5);
    brentSearch->sub_run(model);
    float delta = model->get_lambda();
    model->rotateXY(delta);
    shared_ptr<LinearRegression> lr = shared_ptr<LinearRegression>(new LinearRegression());
    lr->setL1_reg(model->getL1reg());
    lr->setX(model->getRotatedX());
    lr->setY(model->getRoattedY());
    run(lr);
    MatrixXf tmp = lr->getBeta();
    model->updateBeta(tmp);
}


//==================================================================================================
/*!
  @file

  @copyright 2016 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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_TOINT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_TOINT_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/modf.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(toint_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::uint_<A0>, X>
                          )
   {
      using result = bd::as_integer_t<A0, signed> ;
      BOOST_FORCEINLINE result operator()(A0 const& a0) const
      {
        return bitwise_cast<result>(a0);
      }
   };

   BOOST_DISPATCH_OVERLOAD(toint_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::int_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(A0 const& a0) const
     {
        return a0;
     }
   };

  BOOST_DISPATCH_OVERLOAD_IF(toint_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pedantic_tag
                            , bs::pack_<bd::double_<A0>, X>
                          )
   {
     using result = bd::as_integer_t<A0, signed> ;
     BOOST_FORCEINLINE result operator()(pedantic_tag const &
                                        , A0 const& a0) const
     {
       const int32_t Vim32 = Valmin<int32_t>();
       const result Vim =  result(Vim32);
       const result Vax = -result(Vim32);
       constexpr int S = 32;
       A0 z = ldexp(a0, -S);
       A0 t1, t2;
       std::tie(t1, t2) =  modf(z);
       t1 = ldexp(t1, S); // abs(t1) < 2^32
       result z1 = toint(t1)+ ldexp(toint(t2), S);
       return if_else(logical_and(z1 == Vim, is_gtz(a0)), Vax, z1);
     }
   };

   BOOST_DISPATCH_OVERLOAD(toint_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pedantic_tag
                          , bs::pack_<bd::single_<A0>, X>
                          )
   {
     using result = bd::as_integer_t<A0, signed> ;
     BOOST_FORCEINLINE result operator()(pedantic_tag const &
                                        , A0 const& a0) const
     {
       return toint(a0);
     }
   };

} } }

#include <boost/simd/arch/common/simd/function/toint_s.hpp>

#endif


#include <boost/multiprecision/cpp_int.hpp>
#include <graphene/app/balance.hpp>
#include <graphene/app/api.hpp>

namespace graphene { namespace app {

namespace detail {

    void split_payout_to_coauthors(chain::asset paid_price, chain::account_id_type author,
                                   const std::map<chain::account_id_type, uint32_t> &co_authors,
                                   boost::container::flat_map<chain::account_id_type, chain::asset> &result) {
        if (co_authors.empty())
            result.insert(std::make_pair(author, paid_price));
        else {
            chain::asset price = paid_price;
            boost::multiprecision::int128_t price_for_co_author;
            for (auto const &element : co_authors) {
                price_for_co_author = (paid_price.amount.value * element.second) / 10000ll;
                result.insert(std::make_pair(element.first, chain::asset(static_cast<chain::share_type>(price_for_co_author), price.asset_id)));
                price.amount -= price_for_co_author;
            }

            if (price.amount != 0) {
                FC_ASSERT(price.amount > 0);
                auto find = result.find(author);
                if (find != result.end()) {
                    find->second += price;
                } else {
                    result.insert(std::make_pair(author, price));
                }
            }
        }
    };

} //namespace

struct get_balance_history_visitor
{
   chain::account_id_type _account;
   asset_array& _balance;
   chain::asset& _fee;

   get_balance_history_visitor( chain::account_id_type account, asset_array& balance, chain::asset& fee ) : _account(account), _balance(balance), _fee(fee) {}
   typedef void result_type;

   void operator()( const chain::transfer_obsolete_operation& op )
   {
       if (op.from == _account) {
           _balance.asset0 = chain::asset(-op.amount.amount, op.amount.asset_id);
           _balance.asset1 = chain::asset();
       }
       else {
           _balance.asset0 = op.amount;
           _balance.asset1 = chain::asset();
       }
       if (op.fee_payer() == _account)
           _fee = op.fee;
   }

   void operator()( const chain::transfer_operation& op )
   {
       if (op.from == _account) {
           _balance.asset0 = chain::asset(-op.amount.amount, op.amount.asset_id);
           _balance.asset1 = chain::asset();
       }

       if( op.to.is<chain::account_id_type>() && op.to.as<chain::account_id_type>() == _account) {
           _balance.asset0 = op.amount;
           _balance.asset1 = chain::asset();
       }
       if (op.fee_payer() == _account)
           _fee = op.fee;
   }

   void operator()( const chain::asset_issue_operation& op )
   {
       if (op.issue_to_account == _account) {
           _balance.asset0 = op.asset_to_issue;
           _balance.asset1 = chain::asset();
       }
       else {
           _balance.asset0 = _balance.asset1 = chain::asset();
       }
       if (op.fee_payer() == _account)
           _fee = op.fee;
   }

   void operator()( const chain::asset_fund_pools_operation& op )
   {
       if (op.from_account != _account) {
           _balance.asset0 = _balance.asset1 = chain::asset();
           return;
       }

       if( op.uia_asset.amount > 0) {
           _balance.asset0 = chain::asset(-op.uia_asset.amount, op.uia_asset.asset_id);
       }
       if( op.dct_asset.amount > 0) {
           _balance.asset1 = chain::asset(-op.dct_asset.amount, op.dct_asset.asset_id);
       }
       if (op.fee_payer() == _account)
           _fee = op.fee;
   }

   void operator()( const chain::asset_reserve_operation& op )
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;

       if (op.payer != _account) {
           _balance.asset0 = _balance.asset1 = chain::asset();
           return;
       }

       _balance.asset0 = chain::asset(-op.amount_to_reserve.amount, op.amount_to_reserve.asset_id);
       _balance.asset1 = chain::asset();
   }

   void operator()( const chain::asset_claim_fees_operation& op )
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;

       if (op.issuer != _account) {
           _balance.asset0 = _balance.asset1 = chain::asset();
           return;
       }

       if( op.uia_asset.amount > 0) {
           _balance.asset0 = chain::asset(op.uia_asset.amount, op.uia_asset.asset_id);
       }
       if( op.dct_asset.amount > 0) {
           _balance.asset1 = chain::asset(op.dct_asset.amount, op.dct_asset.asset_id);
       }
   }

   void operator()( const chain::vesting_balance_create_operation& op )
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;

       if (op.creator != _account) {
           _balance.asset0 = _balance.asset1 = chain::asset();
           return;
       }

       _balance.asset0 = chain::asset(-op.amount.amount, op.amount.asset_id);
       _balance.asset1 = chain::asset();
   }

   void operator()( const chain::vesting_balance_withdraw_operation& op )
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;

       if (op.owner != _account) {
           _balance.asset0 = _balance.asset1 = chain::asset();
           return;
       }

       _balance.asset0 = op.amount;
       _balance.asset1 = chain::asset();
   }

   void operator()( const chain::withdraw_permission_claim_operation& op )
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;

       if (op.withdraw_from_account == _account) {
           _balance.asset0 = chain::asset(-op.amount_to_withdraw.amount, op.amount_to_withdraw.asset_id);
           _balance.asset1 = chain::asset();
       }
       else if (op.withdraw_to_account == _account) {
           _balance.asset0 = op.amount_to_withdraw;
           _balance.asset1 = chain::asset();
       }
       else {
           _balance.asset0 = _balance.asset1 = chain::asset();
       }
   }

   void operator()( const chain::request_to_buy_operation& op)
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;

       if (op.consumer == _account) {
           _balance.asset0 = -op.price;
           _balance.asset1 = chain::asset();
       }
   }

   void operator()( const chain::return_escrow_buying_operation& op)
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;

       if (op.consumer == _account) {
           _balance.asset0 = op.escrow;
           _balance.asset1 = chain::asset();
       }
   }

   void operator()( const chain::return_escrow_submission_operation& op)
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;

       if (op.author == _account) {
           _balance.asset0 = op.escrow;
           _balance.asset1 = chain::asset();
       }
   }

   void operator()( const chain::content_submit_operation& op)
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;

       if (op.author == _account) {
           _balance.asset0 = -op.publishing_fee;
           _balance.asset1 = chain::asset();
       }
   }

   void operator()( const chain::subscribe_operation& op)
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;

       if (op.from == _account) {
           _balance.asset0 = chain::asset(-op.price.amount, op.price.asset_id);
           _balance.asset1 = chain::asset();
       }
       else if (op.to == _account) {
           _balance.asset0 = op.price;
           _balance.asset1 = chain::asset();
       }
   }

   void operator()( const chain::finish_buying_operation& op)
   {
       //NOTE: consumer balance is already changed in 'request_to_buy_operation'
//     if (fop.consumer == account) {
//        result.a0 = asset(-fop.payout.amount, fop.payout.asset_id);
//        result.a1 = asset();
//     }

       if (op.fee_payer() == _account)
           _fee = op.fee;

       if (op.author == _account && op.co_authors.empty()) {
           _balance.asset0 = op.payout;
           _balance.asset1 = chain::asset();
       }
       else {
           //calculate split to author and co-authors
           boost::container::flat_map<chain::account_id_type, chain::asset> co_authors_split;
           detail::split_payout_to_coauthors(op.payout, op.author, op.co_authors, co_authors_split);

           auto find = co_authors_split.find(_account);
           if (find != co_authors_split.end()) {
               _balance.asset0 = find->second;
               _balance.asset1 = chain::asset();
           }
       }
   }

   void operator()( const chain::pay_seeder_operation& op)
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;

//NOTE: author balance is changed in 'content_submit_operation'
//     if (psop.author == account) {
//         result.a0 = asset(-psop.payout.amount, psop.payout.asset_id);
//         result.a1 = asset();
//     }
       if (op.seeder == _account) {
           _balance.asset0 = op.payout;
           _balance.asset1 = chain::asset();
       }
   }

   void operator()( const chain::account_create_operation& op )
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;
   }

   void operator()( const chain::account_update_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::asset_create_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::non_fungible_token_create_definition_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::non_fungible_token_update_definition_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::non_fungible_token_issue_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::non_fungible_token_transfer_operation& op )
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;
   }

   void operator()( const chain::non_fungible_token_update_data_operation& op )
   {
       if (op.fee_payer() == _account)
           _fee = op.fee;
   }

   void operator()( const chain::update_monitored_asset_operation& op ) {} // 0 fee
   void operator()( const chain::update_user_issued_asset_operation& op ) {} // 0 fee

   void operator()( const chain::update_user_issued_asset_advanced_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::asset_publish_feed_operation& op)
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::miner_create_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::miner_update_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::miner_update_global_parameters_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::proposal_create_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::proposal_update_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::proposal_delete_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::withdraw_permission_create_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::withdraw_permission_update_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::withdraw_permission_delete_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::custom_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::assert_operation& op )
   {
      if (op.fee_payer() == _account)
         _fee = op.fee;
   }

   void operator()( const chain::set_publishing_manager_operation& op ) {} // 0 fee
   void operator()( const chain::set_publishing_right_operation& op ) {} // 0 fee
   void operator()( const chain::content_cancellation_operation& op) {} // 0 fee

   void operator()( const chain::leave_rating_and_comment_operation& op) {} // 0 fee
   void operator()( const chain::ready_to_publish_obsolete_operation& op) {} // 0 fee
   void operator()( const chain::ready_to_publish_operation& op) {} // 0 fee

   void operator()( const chain::proof_of_custody_operation& op) {} // 0 fee
   void operator()( const chain::deliver_keys_operation& op) {} // 0 fee

   void operator()( const chain::subscribe_by_author_operation& op) {} // 0 fee
   void operator()( const chain::automatic_renewal_of_subscription_operation& op) {} // 0 fee
   void operator()( const chain::disallow_automatic_renewal_of_subscription_operation& op) {} // 0 fee
   void operator()( const chain::renewal_of_subscription_operation& op) {} // 0 fee

   void operator()( const chain::report_stats_operation& op) {} // 0 fee

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

};

void operation_get_balance_history( const chain::operation& op, chain::account_id_type account, asset_array& result, chain::asset& fee_result )
{
   get_balance_history_visitor vtor = get_balance_history_visitor(account, result, fee_result );
   op.visit( vtor );
}

} } // graphene::app


#ifndef POLY_HPP
#define POLY_HPP

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include<vector>

using namespace std;

inline bool operator<(const Linepoint& lhs, const Linepoint& rhs)
{
	if(lhs.x != rhs.x)
		return lhs.x < rhs.x;
	else if (lhs.x == rhs.x)
		return lhs.y < rhs.y;

}

void getPolyFit(Line& line_objects, Mat& gray_IPM_image, vector<Linepoint>& x_y_points);
vector<double> polyfit(vector<Linepoint>& x_y_points, int degree);
void print_matrix(const boost::numeric::ublas::matrix<double> &m);
vector<double> getLinePoints(Line& line_obj);
void polyval(const std::vector<double>& oCoeff, Mat& gray_IPM_image, Line& line_obj) ;
void print_d_vector(const std::vector<double>& arr);
vector<Linepoint> getPoints(const vector<Linepoint>& x_y_points, Line& line_obj);
double getSlope(double a, double x, double b );

#endif


#include <iostream>                    // std::cout
#include <fstream>                     // std::ifstream, std::ofstream
#include <filesystem>                  // std::filesystem
#include <string>                      // std::string
#include <chrono>                      // std::chrono
#include <cmath>                       // std::isinf
#include <utility>                     // std::pair
// For External Library
#include <torch/torch.h>               // torch
#include <boost/program_options.hpp>   // boost::program_options
// For Original Header
#include "loss.hpp"                    // Loss
#include "networks.hpp"                // Encoder, Decoder, EstimationNetwork, RelativeEuclideanDistance, CosineSimilarity, load_params
#include "transforms.hpp"              // transforms_Compose
#include "datasets.hpp"                // datasets::ImageFolderWithPaths
#include "dataloader.hpp"              // DataLoader::ImageFolderWithPaths
#include "visualizer.hpp"              // visualizer

// Define Namespace
namespace fs = std::filesystem;
namespace po = boost::program_options;

// ---------------
// Test Function
// ---------------
void test(po::variables_map &vm, torch::Device &device, Encoder &enc, Decoder &dec, EstimationNetwork &est, std::vector<transforms_Compose> &transform){

    constexpr std::pair<float, float> output_range = {-1.0, 1.0};  // range of the value in output images

    // (0) Initialization and Declaration
    float ave_loss, ave_anomaly_score, score;
    double seconds, ave_time;
    std::string path, result_dir, fname;
    std::string dataroot;
    std::ofstream ofs, ofs_loss, ofs_score;
    std::chrono::system_clock::time_point start, end;
    std::tuple<torch::Tensor, std::vector<std::string>> data;
    torch::Tensor image, output;
    torch::Tensor z, z_c, z_r, z_r1, z_r2;
    torch::Tensor mu, sigma, phi;
    torch::Tensor loss, anomaly_score;
    datasets::ImageFolderWithPaths dataset;
    DataLoader::ImageFolderWithPaths dataloader;

    // (1) Get Test Dataset
    dataroot = "datasets/" + vm["dataset"].as<std::string>() + '/' + vm["test_dir"].as<std::string>();
    dataset = datasets::ImageFolderWithPaths(dataroot, transform);
    dataloader = DataLoader::ImageFolderWithPaths(dataset, /*batch_size_=*/1, /*shuffle_=*/false, /*num_workers_=*/0);
    std::cout << "total test images : " << dataset.size() << std::endl << std::endl;

    // (2) Get Model
    path = "checkpoints/" + vm["dataset"].as<std::string>() + "/models/epoch_" + vm["test_load_epoch"].as<std::string>() + "_gmp.dat"; load_params(path, mu, sigma, phi);
    path = "checkpoints/" + vm["dataset"].as<std::string>() + "/models/epoch_" + vm["test_load_epoch"].as<std::string>() + "_enc.pth"; torch::load(enc, path, device);
    path = "checkpoints/" + vm["dataset"].as<std::string>() + "/models/epoch_" + vm["test_load_epoch"].as<std::string>() + "_dec.pth"; torch::load(dec, path, device);
    path = "checkpoints/" + vm["dataset"].as<std::string>() + "/models/epoch_" + vm["test_load_epoch"].as<std::string>() + "_est.pth"; torch::load(est, path, device);
    mu = mu.to(device);
    sigma = sigma.to(device);
    phi = phi.to(device);

    // (3) Set Loss Function
    auto criterion = Loss(vm["loss"].as<std::string>());

    // (4) Initialization of Value
    ave_loss = 0.0;
    ave_anomaly_score = 0.0;
    ave_time = 0.0;

    // (5) Tensor Forward
    torch::NoGradGuard no_grad;
    enc->eval();
    dec->eval();
    est->eval();
    result_dir = vm["test_result_dir"].as<std::string>();  fs::create_directories(result_dir);
    ofs.open(result_dir + "/loss.txt", std::ios::out);
    ofs_loss.open(result_dir + "/reconstruction_error.txt", std::ios::out);
    ofs_score.open(result_dir + "/anomaly_score.txt", std::ios::out);
    while (dataloader(data)){

        image = std::get<0>(data).to(device);

        if (!device.is_cpu()) torch::cuda::synchronize();
        start = std::chrono::system_clock::now();

        // (5.1) Encoder-Decoder Forward
        z_c = enc->forward(image);   // {C,H,W} ===> {ZC,1,1}
        output = dec->forward(z_c);  // {ZC,1,1} ===> {C,H,W}

        // (5.2) Setting Latent Space
        z_c = z_c.view({z_c.size(0), z_c.size(1)});  // {ZC,1,1} ===> {ZC}
        if (vm["RED"].as<bool>()){
            z_r1 = RelativeEuclideanDistance(image, output);
        }
        else{
            z_r1 = AbsoluteEuclideanDistance(image, output);
        }
        z_r2 = CosineSimilarity(image, output);
        z_r = torch::cat({z_r1, z_r2}, /*dim=*/1);  // {1} + {1} ===> {ZR} = {2}
        z = torch::cat({z_c, z_r}, /*dim=*/1);  // {ZC} + {ZR} ===> {Z} = {ZC+ZR}

        // (5.3) Calculation of Anomaly Score
        anomaly_score = est->anomaly_score(z, mu, sigma, phi);

        if (!device.is_cpu()) torch::cuda::synchronize();
        end = std::chrono::system_clock::now();
        seconds = (double)std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() * 0.001 * 0.001;

        loss = criterion(output, image);

        ave_loss += loss.item<float>();
        score = anomaly_score.item<float>();
        score = (std::isinf(score) ? (float)(long)score : score);
        ave_anomaly_score += score;
        ave_time += seconds;

        std::cout << '<' << std::get<1>(data).at(0) << "> " << vm["loss"].as<std::string>() << ':' << loss.item<float>() << " anomaly_score:" << score << std::endl;
        ofs << '<' << std::get<1>(data).at(0) << "> " << vm["loss"].as<std::string>() << ':' << loss.item<float>() << " anomaly_score:" << score << std::endl;
        ofs_loss << loss.item<float>() << std::endl;
        ofs_score << score << std::endl;

        fname = result_dir + '/' + std::get<1>(data).at(0);
        visualizer::save_image(output.detach(), fname, /*range=*/output_range, /*cols=*/1, /*padding=*/0);

    }

    // (6) Calculate Average
    ave_loss = ave_loss / (float)dataset.size();
    ave_anomaly_score = ave_anomaly_score / (float)dataset.size();
    ave_time = ave_time / (double)dataset.size();

    // (7) Average Output
    std::cout << "<All> " << vm["loss"].as<std::string>() << ':' << ave_loss << " anomaly_score:" << ave_anomaly_score << " (time:" << ave_time << ')' << std::endl;
    ofs << "<All> " << vm["loss"].as<std::string>() << ':' << ave_loss << " anomaly_score:" << ave_anomaly_score << " (time:" << ave_time << ')' << std::endl;

    // Post Processing
    ofs.close();
    ofs_loss.close();
    ofs_score.close();

    // End Processing
    return;

}


#include "dSFMTsearch.hpp"
#include "MTToolBox/period.hpp"
#include "Annihilate.h"
#include <NTL/GF2X.h>
#include <errno.h>
#include <stdlib.h>
#include <limits.h>
using namespace MTToolBox;
using namespace std;

void min_max_poly(int &min, int& max, dSFMT& sf);
int main(int argc, char * argv[])
{
    if (argc <= 1) {
        cout << argv[0] << "\n mexp,pos1,sl1,msk1,msk2,fix1,fix2,"
             << "parity1,parity2" << endl;
        return 1;
    }
    dSFMT_param params;
    char * para = argv[1];
    params.mexp = strtoul(para, &para, 10);
    para++;
    params.pos1 = strtoul(para, &para, 10);
    para++;
    params.sl1 = strtoul(para, &para, 10);
    para++;
    params.msk1 = strtoull(para, &para, 16);
    para++;
    params.msk2 = strtoull(para, &para, 16);
    para++;
    params.fix1 = strtoull(para, &para, 16);
    para++;
    params.fix2 = strtoull(para, &para, 16);
    para++;
    params.parity1 = strtoull(para, &para, 16);
    para++;
    params.parity2 = strtoull(para, &para, 16);
    dSFMT sf(params);
    cout << sf.getParamString() << endl;
    int se = 1;
    w128_t seedw;
    int min = INT_MAX;
    int max = -10;
    seedw.u[0] = se;
    sf.seed(seedw);
    min_max_poly(min, max, sf);
    cout << "berore anni ";
    cout << " min = " << dec << min;
    cout << " max = " << dec << max << endl;
    for (int i = 0; i < 2; i++) {
        for (int j = 2; j >= 1; j--) {
            GF2X poly;
            dSFMT sf2 = sf;
            sf2.setStartMode(i);
            sf2.setWeightMode(j);
            if (!anni(sf2)) {
                cout << "annihilate fail" << endl;
                return -1;
            }
            min_max_poly(min, max, sf2);
            cout << "start_mode = " << dec << i;
            cout << " weight_mode = " << dec << j;
            cout << " min = " << dec << min;
            cout << " max = " << dec << max << endl;
        }
    }
    return 0;
}

void min_max_poly(int &min, int&max, dSFMT& sf)
{
    GF2X poly;
    min = INT_MAX;
    max = -10;
    for (int i = 0; i < 52; i++) {
        minpoly<w128_t>(poly, sf, i);
        int d = deg(poly);
        if (d < min) {
            min = d;
        }
        if (d > max) {
            max = d;
        }
    }
    for (int i = 64; i < 104; i++) {
        minpoly<w128_t>(poly, sf, i);
        int d = deg(poly);
        if (d < min) {
            min = d;
        }
        if (d > max) {
            max = d;
        }
    }
}


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::map_pdf::product_pdf::log_unnomralized_pdf.hpp     //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_PRODUCT_PDF_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_PRODUCT_PDF_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <boost/mpl/not.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/statistics/detail/distribution_toolkit/map_pdf/product_pdf/product_pdf.hpp>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{

    template<typename A,typename B,typename T>
    T log_unnormalized_pdf(const product_pdf<A,B>& dist,const T& x)
    {
        return
            (log_unnormalized_pdf(dist.first(),x)
                + log_unnormalized_pdf(dist.second(),x));
    }

}// toolkit
}// distribution
}// detail
}// statistics
}// boost

#endif


#include <chrono>
#include <boost\random\mersenne_twister.hpp>
#include <boost\random\uniform_int_distribution.hpp>

#include "ExerciseHelpers.hpp"

namespace Aid { namespace Utils { namespace ExerciseHelpers {

static boost::random::mt19937_64 g_generator(std::chrono::high_resolution_clock::now().time_since_epoch().count());

typedef std::vector<std::wstring> MessageVector;
static const MessageVector g_tryAgainMessages =
{
    L"Let's try that again, shall we?",
    L"Too bad, try again :(",
    L"\"If at first you don't succeed, get a bigger hammer.\" (- Alan Lewis)",
    L"Looks like you should debug it.",
    L"The answer here is probably not 42 (but it may be).",
    L"If \"Plan A\" didn't work, the alphabet has 25 more letters! Stay cool.",
    L"There are no bugs in this program.",
    L"There may be bugs in this program."
};

void VerifyLoop(std::function<bool()> predicate,
                std::function<void()> break_func)
{
    for (;;)
    {
        break_func();

        try
        {
            if (predicate())
            {
                break;
            }
        }
        catch (const RetryExerciseStepException &)
        {
        }

        auto messageIndex = Rand(0, g_tryAgainMessages.size() - 1);
        auto message = g_tryAgainMessages.at(static_cast<MessageVector::size_type>(messageIndex));
        std::wcout << message << std::endl;
    }
}

void DebugBreak()
{
    __debugbreak();
}

void NoBreak()
{
    // Don't break, as it will be implemented inside
    // the predicate.
    return;
}

std::uint64_t Rand()
{
    return g_generator();
}

std::uint64_t Rand(std::uint64_t range_start, std::uint64_t range_end)
{
    boost::random::uniform_int_distribution<std::uint64_t> distribution(range_start, range_end);

    return distribution(g_generator);
}

uint64_t ReadNumberHex()
{
    // Read a line from input
    std::wcout << L"> ";
    std::wstring user_input;
    std::getline(std::wcin, user_input);

    // Construct a stream
    std::wstringstream stream(user_input);

    // Obtain the value
    uint64_t target;
    stream >> std::hex >> target;

    // Abort if extraction failed
    if (!stream)
    {
        BOOST_THROW_EXCEPTION(RetryExerciseStepException());
    }

    return target;
}

} } }


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

#include <nt2/bessel/functions/yni.hpp>
#include <nt2/include/functions/scalar/rec.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/oneminus.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/cos.hpp>
#include <nt2/include/functions/scalar/cospi.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/is_inf.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/y0.hpp>
#include <nt2/include/functions/scalar/y1.hpp>
#include <nt2/include/constants/digits.hpp>
#include <nt2/include/constants/real.hpp>
#include <boost/simd/sdk/math.hpp>
#include <boost/simd/sdk/config.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A1 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( yni_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< integer_<A0> >)(scalar_< arithmetic_<A1> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0,A1>::type result_type;
    NT2_FUNCTOR_CALL(2)
    {
      return nt2::yni(a0, result_type(a1));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A1 is double
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( yni_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< integer_<A0> >)(scalar_< double_<A1> >)
                            )
  {
    typedef A1 result_type;
    NT2_FUNCTOR_CALL(2)
    {
#ifndef BOOST_SIMD_NO_INVALIDS
      if (is_nan(a1)) return Nan<result_type>();
      if (is_inf(a1)) return Zero<result_type>();
#endif
      if (is_eqz(a1)) return Minf<result_type>();
    #ifdef BOOST_SIMD_HAS__YN
      return ::_yn(a0, a1);
    #elif defined(BOOST_SIMD_HAS_YN)
      return ::yn(a0, a1);
    #else
      #error yn not supported
    #endif
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A1 is float
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( yni_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< integer_<A0> >)(scalar_< single_<A1> >)
                            )
  {

    typedef A1 result_type;

    NT2_FUNCTOR_CALL(2)
    {
      if (is_ltz(a1)) return Nan<result_type>();
#ifndef BOOST_SIMD_NO_INVALIDS
      if (is_nan(a1)) return Nan<result_type>();
      if (is_inf(a1)) return Zero<result_type>();
#endif
      if (is_eqz(a1)) return Minf<result_type>();
      result_type x = a1;
      const size_t n1 = nt2::abs(a0);
      result_type sign = (a0<0)?nt2::cospi(n1):1;
      if( n1 == 0u ) return( sign * nt2::y0(x) );
      if( n1 == 1u ) return( sign * nt2::y1(x) );
      //      result_type an1 = n1;
      /* forward recurrence on n */

      result_type anm2 = nt2::y0(x);
      result_type anm1 = nt2::y1(x);
      size_t k = 1;
      result_type r = result_type(k << 1);
      result_type xinv = rec(x);
      result_type an;
      do
      {
        an = r*anm1*xinv-anm2;
        anm2 = anm1;
        anm1 = an;
        r +=Two<result_type>();
        ++k;
      }
      while( k < n1 );
      return sign*an;
      //     return ::ynf(a0, a1);
    }
  };
} }

#endif