/*
	Copyright (C) 2003-2013 by David White <davewx7@gmail.com>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <boost/array.hpp>
#include <boost/cstdint.hpp>
#include <iostream>
#include <cassert>

#include "asserts.hpp"
#include "foreach.hpp"
#include "json_parser.hpp"
#include "string_utils.hpp"
#include "surface_cache.hpp"
#include "surface.hpp"
#include "unit_test.hpp"
#include "variant.hpp"

namespace graphics {

uint32_t interpolate_pixels (uint32_t sourcePixelOne, uint32_t sourcePixelTwo){
	//Take the average of the colors in two pixels.  Each channel needs to be mixed separately.

	uint32_t resultPixel;
	uint8_t* result_pixel_color_channel = reinterpret_cast<uint8_t*>(&resultPixel);
	uint8_t* pixel_one_channel = reinterpret_cast<uint8_t*>(&sourcePixelOne);
	uint8_t* pixel_two_channel = reinterpret_cast<uint8_t*>(&sourcePixelTwo);

	for ( int color = 0; color < 4; ++color) {
		if(color != 3){
			result_pixel_color_channel[color] = (pixel_one_channel[color] + pixel_two_channel[color]) / 2;
		}else{
			//special handling for alpha channel
			result_pixel_color_channel[color] =  std::min(pixel_one_channel[color], pixel_two_channel[color]);
		}
	}
	return resultPixel;
}

uint32_t interpolate_pixels (uint32_t sourcePixelOne, uint32_t sourcePixelTwo, uint32_t sourcePixelThree, uint32_t sourcePixelFour){
	//Same as above, but with four pixels instead of two.  The implementation was unnecessarily made different, solely for the programmer's edification.

	union PixelUnion {
		uint32_t value;
		uint8_t rgba[4];
	};

	PixelUnion resultPixel, pixel_one, pixel_two, pixel_three, pixel_four;
	pixel_one.value = sourcePixelOne;
	pixel_two.value = sourcePixelTwo;
	pixel_three.value = sourcePixelThree;
	pixel_four.value = sourcePixelFour;

	for ( int color = 0; color < 4; ++color) {
		if(color != 3){
			resultPixel.rgba[color] = (pixel_one.rgba[color] + pixel_two.rgba[color] + pixel_three.rgba[color] + pixel_four.rgba[color]) / 4;
		}else{
			//special handling for alpha channel
			resultPixel.rgba[color] =  std::min(std::min(pixel_one.rgba[color], pixel_two.rgba[color]), std::min(pixel_three.rgba[color], pixel_four.rgba[color]));
		}
	}
	return resultPixel.value;
}

int calculate_difference(uint32_t sourcePixelOne, uint32_t sourcePixelTwo, uint32_t sourcePixelThree, uint32_t sourcePixelFour){
	//determines which of pixels one and two, is more different from pixels three and four
	// return values are between -1 and 1; positive values signify pixel one being more different, and vice versa
	return ((sourcePixelOne != sourcePixelThree || sourcePixelOne != sourcePixelFour) - (sourcePixelTwo != sourcePixelThree || sourcePixelTwo != sourcePixelFour));
}

surface scale_surface_eagle(surface input) {
	surface result(surface::create(input->w*2, input->h*2));

	const uint32_t* in = reinterpret_cast<const uint32_t*>(input->pixels);
	uint32_t* out = reinterpret_cast<uint32_t*>(result->pixels);
	for(int y = 0; y != input->h; ++y) {
		for(int x = 0; x != input->w; ++x) {
			//do nearest neighbor interpolation
			out[(y*2)*result->w + x*2] = in[y*input->w + x];
			out[(y*2 + 1)*result->w + x*2] = in[y*input->w + x];
			out[(y*2 + 1)*result->w + x*2 + 1] = in[y*input->w + x];
			out[(y*2)*result->w + x*2 + 1] = in[y*input->w + x];

			if ((y > 0) && (y < input->h - 1) && (x > 0) && (x < input->w - 1)){
				//do additional eagle interpolation

				// Eagle is a pixel art scaling algorithm designed to smooth rough edges.  It works as follows:  First, it doubles the scale of the art, turning every source pixel into four destination pixels, just like nearest neighbor scaling would.

				// Then, to smooth things out, it conditionally alters each of these four pixels.  Each of the four destination pixels represents a quadrant of the source pixel, and likewise a direction pointing away from the center of the source pixel.  To choose if it 'smoothes an edge" in a given direction, it checks additional, adjacent source pixels in the direction represented by the quadrant.  If they're all the same color, it represents a diagonal slope of pixels, and the given quadrant pixel is filled in with the same color to smooth out the diagonal.

				// The following diagram illustrates first the scaling of a single input pixel into 4 output pixels (left side of diagram), and then the choices made to choose which color the output pixels are given (right side of diagram)

				//   first:        |Then
				//   . . . --\ CC  |S T U  --\ 1 2
				//   . C . --/ CC  |V C W  --/ 3 4
				//   . . .         |X Y Z
				//                 | IF V==S==T => 1=S
				//                 | IF T==U==W => 2=U
				//                 | IF V==X==Y => 3=X
				//                 | IF W==Z==Y => 4=Z

				const int index_up_left = y*input->w + x - input->w - 1;
				 const int index_up = y*input->w + x - input->w;
				 const int index_up_right = y*input->w + x - input->w + 1;
				 const int index_right = y*input->w + x + 1;
				 const int index_down_right = y*input->w + x + input->w + 1;
				 const int index_down = y*input->w + x + input->w;
				 const int index_down_left = y*input->w + x + input->w - 1;
				 const int index_left = y*input->w + x - 1;
				 const int max_index = input->w*input->h;

				 assert(index_down_left < max_index);
				 assert(index_down_right < max_index);
				 assert(index_down < max_index);

				 if ( (in[index_up_left] == in[index_up]) && (in[index_up_left] == in[index_left]) ) {
				 out[(y*2)*result->w + x*2] = in[index_up_left];
				 }
				 if ( (in[index_up_right] == in[index_up]) && (in[index_up_right] == in[index_right]) ) {
				 out[(y*2)*result->w + x*2 + 1] = in[index_up_right];
				 }
				 if ( (in[index_down_left] == in[index_down]) && (in[index_down_left] == in[index_left]) ) {
				 out[(y*2 + 1)*result->w + x*2] = in[index_down_left];
				 }
				 if ( (in[index_down_right] == in[index_down]) && (in[index_down_right] == in[index_right]) ) {
				 out[(y*2 + 1)*result->w + x*2 + 1] = in[index_down_right];
				 }
			}
		}
	}

	return result;
}

surface scale_surface_v1(surface input) {
	surface result(surface::create(input->w*2, input->h*2));

	const uint32_t* in = reinterpret_cast<const uint32_t*>(input->pixels);
	uint32_t* out = reinterpret_cast<uint32_t*>(result->pixels);
	for(int y = 0; y != input->h; ++y) {
		for(int x = 0; x != input->w; ++x) {
			//do nearest neighbor interpolation
			out[(y*2)*result->w + x*2] = in[y*input->w + x];
			out[(y*2 + 1)*result->w + x*2] = in[y*input->w + x];
			out[(y*2 + 1)*result->w + x*2 + 1] = in[y*input->w + x];
			out[(y*2)*result->w + x*2 + 1] = in[y*input->w + x];

			if ((y > 0) && (y < input->h - 2) && (x > 0) && (x < input->w - 2)){

				//do additional 2xSaI interpolation

				//  2xSai works on a square group of sixteen pixels, rather than the square group of nine that Eagle works on.  In Eagle, the current pixel being upsized is the one in the middle of this square of nine; in 2xSai, the current pixel is the one in the upper-left of the middle four.  The other pixels in this group are all input pixels that are being examined to determine if we have an edge to smooth out.

				//  X X X X  |  0  1  2  3
				//  X * X X  |  4  5  6  7
				//  X X X X  |  8  9  10 11
				//  X X X X  |  12 13 14 15
				const int px[4][4] = {{	//[x][y]
					y*input->w + x - input->w - 1,
					y*input->w + x - input->w,
					y*input->w + x - input->w + 1,
				y*input->w + x - input->w + 2},
					{y*input->w + x - 1,
						y*input->w + x,
						y*input->w + x + 1,
					y*input->w + x + 2},
					{y*input->w + x + input->w - 1,
						y*input->w + x + input->w,
						y*input->w + x + input->w + 1,
					y*input->w + x + input->w + 2},
					{y*input->w + x + 2*input->w - 1,
						y*input->w + x + 2*input->w,
						y*input->w + x + 2*input->w + 1,
					y*input->w + x + 2*input->w + 2}};

				//these are the four output pixels corresponding to the one input pixel
				const int upper_left = (y*2)*result->w + x*2;
				const int upper_right = (y*2)*result->w + x*2 + 1;
				const int lower_left = (y*2 + 1)*result->w + x*2;
				const int lower_right = (y*2 + 1)*result->w + x*2 + 1;

				//make sure we're not going out-of-bounds
				const int max_index = input->w*input->h;
				assert(px[0][2] < max_index);
				assert(px[0][3] < max_index);
				assert(px[1][2] < max_index);
				assert(px[1][3] < max_index);
				assert(px[2][0] < max_index);
				assert(px[2][1] < max_index);
				assert(px[2][2] < max_index);
				assert(px[2][3] < max_index);
				assert(px[3][0] < max_index);
				assert(px[3][1] < max_index);
				assert(px[3][2] < max_index);
				assert(px[3][3] < max_index);

				if ( (in[px[1][1]] == in[px[2][2]]) && (in[px[1][2]] != in[px[2][1]]) ) {
					if ( ((in[px[1][1]] == in[px[0][1]]) && (in[px[1][2]] == in[px[2][3]])) || ((in[px[1][1]] == in[px[2][1]]) && (in[px[1][1]] == in[px[0][2]]) && (in[px[1][2]] != in[px[0][1]]) && (in[px[1][2]] == in[px[0][3]]))){
						out[upper_right] = in[px[1][1]];
					}else{
						if( ! ((in[px[1][1]] == in[px[0][1]])) ){
							out[upper_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]]);
						}
					}

					if ( ((in[px[1][1]] == in[px[1][0]]) && (in[px[2][1]] == in[px[3][2]])) || ((in[px[1][1]] == in[px[1][2]]) && (in[px[1][1]] == in[px[2][0]]) && (in[px[1][0]] != in[px[2][1]]) && (in[px[2][1]] == in[px[3][0]]))){
						out[lower_left] = in[px[1][1]];
					}else{
						if( ! ((in[px[1][1]] == in[px[1][0]])) ){
							out[lower_left] = interpolate_pixels(in[px[1][1]],in[px[2][1]]);
						}
					}
					out[lower_right] = in[px[1][1]];
				} else if ( (in[px[1][2]] == in[px[2][1]]) && (in[px[1][1]] != in[px[2][2]]) ) {
					if ( ((in[px[1][2]] == in[px[0][2]]) && (in[px[1][1]] == in[px[2][0]])) || ((in[px[1][2]] == in[px[0][1]]) && (in[px[1][2]] == in[px[2][2]]) && (in[px[1][2]] != in[px[0][2]]) && (in[px[1][1]] == in[px[0][0]]))){
						out[upper_right] = in[px[1][2]];
					}else{
						out[upper_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]]);
					}

					if ( ((in[px[2][1]] == in[px[2][0]]) && (in[px[1][1]] == in[px[0][2]])) || ((in[px[2][1]] == in[px[1][0]]) && (in[px[2][1]] == in[px[2][2]]) && (in[px[1][1]] != in[px[2][0]]) && (in[px[1][1]] == in[px[0][0]]))){
						out[lower_left] = in[px[2][1]];
					}else{
						out[lower_left] = interpolate_pixels(in[px[1][1]],in[px[2][1]]);
					}
					out[lower_right] = in[px[1][2]];

				}else if ( (in[px[1][1]] == in[px[2][2]]) && (in[px[1][2]] == in[px[2][1]]) ) {
					// these are crossed diagonal pairs of pixels in the inner, center set of four.
					// if they're the same, then we weight them against a surrounding ring of pixels, and see which
					// pair is more different from the ring.  The ring is this asterisked set of pixels:
					//  X * * X
					//  * X X *
					//  * X X *
					//  X * * X
					if (in[px[1][1]] == in[px[1][2]]){
						out[lower_right] = in[px[1][1]];
						out[lower_left] = in[px[1][1]];
						out[upper_right] = in[px[1][1]];
						out[upper_left] = in[px[1][1]];

					} else {

						int difference_direction = 0;
						out[upper_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]]);
						out[lower_left] = interpolate_pixels(in[px[1][1]],in[px[2][1]]);

						difference_direction += calculate_difference(in[px[1][2]],in[px[1][1]],in[px[0][1]],in[px[1][0]]);
						difference_direction += calculate_difference(in[px[1][2]],in[px[1][1]],in[px[0][2]],in[px[1][3]]);
						difference_direction += calculate_difference(in[px[1][2]],in[px[1][1]],in[px[3][2]],in[px[2][3]]);
						difference_direction += calculate_difference(in[px[1][2]],in[px[1][1]],in[px[2][0]],in[px[3][1]]);

						if (difference_direction > 0){
							out[lower_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]],in[px[1][2]],in[px[1][2]]);
							out[upper_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]],in[px[1][2]],in[px[1][2]]);
						}else if(difference_direction < 0){
							out[lower_right] = interpolate_pixels(in[px[1][1]],in[px[1][1]],in[px[1][1]],in[px[1][2]]);
							out[upper_right] = interpolate_pixels(in[px[1][1]],in[px[1][1]],in[px[1][1]],in[px[1][2]]);
						}else{
							out[lower_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]],in[px[2][1]],in[px[2][2]]);
							out[upper_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]],in[px[2][1]],in[px[2][2]]);
						}

					}
				} else {
					if ( ( (in[px[1][1]] != in[px[2][1]]) || (in[px[1][1]] != in[px[0][1]]) ) && ( (in[px[0][2]] == in[px[1][2]]) && (in[px[1][2]] == in[px[2][2]]) ) ){
						out[upper_right] = interpolate_pixels(in[px[1][1]],in[px[1][1]],in[px[1][1]],in[px[1][2]]);
						out[lower_right] = interpolate_pixels(in[px[1][1]],in[px[1][1]],in[px[1][1]],in[px[1][2]]);
					}

					if ( ( (in[px[0][2]] == in[px[1][1]]) || (in[px[1][1]] == in[px[2][0]]) ) && ( (in[px[1][1]] != in[px[2][1]]) && (in[px[1][1]] != in[px[1][2]])) ){
						out[upper_right] = interpolate_pixels(in[px[1][1]],in[px[1][1]],in[px[1][1]],in[px[1][2]]);
						out[lower_left] = interpolate_pixels(in[px[1][1]],in[px[1][1]],in[px[1][1]],in[px[2][1]]);
						out[lower_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]],in[px[1][1]],in[px[2][1]]);
					}

					if ( ((in[px[1][1]] == in[px[0][0]]) && (in[px[1][1]] == in[px[1][2]])) && ( (in[px[1][1]] != in[px[1][0]]) && (in[px[1][1]] != in[px[2][1]]) ) ){
						out[upper_left] = interpolate_pixels(in[px[1][1]],in[px[1][1]],in[px[1][1]],in[px[1][0]]);
						out[lower_right] = interpolate_pixels(in[px[1][1]],in[px[1][1]],in[px[1][1]],in[px[2][1]]);
						out[lower_left] = interpolate_pixels(in[px[1][0]],in[px[2][1]]);
					}

					if ( (in[px[1][1]] == in[px[2][1]]) && (in[px[1][1]] == in[px[0][2]]) && (in[px[1][2]] != in[px[0][1]]) && (in[px[1][2]] == in[px[0][3]]) ){
						out[upper_right] = in[px[1][1]];
					} else if ( (in[px[1][2]] == in[px[0][1]]) && (in[px[1][2]] == in[px[2][2]]) && (in[px[1][1]] != in[px[0][2]]) && (in[px[1][1]] == in[px[0][0]]) ){
						out[upper_right] = in[px[1][2]];
					} else {
						//out[upper_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]]);
						//out[lower_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]]);
					}

					if ( (in[px[1][1]] == in[px[1][2]]) && (in[px[1][1]] == in[px[2][0]]) && (in[px[1][0]] != in[px[2][1]]) && (in[px[2][1]] == in[px[3][0]]) ){
						out[lower_left] = in[px[1][1]];
					} else if ( (in[px[2][1]] == in[px[1][0]]) && (in[px[2][1]] == in[px[2][2]]) && (in[px[1][1]] != in[px[2][0]]) && (in[px[1][1]] == in[px[0][0]]) ){
						out[lower_left] = in[px[2][1]];
					} else {
						//out[lower_right] = interpolate_pixels(in[px[1][1]],in[px[2][1]]);
						//out[lower_left] = interpolate_pixels(in[px[1][1]],in[px[2][1]]);
					}
					//out[lower_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]],in[px[2][1]],in[px[2][2]]);
					//out[upper_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]],in[px[2][1]],in[px[2][2]]);
				}

			}
		}
	}

	return result;
}

surface scale_surface_newer(surface input) {
	surface result(surface::create(input->w*2, input->h*2));

	const uint32_t* in = reinterpret_cast<const uint32_t*>(input->pixels);
	uint32_t* out = reinterpret_cast<uint32_t*>(result->pixels);

	for(int y = 0; y != input->h; ++y) {
		for(int x = 0; x != input->w; ++x) {
			uint32_t& out0 = out[y*2*result->w + x*2];
			uint32_t& out1 = out[y*2*result->w + x*2+1];
			uint32_t& out2 = out[(y*2+1)*result->w + x*2];
			uint32_t& out3 = out[(y*2+1)*result->w + x*2+1];

			out0 = out1 = out2 = out3 = in[y*input->w + x];

			const uint32_t matrix[] = {
x >= 2 && y >= 2 ? in[(y-2)*input->w + x-2] : 0, x >= 1 && y >= 2 ? in[(y-2)*input->w + x-1] : 0, y >= 2 ? in[(y-2)*input->w + x] : 0, x < input->w-1 && y >= 2 ? in[(y-2)*input->w + x+1] : 0, x < input->w-2 && y >= 2 ? in[(y-2)*input->w + x+2] : 0,
x >= 2 && y >= 1 ? in[(y-1)*input->w + x-2] : 0, x >= 1 && y >= 2 ? in[(y-1)*input->w + x-1] : 0, y >= 1 ? in[(y-1)*input->w + x] : 0, x < input->w-1 && y >= 1 ? in[(y-1)*input->w + x+1] : 0, x < input->w-2 && y >= 1 ? in[(y-1)*input->w + x+2] : 0,
x >= 2 ? in[(y)*input->w + x-2] : 0, x >= 1 ? in[(y)*input->w + x-1] : 0, in[(y)*input->w + x], x < input->w-1 ? in[(y)*input->w + x+1] : 0, x < input->w-2 ? in[(y)*input->w + x+2] : 0,
x >= 2 && y < input->h-1 ? in[(y+1)*input->w + x-2] : 0, x >= 1 && y < input->h-1 ? in[(y+1)*input->w + x-1] : 0, y < input->h-1 ? in[(y+1)*input->w + x] : 0, x < input->w-1 && y < input->h-1 ? in[(y+1)*input->w + x+1] : 0, x < input->w-2 && y < input->h-1 ? in[(y+1)*input->w + x+2] : 0,
x >= 2 && y < input->h-2 ? in[(y+2)*input->w + x-2] : 0, x >= 1 && y < input->h-2 ? in[(y+2)*input->w + x-1] : 0, y < input->h-2 ? in[(y+2)*input->w + x] : 0, x < input->w-1 && y < input->h-2 ? in[(y+2)*input->w + x+1] : 0, x < input->w-2 && y < input->h-2 ? in[(y+2)*input->w + x+2] : 0,
			};

	union PixelUnion {
		uint32_t value;
		uint8_t rgba[4];
	};
#include "surface_scaling_generated.hpp"
		}
	}

	return result;
}

surface scale_surface(surface input) {
	surface result(surface::create(input->w*2, input->h*2));

	const uint32_t* in = reinterpret_cast<const uint32_t*>(input->pixels);
	uint32_t* out = reinterpret_cast<uint32_t*>(result->pixels);
	for(int y = 0; y != input->h; ++y) {
		for(int x = 0; x != input->w; ++x) {
		  //do nearest neighbor interpolation
			out[(y*2)*result->w + x*2] = in[y*input->w + x];
			out[(y*2 + 1)*result->w + x*2] = in[y*input->w + x];
			out[(y*2 + 1)*result->w + x*2 + 1] = in[y*input->w + x];
			out[(y*2)*result->w + x*2 + 1] = in[y*input->w + x];

			if ((y > 0) && (y < input->h - 2) && (x > 0) && (x < input->w - 2)){

			//do additional 2xSaI interpolation

			//  2xSai works on a square group of sixteen pixels, rather than the square group of nine that Eagle works on.  In Eagle, the current pixel being upsized is the one in the middle of this square of nine; in 2xSai, the current pixel is the one in the upper-left of the middle four.  The other pixels in this group are all input pixels that are being examined to determine if we have an edge to smooth out.

			//  X X X X  |  0  1  2  3
			//  X * X X  |  4  5  6  7
			//  X X X X  |  8  9  10 11
			//  X X X X  |  12 13 14 15
						const int px[4][4] = {{	//[x][y]
							y*input->w + x - input->w - 1,
							y*input->w + x - input->w,
							y*input->w + x - input->w + 1,
							y*input->w + x - input->w + 2},
							{y*input->w + x - 1,
							y*input->w + x,
							y*input->w + x + 1,
							y*input->w + x + 2},
							{y*input->w + x + input->w - 1,
							y*input->w + x + input->w,
							y*input->w + x + input->w + 1,
							y*input->w + x + input->w + 2},
							{y*input->w + x + 2*input->w - 1,
							y*input->w + x + 2*input->w,
							y*input->w + x + 2*input->w + 1,
							y*input->w + x + 2*input->w + 2}};

						//these are the four output pixels corresponding to the one input pixel
						const int upper_left = (y*2)*result->w + x*2;
						const int upper_right = (y*2)*result->w + x*2 + 1;
						const int lower_left = (y*2 + 1)*result->w + x*2;
						const int lower_right = (y*2 + 1)*result->w + x*2 + 1;

						//make sure we're not going out-of-bounds
						const int max_index = input->w*input->h;
						assert(px[0][2] < max_index);
						assert(px[0][3] < max_index);
						assert(px[1][2] < max_index);
						assert(px[1][3] < max_index);
						assert(px[2][0] < max_index);
						assert(px[2][1] < max_index);
						assert(px[2][2] < max_index);
						assert(px[2][3] < max_index);
						assert(px[3][0] < max_index);
						assert(px[3][1] < max_index);
						assert(px[3][2] < max_index);
						assert(px[3][3] < max_index);

						// The following blocks are a visual representation of the conditional right above them.  When I have multiple instances of the same number, such as two 1s, then those places are equal.  When I have two different numbers in the same block, then they *must* be inequal - e.g. the value at 2 does not equal the value at 1, and also does not equal the value at 3. When I have a number, and an alphanumeric character in a block, the character does not need to be different from the number, but must equal itself.
								//  X X X X
								//  X X X X
								//  X X X X
								//  X X X X

						if ( (in[px[1][1]] == in[px[2][2]]) && (in[px[1][2]] != in[px[2][1]]) ) {
								//  X X X X
								//  X 1 2 X
								//  X 3 1 X
								//  X X X X
							if ( ((in[px[1][1]] == in[px[0][1]]) && (in[px[1][2]] == in[px[2][3]])) || ((in[px[1][1]] == in[px[2][1]]) && (in[px[1][1]] == in[px[0][2]]) && (in[px[1][2]] != in[px[0][1]]) && (in[px[1][2]] == in[px[0][3]]))){
								   //  X 1 X X         X 2 1 1
								   //  X 1 A X         X 1 1 X
								   //  X X X A   or    X X X X
								   //  X X X X         X X X X
								out[upper_right] = in[px[1][1]];
							}else{
								if( ! ((in[px[1][1]] == in[px[0][1]])) ){
									//  X 2 X X
									//  X 1 X X
									//  X X X X
									//  X X X X
									out[upper_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]]);
								}
							}

							if ( ((in[px[1][1]] == in[px[1][0]]) && (in[px[2][1]] == in[px[3][2]])) || ((in[px[1][1]] == in[px[1][2]]) && (in[px[1][1]] == in[px[2][0]]) && (in[px[1][0]] != in[px[2][1]]) && (in[px[2][1]] == in[px[3][0]]))){
								//  X X X X           X X X X
								//  1 1 X X           2 A A X
								//  X A X X    or     A 3 X X
								//  X X A X           X 3 X X
								out[lower_left] = in[px[1][1]];
							}else{
								if( ! ((in[px[1][1]] == in[px[1][0]])) ){
										//  X 2 X X
										//  X 1 X X
										//  X X X X
										//  X X X X
									out[lower_left] = interpolate_pixels(in[px[1][1]],in[px[2][1]]);
								}
							}
							out[lower_right] = in[px[1][1]];

						} else if ( (in[px[1][2]] == in[px[2][1]]) && (in[px[1][1]] != in[px[2][2]]) ) {
								//  X X X X
								//  X 2 1 X
								//  X 1 3 X
								//  X X X X

							if ( ((in[px[1][2]] == in[px[0][2]]) && (in[px[1][1]] == in[px[2][0]])) || ((in[px[1][2]] == in[px[0][1]]) && (in[px[1][2]] == in[px[2][2]]) && (in[px[1][2]] != in[px[0][2]]) && (in[px[1][1]] == in[px[0][0]]))){
								//  X X 1 X           2 1 1 X
								//  X A 1 X           X 2 1 X
								//  A X X X    or     X X 1 X
								//  X X X X           X X X X
								out[upper_right] = in[px[1][2]];
							}else{
								out[upper_right] = interpolate_pixels(in[px[1][1]],in[px[1][2]]);
							}

							if ( ((in[px[2][1]] == in[px[2][0]]) && (in[px[1][1]] == in[px[0][2]])) || ((in[px[2][1]] == in[px[1][0]]) && (in[px[2][1]] == in[px[2][2]]) && (in[px[1][1]] != in[px[2][0]]) && (in[px[1][1]] == in[px[0][0]]))){
								//  X X A X           X 2 X X
								//  X A X X           A 2 X X
								//  1 1 X X    or     3 A A X
								//  X X X X           X X X X
								out[lower_left] = in[px[2][1]];
							}else{
								out[lower_left] = interpolate_pixels(in[px[1][1]],in[px[2][1]]);
							}
							out[lower_right] = in[px[1][2]];

						}else if ( (in[px[1][1]] == in[px[2][2]]) && (in[px[1][2]] == in[px[2][1]]) ) {/*

							// these are two crossed diagonal pairs of pixels in the inner, center set of four.
							// If they're the same, then they're just a solid square.
							// but if they're not the same, then we weigh them against a surrounding ring of pixels, and see which
							// pair is more different from the ring.

							// The pair that is more different will end up being visually subdued - the result will be only 1/4 that pair, and 3/4 the other.

							//The ring is this asterisked set of pixels:
							//  X * * X
							//  * X X *
							//  * X X *
							//  X * * X
							if (in[px[1][1]] == in[px[1][2]]){  //First, check if they're the same.  If so, it's just a solid square.
									out[lower_right] = out[upper_right] = out[upper_left] = out[lower_left] = in[px[1][1]];

							} else {

								int difference_direction = 0;
								//These following lines compare the corners in this order, to the center pixels A and B.

								//  X 1 * X          X * 2 X          X * * X             X * * X
								//  1 A B *          * A B 2          * A B *             * A B *
								//  * B A *    then: * B A *    then: * B A 3      then:  4 B A *
								//  X * * X          X * * X          X * 3 X             X 4 * X

							   	// These get summed up.  If the final sum is positive, then A is the non-matching color and gets subdued.
								// if the final sum is negative, then B is the non-matching color and gets subdued.
								difference_direction += calculate_difference(in[px[1][2]],in[px[1][1]],in[px[0][1]],in[px[1][0]]);
								difference_direction += calculate_difference(in[px[1][2]],in[px[1][1]],in[px[1][3]],in[px[0][2]]);
								difference_direction += calculate_difference(in[px[1][2]],in[px[1][1]],in[px[3][2]],in[px[2][3]]);
								difference_direction += calculate_difference(in[px[1][2]],in[px[1][1]],in[px[2][0]],in[px[3][1]]);

								if (difference_direction > 0){
									out[lower_right] = out[upper_right] = out[upper_left] = out[lower_left] = interpolate_pixels(in[px[1][1]],in[px[1][2]],in[px[1][2]],in[px[1][2]]);
								}else if(difference_direction < 0){
									out[lower_right] = out[upper_right] = out[upper_left] = out[lower_left] = interpolate_pixels(in[px[1][1]],in[px[1][1]],in[px[1][1]],in[px[1][2]]);
   								}else{
									out[lower_right] = out[upper_right] = out[upper_left] = out[lower_left] = interpolate_pixels(in[px[1][1]],in[px[1][2]],in[px[2][1]],in[px[2][2]]);
								}

							}
						*/}

			}
		}
	}

	return result;
}

BENCHMARK(surface_scaling)
{
	surface s(graphics::surface_cache::get("characters/frogatto-spritesheet1.png"));
	assert(s.get());

	surface target(SDL_CreateRGBSurface(0,s->w,s->h,32,SURFACE_MASK));
	SDL_SetSurfaceBlendMode(s.get(), SDL_BLENDMODE_NONE);
	SDL_BlitSurface(s.get(), NULL, target.get(), NULL);
	BENCHMARK_LOOP {
		scale_surface(target);
	}
}

namespace {
	typedef boost::array<char, 4> OutputPixels;
	typedef boost::array<char, 25> InputMatrix;

	void flip_matrix(OutputPixels& pixels) {
		std::swap(pixels[0], pixels[1]);
		std::swap(pixels[2], pixels[3]);
	}

	void flop_matrix(OutputPixels& pixels) {
		std::swap(pixels[0], pixels[2]);
		std::swap(pixels[1], pixels[3]);
	}

	void flip_matrix(InputMatrix& pixels) {
		for(int y = 0; y != 5; ++y) {
			for(int x = 0; x < 5/2; ++x) {
				std::swap(pixels[y*5 + x], pixels[y*5 + 4 - x]);
			}
		}
	}

	void flop_matrix(InputMatrix& pixels) {
		for(int x = 0; x != 5; ++x) {
			for(int y = 0; y < 5/2; ++y) {
				std::swap(pixels[y*5 + x], pixels[((5-1) - y)*5 + x]);
			}
		}
	}

	struct ScalingOutput {
		OutputPixels output;
		InputMatrix input;
	};

	struct ScalingPattern {
		InputMatrix input;
		std::vector<ScalingOutput> output;
	};

	void flip_pattern(ScalingPattern& pattern) {
		flip_matrix(pattern.input);
		foreach(ScalingOutput& output, pattern.output) {
			flip_matrix(output.output);
			flip_matrix(output.input);
		}
	}

	void flop_pattern(ScalingPattern& pattern) {
		flop_matrix(pattern.input);
		foreach(ScalingOutput& output, pattern.output) {
			flop_matrix(output.output);
			flop_matrix(output.input);
		}
	}

}

UTILITY(generate_scaling_code)
{
	variant node(json::parse_from_file("./surface_scaling.cfg"));

	std::vector<ScalingPattern> patterns;
	foreach(variant pattern_node, node["pattern"].as_list()) {
		ScalingPattern pattern;
		const std::string& pattern_str = pattern_node["pattern"].as_string();
		int n = 0;
		foreach(char c, pattern_str) {
			if(!util::c_isspace(c)) {
				ASSERT_LOG(n < pattern.input.size(), "SURFACE SCALING PATTERN HAS TOO MANY CHARACTERS: " << pattern_str);
				pattern.input[n++] = c;
			}
		}

		ASSERT_LOG(n == 25, "SURFACE SCALING_PATTERN HAS TOO FEW CHARACTERS: " << pattern_str);

		foreach(variant output_node, pattern_node["output"].as_list()) {
			ScalingOutput output;
			const std::string& input_str = output_node["input"].as_string();
			int n = 0;
			foreach(char c, input_str) {
				if(!util::c_isspace(c)) {
					ASSERT_LOG(n < output.input.size(), "SURFACE SCALING INPUT HAS TOO MANY CHARACTERS: " << input_str);
					output.input[n++] = c;
				}
			}

			ASSERT_LOG(n == 25, "SURFACE SCALING INPUT HAS TOO FEW CHARACTERS: " << input_str);

			const std::string& output_str = output_node["output"].as_string();
			n = 0;
			foreach(char c, output_str) {
				if(!util::c_isspace(c)) {
					ASSERT_LOG(n < output.output.size(), "SURFACE SCALING OUTPUT HAS TOO MANY CHARACTERS: " << output_str);
					output.output[n++] = c;
				}
			}

			ASSERT_LOG(n == 4, "SURFACE SCALING OUTPUT HAS TOO FEW CHARACTERS: " << output_str);

			pattern.output.push_back(output);
		}

		patterns.push_back(pattern);
		flip_pattern(pattern);
		patterns.push_back(pattern);
		flop_pattern(pattern);
		patterns.push_back(pattern);
		flip_pattern(pattern);
		patterns.push_back(pattern);
	}

	foreach(ScalingPattern& pattern, patterns) {
		std::cout << "if(";
		for(int n = 0; n != pattern.input.size(); ++n) {
			if(pattern.input[n] != '*') {
				for(int m = n+1; m != pattern.input.size(); ++m) {
					if(pattern.input[n] == pattern.input[m]) {
						pattern.input[m] = '*';

						std::cout << "matrix[" << n << "] == matrix[" << m << "] &&\n";
					}
				}
			}
		}

		std::cout << "true) {\n";
		foreach(const ScalingOutput& output, pattern.output) {
			std::cout << "\t{\n\tPixelUnion pu;\n\tint red = 0, green = 0, blue = 0, count = 0;\n";
			int value_count = 0;
			for(int n = 0; n != output.input.size(); ++n) {
				if(util::c_isdigit(output.input[n]) && output.input[n] != '0') {
					const int value = output.input[n] - '0';
					value_count += value;
					std::cout << "\tpu.value = matrix[" << n << "];\n";
					std::cout << "\tred += pu.rgba[0]*" << value << "*pu.rgba[3];\n";
					std::cout << "\tgreen += pu.rgba[1]*" << value << "*pu.rgba[3];\n";
					std::cout << "\tblue += pu.rgba[2]*" << value << "*pu.rgba[3];\n";
					std::cout << "\tcount += pu.rgba[3]* " << value << ";\n";
				}
			}

			std::cout << "\tif(count > 0) {\n";
			std::cout << "\t\tred /= count;\n";
			std::cout << "\t\tgreen /= count;\n";
			std::cout << "\t\tblue /= count;\n";
			std::cout << "\t\tpu.rgba[0] = red; pu.rgba[1] = green; pu.rgba[2] = blue; pu.rgba[3] = 255;\n";

			for(int n = 0; n != output.output.size(); ++n) {
				if(output.output[n] != '0') {
					std::cout << "\t\tout" << n << " = pu.value;\n";
				}
			}

			std::cout << "\t}\n}\n";
		}

		std::cout << "\t}\n";
	}
}

}


// (C) Copyright Jeremy Siek 2001.
// 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)

// An example of setting and reading some bits. Note that operator[]
// goes from the least-significant bit at 0 to the most significant
// bit at size()-1. The operator<< for dynamic_bitset prints the
// bitset from most-significant to least-significant, since that is
// the format most people are used to reading.
//
//  The output is:
//
//  11001
//  10011
// ---------------------------------------------------------------------

#include <iostream>
#include <boost/dynamic_bitset.hpp>

int main()
{
    boost::dynamic_bitset<> x(5); // all 0's by default
    x[0] = 1;
    x[1] = 1;
    x[4] = 1;
    for (boost::dynamic_bitset<>::size_type i = 0; i < x.size(); ++i)
        std::cout << x[i];
    std::cout << "\n";
    std::cout << x << "\n";

    return 0;
}


#define PY_ARRAY_UNIQUE_SYMBOL PyArrayHandleCore

#ifndef OPENGM_PYTHON_INTERFACE
#define OPENGM_PYTHON_INTERFACE 1
#endif

#include <stdexcept>
#include <stddef.h>
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include "nifty_iterator.hxx"
#include "iteratorToTuple.hxx"
#include "export_typedes.hxx"
#include "copyhelper.hxx"

#include "opengm/functions/explicit_function.hxx"
#include "opengm/functions/absolute_difference.hxx"
#include "opengm/functions/potts.hxx"
#include "opengm/functions/pottsn.hxx"
#include "opengm/functions/pottsg.hxx"
#include "opengm/functions/squared_difference.hxx"
#include "opengm/functions/truncated_absolute_difference.hxx"
#include "opengm/functions/truncated_squared_difference.hxx"
#
using namespace boost::python;

namespace pyfunction{

   template<class F,class VALUE_TYPE>
   typename F::ValueType getValuePyTuple
   (
      const F & function,
      const boost::python::tuple & labelsequence
   ) {
      IteratorHolder< PythonIntTupleAccessor<VALUE_TYPE,true> > holder(labelsequence);
      return function(holder.begin());
   }

   template<class F,class VALUE_TYPE>
   typename F::ValueType getValuePyList
   (
      const F & function,
      const boost::python::list & labelsequence
   ) {
      typedef PythonIntListAccessor<VALUE_TYPE,true> Accessor;
      typedef opengm::AccessorIterator<Accessor,true> Iterator;
      Accessor accessor(labelsequence);
      Iterator begin(accessor,0);
      return function(begin);
   }

   template<class F,class VALUE_TYPE>
   boost::python::tuple getShapeCallByReturnPyTuple
   (
      const F & function
   ) {
      const size_t dimension=function.dimension();
      typedef typename F::FunctionShapeIteratorType IteratorType;
      typedef VALUE_TYPE V;
      return  iteratorToTuple<IteratorType,V,V>(function.functionShapeBegin(),dimension,static_cast<V>(1));
   }

   template<class FUNCTION>
   void setValueScalar(FUNCTION & f,const size_t index,const typename FUNCTION::ValueType value){
      f(index)=value;
   }

   template<class FUNCTION,class VALUE_TYPE>
   void explicitFunctionAssignPyTuple
   (
      FUNCTION & f,
      const boost::python::tuple & shape
   ) {
      typedef PythonIntTupleAccessor<VALUE_TYPE,true> Accessor;
      typedef opengm::AccessorIterator<Accessor,true> Iterator;
      Accessor accessor(shape);
      Iterator begin(accessor,0);
      Iterator end(accessor,accessor.size());
      f= FUNCTION(begin,end);
   }

   template<class FUNCTION,class VALUE_TYPE>
   void explicitFunctionAssignPyList
   (
      FUNCTION & f,
      const boost::python::list & shape
   ) {
      typedef PythonIntListAccessor<VALUE_TYPE,true> Accessor;
      typedef opengm::AccessorIterator<Accessor,true> Iterator;
      Accessor accessor(shape);
      Iterator begin(accessor,0);
      Iterator end(accessor,accessor.size());
      f= FUNCTION(begin,end);
   }

   template<class FUNCTION,class VALUE_TYPE>
   FUNCTION * explicitFunctionConstructorPyList(const boost::python::list & shape){
      IteratorHolder< PythonIntListAccessor<VALUE_TYPE,true> > holder(shape);
      return new FUNCTION(holder.begin(),holder.end());
   }
   template<class FUNCTION,class VALUE_TYPE>
   FUNCTION * explicitFunctionConstructorPyTuple(const boost::python::tuple & shape){
      IteratorHolder< PythonIntTupleAccessor<VALUE_TYPE,true> > holder(shape);
      return new FUNCTION(holder.begin(),holder.end());
   }

   template<class FUNCTION,class LABEL_VECTOR>
   typename FUNCTION::ValueType & getValueRefPy(FUNCTION & f,const LABEL_VECTOR & state){
      return f(state.begin());
   }
   template<class FUNCTION,class LABEL_VECTOR>
   const typename FUNCTION::ValueType & getValueConstRefPy(const FUNCTION & f,const LABEL_VECTOR & state){
      return f(state.begin());
   }
}

#define FUNCTION_TYPE_EXPORTER_HELPER(CLASS_NAME,CLASS_STRING)\
class_<CLASS_NAME > (CLASS_STRING, init<const CLASS_NAME &> ())\
.def(init<>())\
.add_property("size", &CLASS_NAME::size)\
.add_property("dimension", &CLASS_NAME::dimension)\
.add_property("shape",&pyfunction::getShapeCallByReturnPyTuple< CLASS_NAME,int >)\
.def("__getitem__", &pyfunction::getValuePyList<CLASS_NAME,int>, return_value_policy< return_by_value >())\
.def("__getitem__", &pyfunction::getValuePyTuple<CLASS_NAME,int>, return_value_policy< return_by_value >())\
.def("__copy__", &generic__copy__< CLASS_NAME >)\
//.def("__deepcopy__", &generic__deepcopy__< CLASS_NAME >)

template<class V,class I>
void export_functiontypes(){
   typedef V ValueType;
   typedef I IndexType;
   typedef IndexType LabelType;
   typedef opengm::ExplicitFunction<ValueType,IndexType,LabelType> PyExplicitFunction;
   typedef opengm::PottsFunction<ValueType,IndexType,LabelType> PyPottsFunction;

   FUNCTION_TYPE_EXPORTER_HELPER(PyPottsFunction,"PottsFunction");

   FUNCTION_TYPE_EXPORTER_HELPER(PyExplicitFunction,"ExplicitFunction")
   .def("__init__", make_constructor(&pyfunction::explicitFunctionConstructorPyList<PyExplicitFunction,int> ))
   .def("__init__", make_constructor(&pyfunction::explicitFunctionConstructorPyTuple<PyExplicitFunction,int> ))
   .def("assign",&pyfunction::explicitFunctionAssignPyTuple<PyExplicitFunction,int>)
   .def("assign",&pyfunction::explicitFunctionAssignPyList<PyExplicitFunction,int>)
   ;

//
//
//
  // .
   //;

   //.def("__init__", make_constructor(&pyfunction::explicitFunctionConstructorPy<PyExplicitFunction,LabelVector> ))
   //.def("getShape", &PyExplicitFunction::shape)
   //.def("setValue",pyfunction::setValueScalar<PyExplicitFunction>)
   //.def("value",&pyfunction::getValueConstRefPy<PyExplicitFunction,IndexTypeNumpyVector>,return_value_policy<copy_const_reference > ())
   //.def("__getitem__", &proxy<PyExplicitFunction,float>::get, return_value_policy< return_by_value >())
   //.def("__getitem__", &std_item<PyExplicitFunction,float,IndexTypeNumpyVector>::get, return_value_policy<copy_non_const_reference>())
   //.def("__setitem__", &std_item<PyExplicitFunction,float,IndexTypeNumpyVector>::set, with_custodian_and_ward<1,2>())
   //.def("__getitem__", &std_item<PyExplicitFunction,float,IndexTypeNumpyVector>::getC, return_value_policy<copy_non_const_reference>())
   //.def("__setitem__", &std_item<PyExplicitFunction,float,IndexTypeNumpyVector>::setC, with_custodian_and_ward<1,2>())

}

template void export_functiontypes<GmValueType,GmIndexType>();


//==================================================================================================
/*!
  @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_FUNCTION_COMBINE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COMBINE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-swar
    This function object concatenates two values of the same type to build a boost::simd::pack
    but with a cardinal twice as large.

    @par Header <boost/simd/function/combine.hpp>

    @par Scalar Semantic:
    For any scalar value @c x and @c y of type @c T , the following code:
    @code
    auto c = combine(x,y);
    @endcode
    is equivalent to :
    @code
    pack<T, 2> c(x,y);
    @endcode

    @par SIMD Semantic:
    For any type value @c x and @c y of type boost::simd::pack<T,N>, the following code:
    @code
    pack<T,2*N> c = combine(x,y);
    @endcode
    is equivalent to :
    @code
    pack<T,2*N> c(x[0],....,x[N-1],y[0],....,y[N-1]);
    @endcode
    where @c N is equal to the cardinal of both @c x and @c y.

    @param x First  value to combine
    @param y Second value to combine

    @return The concatenation of both inputs
  **/
  Value combine(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/simd/combine.hpp>

#endif


#include <grit.h>
#include <util.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

template<typename E>
bool test_func( grit::LogicExpression<E> const & condition, grit::Simplex0 const & s)
{
  return condition(s);
}

template<typename E>
bool test_func( grit::LogicExpression<E> const & condition, grit::Simplex1 const & s)
{
  return condition(s);
}

template<typename E>
bool test_func( grit::LogicExpression<E> const & condition, grit::Simplex2 const & s)
{
  return condition(s);
}

BOOST_AUTO_TEST_SUITE(grit);

//
// Unit test mesh, A's are phase 1, all other triangles are phase 0
//
//16 *---*---*---* 19
//   |\  |  /|  /|
//   | \ | / | / |
//   |  \|/  |/  |
//12 *---*---*---* 15
//   |  /|a /| a/|
//   | / | / | / |
//   |/  |/ a|/ a|
// 8 *---*---*---* 11
//   |  /|a /|a /|
//   | / | / | / |
//   |/  |/ a|/ a|
// 4 *---*---*---* 7
//   |  /|  /|\  |
//   | / | / | \ |
//   |/  |/  |  \|
//   *---*---*---*
//   0   1   2   3
//

BOOST_AUTO_TEST_CASE(logic_expression_testing)
{
  grit::engine2d_type        engine;
  grit::param_type           parameters;
  std::string           const txt_filename = util::get_data_file_path("unit_test.txt");

  grit::init_engine_with_mesh_file(txt_filename,parameters,engine);

  // 2015-12-04 Kenny code review: This unit test will crash with expections... it needs some data into the mesh before it makes sense to use it...

  grit::SimplexSet const all = engine.mesh().get_all_simplices();

  grit::Simplex0 const s0  = *(all.begin0());
  grit::Simplex1 const s1  = *(all.begin1());
  grit::Simplex2 const s2  = *(all.begin2());

  BOOST_CHECK_EQUAL( test_func( grit::Bool(true) &&  grit::Bool(true), s0)    , true  );
  BOOST_CHECK_EQUAL( test_func( grit::Bool(false) ||  grit::Bool(true), s1)   , true  );
  BOOST_CHECK_EQUAL( test_func( !grit::Bool(true), s2)                         , false );

  unsigned int label = engine.mesh().label(s2);

  BOOST_CHECK_EQUAL( test_func( grit::InPhase(engine.mesh(),label) , s2)       , true  );
  BOOST_CHECK_EQUAL( test_func( grit::InPhase(engine.mesh(),label+1) , s2)     , false );

  BOOST_CHECK_EQUAL( test_func( grit::IsDimension(engine.mesh(), 0) , s0), true );
  BOOST_CHECK_EQUAL( test_func( grit::IsDimension(engine.mesh(), 1) , s1), true );
  BOOST_CHECK_EQUAL( test_func( grit::IsDimension(engine.mesh(), 2) , s2), true );

  BOOST_CHECK_EQUAL( test_func( grit::IsDimension(engine.mesh(), 1) , s0), false );
  BOOST_CHECK_EQUAL( test_func( grit::IsDimension(engine.mesh(), 2) , s1), false );
  BOOST_CHECK_EQUAL( test_func( grit::IsDimension(engine.mesh(), 0) , s2), false );

  BOOST_CHECK_EQUAL( test_func( grit::IsDimension(engine.mesh(), 2) , s0), false );
  BOOST_CHECK_EQUAL( test_func( grit::IsDimension(engine.mesh(), 0) , s1), false );
  BOOST_CHECK_EQUAL( test_func( grit::IsDimension(engine.mesh(), 1) , s2), false );

  BOOST_CHECK_EQUAL( test_func( grit::IsManifold(engine.mesh()) , s0), true );
  BOOST_CHECK_EQUAL( test_func( grit::IsManifold(engine.mesh()) , s1), true );
  BOOST_CHECK_EQUAL( test_func( grit::IsManifold(engine.mesh()) , s2), true );

  BOOST_CHECK_EQUAL( test_func( grit::IsValid(engine.mesh()) , s0), true);
  BOOST_CHECK_EQUAL( test_func( grit::IsValid(engine.mesh()) , s1), true);
  BOOST_CHECK_EQUAL( test_func( grit::IsValid(engine.mesh()) , s2), true);

  BOOST_CHECK_EQUAL( test_func( grit::IsBoundary(engine.mesh()) , s0), true);
  BOOST_CHECK_EQUAL( test_func( grit::IsBoundary(engine.mesh()) , s1), true);
  //test_func( grit::IsBoundary(engine.mesh()) , s2);

  BOOST_CHECK_EQUAL( test_func( grit::IsInterface(engine.mesh()) , s0), false);
  BOOST_CHECK_EQUAL( test_func( grit::IsInterface(engine.mesh()) , s1), false);
  //test_func( grit::IsInterface(mesh) , s2);

  BOOST_CHECK_EQUAL( test_func( grit::IsSubmeshBoundary(engine.mesh()) , s0), false);
  BOOST_CHECK_EQUAL( test_func( grit::IsSubmeshBoundary(engine.mesh()) , s1), false);
  BOOST_CHECK_EQUAL( test_func( grit::IsSubmeshBoundary(engine.mesh()) , s2), false);

}

BOOST_AUTO_TEST_SUITE_END();


#ifndef QUADRATURE_HPP
#define QUADRATURE_HPP

#include <Eigen/Dense>

namespace polyfem
{
	class Quadrature
	{
	public:
		Eigen::MatrixXd points;
		Eigen::VectorXd weights;
	};
}

#endif //QUADRATURE_HPP


//!
//! \file comms/testserialise.cpp
//! \author Lachlan McCalman
//! \date 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#include <glog/logging.h>
#include "gtest/gtest.h"

#include "comms/messages.hpp"
#include "serial/serial.hpp"
#include <Eigen/Core>

namespace obsidian
{
  bool operator==(const GravResults& g, const GravResults& p)
  {
    return (g.likelihood == p.likelihood) && (g.readings == p.readings);
  }
}
using namespace obsidian;
using namespace obsidian::comms;
namespace stateline
{

  namespace comms
  {

    class Serialise: public testing::Test
    {
    public:
      Eigen::MatrixXd matrix_;
      Eigen::VectorXd vector_;
      std::string matrixString_;
      std::string vectorString_;
      std::vector<char> matrixChars_;
      std::vector<char> vectorChars_;
      GravResults gravResults_;

      virtual void SetUp()
      {
        matrix_ = Eigen::MatrixXd::Zero(3, 2);
        matrix_(0, 0) = 1.0;
        matrix_(1, 1) = 2.0;
        vector_ = Eigen::VectorXd::Zero(4);
        vector_(0) = 1.0;
        vector_(1) = 2.0;
        vector_(2) = 3.0;
        vector_(3) = 4.0;
        matrixChars_ =
        { '\0', '\0', '\0', '\0', '\0', '\0', '\xF0', 0x3F,
          '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0',
          '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0',
          '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0',
          '\0', '\0', '\0', '\0', '\0', '\0', '\0', 0x40,
          '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0'};
        vectorChars_ =
        { '\0', '\0', '\0', '\0', '\0', '\0', '\xF0', 0x3F,
          '\0', '\0', '\0', '\0', '\0', '\0', '\0', 0x40,
          '\0', '\0', '\0', '\0', '\0', '\0', '\b', 0x40,
          '\0', '\0', '\0', '\0', '\0', '\0', 0x10, 0x40};
        matrixString_ = std::string(matrixChars_.begin(), matrixChars_.end());
        vectorString_ = std::string(vectorChars_.begin(), vectorChars_.end());

        gravResults_.readings = Eigen::VectorXd::Random(10);
      }
      virtual void TearDown()
      {
      }
    };

  TEST_F(Serialise, matrixToString)
  {
    std::string s = matrixString(matrix_);
    EXPECT_EQ(matrixString_, s);
  }

  TEST_F(Serialise, vectorToString)
  {
    std::string s = vectorString(vector_);
    EXPECT_EQ(vectorString_, s);
  }

  TEST_F(Serialise, stringToMatrix)
  {
    Eigen::MatrixXd m = stringMatrix(matrixString_, matrix_.rows());
    EXPECT_EQ(m, matrix_);
  }

  TEST_F(Serialise, stringToVector)
  {
    Eigen::VectorXd v = stringVector(vectorString_);
    EXPECT_EQ(v, vector_);
  }

  TEST_F(Serialise, vector)
  {
    std::string s = vectorString(vector_);
    Eigen::VectorXd v = stringVector(s);
    EXPECT_EQ(v, vector_);
  }

  TEST_F(Serialise, matrix)
  {
    std::string s = matrixString(matrix_);
    Eigen::MatrixXd m = stringMatrix(s, matrix_.rows());
    EXPECT_EQ(m, matrix_);
  }

// already tested in serial
//    TEST_F(Serialise, gravResults)
//    {
//      std::string s =  serialise(gravResults_);
//      GravResults p;
//      unserialise(s, p);
//      EXPECT_EQ(gravResults_, p);
//    }

}
 // namespace comms
}// namespace obsidian


#include <iostream>
#include <chrono>
#include <boost/program_options.hpp>

#include <libmotioncapture/vicon.h>

static inline float fsqr(float x) { return x * x; }

int main(int argc, char **argv)
{
  std::string objectName;

  namespace po = boost::program_options;

  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("object", po::value<std::string>(&objectName)->required(), "object name")
  ;

  try
  {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 0;
    }
  }
  catch(po::error& e)
  {
    std::cerr << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return 1;
  }

  libmotioncapture::MotionCapture* mocap = new libmotioncapture::MotionCaptureVicon("vicon",
      /*enableObjects*/ true,
      /*enablePointcloud*/ false);

  libmotioncapture::Object object;

  // set precision to output enough digits (see https://stackoverflow.com/questions/554063/how-do-i-print-a-double-value-with-full-precision-using-cout)
  std::cout.precision(std::numeric_limits<double>::max_digits10);

  std::cout << "time,x,y,z,qx,qy,qz,qw" << std::endl;

  auto startTime = std::chrono::high_resolution_clock::now();

  for (size_t frameId = 0; ; ++frameId) {
    // Get a frame
    mocap->waitForNextFrame();

    auto time = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsedSeconds = time-startTime;

    mocap->getObjectByName(objectName, object);

    if (!object.occluded()) {

      // auto euler = object.rotation().toRotationMatrix().eulerAngles(0, 1, 2);
      // const auto& q = object.rotation();

      // double roll = atan2(2 * (q.w() * q.x() + q.y() * q.z()), 1 - 2 * (fsqr(q.x()) + fsqr(q.y()))); // roll
      // double pitch = asin(2 * (q.w() * q.y() - q.x() * q.z())); // pitch
      // double yaw = atan2(2 * (q.w() * q.z() + q.x() * q.y()), 1 - 2 * (fsqr(q.y()) + fsqr(q.z()))); // yaw

      std::cout << elapsedSeconds.count() << ",";
      std::cout << object.position().x() << ",";
      std::cout << object.position().y() << ",";
      std::cout << object.position().z() << ",";
      std::cout << object.rotation().x() << ",";
      std::cout << object.rotation().y() << ",";
      std::cout << object.rotation().z() << ",";
      std::cout << object.rotation().w();// << ",";
      // std::cout << roll << ",";
      // std::cout << pitch << ",";
      // std::cout << yaw;
      std::cout << std::endl;
    }

    if (elapsedSeconds.count() >= 20.0) {
      break;
    }
  }

  return 0;
}


/*
 * testNestedDissection.cpp
 *
 *   Created on: Nov 29, 2010
 *       Author: nikai
 *  Description: unit tests for NestedDissection
 */

#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <CppUnitLite/TestHarness.h>

#include "SubmapPlanarSLAM.h"
#include "SubmapVisualSLAM.h"
#include "SubmapExamples.h"
#include "SubmapExamples3D.h"
#include "GenericGraph.h"
#include "NonlinearTSAM.h"
#include "partition/NestedDissection-inl.h"

using namespace std;
using namespace gtsam;
using namespace gtsam::partition;

/* ************************************************************************* */
// x1 - x2
//  \  /
//   l1
TEST ( NestedDissection, oneIsland )
{
  using namespace submapPlanarSLAM;
  typedef TSAM2D::SubNLG SubNLG;
  Graph fg;
  fg.addOdometry(1, 2, Pose2(), odoNoise);
  fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise);
  fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise);
  fg.addPoseConstraint(1, Pose2());

  Ordering ordering; ordering += x1, x2, l1;

  int numNodeStopPartition = 1e3;
  int minNodesPerMap = 1e3;
  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
  LONGS_EQUAL(4, nd.root()->size());
  LONGS_EQUAL(3, nd.root()->frontal().size());
  LONGS_EQUAL(0, nd.root()->children().size());
}

/* ************************************************************************* */
// x1\  /x4
// |  x3  |
// x2/  \x5
TEST ( NestedDissection, TwoIslands )
{
  using namespace submapPlanarSLAM;
  typedef TSAM2D::SubNLG SubNLG;
  Graph fg;
  fg.addOdometry(1, 2, Pose2(), odoNoise);
  fg.addOdometry(1, 3, Pose2(), odoNoise);
  fg.addOdometry(2, 3, Pose2(), odoNoise);
  fg.addOdometry(3, 4, Pose2(), odoNoise);
  fg.addOdometry(4, 5, Pose2(), odoNoise);
  fg.addOdometry(3, 5, Pose2(), odoNoise);
  fg.addPoseConstraint(1, Pose2());
  fg.addPoseConstraint(4, Pose2());
  Ordering ordering; ordering += x1, x2, x3, x4, x5;

  int numNodeStopPartition = 2;
  int minNodesPerMap = 1;
  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
  // root submap
  LONGS_EQUAL(0, nd.root()->size());
  LONGS_EQUAL(1, nd.root()->frontal().size());
  LONGS_EQUAL(0, nd.root()->separator().size());
  LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps

  // the 1st submap
  LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size());
  LONGS_EQUAL(4, nd.root()->children()[0]->size());

  // the 2nd submap
  LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size());
  LONGS_EQUAL(4, nd.root()->children()[1]->size());
}

/* ************************************************************************* */
// x1\  /x4
//    x3
// x2/  \x5
TEST ( NestedDissection, FourIslands )
{
  using namespace submapPlanarSLAM;
  typedef TSAM2D::SubNLG SubNLG;
  Graph fg;
  fg.addOdometry(1, 3, Pose2(), odoNoise);
  fg.addOdometry(2, 3, Pose2(), odoNoise);
  fg.addOdometry(3, 4, Pose2(), odoNoise);
  fg.addOdometry(3, 5, Pose2(), odoNoise);
  fg.addPoseConstraint(1, Pose2());
  fg.addPoseConstraint(4, Pose2());
  Ordering ordering; ordering += x1, x2, x3, x4, x5;

  int numNodeStopPartition = 2;
  int minNodesPerMap = 1;
  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
  LONGS_EQUAL(0, nd.root()->size());
  LONGS_EQUAL(1, nd.root()->frontal().size());
  LONGS_EQUAL(0, nd.root()->separator().size());
  LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps

  // the 1st submap
  LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size());
  LONGS_EQUAL(2, nd.root()->children()[0]->size());

  // the 2nd submap
  LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size());
  LONGS_EQUAL(2, nd.root()->children()[1]->size());

  // the 3rd submap
  LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size());
  LONGS_EQUAL(1, nd.root()->children()[2]->size());

  // the 4th submap
  LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size());
  LONGS_EQUAL(1, nd.root()->children()[3]->size());
}

/* ************************************************************************* */
// x1\  /x3
//  | x2  |
// l6/  \x4
//  |
// x5
TEST ( NestedDissection, weekLinks )
{
  using namespace submapPlanarSLAM;
  typedef TSAM2D::SubNLG SubNLG;
  Graph fg;
  fg.addOdometry(1, 2, Pose2(), odoNoise);
  fg.addOdometry(2, 3, Pose2(), odoNoise);
  fg.addOdometry(2, 4, Pose2(), odoNoise);
  fg.addOdometry(3, 4, Pose2(), odoNoise);
  fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise);
  fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise);
  fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise);
  fg.addPoseConstraint(1, Pose2());
  fg.addPoseConstraint(4, Pose2());
  fg.addPoseConstraint(5, Pose2());
  Ordering ordering; ordering += x1, x2, x3, x4, x5, l6;

  int numNodeStopPartition = 2;
  int minNodesPerMap = 1;
  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
  LONGS_EQUAL(0, nd.root()->size()); // one weeklink
  LONGS_EQUAL(1, nd.root()->frontal().size());
  LONGS_EQUAL(0, nd.root()->separator().size());
  LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps
  LONGS_EQUAL(1, nd.root()->weeklinks().size());

  // the 1st submap
  LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4
  LONGS_EQUAL(4, nd.root()->children()[0]->size());

  // the 2nd submap
  LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6
  LONGS_EQUAL(4, nd.root()->children()[1]->size());
  //
  // the 3rd submap
  LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5
  LONGS_EQUAL(1, nd.root()->children()[2]->size());
}

/* ************************************************************************* */
/**
 *  l1   l2   l3
 *   | X  | X |
 *  x0 - x1 - x2
 *   | X  | X |
 *  l4   l5   l6
 */
TEST ( NestedDissection, manual_cuts )
{
  using namespace submapPlanarSLAM;
  typedef partition::Cuts Cuts;
  typedef TSAM2D::SubNLG SubNLG;
  typedef partition::PartitionTable PartitionTable;
  Graph fg;
  fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise);
  fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise);

  fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1,       bearingRangeNoise);
  fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1,       bearingRangeNoise);
  fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise);
  fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise);

  fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
  fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2),     1,       bearingRangeNoise);
  fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4),     sqrt(2), bearingRangeNoise);
  fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
  fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2),     1,       bearingRangeNoise);
  fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4),     sqrt(2), bearingRangeNoise);

  fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
  fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
  fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2),     1,       bearingRangeNoise);
  fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2),     1,       bearingRangeNoise);

  fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise);

  // generate ordering
  Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6;

  // define cuts
  boost::shared_ptr<Cuts> cuts(new Cuts());
  cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable;
  //x0        x1         x2         l1         l2         l3         l4         l5         l6
  (*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2;

  cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
  cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable;
  //x0        x1          x2          l1         l2          l3          l4         l5          l6
  (*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1;

  cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
  cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable;
  //x0         x1          x2         l1          l2          l3         l4          l5          l6
  (*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2;

  // nested dissection
  NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, cuts);
  LONGS_EQUAL(2, nd.root()->size());
  LONGS_EQUAL(3, nd.root()->frontal().size());
  LONGS_EQUAL(0, nd.root()->separator().size());
  LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
  LONGS_EQUAL(0, nd.root()->weeklinks().size());

  // the 1st submap
  LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0
  LONGS_EQUAL(4, nd.root()->children()[0]->size());
  LONGS_EQUAL(2, nd.root()->children()[0]->children().size());

  // the 1-1st submap
  LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1
  LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size());

  // the 1-2nd submap
  LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4
  LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size());

  // the 2nd submap
  LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2
  LONGS_EQUAL(3, nd.root()->children()[1]->size());
  LONGS_EQUAL(2, nd.root()->children()[1]->children().size());

  // the 2-1st submap
  LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3
  LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size());

  // the 2-2nd submap
  LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6
  LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size());

}

/* ************************************************************************* */
//   l1-l8   l9-16    l17-124
//    / |    /    \    | \
// x0  x1             x2  x3
TEST( NestedDissection, Graph3D) {
  using namespace gtsam::submapVisualSLAM;
  typedef TSAM3D::SubNLG SubNLG;
  typedef partition::PartitionTable PartitionTable;
  vector<GeneralCamera> cameras;
  cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.))));
  cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.))));
  cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.))));
  cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.))));

  vector<Point3> points;
  for (int cube_index = 0; cube_index <= 3; cube_index++) {
    Point3 center((cube_index-1) * 3, 0.5, 10.);
    points.push_back(center + Point3(-0.5, -0.5, -0.5));
    points.push_back(center + Point3(-0.5,  0.5, -0.5));
    points.push_back(center + Point3( 0.5,  0.5, -0.5));
    points.push_back(center + Point3( 0.5, -0.5, -0.5));
    points.push_back(center + Point3(-0.5, -0.5,  0.5));
    points.push_back(center + Point3(-0.5,  0.5,  0.5));
    points.push_back(center + Point3( 0.5,  0.5,  0.5));
    points.push_back(center + Point3( 0.5,  0.5,  0.5));
  }

  Graph graph;
  SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.));
  SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.));
  for (int j=1; j<=8; j++)
    graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
  for (int j=1; j<=16; j++)
    graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
  for (int j=9; j<=24; j++)
    graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
  for (int j=17; j<=24; j++)
    graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);

  // make an easy ordering
  Ordering ordering; ordering += x0, x1, x2, x3;
  for (int j=1; j<=24; j++)
    ordering += Symbol('l', j);

  // nested dissection
  const int numNodeStopPartition = 10;
  const int minNodesPerMap = 5;
  NestedDissection<Graph, SubNLG, GenericGraph3D> nd(graph, ordering, numNodeStopPartition, minNodesPerMap);

  LONGS_EQUAL(0, nd.root()->size());
  LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16
  LONGS_EQUAL(0, nd.root()->separator().size());
  LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
  LONGS_EQUAL(0, nd.root()->weeklinks().size());

  // the 1st submap
  LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8
  LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16
  LONGS_EQUAL(0, nd.root()->children()[0]->children().size());

  // the 2nd submap
  LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8
  LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8
  LONGS_EQUAL(0, nd.root()->children()[1]->children().size());
}

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


#ifndef PWALK_BALL_WALKER_HPP_
#define PWALK_BALL_WALKER_HPP_

#include <Eigen/Dense>
#include "util/math_functions.hpp"
#include "walker.hpp"

namespace pwalk {

template <typename Dtype>
class BallWalker: public Walker<Dtype> {
public:
  BallWalker(const Eigen::Matrix<Dtype, Eigen::Dynamic, 1>& initialization, const Eigen::Matrix<Dtype, Eigen::Dynamic, Eigen::Dynamic>& cons_A, const Eigen::Matrix<Dtype, Eigen::Dynamic, 1>& cons_b, const Dtype r) : Walker<Dtype>(initialization, cons_A, cons_b), r_(r){}

  // getter for radius
  Dtype getRadius() {
    return r_;
  }

  void proposal(Eigen::Matrix<Dtype, Eigen::Dynamic, 1>& new_sample);

  bool acceptRejectReverse(const Eigen::Matrix<Dtype, Eigen::Dynamic, 1>& new_sample);

  bool doSample(Eigen::Matrix<Dtype, Eigen::Dynamic, 1>& new_sample, const Dtype lazy = Dtype(0.5));

private:
  const Dtype r_;
};

} // namespace pwalk

#endif // PWALK_BALL_WALKER_HPP_


//==================================================================================================
/**
  Copyright 201 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/detail/nsm.hpp>
#include <boost/simd/function/scalar/shuffle.hpp>
#include <scalar_test.hpp>

using namespace boost::simd;

STF_CASE_TPL("Check unary shuffle behavior with direct permutation", STF_NUMERIC_TYPES)
{
  STF_EQUAL( shuffle<-1>(T(4)), T(0) );
  STF_EQUAL( shuffle<0>(T(4)) , T(4) );
}

STF_CASE_TPL("Check binary shuffle behavior with direct permutation", STF_NUMERIC_TYPES)
{
  STF_EQUAL( shuffle<-1>(T(4),T(7)) , T(0) );
  STF_EQUAL( shuffle< 0>(T(4),T(7)) , T(4) );
  STF_EQUAL( shuffle< 1>(T(4),T(7)) , T(7) );
}

template<int N> struct grab_
{
  template<typename I, typename C>
  struct apply : nsm::type_traits::integral_constant<int, N>
  {};
};

STF_CASE_TPL("Check unary shuffle behavior with direct meta-permutation", STF_NUMERIC_TYPES)
{
  STF_EQUAL( shuffle<grab_<0>>(T(4)) , T(4) );
}

STF_CASE_TPL("Check binary shuffle behavior with direct meta-permutation", STF_NUMERIC_TYPES)
{
  STF_EQUAL( shuffle<grab_<0>>(T(4),T(7)) , T(4) );
  STF_EQUAL( shuffle<grab_<1>>(T(4),T(7)) , T(7) );
}

#if !defined(BOOST_NO_CONSTEXPR)
// -------------------------------------------------------------------------------------------------
// Not much variation when cardinal is equal to 1, so we just make two "permutations"

constexpr int grab0(int /*i*/, int /*c*/) { return 0; }
constexpr int grab1(int /*i*/, int /*c*/) { return 1; }

STF_CASE_TPL("Check unary shuffle behavior with constexpr-permutation", STF_NUMERIC_TYPES)
{
  STF_EQUAL( shuffle<pattern<grab0>>(T(4),T(7)) , T(4) );
  STF_EQUAL( shuffle<pattern<grab1>>(T(4),T(7)) , T(7) );
}
#endif


#include <iostream>
#include <fstream>
#include <nlohmann/json.hpp>
#include <boost/algorithm/string.hpp>
#include <vector>
#include <cstring>
#include <dirent.h>
#include <sys/types.h>
using namespace std;
using json = nlohmann::json;

void accumulateDatasets(vector< vector<float> > &finalDataset, vector<int> &prohibitedIndices,
 vector< vector<float> > &normalDataset, int numNormal, vector< vector<float> > &anomalousDataset, int numAnomalous, int flag){

   srand(time(0));
   vector<pair<int, int> > selectnormalIndex;
   vector<pair<int, int> > selectanomalousIndex;
   vector< vector<float> > dataset;

   for(int j=0; j<normalDataset.size(); j++){
         selectnormalIndex.push_back(make_pair(rand(), j));
   }
   sort(selectnormalIndex.begin(), selectnormalIndex.end());
   for(int j=0; j<numNormal; j++){
      vector <float> temp;
      for(int k=0; k<normalDataset[0].size(); k++){
         vector <int>::iterator it;
         it=find(prohibitedIndices.begin(), prohibitedIndices.end(), k);
         if(it==prohibitedIndices.end()){
            temp.push_back(normalDataset[selectnormalIndex[j].second][k]);
         }
         else{
            temp.push_back(1);
         }
      }
      temp.push_back(0);                //0 means normal instance.
      dataset.push_back(temp);
   }

   for(int j=0; j<anomalousDataset.size(); j++){
         selectanomalousIndex.push_back(make_pair(rand(), j));
   }
   sort(selectanomalousIndex.begin(), selectanomalousIndex.end());
   for(int j=0; j<numAnomalous; j++){
      vector <float> temp;
      for(int k=0; k<anomalousDataset[0].size(); k++){
         vector <int>::iterator it;
         it=find(prohibitedIndices.begin(), prohibitedIndices.end(), k);
         if(it==prohibitedIndices.end()){
            temp.push_back(anomalousDataset[selectanomalousIndex[j].second][k]);
         }
         else{
            temp.push_back(1);
         }
      }
      temp.push_back(1);                //1 means anomalous instance.
      dataset.push_back(temp);
   }

   if(flag==0){
      for(int i=0; i<dataset.size(); i++){
         vector <float> temp;
         for(int j=0; j<dataset[0].size(); j++){
            temp.push_back(dataset[i][j]);
         }
         finalDataset.push_back(temp);
      }
   }
   else{
      vector<pair<int, int> > selectIndex;
      srand(time(0));
      for(int j=0; j<dataset.size(); j++){
         selectIndex.push_back(make_pair(rand(), j));
      }
      sort(selectIndex.begin(), selectIndex.end());
      for(int i=0; i<dataset.size(); i++){
         vector <float> temp;
         for(int j=0; j<dataset[0].size(); j++){
            temp.push_back(dataset[selectIndex[i].second][j]);
         }
         finalDataset.push_back(temp);
      }
   }

}

void fetchDataset(vector< vector<float> > &dataset, string filepath){

   ifstream file (filepath);
   string line;
   while (getline(file,line))
   {
      vector<string> attr;
      boost::split(attr, line, boost::is_any_of(" "));
      vector <float> temp;
      for(int k=0; k<attr.size()-1; k++){
         temp.push_back(stof(attr[k]));
      }
      dataset.push_back(temp);

   }
   file.close();

}

void minMax(vector< vector<float> > &dataset, float* min, float* max, int attr){

   for(int j=0; j<dataset.size(); j++){
      if(dataset[j][attr]>*max){
         *max=dataset[j][attr];
      }
      if(dataset[j][attr]<*min){
         *min=dataset[j][attr];
      }
   }

}

void scaleDataset(vector< vector<float> > &dataset, float min, float max, int attr){

   for(int j=0; j<dataset.size(); j++){
      dataset[j][attr] = (dataset[j][attr] - min)/(max - min);
   }

}

void getDataset(vector< vector<float> > &dataset, string filePath, int flag){         //1 for random display

   ofstream file;
   file.open(filePath);

   if(flag==0){
      for(int i=0; i<dataset.size(); i++){
         //cout<<i<<": ";
         for(int j=0; j<dataset[0].size(); j++){
            //cout<<dataset[i][j]<<" ";
            file<<dataset[i][j]<<" ";
         }
         //cout<<"\n";
         file<<"\n";
      }
   }
   else{
      vector<pair<int, int> > selectIndex;
      srand(time(0));
      for(int j=0; j<dataset.size(); j++){
         selectIndex.push_back(make_pair(rand(), j));
      }
      sort(selectIndex.begin(), selectIndex.end());
      for(int i=0; i<dataset.size(); i++){
         //cout<<i<<": ";
         for(int j=0; j<dataset[0].size(); j++){
            //cout<<dataset[selectIndex[i].second][j]<<" ";
            file<<dataset[selectIndex[i].second][j]<<" ";
         }
         //cout<<"\n";
         file<<"\n";
      }
   }

   file.close();
   cout<<"Dataset Completed: "<<dataset.size()<<"x"<<dataset[0].size()<<"\n";

}

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

   vector <int> prohibitedIndices;
   int dictSize = 519;

   vector< vector<float> > normalDataset1;
   fetchDataset(normalDataset1, "normalDataset1.txt");
   vector< vector<float> > anomalousDataset1;
   fetchDataset(anomalousDataset1, "anomalousDataset1.txt");

   vector< vector<float> > normalDataset2;
   fetchDataset(normalDataset2, "normalDataset2.txt");
   vector< vector<float> > anomalousDataset2;
   fetchDataset(anomalousDataset2, "anomalousDataset2.txt");

   vector< vector<float> > normalDataset3;
   fetchDataset(normalDataset3, "normalDataset3.txt");
   vector< vector<float> > anomalousDataset3;
   fetchDataset(anomalousDataset3, "anomalousDataset3.txt");

   vector< vector<float> > normalDataset4;
   fetchDataset(normalDataset4, "normalDataset4.txt");
   vector< vector<float> > anomalousDataset4;
   fetchDataset(anomalousDataset4, "anomalousDataset4.txt");

   vector< vector<float> > normalDataset5;
   fetchDataset(normalDataset5, "normalDataset5.txt");
   vector< vector<float> > anomalousDataset5;
   fetchDataset(anomalousDataset5, "anomalousDataset5.txt");

   //Perform min-max scaling.
   for(int i=0; i<dictSize; i++){

      float min = normalDataset1[0][i];
      float max = normalDataset1[0][i];

      minMax(normalDataset1, &min, &max, i);
      minMax(anomalousDataset1, &min, &max, i);
      minMax(normalDataset2, &min, &max, i);
      minMax(anomalousDataset2, &min, &max, i);
      minMax(normalDataset3, &min, &max, i);
      minMax(anomalousDataset3, &min, &max, i);
      minMax(normalDataset4, &min, &max, i);
      minMax(anomalousDataset4, &min, &max, i);
      minMax(normalDataset5, &min, &max, i);
      minMax(anomalousDataset5, &min, &max, i);

      scaleDataset(normalDataset1, min, max, i);
      scaleDataset(anomalousDataset1, min, max, i);

      scaleDataset(normalDataset2, min, max, i);
      scaleDataset(anomalousDataset2, min, max, i);

      scaleDataset(normalDataset3, min, max, i);
      scaleDataset(anomalousDataset3, min, max, i);

      scaleDataset(normalDataset4, min, max, i);
      scaleDataset(anomalousDataset4, min, max, i);

      scaleDataset(normalDataset5, min, max, i);
      scaleDataset(anomalousDataset5, min, max, i);

      if(min==max){
         prohibitedIndices.push_back(i);
      }
   }

   getDataset(normalDataset1, "scaledNormalDataset1.txt", 0);      //1000
   getDataset(anomalousDataset1, "scaledAnomalousDataset1.txt", 0);     //774
   getDataset(normalDataset2, "scaledNormalDataset2.txt", 0);      //1120
   getDataset(anomalousDataset2, "scaledAnomalousDataset2.txt", 0);      //984
   getDataset(normalDataset3, "scaledNormalDataset3.txt", 0);      //2392
   getDataset(anomalousDataset3, "scaledAnomalousDataset3.txt", 0);     //1054
   getDataset(normalDataset4, "scaledNormalDataset4.txt", 0);     //5000
   getDataset(anomalousDataset4, "scaledAnomalousDataset4.txt", 0);     //2500
   getDataset(normalDataset5, "scaledNormalDataset5.txt", 0);       //962
   getDataset(anomalousDataset5, "scaledAnomalousDataset5.txt", 0);    //500

   //Total Normal Instances = 10474
   //Total Anomalous Instances = 5812

   for(int i=0; i<prohibitedIndices.size(); i++){
      cout<<prohibitedIndices[i]<<"\n";
   }

   vector< vector<float> > dataset;

   float anomalyFraction = 0.05;
   cout<<(int)(normalDataset1.size()*anomalyFraction)<<"\n";

   accumulateDatasets(dataset, prohibitedIndices, normalDataset1, normalDataset1.size(), anomalousDataset1,
   (int)(normalDataset1.size()*anomalyFraction), 1);

   accumulateDatasets(dataset, prohibitedIndices, normalDataset2, normalDataset2.size(), anomalousDataset2,
   (int)(normalDataset2.size()*anomalyFraction), 1);

   accumulateDatasets(dataset, prohibitedIndices, normalDataset3, normalDataset3.size(), anomalousDataset3,
   (int)(normalDataset3.size()*anomalyFraction), 1);

   accumulateDatasets(dataset, prohibitedIndices, normalDataset4, normalDataset4.size(), anomalousDataset4,
   (int)(normalDataset4.size()*anomalyFraction), 1);

   accumulateDatasets(dataset, prohibitedIndices, normalDataset5, normalDataset5.size(), anomalousDataset5,
   (int)(normalDataset5.size()*anomalyFraction), 1);

   cout<<"Dataset: "<<dataset.size()<<"x"<<dataset[0].size()<<"\n";

   getDataset(dataset, "dataset.txt", 0);   //0 gives sequential dataset(evolving), while 1 gives randomly distributed dataset.

}


//==================================================================================================
/*!
  @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_FUNCTION_IS_NOT_INFINITE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_NOT_INFINITE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-predicates

    This function object returns @ref True if x is finite or @ref Nan

    @par Header <boost/simd/function/is_not_infinite.hpp>

    @par Note

     Using `is_not_infinite(x)` is similar to `(x !=  Inf) && (x != -Inf)` for floating types
     and is always True for integral types

    @par Example:

      @snippet is_not_infinite.cpp is_not_infinite

    @par Possible output:

      @snippet is_not_infinite.txt is_not_infinite

  **/
  as_logical_t<Value> is_not_infinite(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/is_not_infinite.hpp>
#include <boost/simd/function/simd/is_not_infinite.hpp>

#endif


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

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

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

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

#include <ql/cashflows/iborcoupon.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <qle/pricingengines/analyticlgmswaptionengine.hpp>

#include <boost/bind.hpp>

namespace QuantExt {

AnalyticLgmSwaptionEngine::AnalyticLgmSwaptionEngine(const boost::shared_ptr<LinearGaussMarkovModel>& model,
                                                     const Handle<YieldTermStructure>& discountCurve,
                                                     const FloatSpreadMapping floatSpreadMapping)
    : GenericEngine<Swaption::arguments, Swaption::results>(), p_(model->parametrization()),
      c_(discountCurve.empty() ? p_->termStructure() : discountCurve), floatSpreadMapping_(floatSpreadMapping),
      caching_(false) {
    registerWith(model);
    registerWith(c_);
}

AnalyticLgmSwaptionEngine::AnalyticLgmSwaptionEngine(const boost::shared_ptr<CrossAssetModel>& model, const Size ccy,
                                                     const Handle<YieldTermStructure>& discountCurve,
                                                     const FloatSpreadMapping floatSpreadMapping)
    : GenericEngine<Swaption::arguments, Swaption::results>(), p_(model->irlgm1f(ccy)),
      c_(discountCurve.empty() ? p_->termStructure() : discountCurve), floatSpreadMapping_(floatSpreadMapping),
      caching_(false) {
    registerWith(model);
    registerWith(c_);
}

AnalyticLgmSwaptionEngine::AnalyticLgmSwaptionEngine(const boost::shared_ptr<IrLgm1fParametrization> irlgm1f,
                                                     const Handle<YieldTermStructure>& discountCurve,
                                                     const FloatSpreadMapping floatSpreadMapping)
    : GenericEngine<Swaption::arguments, Swaption::results>(), p_(irlgm1f),
      c_(discountCurve.empty() ? p_->termStructure() : discountCurve), floatSpreadMapping_(floatSpreadMapping),
      caching_(false) {
    registerWith(c_);
}

void AnalyticLgmSwaptionEngine::enableCache(const bool lgm_H_constant, const bool lgm_alpha_constant) {
    caching_ = true;
    lgm_H_constant_ = lgm_H_constant;
    lgm_alpha_constant_ = lgm_alpha_constant;
    clearCache();
}

void AnalyticLgmSwaptionEngine::clearCache() {
    S_.clear();             // indicates that H / alpha independent variables are not yet computed
    Hj_.clear();            // indicates that H dependent variables not not yet computed
    zetaex_ = Null<Real>(); // indicates that alpha dependent variables are not yet computed
}

void AnalyticLgmSwaptionEngine::calculate() const {

    QL_REQUIRE(arguments_.settlementType == Settlement::Physical, "cash-settled swaptions are not supported ...");

    Date reference = p_->termStructure()->referenceDate();

    Date expiry = arguments_.exercise->dates().back();

    if (expiry <= reference) {
        // swaption is expired, possibly generated swap is not
        // valued by this engine, so we set the npv to zero
        results_.value = 0.0;
        return;
    }

    if (!caching_ || S_.empty()) {

        Option::Type type = arguments_.type == VanillaSwap::Payer ? Option::Call : Option::Put;
        const Schedule& fixedSchedule = arguments_.swap->fixedSchedule();
        const Schedule& floatSchedule = arguments_.swap->floatingSchedule();

        j1_ = std::lower_bound(fixedSchedule.dates().begin(), fixedSchedule.dates().end(), expiry) -
              fixedSchedule.dates().begin();
        k1_ = std::lower_bound(floatSchedule.dates().begin(), floatSchedule.dates().end(), expiry) -
              floatSchedule.dates().begin();

        // compute S_i, i.e. equivalent fixed rate spreads compensating for
        // a) a possibly non-zero float spread and
        // b) a spread between the ibor indices forwarding curve and the
        //     discounting curve
        // here, we do not work with a spread corrections directly, but
        // with this multiplied by the nominal and accrual basis,
        // so S_i is really an amount correction.

        S_.resize(arguments_.fixedCoupons.size() - j1_);
        for (Size i = 0; i < S_.size(); ++i) {
            S_[i] = 0.0;
        }
        S_m1 = 0.0;
        Size ratio = static_cast<Size>(static_cast<Real>(arguments_.floatingCoupons.size()) /
                                           static_cast<Real>(arguments_.fixedCoupons.size()) +
                                       0.5);
        QL_REQUIRE(ratio >= 1, "floating leg's payment frequency must be equal or "
                               "higher than fixed leg's payment frequency in "
                               "analytic lgm swaption engine");

        Size k = k1_;
        // The method reduces the problem to a one curve configuration w.r.t. the discount curve and
        // apply a correction for the discount curve / forwarding curve spread. Furthermore the method
        // assumes that no historical fixings are present in the floating rate coupons.
        auto index = arguments_.swap->iborIndex();
        for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
            Real sum1 = 0.0, sum2 = 0.0;
            for (Size rr = 0; rr < ratio && k < arguments_.floatingCoupons.size(); ++rr, ++k) {
                Real amount = arguments_.floatingCoupons[k];
                Real lambda1 = 0.0, lambda2 = 1.0;
                if (floatSpreadMapping_ == proRata) {
                    // we do not use the exact pay dates but the ratio to determine
                    // the distance to the adjacent payment dates
                    lambda2 = static_cast<Real>(rr + 1) / static_cast<Real>(ratio);
                    lambda1 = 1.0 - lambda2;
                }
                if (amount != Null<Real>()) {
                    Real flatAmount;
                    if (IborCoupon::usingAtParCoupons()) {
                        // if par coupons are used, we mimick the fixing estimation in IborCoupon; we make
                        // sure that the estimation period does not start in the past and we do not use
                        // historical fixings
                        Date fixingValueDate = index->fixingCalendar().advance(arguments_.floatingFixingDates[k],
                                                                               index->fixingDays(), Days);
                        fixingValueDate = std::max(fixingValueDate, reference);
                        auto cpn = boost::dynamic_pointer_cast<Coupon>(arguments_.swap->floatingLeg()[k]);
                        QL_REQUIRE(cpn, "AnalyticalLgmSwaptionEngine::calculate(): coupon expected on underlying swap "
                                        "floating leg, could not cast");
                        Date nextFixingDate = index->fixingCalendar().advance(
                            cpn->accrualEndDate(), -static_cast<Integer>(index->fixingDays()), Days);
                        Date fixingEndDate = index->fixingCalendar().advance(nextFixingDate, index->fixingDays(), Days);
                        fixingEndDate = std::max(fixingEndDate, fixingValueDate + 1);
                        Real spanningTime = index->dayCounter().yearFraction(fixingValueDate, fixingEndDate);
                        DiscountFactor disc1 = c_->discount(fixingValueDate);
                        DiscountFactor disc2 = c_->discount(fixingEndDate);
                        Real fixing = (disc1 / disc2 - 1.0) / spanningTime;
                        flatAmount = fixing * arguments_.floatingAccrualTimes[k] * arguments_.nominal;
                    } else {
                        // if indexed coupons are used, we use a proper fixing, but make sure that the fixing date
                        // is not in the past and we do not use a historical fixing for "today"
                        auto flatIbor = boost::make_shared<IborIndex>(
                            index->familyName() + " (no fixings)", index->tenor(), index->fixingDays(),
                            index->currency(), index->fixingCalendar(), index->businessDayConvention(),
                            index->endOfMonth(), index->dayCounter(), c_);
                        Date fixingDate =
                            flatIbor->fixingCalendar().adjust(std::max(arguments_.floatingFixingDates[k], reference));
                        flatAmount =
                            flatIbor->fixing(fixingDate) * arguments_.floatingAccrualTimes[k] * arguments_.nominal;
                    }
                    Real correction = (amount - flatAmount) * c_->discount(arguments_.floatingPayDates[k]);
                    sum1 += lambda1 * correction;
                    sum2 += lambda2 * correction;
                } else {
                    // if no amount is given, we do not need a spread correction
                    // due to different forward / discounting curves since then
                    // no curve is attached to the swap's ibor index and so we
                    // assume a one curve setup;
                    // but we can still have a float spread that has to be converted
                    // into a fixed leg's payment
                    Real correction = arguments_.nominal * arguments_.floatingSpreads[k] *
                                      arguments_.floatingAccrualTimes[k] * c_->discount(arguments_.floatingPayDates[k]);
                    sum1 += lambda1 * correction;
                    sum2 += lambda2 * correction;
                }
            }
            if (j > j1_) {
                S_[j - j1_ - 1] += sum1 / c_->discount(arguments_.fixedPayDates[j - 1]);
            } else {
                S_m1 += sum1 / c_->discount(arguments_.floatingResetDates[k1_]);
            }
            S_[j - j1_] += sum2 / c_->discount(arguments_.fixedPayDates[j]);
        }

        w_ = type == Option::Call ? -1.0 : 1.0;
        D0_ = c_->discount(arguments_.floatingResetDates[k1_]);
        Dj_.resize(arguments_.fixedCoupons.size() - j1_);
        for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
            Dj_[j - j1_] = c_->discount(arguments_.fixedPayDates[j - j1_]);
        }
    }

    if (!caching_ || !lgm_H_constant_ || Hj_.empty()) {
        // it is a requirement that H' does not change its sign,
        // with u = -1.0 we handle the case H' < 0
        u_ = p_->Hprime(0.0) > 0.0 ? 1.0 : -1.0;

        H0_ = p_->H(p_->termStructure()->timeFromReference(arguments_.floatingResetDates[k1_]));
        Hj_.resize(arguments_.fixedCoupons.size() - j1_);
        for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
            Hj_[j - j1_] = p_->H(p_->termStructure()->timeFromReference(arguments_.fixedPayDates[j]));
        }
    }

    if (!caching_ || !lgm_alpha_constant_ || zetaex_ == Null<Real>()) {
        zetaex_ = p_->zeta(p_->termStructure()->timeFromReference(expiry));
    }

    Brent b;
    Real yStar;
    try {
        yStar = b.solve(boost::bind(&AnalyticLgmSwaptionEngine::yStarHelper, this, _1), 1.0E-6, 0.0, 0.01);
    } catch (const std::exception& e) {
        QL_FAIL("AnalyticLgmSwaptionEngine, failed to compute yStar, " << e.what());
    }

    CumulativeNormalDistribution N;
    Real sqrt_zetaex = std::sqrt(zetaex_);
    Real sum = 0.0;
    for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
        sum += w_ * (arguments_.fixedCoupons[j] - S_[j - j1_]) * Dj_[j - j1_] *
               N(u_ * w_ * (yStar + (Hj_[j - j1_] - H0_) * zetaex_) / sqrt_zetaex);
    }
    sum += -w_ * S_m1 * D0_ * N(u_ * w_ * yStar / sqrt_zetaex);
    sum += w_ * (arguments_.nominal * Dj_.back() * N(u_ * w_ * (yStar + (Hj_.back() - H0_) * zetaex_) / sqrt_zetaex) -
                 arguments_.nominal * D0_ * N(u_ * w_ * yStar / sqrt_zetaex));
    results_.value = sum;

    results_.additionalResults["fixedAmountCorrectionSettlement"] = S_m1;
    results_.additionalResults["fixedAmountCorrections"] = S_;

} // calculate

Real AnalyticLgmSwaptionEngine::yStarHelper(const Real y) const {
    Real sum = 0.0;
    for (Size j = j1_; j < arguments_.fixedCoupons.size(); ++j) {
        sum += (arguments_.fixedCoupons[j] - S_[j - j1_]) * Dj_[j - j1_] *
               std::exp(-(Hj_[j - j1_] - H0_) * y - 0.5 * (Hj_[j - j1_] - H0_) * (Hj_[j - j1_] - H0_) * zetaex_);
    }
    sum += -S_m1 * D0_;
    sum += Dj_.back() * arguments_.nominal *
           std::exp(-(Hj_.back() - H0_) * y - 0.5 * (Hj_.back() - H0_) * (Hj_.back() - H0_) * zetaex_);
    sum -= D0_ * arguments_.nominal;
    return sum;
}

} // namespace QuantExt