#ifndef TVMTL_VISUALIZATION3D_HPP
#define TVMTL_VISUALIZATION3D_HPP

//System includes
#include <iostream>
#include <cmath>
#include <string>

//Eigen includes
#include <Eigen/Geometry>

//OpenCV includes
#include <opencv2/opencv.hpp>

//OpenGL includes
#include <GL/freeglut.h>

// video++ includes
#include <vpp/vpp.hh>

// own includes
#include "enumerators.hpp"
#include "func_ptr_utils.hpp"
#include "manifold.hpp"
#include "data.hpp"

namespace tvmtl{

// Specialization SO(3)
template <class DATA>
class Visualization< SO, 3, DATA, 3>{
    public:
        typedef Manifold<SO,3> mf_t;
	typedef Visualization<SO,3, DATA, 3> myType;

	// Constructor
	Visualization(DATA& dat): data_(dat), filename_(""), paint_inpainted_pixel_(false), cam_(Camera()) {}

	// Static members GL Interface

	// Class members GL Intergace
	void GLInit(const char* windowname);
	void reshape(int x, int y);
	void draw(void);
	void saveImage(std::string filename);
	void keyboard(unsigned char key, int x, int y);
	void specialKeys(int key, int x, int y);
	//
	//Acces methods
	void paint_inpainted_pixel(bool setFlag) {paint_inpainted_pixel_ = setFlag; }

    private:
	void storeImage(std::string filename);

	DATA& data_;
	bool paint_inpainted_pixel_;
	std::string filename_;
	int width_, height_;

	Camera cam_;

};

// Specialization EUCLIDIAN
template <class DATA, int N>
class Visualization< EUCLIDIAN, N, DATA, 3>{
    public:
        typedef Manifold<EUCLIDIAN, N> mf_t;
	typedef Visualization<EUCLIDIAN, N, DATA, 3> myType;

	// Constructor
	Visualization(DATA& dat): data_(dat), filename_(""), paint_inpainted_pixel_(false), cam_(Camera()) {}

	// Static members GL Interface

	// Class members GL Intergace
	void GLInit(const char* windowname);
	void initTextures();
	void reshape(int x, int y);
	void animation();
	void draw(void);
	void saveImage(std::string filename);
	void keyboard(unsigned char key, int x, int y);
	void specialKeys(int key, int x, int y);
	//
	//Acces methods
	void paint_inpainted_pixel(bool setFlag) {paint_inpainted_pixel_ = setFlag; }

    private:
	void storeImage(std::string filename);

	DATA& data_;
	bool paint_inpainted_pixel_;
	std::string filename_;
	int width_, height_;

	Camera cam_;

	GLuint texture3d_id;
};

// Specialization SPD(3)
template <class DATA>
class Visualization< SPD, 3, DATA, 3>{
    public:
        typedef Manifold<SPD,3> mf_t;
	typedef Visualization<SPD,3, DATA, 3> myType;

	// Constructor
	Visualization(DATA& dat): data_(dat), filename_(""), paint_inpainted_pixel_(false), cam_(Camera()) {}

	// Static members GL Interface
	static void initLighting();

	// Class members GL Interface
	void GLInit(const char* windowname);
	void reshape(int x, int y);
	void draw(void);
	void saveImage(std::string filename);
	void keyboard(unsigned char key, int x, int y);
	void specialKeys(int key, int x, int y);

	//Acces methods
	void paint_inpainted_pixel(bool setFlag) {paint_inpainted_pixel_ = setFlag; }

    private:
	void storeImage(std::string filename);

	DATA& data_;
	bool paint_inpainted_pixel_;
	std::string filename_;
	int width_, height_;

	Camera cam_;
};

// IMPLEMENTATION SO(3)
template <class DATA>
void Visualization<SO, 3, DATA, 3>::draw(void)
{

    glMatrixMode(GL_MODELVIEW);
    // clear the drawing buffer.
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
    // Enable depth test
    glEnable(GL_DEPTH_TEST);
    // Accept fragment if it closer to the camera than the former one
    glDepthFunc(GL_LESS);

    int nz = data_.img_.nslices();
    int nx = data_.img_.ncols();
    int ny = data_.img_.nrows();

   int max = std::max(nx,std::max(ny,nz));

    float scaling = 2.0 / (3.0 * max);
    float spacing = 4.0 * scaling;
    float z_distance= -4.5;
    scaling *= 0.5;

    #ifdef TV_VISUAL_DEBUG
	std::cout << "ny: " << ny << " nx: " << nx << std::endl;
	std::cout << "Maximum: " << max << " Scaling " << scaling << std::endl;
    #endif

    glLoadIdentity();
    gluLookAt(cam_.xPos_, cam_.yPos_, cam_.zPos_, cam_.xDir_, cam_.yDir_, cam_.zDir_, 0.0f, 1.0f, 0.0f);

    glTranslatef(-1.0, -1.25, z_distance); // Left->Right, Bottom->Top

    for(int s = 0; s < nz; ++s){
	for(int r = 0; r < ny; ++r){
	// Start of row pointers
	const typename mf_t::value_type* v = &data_.img_(s, r, 0);
	const typename DATA::inp_type* i = &data_.inp_(s, r, 0);
	for(int c = 0; c < nx; ++c){
	    if(paint_inpainted_pixel_ || !i[c]){
		glPushMatrix();
	        glTranslatef(c * spacing, r * spacing, s * spacing); //Left->Right, Bottom->Top

		Eigen::Affine3f t = Eigen::Affine3f::Identity();
		t.linear() = v[c].cast<float>();

		#ifdef TV_VISUAL_DEBUG
		    std::cout << "Transformation matrix:\n" << t.matrix() << std::endl;
		#endif

		glMultMatrixf(t.data());

		glScalef(scaling, scaling, scaling);

		glBegin(GL_QUADS);        // Draw The Cube Using quads
		glColor3f(0.0f,1.0f,0.0f);    // Color Blue
		glVertex3f( 1.0f, 1.0f,-1.0f);    // Top Right Of The Quad (Top)
		glVertex3f(-1.0f, 1.0f,-1.0f);    // Top Left Of The Quad (Top)
		glVertex3f(-1.0f, 1.0f, 1.0f);    // Bottom Left Of The Quad (Top)
		glVertex3f( 1.0f, 1.0f, 1.0f);    // Bottom Right Of The Quad (Top)
		glColor3f(1.0f,0.5f,0.0f);    // Color Orange
		glVertex3f( 1.0f,-1.0f, 1.0f);    // Top Right Of The Quad (Bottom)
		glVertex3f(-1.0f,-1.0f, 1.0f);    // Top Left Of The Quad (Bottom)
		glVertex3f(-1.0f,-1.0f,-1.0f);    // Bottom Left Of The Quad (Bottom)
		glVertex3f( 1.0f,-1.0f,-1.0f);    // Bottom Right Of The Quad (Bottom)
		glColor3f(1.0f,0.0f,0.0f);    // Color Red
		glVertex3f( 1.0f, 1.0f, 1.0f);    // Top Right Of The Quad (Front)
		glVertex3f(-1.0f, 1.0f, 1.0f);    // Top Left Of The Quad (Front)
		glVertex3f(-1.0f,-1.0f, 1.0f);    // Bottom Left Of The Quad (Front)
		glVertex3f( 1.0f,-1.0f, 1.0f);    // Bottom Right Of The Quad (Front)
		glColor3f(1.0f,1.0f,0.0f);    // Color Yellow
		glVertex3f( 1.0f,-1.0f,-1.0f);    // Top Right Of The Quad (Back)
		glVertex3f(-1.0f,-1.0f,-1.0f);    // Top Left Of The Quad (Back)
		glVertex3f(-1.0f, 1.0f,-1.0f);    // Bottom Left Of The Quad (Back)
		glVertex3f( 1.0f, 1.0f,-1.0f);    // Bottom Right Of The Quad (Back)
		glColor3f(0.0f,0.0f,1.0f);    // Color Blue
		glVertex3f(-1.0f, 1.0f, 1.0f);    // Top Right Of The Quad (Left)
		glVertex3f(-1.0f, 1.0f,-1.0f);    // Top Left Of The Quad (Left)
		glVertex3f(-1.0f,-1.0f,-1.0f);    // Bottom Left Of The Quad (Left)
		glVertex3f(-1.0f,-1.0f, 1.0f);    // Bottom Right Of The Quad (Left)
		glColor3f(1.0f,0.0f,1.0f);    // Color Violet
		glVertex3f( 1.0f, 1.0f,-1.0f);    // Top Right Of The Quad (Right)
		glVertex3f( 1.0f, 1.0f, 1.0f);    // Top Left Of The Quad (Right)
		glVertex3f( 1.0f,-1.0f, 1.0f);    // Bottom Left Of The Quad (Right)
		glVertex3f( 1.0f,-1.0f,-1.0f);    // Bottom Right Of The Quad (Right)
		glEnd();            // End Drawing The Cube
		glPopMatrix();
	    }
	}
    }
}
    glutSwapBuffers();
}

template <class DATA>
void Visualization<SO, 3, DATA, 3>::reshape(int x, int y)
{
    if (y == 0 || x == 0) return;  //Nothing is visible then, so return

    width_ = x;
    height_ = y;

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(45.0,(GLdouble)x/(GLdouble)y,0.1,100.0);
    glViewport(0.0, 0.0 , x, y);  //Use the whole window for rendering
}

template <class DATA>
/* Callback handler for normal-key event */
void Visualization<SO, 3, DATA, 3>::keyboard(unsigned char key, int x, int y) {
   switch (key) {
         case 27:     // ESC key
	 case 113:	// q ley
	    glutLeaveMainLoop();
            break;
	case 115:
	    if(filename_ != "") storeImage(filename_);
	    break;
    }
}

template <class DATA>
/* Callback handler for special-key event */
void Visualization<SO, 3, DATA, 3>::specialKeys(int key, int x, int y) {
   #ifdef TV_VISUAL_CONTROLS_DEBUG
	std::cout << "BEFORE keypress: ";
	cam_.printParams();
    #endif
	switch (key) {
	case GLUT_KEY_RIGHT:
	    cam_.yAngle_ += 0.01f;
	    cam_.xDir_ = std::sin(cam_.yAngle_);
	    cam_.zDir_ = -std::cos(cam_.yAngle_);
	    break;
	case GLUT_KEY_LEFT:
	    cam_.yAngle_ -= 0.01f;
	    cam_.xDir_ = std::sin(cam_.yAngle_);
	    cam_.zDir_ = -std::cos(cam_.yAngle_);
	    break;
	case GLUT_KEY_UP:
	    cam_.xPos_ += cam_.xDir_ * cam_.step_;
	    cam_.yPos_ += cam_.yDir_ * cam_.step_;
	    cam_.zPos_ += cam_.zDir_ * cam_.step_;
	    break;
	case GLUT_KEY_DOWN:
	    cam_.xPos_ -= cam_.xDir_ * cam_.step_;
	    cam_.yPos_ -= cam_.yDir_ * cam_.step_;
	    cam_.zPos_ -= cam_.zDir_ * cam_.step_;
	    break;
	case GLUT_KEY_PAGE_UP:
	    break;
	case GLUT_KEY_PAGE_DOWN:
	    break;
      }

    #ifdef TV_VISUAL_CONTROLS_DEBUG
	std::cout << "AFTER keypress: ";
	cam_.printParams();
    #endif

}

template <class DATA>
void Visualization<SO, 3, DATA, 3>::saveImage(std::string filename){
    filename_ = filename;
}

template <class DATA>
void Visualization<SO, 3, DATA, 3>::storeImage(std::string filename){
    cv::Mat img(height_, width_, CV_8UC3);
    glPixelStorei(GL_PACK_ALIGNMENT, (img.step & 3)?1:4);
    glPixelStorei(GL_PACK_ROW_LENGTH, img.step/img.elemSize());
    glReadPixels(0, 0, img.cols, img.rows, GL_BGR_EXT, GL_UNSIGNED_BYTE, img.data);
    cv::Mat flipped(img);
    cv::flip(img, flipped, 0);
    cv::imwrite(filename, img);
}

template <class DATA>
void Visualization<SO, 3, DATA, 3>::GLInit(const char* window_name){

    int argc = 1;
    char** argv=0;
    glutInit(&argc, argv);

    //we initialilze the glut. functions
    glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_GLUTMAINLOOP_RETURNS);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB| GLUT_DEPTH);
    glutInitWindowPosition(100, 100);
    glutCreateWindow(window_name);

    glClearColor(1.0,1.0,1.0,0);

    glutDisplayFunc(getCFunctionPointer(&myType::draw,this));
    glutIdleFunc(getCFunctionPointer(&myType::draw,this));

	typedef void (*reshape_mptr)(int, int);
	Callback<void(int, int)>::func = std::bind(&myType::reshape, this, std::placeholders::_1, std::placeholders::_2);
	reshape_mptr reshape_ptr = static_cast<reshape_mptr>(Callback<void(int, int)>::callback);
    glutReshapeFunc(reshape_ptr);

	typedef void (*keyboard_mptr)(unsigned char, int, int);
	Callback<void(unsigned char, int, int)>::func = std::bind(&myType::keyboard, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
	keyboard_mptr keyboard_ptr = static_cast<keyboard_mptr>(Callback<void(unsigned char, int, int)>::callback);
    glutKeyboardFunc(keyboard_ptr);

	typedef void (*specialkeyboard_mptr)(int, int, int);
	Callback<void(int, int, int)>::func = std::bind(&myType::specialKeys, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
	specialkeyboard_mptr skeyboard_ptr = static_cast<specialkeyboard_mptr>(Callback<void(int, int, int)>::callback);
    glutSpecialFunc(skeyboard_ptr);

    glutMainLoop();
}

// IMPLEMENTATION SPD

template <class DATA>
void Visualization<SPD, 3, DATA, 3>::initLighting()
{

    // Set lighting intensity and color
    GLfloat light_ambient[]  = { 0.0f, 0.0f, 0.0f, 1.0f };
    GLfloat light_diffuse[]  = { 1.0f, 1.0f, 1.0f, 1.0f };
    GLfloat light_specular[] = { 1.0f, 1.0f, 1.0f, 1.0f };
    // Light source position
    GLfloat light_position[] = { 2.0f, 5.0f, 5.0f, 0.0f };

    // Enable lighting
    glEnable(GL_LIGHT0);
    glEnable(GL_NORMALIZE);
    glEnable(GL_COLOR_MATERIAL);
    glEnable(GL_LIGHTING);

     // Set lighting intensity and color
    glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
    glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
    glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);

    // Set the light position
     glLightfv(GL_LIGHT0, GL_POSITION, light_position);

}

template <class DATA>
void Visualization<SPD, 3, DATA, 3>::draw(void)
{
    // Color Definitions
    GLfloat mat_ambient[]    = { 0.7f, 0.7f, 0.7f, 1.0f };
    GLfloat mat_diffuse[]    = { 0.8f, 0.8f, 0.8f, 1.0f };
    GLfloat mat_specular[]   = { 1.0f, 1.0f, 1.0f, 1.0f };
    GLfloat high_shininess =  100.0f;

    glMatrixMode(GL_MODELVIEW);
    // clear the drawing buffer.
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
    // Enable depth test
    glEnable(GL_DEPTH_TEST);
    // Accept fragment if it closer to the camera than the former one
    glDepthFunc(GL_LESS);

    // Set material properties
    glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
    glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuse);
    glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
    glMaterialf(GL_FRONT, GL_SHININESS, high_shininess);

    int nz=data_.img_.nslices();
    int nx=data_.img_.ncols();
    int ny=data_.img_.nrows();

    int max = std::max(nx,std::max(ny,nz));

    float scaling = 2.0 / (3.0 * max);
    float spacing = 4.0 * scaling;

    float z_distance= -4.5;
    scaling *=0.5;

    #ifdef TV_VISUAL_DEBUG
	std::cout << "ny: " << ny << " nx: " << nx << std::endl;
	std::cout << "Maximum: " << max << " Scaling " << scaling << std::endl;
    #endif

    glLoadIdentity();

    gluLookAt(cam_.xPos_, cam_.yPos_, cam_.zPos_, cam_.xDir_, cam_.yDir_, cam_.zDir_, 0.0f, 1.0f, 0.0f);

    //glTranslatef(-1.2, 1.2, z_distance); // Left->Right, Top->Bottom
    glTranslatef(-1.0, -1.25, z_distance); // Left->Right, Bottom->Top

    for(int s = 0; s < nz; ++s){
	for(int r = 0; r < ny; ++r){
	    // Start of row pointers
	    const typename mf_t::value_type* v = &data_.img_(s, r, 0);
	    const typename DATA::inp_type* i = &data_.inp_(s, r, 0);
	    for(int c = 0; c < nx; ++c){
		if(paint_inpainted_pixel_ || !i[c]){
		    glPushMatrix();

		    //glTranslatef(coords(1) * spacing, -coords(0) * spacing, 0.0); //Left->Right, Top->Bottom
		    glTranslatef(c * spacing, r * spacing, s * spacing); //Left->Right, Bottom->Top

		    Eigen::Affine3f t = Eigen::Affine3f::Identity();
		    Eigen::SelfAdjointEigenSolver<typename mf_t::value_type> es(v[c]);

		    double mean_diffusity = es.eigenvalues().sum() / 3.0;

		    // anisotropic scaling transfomation and rotation
		    t.linear() = (es.eigenvectors() * (es.eigenvalues() / mean_diffusity).asDiagonal()).cast<float>();
		    //t.linear() = es.eigenvalues().cast<float>().asDiagonal();
		    glMultMatrixf(t.data());

		    #ifdef TV_VISUAL_DEBUG
			std::cout << "Transformation matrix:\n" << t.matrix() << std::endl;
		    #endif

		    // isotropic scaling transformation
		    glScalef(5.0 * scaling, 5.0 * scaling, 5.0 * scaling);

		    int argmax;
		    es.eigenvalues().maxCoeff(&argmax);
		    Eigen::Vector3d principal_direction = es.eigenvectors().col(argmax);
		    //principal_direction /= principal_direction.maxCoeff();
		    principal_direction.normalize();

		    glColor3d(std::abs(principal_direction(0)), std::abs(principal_direction(1)),std::abs(principal_direction(2)));
		    // draw sphere
		    glutSolidSphere(0.5, 35, 35);

		    // scaling to correct global size
		    glPopMatrix();
		}

	    }
	}
    }
    glutSwapBuffers();
}

template <class DATA>
void Visualization<SPD, 3, DATA, 3>::reshape(int x, int y)
{
    if (y == 0 || x == 0) return;  //Nothing is visible then, so return

    width_ = x;
    height_ = y;

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(45.0,(GLdouble)x/(GLdouble)y,0.1,100.0);
    glViewport(0.0, 0.0 , x, y);  //Use the whole window for rendering
}

template <class DATA>
/* Callback handler for normal-key event */
void Visualization<SPD, 3, DATA, 3>::keyboard(unsigned char key, int x, int y) {
   switch (key) {
         case 27:	// ESC key
	 case 113:	// q ley
	    glutLeaveMainLoop();
            break;
	case 115:
	    if(filename_ != "") storeImage(filename_);
	    break;
      }
}

template <class DATA>
/* Callback handler for special-key event */
void Visualization<SPD, 3, DATA, 3>::specialKeys(int key, int x, int y) {
   #ifdef TV_VISUAL_CONTROLS_DEBUG
	std::cout << "BEFORE keypress: ";
	cam_.printParams();
    #endif
	switch (key) {
	case GLUT_KEY_RIGHT:
	    cam_.yAngle_ += 0.01f;
	    cam_.xDir_ = std::sin(cam_.yAngle_);
	    cam_.zDir_ = -std::cos(cam_.yAngle_);
	    break;
	case GLUT_KEY_LEFT:
	    cam_.yAngle_ -= 0.01f;
	    cam_.xDir_ = std::sin(cam_.yAngle_);
	    cam_.zDir_ = -std::cos(cam_.yAngle_);
	    break;
	case GLUT_KEY_UP:
	    cam_.xPos_ += cam_.xDir_ * cam_.step_;
	    cam_.yPos_ += cam_.yDir_ * cam_.step_;
	    cam_.zPos_ += cam_.zDir_ * cam_.step_;
	    break;
	case GLUT_KEY_DOWN:
	    cam_.xPos_ -= cam_.xDir_ * cam_.step_;
	    cam_.yPos_ -= cam_.yDir_ * cam_.step_;
	    cam_.zPos_ -= cam_.zDir_ * cam_.step_;
	    break;
	case GLUT_KEY_PAGE_UP:
	    break;
	case GLUT_KEY_PAGE_DOWN:
	    break;
      }

    #ifdef TV_VISUAL_CONTROLS_DEBUG
	std::cout << "AFTER keypress: ";
	cam_.printParams();
    #endif

}

template <class DATA>
void Visualization<SPD, 3, DATA, 3>::saveImage(std::string filename){
    filename_ = filename;
}

template <class DATA>
void Visualization<SPD, 3, DATA, 3>::storeImage(std::string filename)
{
    std::cout << "Saving image...\n Width: " << width_ << "\n Height: " << height_ << "\n Filename: " << filename << std::endl;
    cv::Mat img(height_, width_, CV_8UC3);
    glPixelStorei(GL_PACK_ALIGNMENT, (img.step & 3)?1:4);
    glPixelStorei(GL_PACK_ROW_LENGTH, img.step/img.elemSize());
    glReadPixels(0, 0, img.cols, img.rows, GL_BGR_EXT, GL_UNSIGNED_BYTE, img.data);
    cv::Mat flipped(img);
    cv::flip(img, flipped, 0);
    cv::imwrite(filename, img);
}

template <class DATA>
void Visualization<SPD, 3, DATA, 3>::GLInit(const char* window_name){

    int argc = 1;
    char** argv=0;
    glutInit(&argc, argv);

    //we initialilze the glut. functions
    glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_GLUTMAINLOOP_RETURNS);

    glutInitDisplayMode(GLUT_DOUBLE| GLUT_RGB| GLUT_DEPTH);
    glutInitWindowPosition(100, 100);
    glutCreateWindow(window_name);
    initLighting();

    glClearColor(1.0,1.0,1.0,0);

    glutDisplayFunc(getCFunctionPointer(&myType::draw,this));
    glutIdleFunc(getCFunctionPointer(&myType::draw,this));

    typedef void (*reshape_mptr)(int, int);
    Callback<void(int, int)>::func = std::bind(&myType::reshape, this, std::placeholders::_1, std::placeholders::_2);
    reshape_mptr reshape_ptr = static_cast<reshape_mptr>(Callback<void(int, int)>::callback);
	glutReshapeFunc(reshape_ptr);

    typedef void (*keyboard_mptr)(unsigned char, int, int);
    Callback<void(unsigned char, int, int)>::func = std::bind(&myType::keyboard, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
    keyboard_mptr keyboard_ptr = static_cast<keyboard_mptr>(Callback<void(unsigned char, int, int)>::callback);
	    glutKeyboardFunc(keyboard_ptr);

    typedef void (*specialkeyboard_mptr)(int, int, int);
    Callback<void(int, int, int)>::func = std::bind(&myType::specialKeys, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
    specialkeyboard_mptr skeyboard_ptr = static_cast<specialkeyboard_mptr>(Callback<void(int, int, int)>::callback);
	    glutSpecialFunc(skeyboard_ptr);

    glutMainLoop();
}

//Implementation EUCLIDIAN
//

template <class DATA, int N>
void Visualization<EUCLIDIAN, N, DATA, 3>::initTextures(){

    int nz = data_.img_.nslices();
    int ny = data_.img_.nrows();
    int nx = data_.img_.ncols();

    int size = nz * ny * nx;
    char* RGBAbuffer = new char[size * 4];

    static_assert(N == 3 || N == 1, "Cube Visualization only possible for N=1 or N=3");

    int k = 0;
    if(N == 3){
	for(auto& p : data_.img_){
	    typename mf_t::value_type v = p * (double) std::numeric_limits<unsigned char>::max();
	    //std::cout << "Image Vector:\n" << p << std::endl;
	    //std::cout << "Scaling Factor: " << (int) std::numeric_limits<unsigned char>::max() << std::endl;
	    //std::cout << "Scaled Vector:\n " << v << std::endl;
	    double avg = v.sum() /3;
	    RGBAbuffer[k * 4]	= static_cast<char>(v[0]);
	    RGBAbuffer[k * 4 + 1]	= static_cast<char>(v[1]);
	    RGBAbuffer[k * 4 + 2]	= static_cast<char>(v[2]);
	    RGBAbuffer[k * 4 + 3]	= static_cast<char>(avg);
	    //for(int i = 0; i<4; ++i) std::cout << (int) RGBAbuffer[k*4 + i] << ", ";
	    //std::cout << std::endl;
	    ++k;
	}
    }
    else{
	for(auto& p : data_.img_){
	    double  v = p[0] * (double) std::numeric_limits<unsigned char>::max();
	    RGBAbuffer[k * 4]		= static_cast<char>(v);
	    RGBAbuffer[k * 4 + 1]	= static_cast<char>(v);
	    RGBAbuffer[k * 4 + 2]	= static_cast<char>(v);
	    RGBAbuffer[k * 4 + 3]	= static_cast<char>(v);
	    ++k;
	}
    }

    glGenTextures(1, (GLuint*) &texture3d_id);
    glBindTexture( GL_TEXTURE_3D, texture3d_id );
    glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);
    glTexParameteri(GL_TEXTURE_3D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);
    glTexParameteri(GL_TEXTURE_3D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER);
    glTexParameteri(GL_TEXTURE_3D, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_BORDER);
    glTexParameteri(GL_TEXTURE_3D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_3D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);

    glTexImage3D(GL_TEXTURE_3D, 0, GL_RGBA, nx, ny, nz, 0, GL_RGBA, GL_UNSIGNED_BYTE, RGBAbuffer );
    glBindTexture( GL_TEXTURE_3D, 0 );

    delete[] RGBAbuffer;
}

template <class DATA, int N>
void Visualization<EUCLIDIAN, N, DATA, 3>::draw(void)
{

    float dOrthoSize = 1.0;

    glMatrixMode(GL_MODELVIEW);
    // clear the drawing buffer.
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
    // Enable depth test
    //glEnable(GL_DEPTH_TEST);
    //glDepthFunc(GL_LESS);

    glEnable(GL_ALPHA_TEST );
    glAlphaFunc(GL_GREATER, 0.05f);

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

    int nz = data_.img_.nslices();
    int nx = data_.img_.ncols();
    int ny = data_.img_.nrows();

    #ifdef TV_VISUAL_DEBUG
	std::cout << "ny: " << ny << " nx: " << nx << std::endl;
    #endif

    glMatrixMode(GL_TEXTURE);
    glLoadIdentity();
    glTranslatef(0.5f, 0.5f, 0.5f);
    glScaled( (float)nx /(float) nx, -1.0f*(float) nx/(float)ny, (float)nx/(float)nz);
    glRotated(cam_.yAngle_, 0.0, 1.0, 0.0);
    glTranslatef(-0.5f, -0.5f, -0.5f);

    //gluLookAt(cam_.xPos_, cam_.yPos_, cam_.zPos_, cam_.xDir_, cam_.yDir_, cam_.zDir_, 0.0f, 1.0f, 0.0f);

    glEnable(GL_TEXTURE_3D);
    glBindTexture(GL_TEXTURE_3D, texture3d_id);

    for (float fIndx = -1.0f; fIndx <= 1.0f; fIndx+=0.01f ){
	glBegin(GL_QUADS);
	glTexCoord3f(0.0f, 0.0f, ((float)fIndx+1.0f)/2.0f);
        glVertex3f(-dOrthoSize,-dOrthoSize,fIndx);
        glTexCoord3f(1.0f, 0.0f, ((float)fIndx+1.0f)/2.0f);
        glVertex3f(dOrthoSize,-dOrthoSize,fIndx);
        glTexCoord3f(1.0f, 1.0f, ((float)fIndx+1.0f)/2.0f);
        glVertex3f(dOrthoSize,dOrthoSize,fIndx);
        glTexCoord3f(0.0f, 1.0f, ((float)fIndx+1.0f)/2.0f);
        glVertex3f(-dOrthoSize,dOrthoSize,fIndx);
	glEnd();
    }

    glutSwapBuffers();
}

template <class DATA, int N>
void Visualization<EUCLIDIAN, N, DATA, 3>::reshape(int x, int y)
{
    width_ = x;    height_ = y;
    GLfloat dOrthoSize = 1.0f;
    GLdouble AspectRatio = ( GLdouble )(x) / ( GLdouble )(y);

    glViewport(0.0, 0.0 , x, y);  //Use the whole window for rendering
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();

    if( x <= y )
	glOrtho( -dOrthoSize, dOrthoSize, -( dOrthoSize / AspectRatio ) ,dOrthoSize / AspectRatio, 2.0f*-dOrthoSize, 2.0f*dOrthoSize );
    else
	glOrtho( -dOrthoSize * AspectRatio, dOrthoSize * AspectRatio, -dOrthoSize, dOrthoSize, 2.0f*-dOrthoSize, 2.0f*dOrthoSize );

    glMatrixMode( GL_MODELVIEW );
    glLoadIdentity();
}

template <class DATA, int N>
/* Callback handler for normal-key event */
void Visualization<EUCLIDIAN, N, DATA, 3>::keyboard(unsigned char key, int x, int y) {
   switch (key) {
         case 27:     // ESC key
	 case 113:	// q ley
	    glutLeaveMainLoop();
            break;
	case 115:
	    if(filename_ != "") storeImage(filename_);
	    break;
    }
}

template <class DATA, int N>
/* Callback handler for special-key event */
void Visualization<EUCLIDIAN, N, DATA, 3>::specialKeys(int key, int x, int y) {
   #ifdef TV_VISUAL_CONTROLS_DEBUG
	std::cout << "BEFORE keypress: ";
	cam_.printParams();
    #endif
	switch (key) {
	case GLUT_KEY_RIGHT:
	    cam_.yAngle_ += 0.01f;
	    cam_.xDir_ = std::sin(cam_.yAngle_);
	    cam_.zDir_ = -std::cos(cam_.yAngle_);
	    break;
	case GLUT_KEY_LEFT:
	    cam_.yAngle_ -= 0.01f;
	    cam_.xDir_ = std::sin(cam_.yAngle_);
	    cam_.zDir_ = -std::cos(cam_.yAngle_);
	    break;
	case GLUT_KEY_UP:
	    cam_.xPos_ += cam_.xDir_ * cam_.step_;
	    cam_.yPos_ += cam_.yDir_ * cam_.step_;
	    cam_.zPos_ += cam_.zDir_ * cam_.step_;
	    break;
	case GLUT_KEY_DOWN:
	    cam_.xPos_ -= cam_.xDir_ * cam_.step_;
	    cam_.yPos_ -= cam_.yDir_ * cam_.step_;
	    cam_.zPos_ -= cam_.zDir_ * cam_.step_;
	    break;
	case GLUT_KEY_PAGE_UP:
	    break;
	case GLUT_KEY_PAGE_DOWN:
	    break;
      }

    #ifdef TV_VISUAL_CONTROLS_DEBUG
	std::cout << "AFTER keypress: ";
	cam_.printParams();
    #endif

}

template <class DATA, int N>
void Visualization<EUCLIDIAN, N, DATA, 3>::animation(){
    cam_.yAngle_+=0.50;
    draw();
}

template <class DATA, int N>
void Visualization<EUCLIDIAN, N, DATA, 3>::saveImage(std::string filename){
    filename_ = filename;
}

template <class DATA, int N>
void Visualization<EUCLIDIAN, N, DATA, 3>::storeImage(std::string filename){
    cv::Mat img(height_, width_, CV_8UC3);
    glPixelStorei(GL_PACK_ALIGNMENT, (img.step & 3)?1:4);
    glPixelStorei(GL_PACK_ROW_LENGTH, img.step/img.elemSize());
    glReadPixels(0, 0, img.cols, img.rows, GL_BGR_EXT, GL_UNSIGNED_BYTE, img.data);
    cv::Mat flipped(img);
    cv::flip(img, flipped, 0);
    cv::imwrite(filename, img);
}

template <class DATA, int N>
void Visualization<EUCLIDIAN, N, DATA, 3>::GLInit(const char* window_name){

    int argc = 1;
    char** argv=0;
    glutInit(&argc, argv);

    //we initialilze the glut. functions
    glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_GLUTMAINLOOP_RETURNS);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
    glutInitWindowPosition(100, 100);
    glutCreateWindow(window_name);

    glClearColor(0.0, 0.0, 0.0, 0.0);
    glewInit();
    initTextures();

    glutDisplayFunc(getCFunctionPointer(&myType::draw,this));
    glutIdleFunc(getCFunctionPointer(&myType::animation,this));

	typedef void (*reshape_mptr)(int, int);
	Callback<void(int, int)>::func = std::bind(&myType::reshape, this, std::placeholders::_1, std::placeholders::_2);
	reshape_mptr reshape_ptr = static_cast<reshape_mptr>(Callback<void(int, int)>::callback);
    glutReshapeFunc(reshape_ptr);

	typedef void (*keyboard_mptr)(unsigned char, int, int);
	Callback<void(unsigned char, int, int)>::func = std::bind(&myType::keyboard, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
	keyboard_mptr keyboard_ptr = static_cast<keyboard_mptr>(Callback<void(unsigned char, int, int)>::callback);
    glutKeyboardFunc(keyboard_ptr);

	typedef void (*specialkeyboard_mptr)(int, int, int);
	Callback<void(int, int, int)>::func = std::bind(&myType::specialKeys, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
	specialkeyboard_mptr skeyboard_ptr = static_cast<specialkeyboard_mptr>(Callback<void(int, int, int)>::callback);
    glutSpecialFunc(skeyboard_ptr);

    glutMainLoop();
}

} // end namespace tvmtl
#endif


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2013-2013 by Ryo IGARASHI <rigarash@issp.u-tokyo.ac.jp>,
*               2014-2014 by Synge Todo <wistaria@comp-phys.org>,
*               2014-2014 by Tatsuya Sakashita <t-sakashita@issp.u-tokyo.ac.jp>
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>

#include <rokko/solver.hpp>

#define BOOST_TEST_MODULE test_parallel_dense_solvers
#ifndef BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

BOOST_AUTO_TEST_CASE(test_parallel_dense_solvers) {
  BOOST_FOREACH(std::string name, rokko::parallel_dense_solver::solvers()) {
    std::cerr << name << std::endl;
  }
}


//=====================================================
// File   :  hand_vec_interface.hh
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
//=====================================================
//
// 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, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
#ifndef HAND_VEC_INTERFACE_HH
#define HAND_VEC_INTERFACE_HH

#include <Eigen/Core>
#include "f77_interface.hh"

using namespace Eigen;

template<class real>
class hand_vec_interface : public f77_interface_base<real> {

public :

  typedef typename internal::packet_traits<real>::type Packet;
  static const int PacketSize = internal::packet_traits<real>::size;

  typedef typename f77_interface_base<real>::stl_matrix stl_matrix;
  typedef typename f77_interface_base<real>::stl_vector stl_vector;
  typedef typename f77_interface_base<real>::gene_matrix gene_matrix;
  typedef typename f77_interface_base<real>::gene_vector gene_vector;

  static void free_matrix(gene_matrix & A, int N){
    internal::aligned_free(A);
  }

  static void free_vector(gene_vector & B){
    internal::aligned_free(B);
  }

  static inline void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){
    int N = A_stl.size();
    A = (real*)internal::aligned_malloc(N*N*sizeof(real));
    for (int j=0;j<N;j++)
      for (int i=0;i<N;i++)
        A[i+N*j] = A_stl[j][i];
  }

  static inline void vector_from_stl(gene_vector & B, stl_vector & B_stl){
    int N = B_stl.size();
    B = (real*)internal::aligned_malloc(N*sizeof(real));
    for (int i=0;i<N;i++)
      B[i] = B_stl[i];
  }

  static inline std::string name() {
    #ifdef PEELING
    return "hand_vectorized_peeling";
    #else
    return "hand_vectorized";
    #endif
  }

  static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
  {
    asm("#begin matrix_vector_product");
    int AN = (N/PacketSize)*PacketSize;
    int ANP = (AN/(2*PacketSize))*2*PacketSize;
    int bound = (N/4)*4;
    for (int i=0;i<N;i++)
      X[i] = 0;

    for (int i=0;i<bound;i+=4)
    {
      register real* __restrict__ A0 = A + i*N;
      register real* __restrict__ A1 = A + (i+1)*N;
      register real* __restrict__ A2 = A + (i+2)*N;
      register real* __restrict__ A3 = A + (i+3)*N;

      Packet ptmp0 = internal::pset1(B[i]);
      Packet ptmp1 = internal::pset1(B[i+1]);
      Packet ptmp2 = internal::pset1(B[i+2]);
      Packet ptmp3 = internal::pset1(B[i+3]);
//       register Packet ptmp0, ptmp1, ptmp2, ptmp3;
//       asm(
//
//           "movss     (%[B],%[j],4), %[ptmp0]  \n\t"
//           "shufps   $0,%[ptmp0],%[ptmp0] \n\t"
//           "movss    4(%[B],%[j],4), %[ptmp1]  \n\t"
//           "shufps   $0,%[ptmp1],%[ptmp1] \n\t"
//           "movss    8(%[B],%[j],4), %[ptmp2]  \n\t"
//           "shufps   $0,%[ptmp2],%[ptmp2] \n\t"
//           "movss   12(%[B],%[j],4), %[ptmp3]  \n\t"
//           "shufps   $0,%[ptmp3],%[ptmp3] \n\t"
//           : [ptmp0] "=x" (ptmp0),
//             [ptmp1] "=x" (ptmp1),
//             [ptmp2] "=x" (ptmp2),
//             [ptmp3] "=x" (ptmp3)
//           : [B] "r" (B),
//             [j] "r" (size_t(i))
//           : );

      if (AN>0)
      {
//         for (size_t j = 0;j<ANP;j+=8)
//         {
//           asm(
//
//           "movaps     (%[A0],%[j],4), %%xmm8  \n\t"
//           "movaps   16(%[A0],%[j],4), %%xmm12 \n\t"
//           "movups     (%[A3],%[j],4), %%xmm11 \n\t"
//           "movups   16(%[A3],%[j],4), %%xmm15 \n\t"
//           "movups     (%[A2],%[j],4), %%xmm10 \n\t"
//           "movups   16(%[A2],%[j],4), %%xmm14 \n\t"
//           "movups     (%[A1],%[j],4), %%xmm9  \n\t"
//           "movups   16(%[A1],%[j],4), %%xmm13 \n\t"
//
//           "mulps %[ptmp0], %%xmm8  \n\t"
//           "addps (%[res0],%[j],4), %%xmm8  \n\t"
//           "mulps %[ptmp3], %%xmm11 \n\t"
//           "addps %%xmm11, %%xmm8  \n\t"
//           "mulps %[ptmp2], %%xmm10 \n\t"
//           "addps %%xmm10, %%xmm8  \n\t"
//           "mulps %[ptmp1], %%xmm9  \n\t"
//           "addps %%xmm9, %%xmm8   \n\t"
//           "movaps %%xmm8, (%[res0],%[j],4)  \n\t"
//
//           "mulps %[ptmp0], %%xmm12 \n\t"
//           "addps 16(%[res0],%[j],4), %%xmm12  \n\t"
//           "mulps %[ptmp3], %%xmm15 \n\t"
//           "addps %%xmm15, %%xmm12  \n\t"
//           "mulps %[ptmp2], %%xmm14 \n\t"
//           "addps %%xmm14, %%xmm12  \n\t"
//           "mulps %[ptmp1], %%xmm13 \n\t"
//           "addps %%xmm13, %%xmm12  \n\t"
//           "movaps %%xmm12, 16(%[res0],%[j],4) \n\t"
//           :
//           : [res0] "r" (X), [j] "r" (j),[A0] "r" (A0),
//             [A1] "r" (A1),
//             [A2] "r" (A2),
//             [A3] "r" (A3),
//             [ptmp0] "x" (ptmp0),
//             [ptmp1] "x" (ptmp1),
//             [ptmp2] "x" (ptmp2),
//             [ptmp3] "x" (ptmp3)
//           : "%xmm8", "%xmm9", "%xmm10", "%xmm11", "%xmm12", "%xmm13", "%xmm14", "%xmm15", "%r14");
//         }
          register Packet A00;
          register Packet A01;
          register Packet A02;
          register Packet A03;
          register Packet A10;
          register Packet A11;
          register Packet A12;
          register Packet A13;
          for (int j = 0;j<ANP;j+=2*PacketSize)
          {
//             A00 = internal::pload(&A0[j]);
//             A01 = internal::ploadu(&A1[j]);
//             A02 = internal::ploadu(&A2[j]);
//             A03 = internal::ploadu(&A3[j]);
//             A10 = internal::pload(&A0[j+PacketSize]);
//             A11 = internal::ploadu(&A1[j+PacketSize]);
//             A12 = internal::ploadu(&A2[j+PacketSize]);
//             A13 = internal::ploadu(&A3[j+PacketSize]);
//
//             A00 = internal::pmul(ptmp0, A00);
//             A01 = internal::pmul(ptmp1, A01);
//             A02 = internal::pmul(ptmp2, A02);
//             A03 = internal::pmul(ptmp3, A03);
//             A10 = internal::pmul(ptmp0, A10);
//             A11 = internal::pmul(ptmp1, A11);
//             A12 = internal::pmul(ptmp2, A12);
//             A13 = internal::pmul(ptmp3, A13);
//
//             A00 = internal::padd(A00,A01);
//             A02 = internal::padd(A02,A03);
//             A00 = internal::padd(A00,internal::pload(&X[j]));
//             A00 = internal::padd(A00,A02);
//             internal::pstore(&X[j],A00);
//
//             A10 = internal::padd(A10,A11);
//             A12 = internal::padd(A12,A13);
//             A10 = internal::padd(A10,internal::pload(&X[j+PacketSize]));
//             A10 = internal::padd(A10,A12);
//             internal::pstore(&X[j+PacketSize],A10);

            internal::pstore(&X[j],
              internal::padd(internal::pload(&X[j]),
                internal::padd(
                  internal::padd(internal::pmul(ptmp0,internal::pload(&A0[j])),internal::pmul(ptmp1,internal::ploadu(&A1[j]))),
                  internal::padd(internal::pmul(ptmp2,internal::ploadu(&A2[j])),internal::pmul(ptmp3,internal::ploadu(&A3[j]))) )));

            internal::pstore(&X[j+PacketSize],
              internal::padd(internal::pload(&X[j+PacketSize]),
                internal::padd(
                  internal::padd(internal::pmul(ptmp0,internal::pload(&A0[j+PacketSize])),internal::pmul(ptmp1,internal::ploadu(&A1[j+PacketSize]))),
                  internal::padd(internal::pmul(ptmp2,internal::ploadu(&A2[j+PacketSize])),internal::pmul(ptmp3,internal::ploadu(&A3[j+PacketSize]))) )));
          }
          for (int j = ANP;j<AN;j+=PacketSize)
            internal::pstore(&X[j],
              internal::padd(internal::pload(&X[j]),
                internal::padd(
                  internal::padd(internal::pmul(ptmp0,internal::pload(&A0[j])),internal::pmul(ptmp1,internal::ploadu(&A1[j]))),
                  internal::padd(internal::pmul(ptmp2,internal::ploadu(&A2[j])),internal::pmul(ptmp3,internal::ploadu(&A3[j]))) )));
      }
      // process remaining scalars
      for (int j=AN;j<N;j++)
        X[j] += internal::pfirst(ptmp0) * A0[j] + internal::pfirst(ptmp1) * A1[j] + internal::pfirst(ptmp2) * A2[j] + internal::pfirst(ptmp3) * A3[j];
    }
    for (int i=bound;i<N;i++)
    {
      real tmp0 = B[i];
      Packet ptmp0 = internal::pset1(tmp0);
      int iN0 = i*N;
      if (AN>0)
      {
        bool aligned0 = (iN0 % PacketSize) == 0;
        if (aligned0)
          for (int j = 0;j<AN;j+=PacketSize)
            internal::pstore(&X[j], internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),internal::pload(&X[j])));
        else
          for (int j = 0;j<AN;j+=PacketSize)
            internal::pstore(&X[j], internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),internal::pload(&X[j])));
      }
      // process remaining scalars
      for (int j=AN;j<N;j++)
        X[j] += tmp0 * A[j+iN0];
    }
    asm("#end matrix_vector_product");
  }

  static inline void symv(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
  {

//     int AN = (N/PacketSize)*PacketSize;
//     int ANP = (AN/(2*PacketSize))*2*PacketSize;
//     int bound = (N/4)*4;
    for (int i=0;i<N;i++)
      X[i] = 0;

    int bound = std::max(0,N-8) & 0xfffffffE;

    for (int j=0;j<bound;j+=2)
    {
      register real* __restrict__ A0 = A + j*N;
      register real* __restrict__ A1 = A + (j+1)*N;

      real t0 = B[j];
      Packet ptmp0 = internal::pset1(t0);
      real t1 = B[j+1];
      Packet ptmp1 = internal::pset1(t1);

      real t2 = 0;
      Packet ptmp2 = internal::pset1(t2);
      real t3 = 0;
      Packet ptmp3 = internal::pset1(t3);

      int starti = j+2;
      int alignedEnd = starti;
      int alignedStart = (starti) + internal::first_aligned(&X[starti], N-starti);
      alignedEnd = alignedStart + ((N-alignedStart)/(PacketSize))*(PacketSize);

      X[j]   += t0 * A0[j];
      X[j+1] += t1 * A1[j];

      X[j+1] += t0 * A0[j+1];
      t2 += A0[j+1] * B[j+1];

//       alignedStart = alignedEnd;
      for (int i=starti; i<alignedStart; ++i) {
        X[i] += t0 * A0[i] + t1 * A1[i];
        t2 += A0[i] * B[i];
        t3 += A1[i] * B[i];
      }
      asm("#begin symv");
      for (size_t i=alignedStart; i<alignedEnd; i+=PacketSize) {
        Packet A0i = internal::ploadu(&A0[i]);
        Packet A1i = internal::ploadu(&A1[i]);
//         Packet A0i1 = internal::ploadu(&A0[i+PacketSize]);
        Packet Xi = internal::pload(&X[i]);
        Packet Bi = internal::pload/*u*/(&B[i]);
//         Packet Xi1 = internal::pload(&X[i+PacketSize]);
//         Packet Bi1 = internal::pload/*u*/(&B[i+PacketSize]);
        Xi = internal::padd(internal::padd(Xi, internal::pmul(ptmp0, A0i)), internal::pmul(ptmp1, A1i));
        ptmp2 = internal::padd(ptmp2, internal::pmul(A0i, Bi));
        ptmp3 = internal::padd(ptmp3, internal::pmul(A1i, Bi));
//         Xi1 = internal::padd(Xi1, internal::pmul(ptmp1, A0i1));
//         ptmp2 = internal::padd(ptmp2, internal::pmul(A0i1, Bi1));
//
        internal::pstore(&X[i],Xi);
//         internal::pstore(&X[i+PacketSize],Xi1);
//         asm(
//           "prefetchnta   64(%[A0],%[i],4)   \n\t"
//           //"movups     (%[A0],%[i],4), %%xmm8  \n\t"
//           "movsd       (%[A0],%[i],4), %%xmm8  \n\t"
//           "movhps     8(%[A0],%[i],4), %%xmm8  \n\t"
// //           "movups   16(%[A0],%[i],4), %%xmm9  \n\t"
// //           "movups   64(%[A0],%[i],4), %%xmm15  \n\t"
//           "movaps     (%[B], %[i],4), %%xmm12 \n\t"
// //           "movaps   16(%[B], %[i],4), %%xmm13 \n\t"
//           "movaps     (%[X], %[i],4), %%xmm10 \n\t"
// //           "movaps   16(%[X], %[i],4), %%xmm11 \n\t"
//
//           "mulps %%xmm8, %%xmm12  \n\t"
// //           "mulps %%xmm9, %%xmm13  \n\t"
//
//           "mulps %[ptmp1], %%xmm8  \n\t"
//           "addps %%xmm12, %[ptmp2]  \n\t"
//           "addps %%xmm8, %%xmm10  \n\t"
//
//
//
//
// //           "mulps %[ptmp1], %%xmm9  \n\t"
//
// //           "addps %%xmm9, %%xmm11  \n\t"
// //           "addps %%xmm13, %[ptmp2]  \n\t"
//
//           "movaps %%xmm10,   (%[X],%[i],4) \n\t"
// //           "movaps %%xmm11, 16(%[X],%[i],4) \n\t"
//           :
//           : [X] "r" (X), [i] "r" (i), [A0] "r" (A0),
//             [B] "r" (B),
//             [ptmp1] "x" (ptmp1),
//             [ptmp2] "x" (ptmp2)
//           : "%xmm8", "%xmm9", "%xmm10", "%xmm11", "%xmm12", "%xmm13", "%xmm15");
      }
      asm("#end symv");
      for (int i=alignedEnd; i<N; i++) {
        X[i] += t0 * A0[i] + t1 * A1[i];
        t2 += A0[i] * B[i];
        t3 += A1[i] * B[i];
      }

      X[j]   += t2 + internal::predux(ptmp2);
      X[j+1] += t3 + internal::predux(ptmp3);
    }
    for (int j=bound;j<N;j++)
    {
      register real* __restrict__ A0 = A + j*N;

      real t1 = B[j];
      real t2 = 0;
      X[j] += t1 * A0[j];
      for (int i=j+1; i<N; i+=PacketSize) {
        X[i] += t1 * A0[i];
        t2 += A0[i] * B[i];
      }
      X[j] += t2;
    }

  }

//   static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
//   {
//     asm("#begin matrix_vector_product");
//     int AN = (N/PacketSize)*PacketSize;
//     int ANP = (AN/(2*PacketSize))*2*PacketSize;
//     int bound = (N/4)*4;
//     for (int i=0;i<N;i++)
//       X[i] = 0;
//
//     for (int i=0;i<bound;i+=4)
//     {
//       real tmp0 = B[i];
//       Packet ptmp0 = internal::pset1(tmp0);
//       real tmp1 = B[i+1];
//       Packet ptmp1 = internal::pset1(tmp1);
//       real tmp2 = B[i+2];
//       Packet ptmp2 = internal::pset1(tmp2);
//       real tmp3 = B[i+3];
//       Packet ptmp3 = internal::pset1(tmp3);
//       int iN0 = i*N;
//       int iN1 = (i+1)*N;
//       int iN2 = (i+2)*N;
//       int iN3 = (i+3)*N;
//       if (AN>0)
//       {
// //         int aligned0 = (iN0 % PacketSize);
//         int aligned1 = (iN1 % PacketSize);
//
//         if (aligned1==0)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pload(&X[j]),
//                 internal::padd(
//                   internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),internal::pmul(ptmp1,internal::pload(&A[j+iN1]))),
//                   internal::padd(internal::pmul(ptmp2,internal::pload(&A[j+iN2])),internal::pmul(ptmp3,internal::pload(&A[j+iN3]))) )));
//           }
//         }
//         else if (aligned1==2)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pload(&X[j]),
//                 internal::padd(
//                   internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),internal::pmul(ptmp1,internal::ploadu(&A[j+iN1]))),
//                   internal::padd(internal::pmul(ptmp2,internal::pload(&A[j+iN2])),internal::pmul(ptmp3,internal::ploadu(&A[j+iN3]))) )));
//           }
//         }
//         else
//         {
//           for (int j = 0;j<ANP;j+=2*PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pload(&X[j]),
//                 internal::padd(
//                   internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),internal::pmul(ptmp1,internal::ploadu(&A[j+iN1]))),
//                   internal::padd(internal::pmul(ptmp2,internal::ploadu(&A[j+iN2])),internal::pmul(ptmp3,internal::ploadu(&A[j+iN3]))) )));
//
//             internal::pstore(&X[j+PacketSize],
//               internal::padd(internal::pload(&X[j+PacketSize]),
//                 internal::padd(
//                   internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+PacketSize+iN0])),internal::pmul(ptmp1,internal::ploadu(&A[j+PacketSize+iN1]))),
//                   internal::padd(internal::pmul(ptmp2,internal::ploadu(&A[j+PacketSize+iN2])),internal::pmul(ptmp3,internal::ploadu(&A[j+PacketSize+iN3]))) )));
//
// //             internal::pstore(&X[j+2*PacketSize],
// //               internal::padd(internal::pload(&X[j+2*PacketSize]),
// //                 internal::padd(
// //                   internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+2*PacketSize+iN0])),internal::pmul(ptmp1,internal::ploadu(&A[j+2*PacketSize+iN1]))),
// //                   internal::padd(internal::pmul(ptmp2,internal::ploadu(&A[j+2*PacketSize+iN2])),internal::pmul(ptmp3,internal::ploadu(&A[j+2*PacketSize+iN3]))) )));
// //
// //             internal::pstore(&X[j+3*PacketSize],
// //               internal::padd(internal::pload(&X[j+3*PacketSize]),
// //                 internal::padd(
// //                   internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+3*PacketSize+iN0])),internal::pmul(ptmp1,internal::ploadu(&A[j+3*PacketSize+iN1]))),
// //                   internal::padd(internal::pmul(ptmp2,internal::ploadu(&A[j+3*PacketSize+iN2])),internal::pmul(ptmp3,internal::ploadu(&A[j+3*PacketSize+iN3]))) )));
//
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             internal::pstore(&X[j],
//               internal::padd(internal::pload(&X[j]),
//                 internal::padd(
//                   internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),internal::pmul(ptmp1,internal::ploadu(&A[j+iN1]))),
//                   internal::padd(internal::pmul(ptmp2,internal::ploadu(&A[j+iN2])),internal::pmul(ptmp3,internal::ploadu(&A[j+iN3]))) )));
//         }
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp0 * A[j+iN0] + tmp1 * A[j+iN1] + tmp2 * A[j+iN2] + tmp3 * A[j+iN3];
//     }
//     for (int i=bound;i<N;i++)
//     {
//       real tmp0 = B[i];
//       Packet ptmp0 = internal::pset1(tmp0);
//       int iN0 = i*N;
//       if (AN>0)
//       {
//         bool aligned0 = (iN0 % PacketSize) == 0;
//         if (aligned0)
//           for (int j = 0;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),internal::pload(&X[j])));
//         else
//           for (int j = 0;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),internal::pload(&X[j])));
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp0 * A[j+iN0];
//     }
//     asm("#end matrix_vector_product");
//   }

//   static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
//   {
//     asm("#begin matrix_vector_product");
//     int AN = (N/PacketSize)*PacketSize;
//     for (int i=0;i<N;i++)
//       X[i] = 0;
//
//     for (int i=0;i<N;i+=2)
//     {
//       real tmp0 = B[i];
//       Packet ptmp0 = internal::pset1(tmp0);
//       real tmp1 = B[i+1];
//       Packet ptmp1 = internal::pset1(tmp1);
//       int iN0 = i*N;
//       int iN1 = (i+1)*N;
//       if (AN>0)
//       {
//         bool aligned0 = (iN0 % PacketSize) == 0;
//         bool aligned1 = (iN1 % PacketSize) == 0;
//
//         if (aligned0 && aligned1)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::pload(&A[j+iN1])),internal::pload(&X[j]))));
//           }
//         }
//         else if (aligned0)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::ploadu(&A[j+iN1])),internal::pload(&X[j]))));
//           }
//         }
//         else if (aligned1)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::pload(&A[j+iN1])),internal::pload(&X[j]))));
//           }
//         }
//         else
//         {
//           int ANP = (AN/(4*PacketSize))*4*PacketSize;
//           for (int j = 0;j<ANP;j+=4*PacketSize)
//           {
//             internal::pstore(&X[j],
//               internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::ploadu(&A[j+iN1])),internal::pload(&X[j]))));
//
//             internal::pstore(&X[j+PacketSize],
//               internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+PacketSize+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::ploadu(&A[j+PacketSize+iN1])),internal::pload(&X[j+PacketSize]))));
//
//             internal::pstore(&X[j+2*PacketSize],
//               internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+2*PacketSize+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::ploadu(&A[j+2*PacketSize+iN1])),internal::pload(&X[j+2*PacketSize]))));
//
//             internal::pstore(&X[j+3*PacketSize],
//               internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+3*PacketSize+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::ploadu(&A[j+3*PacketSize+iN1])),internal::pload(&X[j+3*PacketSize]))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             internal::pstore(&X[j],
//               internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),
//               internal::padd(internal::pmul(ptmp1,internal::ploadu(&A[j+iN1])),internal::pload(&X[j]))));
//         }
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp0 * A[j+iN0] + tmp1 * A[j+iN1];
//     }
//     int remaining = (N/2)*2;
//     for (int i=remaining;i<N;i++)
//     {
//       real tmp0 = B[i];
//       Packet ptmp0 = internal::pset1(tmp0);
//       int iN0 = i*N;
//       if (AN>0)
//       {
//         bool aligned0 = (iN0 % PacketSize) == 0;
//         if (aligned0)
//           for (int j = 0;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pmul(ptmp0,internal::pload(&A[j+iN0])),internal::pload(&X[j])));
//         else
//           for (int j = 0;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pmul(ptmp0,internal::ploadu(&A[j+iN0])),internal::pload(&X[j])));
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp0 * A[j+iN0];
//     }
//     asm("#end matrix_vector_product");
//   }

//   static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
//   {
//     asm("#begin matrix_vector_product");
//     int AN = (N/PacketSize)*PacketSize;
//     for (int i=0;i<N;i++)
//       X[i] = 0;
//     for (int i=0;i<N;i++)
//     {
//       real tmp = B[i];
//       Packet ptmp = internal::pset1(tmp);
//       int iN = i*N;
//       if (AN>0)
//       {
//         bool aligned = (iN % PacketSize) == 0;
//         if (aligned)
//         {
//           #ifdef PEELING
//           Packet A0, A1, A2, X0, X1, X2;
//           int ANP = (AN/(8*PacketSize))*8*PacketSize;
//           for (int j = 0;j<ANP;j+=PacketSize*8)
//           {
//             A0 = internal::pload(&A[j+iN]);
//             X0 = internal::pload(&X[j]);
//             A1 = internal::pload(&A[j+PacketSize+iN]);
//             X1 = internal::pload(&X[j+PacketSize]);
//             A2 = internal::pload(&A[j+2*PacketSize+iN]);
//             X2 = internal::pload(&X[j+2*PacketSize]);
//             internal::pstore(&X[j], internal::padd(X0, internal::pmul(ptmp,A0)));
//             A0 = internal::pload(&A[j+3*PacketSize+iN]);
//             X0 = internal::pload(&X[j+3*PacketSize]);
//             internal::pstore(&X[j+PacketSize], internal::padd(internal::pload(&X1), internal::pmul(ptmp,A1)));
//             A1 = internal::pload(&A[j+4*PacketSize+iN]);
//             X1 = internal::pload(&X[j+4*PacketSize]);
//             internal::pstore(&X[j+2*PacketSize], internal::padd(internal::pload(&X2), internal::pmul(ptmp,A2)));
//             A2 = internal::pload(&A[j+5*PacketSize+iN]);
//             X2 = internal::pload(&X[j+5*PacketSize]);
//             internal::pstore(&X[j+3*PacketSize], internal::padd(internal::pload(&X0), internal::pmul(ptmp,A0)));
//             A0 = internal::pload(&A[j+6*PacketSize+iN]);
//             X0 = internal::pload(&X[j+6*PacketSize]);
//             internal::pstore(&X[j+4*PacketSize], internal::padd(internal::pload(&X1), internal::pmul(ptmp,A1)));
//             A1 = internal::pload(&A[j+7*PacketSize+iN]);
//             X1 = internal::pload(&X[j+7*PacketSize]);
//             internal::pstore(&X[j+5*PacketSize], internal::padd(internal::pload(&X2), internal::pmul(ptmp,A2)));
//             internal::pstore(&X[j+6*PacketSize], internal::padd(internal::pload(&X0), internal::pmul(ptmp,A0)));
//             internal::pstore(&X[j+7*PacketSize], internal::padd(internal::pload(&X1), internal::pmul(ptmp,A1)));
// //
// //             internal::pstore(&X[j], internal::padd(internal::pload(&X[j]), internal::pmul(ptmp,internal::pload(&A[j+iN]))));
// //             internal::pstore(&X[j+PacketSize], internal::padd(internal::pload(&X[j+PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+PacketSize+iN]))));
// //             internal::pstore(&X[j+2*PacketSize], internal::padd(internal::pload(&X[j+2*PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+2*PacketSize+iN]))));
// //             internal::pstore(&X[j+3*PacketSize], internal::padd(internal::pload(&X[j+3*PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+3*PacketSize+iN]))));
// //             internal::pstore(&X[j+4*PacketSize], internal::padd(internal::pload(&X[j+4*PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+4*PacketSize+iN]))));
// //             internal::pstore(&X[j+5*PacketSize], internal::padd(internal::pload(&X[j+5*PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+5*PacketSize+iN]))));
// //             internal::pstore(&X[j+6*PacketSize], internal::padd(internal::pload(&X[j+6*PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+6*PacketSize+iN]))));
// //             internal::pstore(&X[j+7*PacketSize], internal::padd(internal::pload(&X[j+7*PacketSize]), internal::pmul(ptmp,internal::pload(&A[j+7*PacketSize+iN]))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pload(&X[j]), internal::pmul(ptmp,internal::pload(&A[j+iN]))));
//           #else
//           for (int j = 0;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pload(&X[j]), internal::pmul(ptmp,internal::pload(&A[j+iN]))));
//           #endif
//         }
//         else
//         {
//           #ifdef PEELING
//           int ANP = (AN/(8*PacketSize))*8*PacketSize;
//           for (int j = 0;j<ANP;j+=PacketSize*8)
//           {
//             internal::pstore(&X[j], internal::padd(internal::pload(&X[j]), internal::pmul(ptmp,internal::ploadu(&A[j+iN]))));
//             internal::pstore(&X[j+PacketSize], internal::padd(internal::pload(&X[j+PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+PacketSize+iN]))));
//             internal::pstore(&X[j+2*PacketSize], internal::padd(internal::pload(&X[j+2*PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+2*PacketSize+iN]))));
//             internal::pstore(&X[j+3*PacketSize], internal::padd(internal::pload(&X[j+3*PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+3*PacketSize+iN]))));
//             internal::pstore(&X[j+4*PacketSize], internal::padd(internal::pload(&X[j+4*PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+4*PacketSize+iN]))));
//             internal::pstore(&X[j+5*PacketSize], internal::padd(internal::pload(&X[j+5*PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+5*PacketSize+iN]))));
//             internal::pstore(&X[j+6*PacketSize], internal::padd(internal::pload(&X[j+6*PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+6*PacketSize+iN]))));
//             internal::pstore(&X[j+7*PacketSize], internal::padd(internal::pload(&X[j+7*PacketSize]), internal::pmul(ptmp,internal::ploadu(&A[j+7*PacketSize+iN]))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pload(&X[j]), internal::pmul(ptmp,internal::ploadu(&A[j+iN]))));
//           #else
//           for (int j = 0;j<AN;j+=PacketSize)
//             internal::pstore(&X[j], internal::padd(internal::pload(&X[j]), internal::pmul(ptmp,internal::ploadu(&A[j+iN]))));
//           #endif
//         }
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp * A[j+iN];
//     }
//     asm("#end matrix_vector_product");
//   }

    static inline void atv_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
  {
    int AN = (N/PacketSize)*PacketSize;
    int bound = (N/4)*4;
    for (int i=0;i<bound;i+=4)
    {
      real tmp0 = 0;
      Packet ptmp0 = internal::pset1(real(0));
      real tmp1 = 0;
      Packet ptmp1 = internal::pset1(real(0));
      real tmp2 = 0;
      Packet ptmp2 = internal::pset1(real(0));
      real tmp3 = 0;
      Packet ptmp3 = internal::pset1(real(0));
      int iN0 = i*N;
      int iN1 = (i+1)*N;
      int iN2 = (i+2)*N;
      int iN3 = (i+3)*N;
      if (AN>0)
      {
        int align1 = (iN1 % PacketSize);
        if (align1==0)
        {
          for (int j = 0;j<AN;j+=PacketSize)
          {
            Packet b = internal::pload(&B[j]);
            ptmp0 = internal::padd(ptmp0, internal::pmul(b, internal::pload(&A[j+iN0])));
            ptmp1 = internal::padd(ptmp1, internal::pmul(b, internal::pload(&A[j+iN1])));
            ptmp2 = internal::padd(ptmp2, internal::pmul(b, internal::pload(&A[j+iN2])));
            ptmp3 = internal::padd(ptmp3, internal::pmul(b, internal::pload(&A[j+iN3])));
          }
        }
        else if (align1==2)
        {
          for (int j = 0;j<AN;j+=PacketSize)
          {
            Packet b = internal::pload(&B[j]);
            ptmp0 = internal::padd(ptmp0, internal::pmul(b, internal::pload(&A[j+iN0])));
            ptmp1 = internal::padd(ptmp1, internal::pmul(b, internal::ploadu(&A[j+iN1])));
            ptmp2 = internal::padd(ptmp2, internal::pmul(b, internal::pload(&A[j+iN2])));
            ptmp3 = internal::padd(ptmp3, internal::pmul(b, internal::ploadu(&A[j+iN3])));
          }
        }
        else
        {
          for (int j = 0;j<AN;j+=PacketSize)
          {
            Packet b = internal::pload(&B[j]);
            ptmp0 = internal::padd(ptmp0, internal::pmul(b, internal::pload(&A[j+iN0])));
            ptmp1 = internal::padd(ptmp1, internal::pmul(b, internal::ploadu(&A[j+iN1])));
            ptmp2 = internal::padd(ptmp2, internal::pmul(b, internal::ploadu(&A[j+iN2])));
            ptmp3 = internal::padd(ptmp3, internal::pmul(b, internal::ploadu(&A[j+iN3])));
          }
        }
        tmp0 = internal::predux(ptmp0);
        tmp1 = internal::predux(ptmp1);
        tmp2 = internal::predux(ptmp2);
        tmp3 = internal::predux(ptmp3);
      }
      // process remaining scalars
      for (int j=AN;j<N;j++)
      {
        tmp0 += B[j] * A[j+iN0];
        tmp1 += B[j] * A[j+iN1];
        tmp2 += B[j] * A[j+iN2];
        tmp3 += B[j] * A[j+iN3];
      }
      X[i+0] = tmp0;
      X[i+1] = tmp1;
      X[i+2] = tmp2;
      X[i+3] = tmp3;
    }

    for (int i=bound;i<N;i++)
    {
      real tmp0 = 0;
      Packet ptmp0 = internal::pset1(real(0));
      int iN0 = i*N;
      if (AN>0)
      {
        if (iN0 % PacketSize==0)
          for (int j = 0;j<AN;j+=PacketSize)
            ptmp0 = internal::padd(ptmp0, internal::pmul(internal::pload(&B[j]), internal::pload(&A[j+iN0])));
        else
          for (int j = 0;j<AN;j+=PacketSize)
            ptmp0 = internal::padd(ptmp0, internal::pmul(internal::pload(&B[j]), internal::ploadu(&A[j+iN0])));
        tmp0 = internal::predux(ptmp0);
      }
      // process remaining scalars
      for (int j=AN;j<N;j++)
        tmp0 += B[j] * A[j+iN0];
      X[i+0] = tmp0;
    }
  }

//   static inline void atv_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
//   {
//     int AN = (N/PacketSize)*PacketSize;
//     for (int i=0;i<N;i++)
//       X[i] = 0;
//     for (int i=0;i<N;i++)
//     {
//       real tmp = 0;
//       Packet ptmp = internal::pset1(real(0));
//       int iN = i*N;
//       if (AN>0)
//       {
//         bool aligned = (iN % PacketSize) == 0;
//         if (aligned)
//         {
//           #ifdef PEELING
//           int ANP = (AN/(8*PacketSize))*8*PacketSize;
//           for (int j = 0;j<ANP;j+=PacketSize*8)
//           {
//             ptmp =
//               internal::padd(internal::pmul(internal::pload(&B[j]), internal::pload(&A[j+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+PacketSize]), internal::pload(&A[j+PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+2*PacketSize]), internal::pload(&A[j+2*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+3*PacketSize]), internal::pload(&A[j+3*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+4*PacketSize]), internal::pload(&A[j+4*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+5*PacketSize]), internal::pload(&A[j+5*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+6*PacketSize]), internal::pload(&A[j+6*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+7*PacketSize]), internal::pload(&A[j+7*PacketSize+iN])),
//               ptmp))))))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             ptmp = internal::padd(ptmp, internal::pmul(internal::pload(&B[j]), internal::pload(&A[j+iN])));
//           #else
//           for (int j = 0;j<AN;j+=PacketSize)
//             ptmp = internal::padd(ptmp, internal::pmul(internal::pload(&B[j]), internal::pload(&A[j+iN])));
//           #endif
//         }
//         else
//         {
//           #ifdef PEELING
//           int ANP = (AN/(8*PacketSize))*8*PacketSize;
//           for (int j = 0;j<ANP;j+=PacketSize*8)
//           {
//             ptmp =
//               internal::padd(internal::pmul(internal::pload(&B[j]), internal::ploadu(&A[j+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+PacketSize]), internal::ploadu(&A[j+PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+2*PacketSize]), internal::ploadu(&A[j+2*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+3*PacketSize]), internal::ploadu(&A[j+3*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+4*PacketSize]), internal::ploadu(&A[j+4*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+5*PacketSize]), internal::ploadu(&A[j+5*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+6*PacketSize]), internal::ploadu(&A[j+6*PacketSize+iN])),
//               internal::padd(internal::pmul(internal::pload(&B[j+7*PacketSize]), internal::ploadu(&A[j+7*PacketSize+iN])),
//               ptmp))))))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             ptmp = internal::padd(ptmp, internal::pmul(internal::pload(&B[j]), internal::ploadu(&A[j+iN])));
//           #else
//           for (int j = 0;j<AN;j+=PacketSize)
//             ptmp = internal::padd(ptmp, internal::pmul(internal::pload(&B[j]), internal::ploadu(&A[j+iN])));
//           #endif
//         }
//         tmp = internal::predux(ptmp);
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         tmp += B[j] * A[j+iN];
//       X[i] = tmp;
//     }
//   }

  static inline void axpy(real coef, const gene_vector & X, gene_vector & Y, int N){
    int AN = (N/PacketSize)*PacketSize;
    if (AN>0)
    {
      Packet pcoef = internal::pset1(coef);
      #ifdef PEELING
      const int peelSize = 3;
      int ANP = (AN/(peelSize*PacketSize))*peelSize*PacketSize;
      float* X1 = X + PacketSize;
      float* Y1 = Y + PacketSize;
      float* X2 = X + 2*PacketSize;
      float* Y2 = Y + 2*PacketSize;
      Packet x0,x1,x2,y0,y1,y2;
      for (int j = 0;j<ANP;j+=PacketSize*peelSize)
      {
        x0 = internal::pload(X+j);
        x1 = internal::pload(X1+j);
        x2 = internal::pload(X2+j);

        y0 = internal::pload(Y+j);
        y1 = internal::pload(Y1+j);
        y2 = internal::pload(Y2+j);

        y0 = internal::pmadd(pcoef, x0, y0);
        y1 = internal::pmadd(pcoef, x1, y1);
        y2 = internal::pmadd(pcoef, x2, y2);

        internal::pstore(Y+j,  y0);
        internal::pstore(Y1+j, y1);
        internal::pstore(Y2+j, y2);
//         internal::pstore(&Y[j+2*PacketSize], internal::padd(internal::pload(&Y[j+2*PacketSize]), internal::pmul(pcoef,internal::pload(&X[j+2*PacketSize]))));
//         internal::pstore(&Y[j+3*PacketSize], internal::padd(internal::pload(&Y[j+3*PacketSize]), internal::pmul(pcoef,internal::pload(&X[j+3*PacketSize]))));
//         internal::pstore(&Y[j+4*PacketSize], internal::padd(internal::pload(&Y[j+4*PacketSize]), internal::pmul(pcoef,internal::pload(&X[j+4*PacketSize]))));
//         internal::pstore(&Y[j+5*PacketSize], internal::padd(internal::pload(&Y[j+5*PacketSize]), internal::pmul(pcoef,internal::pload(&X[j+5*PacketSize]))));
//         internal::pstore(&Y[j+6*PacketSize], internal::padd(internal::pload(&Y[j+6*PacketSize]), internal::pmul(pcoef,internal::pload(&X[j+6*PacketSize]))));
//         internal::pstore(&Y[j+7*PacketSize], internal::padd(internal::pload(&Y[j+7*PacketSize]), internal::pmul(pcoef,internal::pload(&X[j+7*PacketSize]))));
      }
      for (int j = ANP;j<AN;j+=PacketSize)
        internal::pstore(&Y[j], internal::padd(internal::pload(&Y[j]), internal::pmul(pcoef,internal::pload(&X[j]))));
      #else
      for (int j = 0;j<AN;j+=PacketSize)
        internal::pstore(&Y[j], internal::padd(internal::pload(&Y[j]), internal::pmul(pcoef,internal::pload(&X[j]))));
      #endif
    }
    // process remaining scalars
    for (int i=AN;i<N;i++)
      Y[i] += coef * X[i];
  }

};

#endif


//-*-Mode:C++;coding:utf-8;tab-width:4;c-basic-offset:4;indent-tabs-mode:()-*-
// ex: set ft=cpp fenc=utf-8 sts=4 ts=4 sw=4 et nomod:
//
// MIT License
//
// Copyright (c) 2011-2021 Michael Truog <mjtruog at protonmail dot com>
//
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
#include "cloudi.hpp"
#include "timer.hpp"
#include "thread_pool.hpp"
#include "piqpr8_gmp.hpp"
#include "piqpr8_gmp_verify.hpp"
#include <boost/thread/mutex.hpp>
#include <boost/thread/locks.hpp>
#include <boost/thread/condition_variable.hpp>
#include <sstream>
#include <iostream>
#include <string>
#include "assert.hpp"

class thread_data : public thread_pool_thread_data
{
    public:
        virtual ~thread_data() throw() {}
};

class task_output_data : public thread_pool_output_data
{
    public:
        task_output_data(CloudI::API::return_value_type const error) :
            m_error(error)
        {
        }

        virtual ~task_output_data() throw() {}

        int error() const
        {
            int error_value = 0;
            if (m_error && m_error != CloudI::API::return_value::terminate)
                error_value = m_error;
            return error_value;
        }

    private:
        CloudI::API::return_value_type const m_error;
};

class task : public thread_pool_input<thread_data, task_output_data>
{
    public:
        task(unsigned int const thread_index) :
            m_stop_default(false),
            m_stop(m_stop_default),
            m_api(thread_index, true)
        {
            task::m_timeout_terminate = m_api.timeout_terminate();
        }

        virtual ~task() throw() {}

        static uint32_t timeout_terminate()
        {
            return task::m_timeout_terminate;
        }

        void hexpi(CloudI::API const & api,
                   int const request_type,
                   std::string const & name,
                   std::string const & pattern,
                   void const * const /*request_info*/,
                   uint32_t const /*request_info_size*/,
                   void const * const request,
                   uint32_t const request_size,
                   uint32_t timeout,
                   int8_t /*priority*/,
                   char const * const trans_id,
                   char const * const pid,
                   uint32_t const pid_size)
        {
            uint32_t const * parameters =
                reinterpret_cast<uint32_t const *>(request);
            assert(request_size > (sizeof(uint32_t) * 2));
            uint32_t const & iteration_count = parameters[0];
            uint32_t const & digit_step = parameters[1];
            char const * digit_index =
                reinterpret_cast<char const *>(&parameters[2]);

            // perform the work
            timer t;
            std::ostringstream result;
            for (uint32_t i = 0; i < iteration_count; ++i)
            {
                if (m_stop)
                {
                    return;
                }
                else
                {
                    std::string pi_sequence;
                    if (! bbp_pi(m_stop, digit_index,
                                 digit_step * i, pi_sequence))
                        return;
                    result << pi_sequence;
                }
            }
            std::string pi_result = result.str();
            if (! bbp_pi_verify(digit_index, pi_result))
            {
                std::cerr << "index: " << digit_index << ", \"" <<
                    pi_result << "\" failed validation" << std::endl;
                return;
            }

            // insert the elapsed time as a 32 bit float
            pi_result.insert(0, "    ");
            float * elapsed_hours = reinterpret_cast<float *>(
                const_cast<char *>(pi_result.c_str()));
            *elapsed_hours = static_cast<float>(t.elapsed() / 3600.0);

            api.return_(request_type, name, pattern, "", 0,
                        pi_result.c_str(), pi_result.size(),
                        timeout, trans_id, pid, pid_size);
            std::cout << "execution never gets here" << std::endl;
        }

        task_output_data process(bool const & stop, thread_data & /*data*/)
        {
            // a return from this task object function, provides
            // a single task_output_data object (with the error value) to the
            // single shared task_output object, which will then cause the
            // thread pool to use the exit() function
            // (m_stop is set to true in all threads,
            //  so all threads are asked to abort their processing)
            m_stop = stop;
            // it is best to do CloudI API subscribe calls here due to
            // ownership of *this in the thread_pool
            // when the process function is executed
            // (makes it easier to use the thread_data variable,
            //  if necessary,  because *this isn't getting copied)
            int error = m_api.subscribe_count("hexpi");
            if (! error)
            {
                assert(m_api.get_subscribe_count() == 0);
                error = m_api.subscribe("hexpi", *this, &task::hexpi);
            }
            if (! error)
            {
                error = m_api.subscribe_count("hexpi");
            }
            if (! error)
            {
                assert(m_api.get_subscribe_count() == 1);
                while (CloudI::API::return_value::timeout ==
                       (error = m_api.poll(1000)))
                {
                    if (stop)
                    {
                        error = CloudI::API::return_value::success;
                        break;
                    }
                }
            }
            return task_output_data(error);
        }

    private:
        bool m_stop_default;
        bool & m_stop;

        CloudI::API m_api;
        static uint32_t m_timeout_terminate;
};
uint32_t task::m_timeout_terminate = 0;

class task_output : public thread_pool_output<task_output_data>
{
    public:
        task_output() :
            m_status(0),
            m_terminate(false)
        {
        }

        virtual ~task_output() throw() {}

        void output(task_output_data & data)
        {
            bool terminated;
            {
                boost::lock_guard<boost::mutex> lock(m_terminate_mutex);
                int error = data.error();
                if (error)
                {
                    std::cerr << "C/C++ CloudI API error " <<
                        error << std::endl;
                    m_status = error;
                }
                terminated = m_terminate;
                m_terminate = true;
            }
            if (terminated == false)
                m_terminate_conditional.notify_all();
        }

        int wait_on_terminate()
        {
            if (m_terminate == false)
            {
                boost::unique_lock<boost::mutex> lock(m_terminate_mutex);
                m_terminate_conditional.wait(lock);
            }
            return m_status;
        }

    private:
        int m_status;
        bool m_terminate;
        boost::mutex m_terminate_mutex;
        boost::condition_variable m_terminate_conditional;
};

int main(int, char **)
{
    int status = 0;

    assert_initialize();
    try
    {
        unsigned int const thread_count = CloudI::API::thread_count();

        task_output output_object;
        thread_pool<task, thread_data, task_output, task_output_data>
            thread_pool(thread_count, thread_count, output_object);

        for (unsigned int i = 0; i < thread_count; ++i)
        {
            task task_input(i);
            bool const added = thread_pool.input(task_input);
            assert(added);
        }

        assert(task::timeout_terminate() >= 1000);
        status = output_object.wait_on_terminate();
        thread_pool.exit(task::timeout_terminate() - 100);
    }
    catch (CloudI::API::invalid_input_exception const & e)
    {
        std::cerr << e.what() << std::endl;
    }
    catch (CloudI::API::terminate_exception const &)
    {
    }

    std::cout << "terminate hexpi c++" << std::endl;

    return status;
}


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2013 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#ifndef MAPNIK_POLYGON_CLIPPER_HPP
#define MAPNIK_POLYGON_CLIPPER_HPP

// stl
#include <iostream>
#include <deque>

// mapnik
#include <mapnik/box2d.hpp>
#include <mapnik/geometry.hpp>

// boost
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/register/point.hpp>

BOOST_GEOMETRY_REGISTER_POINT_2D(mapnik::coord2d, double, cs::cartesian, x, y)

// register mapnik::box2d<double>
namespace boost { namespace geometry { namespace traits
{

template<> struct tag<mapnik::box2d<double> > { typedef box_tag type; };

template<> struct point_type<mapnik::box2d<double> > { typedef mapnik::coord2d type; };

template <>
struct indexed_access<mapnik::box2d<double>, min_corner, 0>
{
    typedef coordinate_type<mapnik::coord2d>::type ct;
    static inline ct get(mapnik::box2d<double> const& b) { return b.minx();}
    static inline void set(mapnik::box2d<double> &b, ct const& value) { b.set_minx(value); }
};

template <>
struct indexed_access<mapnik::box2d<double>, min_corner, 1>
{
    typedef coordinate_type<mapnik::coord2d>::type ct;
    static inline ct get(mapnik::box2d<double> const& b) { return b.miny();}
    static inline void set(mapnik::box2d<double> &b, ct const& value) { b.set_miny(value); }
};

template <>
struct indexed_access<mapnik::box2d<double>, max_corner, 0>
{
    typedef coordinate_type<mapnik::coord2d>::type ct;
    static inline ct get(mapnik::box2d<double> const& b) { return b.maxx();}
    static inline void set(mapnik::box2d<double> &b, ct const& value) { b.set_maxx(value); }
};

template <>
struct indexed_access<mapnik::box2d<double>, max_corner, 1>
{
    typedef coordinate_type<mapnik::coord2d>::type ct;
    static inline ct get(mapnik::box2d<double> const& b) { return b.maxy();}
    static inline void set(mapnik::box2d<double> &b , ct const& value) { b.set_maxy(value); }
};

}}}

namespace mapnik {

using namespace boost::geometry;

template <typename Geometry>
struct polygon_clipper
{
    typedef mapnik::coord2d point_2d;
    typedef model::polygon<mapnik::coord2d> polygon_2d;
    typedef std::deque<polygon_2d> polygon_list;

    polygon_clipper(Geometry & geom)
        : clip_box_(),
          geom_(geom)
    {

    }

    polygon_clipper( box2d<double> const& clip_box,Geometry & geom)
        : clip_box_(clip_box),
          geom_(geom)
    {
        init();
    }

    void set_clip_box(box2d<double> const& clip_box)
    {
        clip_box_ = clip_box;
        init();
    }

    unsigned type() const
    {
        return geom_.type();
    }

    void rewind(unsigned path_id)
    {
        output_.rewind(path_id);
    }

    unsigned vertex (double * x, double * y)
    {
        return output_.vertex(x,y);
    }

private:

    void init()
    {
        polygon_2d subject_poly;
        double x,y;
        double prev_x, prev_y;
        geom_.rewind(0);
        unsigned ring_count = 0;
        while (true)
        {
            unsigned cmd = geom_.vertex(&x,&y);
            if (cmd == SEG_END) break;
            if (cmd == SEG_MOVETO)
            {
                prev_x = x;
                prev_y = y;
                if (ring_count == 0)
                {
                    append(subject_poly, make<point_2d>(x,y));
                }
                else
                {
                    subject_poly.inners().push_back(polygon_2d::inner_container_type::value_type());
                    append(subject_poly.inners().back(),make<point_2d>(x,y));
                }
                ++ring_count;
            }
            else if (cmd == SEG_LINETO)
            {
                if (std::abs(x - prev_x) < 1e-12 && std::abs(y - prev_y) < 1e-12)
                {
                    std::cerr << std::setprecision(12) << "coincident vertices:(" << prev_x << ","
                              <<  prev_y << ") , (" << x << "," << y <<  ")" << std::endl;
                    continue;
                }
                prev_x = x;
                prev_x = y;
                if (ring_count == 1)
                {
                    append(subject_poly, make<point_2d>(x,y));
                }
                else
                {
                    append(subject_poly.inners().back(),make<point_2d>(x,y));
                }
            }
        }

        polygon_list clipped_polygons;

        try
        {
            boost::geometry::intersection(clip_box_, subject_poly, clipped_polygons);
        }
        catch (boost::geometry::exception const& ex)
        {
            std::cerr << ex.what() << std::endl;
        }

        BOOST_FOREACH(polygon_2d const& poly, clipped_polygons)
        {
            bool move_to = true;
            BOOST_FOREACH(point_2d const& c, boost::geometry::exterior_ring(poly))
            {
                if (move_to)
                {
                    move_to = false;
                    output_.move_to(c.x,c.y);
                }
                else
                {
                    output_.line_to(c.x,c.y);
                }
            }
            output_.close_path();
            // interior rings
            BOOST_FOREACH(polygon_2d::inner_container_type::value_type const& ring, boost::geometry::interior_rings(poly))
            {
                move_to = true;
                BOOST_FOREACH(point_2d const& c, ring)
                {
                    if (move_to)
                    {
                        move_to = false;
                        output_.move_to(c.x,c.y);
                    }
                    else
                    {
                        output_.line_to(c.x,c.y);
                    }
                }
                output_.close_path();
            }
        }
    }

    box2d<double> clip_box_;
    Geometry & geom_;
    mapnik::geometry_type output_;

};

}

#endif //MAPNIK_POLYGON_CLIPPER_HPP


/*******************************************************************************
 * segmentMex.cpp - nnf segmentation for matlab
 *******************************************************************************
 * Add license here...
 *******************************/

// other stuff
#include <mexutil.h>
#include <boost/unordered_map.hpp>
// our stuff
#include <nnf.h>
#include <patch/affine.h>
#include <texture.h>
#include <segment.h>

template <typename Patch, typename Scalar>
void typedMexFunction(int nout, mxArray *out[], const mxArray *nnfArray);

#define mexArgs nout, out, in[0]

/**
 * Usage:
 *
 * [boundaries, labelmap] = segmentmex( nnf, patch_type )
 *
 */
void mexFunction(int nout, mxArray *out[], int nin, const mxArray *in[]) {

	// checking the input
	if (nin > 2) {
		mexErrMsgIdAndTxt("MATLAB:segment:invalidNumInputs",
				"Only two arguments allowed.");
	}
	// checking the output
	if (nout > 2) {
		mexErrMsgIdAndTxt("MATLAB:segment:maxlhs",
				"Too many output arguments.");
	}

	pm::PatchType patchType = pm::Basic;
	if (nin == 2){
        const mxArray *patch_type = in[1];
		if (mxStringEquals(patch_type, "affine"))
			patchType = pm::Affine;
		else if(mxStringEquals(patch_type, "basicf"))
			patchType = pm::BasicFloat;
		else if(mxStringEquals(patch_type, "basic"))
			patchType = pm::Basic;
	}

	switch(patchType) {
		case pm::Basic: typedMexFunction<pm::BasicPatchX, float>(mexArgs); break;
		case pm::BasicFloat: typedMexFunction<pm::BasicFloatPatchX, float>(mexArgs); break;
		case pm::Affine: typedMexFunction<pm::AffinePatchX, float>(mexArgs); break;
		default:
			mexErrMsgIdAndTxt("MATLAB:nnf:patch_type", "Invalid patch type!");
	}

}

template <typename Patch, typename Scalar>
void typedMexFunction(int nout, mxArray *out[], const mxArray *nnfArray) {

    // - size
    const int h = mxGetDimensions(nnfArray)[0];
	const int w = mxGetDimensions(nnfArray)[1];

	// - loading the nnf map
	Patch::width(10);
	typedef pm::NearestNeighborField<Patch, Scalar> NNF;
	NNF nnf(w, h); // no need for the distances
	mxLoadNNF(nnf, nnfArray, "Invalid nnf!");

    // - segmentation
    pm::BaseSegmentation* s = pm::segment<pm::NoData>(&nnf);

	// - boundaries
	switch(nout){
		case 2:
		{
			// label storage
			out[1] = mxCreateMatrix(h, w, mxSINGLE_CLASS);
			float *data = reinterpret_cast<float *>(mxGetData(out[1])); // unsigned int?
			// label map
			// create simplest labels from 1 to N
			unsigned int N = 1;
			typedef pm::BaseSegmentation::Label Label;
			typedef boost::unordered_map<Label, unsigned int> LabelMap;
			LabelMap labelMap;
			// loop
			for (int y = 0; y < s->rows; ++y) {
				for (int x = 0; x < s->cols; ++x) {
					// map the label to a simple one
					unsigned int simpleLabel; //< to find
					Label label = s->at(y, x).label();
					LabelMap::iterator it = labelMap.find(label);
					if(it == labelMap.end()) {
						simpleLabel = N++; // new label
						labelMap[label] = simpleLabel;
					} else
						simpleLabel = it->second; // already used before

					// store the simple label
					data[y + x * s->rows] = simpleLabel;
				}
			}
		}
		case 1:
		default:
		{
			out[0] = mxCreateMatrix(h, w, mxLOGICAL_CLASS);
			bool *data = reinterpret_cast<bool *>(mxGetData(out[0]));
#if _OPENMP
#pragma omp parallel for collapse(2)
#endif
			for (int y = 0; y < s->rows; ++y) {
				for (int x = 0; x < s->cols; ++x) {
					data[y + x * s->rows] = s->at(y, x).isBoundary();
				}
			}
		}
	}
	// release memory
	if (s != NULL) delete s;
}