/* image_blend_plugin.cpp
 *
 * 2014-01-28 : by Zhi Zhou
 */

#include "v3d_message.h"
#include <vector>
#include "image_blend_plugin.h"
#include <fstream>
#include <iostream>
#include <boost/lexical_cast.hpp>
#include "../../../v3d_main/jba/c++/convert_type2uint8.h"
#include "../plugin_loader/v3d_plugin_loader.h"
#include "../../../v3d_external/v3d_main/basic_c_fun/stackutil.h"
#include "../../../released_plugins/v3d_plugins/swc_to_maskimage/filter_dialog.h"
#include "basic_surf_objs.h"
#include "../../../v3d_external/v3d_main/v3d/colormap.h"

using namespace std;
Q_EXPORT_PLUGIN2(image_blend, image_blend);

template <class T>
void BinaryProcess(T *apsInput, T * aspOutput, V3DLONG iImageWidth, V3DLONG iImageHeight, V3DLONG iImageLayer, V3DLONG h, V3DLONG d)
{
    V3DLONG i, j,k,n,count;
    double t, temp;

    V3DLONG mCount = iImageHeight * iImageWidth;
    for (i=0; i<iImageLayer; i++)
    {
        for (j=0; j<iImageHeight; j++)
        {
            for (k=0; k<iImageWidth; k++)
            {
                V3DLONG curpos = i * mCount + j*iImageWidth + k;
                V3DLONG curpos1 = i* mCount + j*iImageWidth;
                V3DLONG curpos2 = j* iImageWidth + k;
                temp = 0;
                count = 0;
                for(n =1 ; n <= d  ;n++)
                {
                    if (k>h*n) {temp += apsInput[curpos1 + k-(h*n)]; count++;}
                    if (k+(h*n)< iImageWidth) { temp += apsInput[curpos1 + k+(h*n)]; count++;}
                    if (j>h*n) {temp += apsInput[i* mCount + (j-(h*n))*iImageWidth + k]; count++;}//
                    if (j+(h*n)<iImageHeight) {temp += apsInput[i* mCount + (j+(h*n))*iImageWidth + k]; count++;}//
                    if (i>(h*n)) {temp += apsInput[(i-(h*n))* mCount + curpos2]; count++;}//
                    if (i+(h*n)< iImageLayer) {temp += apsInput[(i+(h*n))* mCount + j* iImageWidth + k ]; count++;}
                }
                t =  apsInput[curpos]-temp/(count);
                aspOutput[curpos]= (t > 0)? t : 0;
            }
        }
    }
}

template <class T> void median_filter(T* data1d,
                                      V3DLONG *in_sz,
                                      unsigned int Wx,
                                      unsigned int Wy,
                                      unsigned int Wz,
                                      unsigned int c,
                                      T* &outimg);

QStringList image_blend::menulist() const
{
	return QStringList()
		<<tr("about");
}

QStringList image_blend::funclist() const
{
	return QStringList()
		<<tr("image_blend")
        <<tr("image_modulate")
        <<tr("image_align")
        <<tr("xy_yz_combine")
        <<tr("image_average_batch")
        <<tr("image_difference")
		<<tr("help");
}

void image_blend::domenu(const QString &menu_name, V3DPluginCallback2 &callback, QWidget *parent)
{
    if (menu_name == tr("about"))
	{
        v3d_msg(tr("This plugin blends two images. This plugin is designed for command line use ONLY right now. "
			"Developed by Zhi Zhou, 2014-01-28"));
	}
}

bool image_blend::dofunc(const QString & func_name, const V3DPluginArgList & input, V3DPluginArgList & output, V3DPluginCallback2 & callback,  QWidget * parent)
{
    if (func_name == tr("image_average_batch")){
        // Take the average of all images under a folder
        // Peng Xie 2019-03-29

        // Step 1: Find image files under a folder;
        vector<char*> infiles;
        if(input.size() >= 1) infiles = *((vector<char*> *)input.at(0).p);
        QString qs_dir_img;
        if(infiles.size()>=1){
            char * indir = infiles.at(0);
            qs_dir_img = QString(indir);
        }
        else{
            //choose a directory that contain image files
            qs_dir_img = QFileDialog::getExistingDirectory(parent,
                                                           QString(QObject::tr("Choose the directory that contains files to be processed")));
            qs_dir_img = qs_dir_img+"/";
        }
        QDir dir(qs_dir_img);
        QStringList qsl_filelist, qsl_filters;
        qsl_filters << "*.v3draw" << "*.raw" << "*.tif";

        foreach(QString file, dir.entryList(qsl_filters, QDir::Files))
        {
            qsl_filelist+=file;
        }

        if(qsl_filelist.size()==0)
        {
            v3d_msg("Cannot find image files files in the given directory!\nTry another diretory");
            return 0;
        }

        // Step 2: Specify output folder
        const QString output_dir("Average_image");
        if(!dir.mkdir(output_dir)){
            qDebug()<<(QString("Cannot create dir \"%1\" or \"%2\" already exists. Please double check.").arg(output_dir).arg(output_dir));
        }

        // Step 3: Signal summation through a loop
        V3DLONG pagesz;
        V3DLONG in_sz[4];
        unsigned short int *data_average = 0;
        V3DLONG file_ct = 0;
        for(int i=0; i<qsl_filelist.size(); i++){
//        for(int i=0; i<2; i++){

            QString inimg_file = qsl_filelist.at(i);
            qDebug()<<"Reading file "<<(i+1)<<" "<<inimg_file;

            unsigned char * image = 0;
            V3DLONG cur_in_sz[4];
            int datatype;
            if(!simple_loadimage_wrapper(callback, inimg_file.toStdString().c_str(), image, cur_in_sz, datatype))
            {
                cerr<<"load image "<<inimg_file.toStdString()<<" error!"<<endl;
                if (image) {delete []image; image=0;}
                return false;
            }
//            qDebug()<<QString("File_size: %1 %2 %3\nChannels: %4\nDatatype: %5").arg(cur_in_sz[0]).arg(cur_in_sz[1]).arg(cur_in_sz[2]).arg(cur_in_sz[3]).arg(datatype);

            if(i==0){
                in_sz[0] = cur_in_sz[0];
                in_sz[1] = cur_in_sz[1];
                in_sz[2] = cur_in_sz[2];
                in_sz[3] = cur_in_sz[3];
                pagesz = in_sz[0] * in_sz[1] * in_sz[2];
                try {data_average = new unsigned short int [pagesz];}
                catch(...)  {v3d_msg("cannot allocate memory for data_blended."); return false;}
                for(V3DLONG j = 0; j < pagesz; j++)
                {
                    data_average[j] = image[j];
                }
                file_ct++;
                qDebug()<<"Added signal from: "<<(i+1)<<" "<<inimg_file;
            }
            else{
                if((in_sz[0]==cur_in_sz[0]) && (in_sz[1]==cur_in_sz[1]) && (in_sz[2]==cur_in_sz[2]) && (in_sz[3]==cur_in_sz[3])){
                    for(V3DLONG j = 0; j < pagesz; j++)
                    {
                        data_average[j] += image[j];
                    }
                    file_ct++;
                    qDebug()<<"Added signal from: "<<(i+1)<<" "<<inimg_file;
                }
            }
        }
        // Take average
        if(file_ct==0){
            qDebug()<<"No file loaded.";
            return 1;
        }
        for(V3DLONG i = 0; i < pagesz; i++)
        {
            data_average[i] /= file_ct;
        }

        // Convert to UNIT_8
        unsigned char *data1 = 0;
        try {data1 = new unsigned char [pagesz];}
        catch(...)  {v3d_msg("cannot allocate memory for image_average."); return false;}
        for(V3DLONG i=0; i<pagesz; i++){
            data1[i] = data_average[i];
        }

        simple_saveimage_wrapper(callback, "Average_image/Average.v3draw", (unsigned char *)data1, in_sz, 1);
        return 1;

    }
    if (func_name == tr("image_difference")){
        // Step 1: Find image files under a folder;
        vector<char*> infiles, outfiles;
        if(input.size() >= 1) infiles = *((vector<char*> *)input.at(0).p);
        if(output.size() >= 1) outfiles = *((vector<char*> *)output.at(0).p);
        QString img_file_1;
        QString img_file_2;
        if(infiles.size()>=2){
            img_file_1 = QString(infiles[0]);
            img_file_2 = QString(infiles[1]);
        }
        else{
            qDebug()<<"Error: Two input image files required.";
            return 1;
        }

        QString out_img_file;
        if(outfiles.size()>=1){
            out_img_file = QString(outfiles[0]);
        }
        else{
            out_img_file = "Difference.v3draw";
        }

        // Step 2: signal difference
        V3DLONG pagesz;
        V3DLONG in_sz[4];
        unsigned char *data_difference = 0;

        // Image 1
        unsigned char * image1 = 0;
        V3DLONG cur_in_sz[4];
        int datatype;
        if(!simple_loadimage_wrapper(callback, img_file_1.toStdString().c_str(), image1, cur_in_sz, datatype))
        {
            cerr<<"load image "<<img_file_1.toStdString()<<" error!"<<endl;
            if (image1) {delete []image1; image1=0;}
            return false;
        }
        in_sz[0] = cur_in_sz[0];
        in_sz[1] = cur_in_sz[1];
        in_sz[2] = cur_in_sz[2];
        in_sz[3] = cur_in_sz[3];
        // Image 2
        unsigned char * image2 = 0;
        if(!simple_loadimage_wrapper(callback, img_file_2.toStdString().c_str(), image2, cur_in_sz, datatype))
        {
            cerr<<"load image "<<img_file_2.toStdString()<<" error!"<<endl;
            if (image2) {delete []image2; image2=0;}
            return false;
        }

        if((in_sz[0] != cur_in_sz[0]) || (in_sz[1] != cur_in_sz[1]) || (in_sz[2] != cur_in_sz[2]) || (in_sz[3] != cur_in_sz[3]))
        {
            qDebug()<<"Error: input image sizes don't match!";
            return 1;
        }

        // Output image
        pagesz = in_sz[0] * in_sz[1] * in_sz[2];
        try {data_difference = new unsigned char [pagesz];}
        catch(...)  {v3d_msg("cannot allocate memory for data_blended."); return false;}

        for(V3DLONG i = 0; i < pagesz; i++)
        {
            if(image1[i]>image2[i]){
                data_difference[i] = image1[i] - image2[i];
            }
            else{
                data_difference[i] = 0;
            }
        }

       simple_saveimage_wrapper(callback, out_img_file.toStdString().c_str(), (unsigned char *)data_difference, in_sz, 1);
       return 1;

    }
	if (func_name == tr("image_blend"))
	{
        if(input.size()<1 || (input.size()==1 && output.size()<1) ) // no inputs
        {
            return true;
        }

        char * output_file = ((vector<char*> *)(output.at(0).p))->at(0);

        vector<char*> * paralist;
        char * paras = NULL; // parameters
        if(input.size()>1) { paralist = (vector<char*> *)(input.at(1).p); paras =  paralist->at(0);} // parameters

        QString inimg_file1,inimg_file2;
        V3DLONG channel_input1 = 0;
        V3DLONG channel_input2 = 0;
        V3DLONG channel_output1 = 0;
        V3DLONG channel_output2 = 0;

        if(paras)
        {
            int argc = 0;
            int len = strlen(paras);
            int posb[2048];

            for(int i = 0; i < len; i++)
            {
                if(i==0 && paras[i] != ' ' && paras[i] != '\t')
                {
                    posb[argc++] = i;
                }
                else if((paras[i-1] == ' ' || paras[i-1] == '\t') && (paras[i] != ' ' && paras[i] != '\t'))
                {
                    posb[argc++] = i;
                }
            }

            char **argv = NULL;
            try
            {
                argv =  new char* [argc];
                for(int i = 0; i < argc; i++)
                {
                    argv[i] = paras + posb[i];
                }
            }
            catch(...)
            {
                printf("\nError: fail to allocate memory!\n");
                return false;
            }

            for(int i = 0; i < len; i++)
            {
                if(paras[i]==' ' || paras[i]=='\t')
                    paras[i]='\0';
            }

            char* key;
            for(int i=0; i<argc; i++)
            {
                if(i+1 != argc) // check that we haven't finished parsing yet
                {
                    key = argv[i];

                    qDebug()<<">>key ..."<<key;

                    if (*key == '^')
                    {
                        while(*++key)
                        {
                            if (!strcmp(key, "i1"))
                            {
                                key++;
                                inimg_file1 = QString( argv[i+1] );
                                qDebug()<<"filename_img_1=["<<inimg_file1<<"]";
                                i++;
                            }
                            else if (!strcmp(key, "c1"))
                            {
                                key++;
                                channel_input1 = atoi( argv[i+1] ); // red 1 green 2 blue 3
                                qDebug()<<"input channel for filename_img_1 = "<<channel_input1<<"";
                                i++;
                            }
                            else if (!strcmp(key, "o1"))
                            {
                                key++;
                                channel_output1 = atoi( argv[i+1] ); // red 1 green 2 blue 3
                                qDebug()<<"output channel for filename_img_1 = "<<channel_output1<<"";
                                i++;
                            }
                            else if (!strcmp(key, "i2"))
                            {
                                key++;
                                inimg_file2 = QString( argv[i+1] );
                                qDebug()<<"filename_img_2=["<<inimg_file2<<"]";
                                i++;
                            }
                            else if (!strcmp(key, "c2"))
                            {
                                key++;
                                channel_input2 = atoi( argv[i+1] ); // red 1 green 2 blue 3
                                qDebug()<<"input channel for filename_img_2 = "<<channel_input2<<"";
                                i++;
                            }
                            else if (!strcmp(key, "o2"))
                            {
                                key++;
                                channel_output2 = atoi( argv[i+1] ); // red 1 green 2 blue 3
                                qDebug()<<"output channel for filename_img_2 ="<<channel_output2<<"";
                                i++;
                            }
                            else
                            {
                                cout<<"parsing ..."<<key<<i<<"Unknown command. Type 'v3d -x plugin_name -f function_name' for usage"<<endl;
                                return false;
                            }

                        }
                    }
                    else
                    {
                        cout<<"parsing ..."<<key<<i<<"Unknown command. Type 'v3d -x plugin_name -f function_name' for usage"<<endl;
                        return false;
                    }
                }
            }

          if (argv) {delete []argv; argv=0;}
        }

        if(channel_output1 == channel_output2 || channel_output1 > 3 || channel_output2 > 3)
        {
            cerr<<"Invalid channel number for: "<<output_file<<" error!"<<endl;
            return false;
        }

        unsigned char * image1 = 0;
        V3DLONG in_sz1[4];
        int datatype1;
        if(!simple_loadimage_wrapper(callback, inimg_file1.toStdString().c_str(), image1, in_sz1, datatype1))
        {
            cerr<<"load image1 "<<inimg_file1.toStdString()<<" error!"<<endl;
            if (image1) {delete []image1; image1=0;}
            return false;
        }

        if(channel_input1 > in_sz1[3] || channel_input1 < 1)
        {
            cerr<<"Invalid channel number for: "<<inimg_file1.toStdString()<<" error!"<<endl;
            if (image1) {delete []image1; image1=0;}
            return false;
        }
        unsigned char * image2 = 0;
        V3DLONG in_sz2[4];
        int datatype2;
        if(!simple_loadimage_wrapper(callback, inimg_file2.toStdString().c_str(), image2, in_sz2, datatype2))
        {
            cerr<<"load image2 "<<inimg_file2.toStdString()<<" error!"<<endl;
             if (image2) {delete []image2; image2=0;}
            return false;
        }
        if(channel_input2 > in_sz2[3] || channel_input2 < 1)
        {
            cerr<<"Invalid channel number for: "<<inimg_file2.toStdString()<<" error!"<<endl;
            if (image2) {delete []image2; image2=0;}
            return false;
        }

        if(in_sz1[0] != in_sz2[0] || in_sz1[1] != in_sz2[1] || in_sz1[2] != in_sz2[2])
        {
             v3d_msg("Two images have differnt size.");
             if (image1) {delete []image1; image1=0;}
             if (image2) {delete []image2; image2=0;}
             return false;
        }

        V3DLONG pagesz = in_sz1[0]*in_sz1[1]*in_sz1[2];
        V3DLONG in_sz[4];
        in_sz[0] = in_sz1[0];
        in_sz[1] = in_sz1[1];
        in_sz[2] = in_sz1[2];
        in_sz[3] = 3;
        unsigned char *data_blended = 0;
        try {data_blended = new unsigned char [pagesz*3];}
        catch(...)  {v3d_msg("cannot allocate memory for data_blended."); return false;}

        for(V3DLONG i = 0; i < pagesz*3; i++)
            data_blended[i] = 0;
        V3DLONG j = 0;
        for(V3DLONG i = (channel_output1-1)*pagesz; i < channel_output1*pagesz; i++)
        {
            data_blended[i] = image1[(channel_input1-1)*pagesz + j];
            j++;
        }
        j = 0;
        for(V3DLONG i = (channel_output2-1)*pagesz; i < channel_output2*pagesz; i++)
        {
            data_blended[i] = image2[(channel_input2-1)*pagesz + j];
            j++;
        }

        simple_saveimage_wrapper(callback, output_file, (unsigned char *)data_blended, in_sz, 1);

        if (image1) {delete []image1; image1=0;}
        if (image2) {delete []image2; image2=0;}
        if (data_blended) {delete []data_blended; data_blended=0;}

	}
    else if (func_name == tr("image_modulate"))
    {
        if (output.size() != 1) return false;
        char * inimg_file = ((vector<char*> *)(input.at(0).p))->at(0);
       // char * inimg_enhanced = ((vector<char*> *)(input.at(1).p))->at(0);
        char * outimg_file = ((vector<char*> *)(output.at(0).p))->at(0);

       /* unsigned char * image1 = 0;
        V3DLONG in_sz1[4];
        int datatype1;
        if(!simple_loadimage_wrapper(callback, inimg_file, image1, in_sz1, datatype1))
        {
            if (image1) {delete []image1; image1=0;}
            return false;
        }*/

        unsigned char * image1 = 0;
        V3DLONG * in_sz1 = 0;

        int datatype1;
        if(!loadImage(inimg_file, image1, in_sz1, datatype1))
        {
            cerr<<"load image "<<inimg_file<<" error!"<<endl;
            return false;
        }

     /*   unsigned char * image2 = 0;
        V3DLONG in_sz2[4];
        int datatype2;
        if(!simple_loadimage_wrapper(callback, inimg_enhanced, image2, in_sz2, datatype2))
        {
            if (image2) {delete []image2; image2=0;}
            return false;
        }*/

        V3DLONG in_sz[4];
        in_sz[0] = in_sz1[0];
        in_sz[1] = in_sz1[1];
        in_sz[2] = in_sz1[2];
        in_sz[3] = 1;

        V3DLONG pagesz = in_sz[0]*in_sz[1]*in_sz1[2];

       /* V3DPluginArgItem arg;
        V3DPluginArgList input;
        V3DPluginArgList output;

        arg.type = "random";std::vector<char*> args1;
        args1.push_back(inimg_enhanced); arg.p = (void *) & args1; input<< arg;
        arg.type = "random";std::vector<char*> args;
        args.push_back("3");args.push_back("3");args.push_back("3");args.push_back("1"); args.push_back("1"); arg.p = (void *) & args; input << arg;
        arg.type = "random";std::vector<char*> args2;args2.push_back("gfImage.v3draw"); arg.p = (void *) & args2; output<< arg;

        QString full_plugin_name = "gaussian";
        QString func_name = "gf";

        callback.callPluginFunc(full_plugin_name,func_name, input,output);

        unsigned char * data1d_Gf = 0;
        int datatype;
        V3DLONG in_zz[4];

        simple_loadimage_wrapper(callback, "gfImage.v3draw", data1d_Gf, in_zz, datatype);
        remove("gfImage.v3draw");
        unsigned char* image2 = 0;
        image2 = new unsigned char [pagesz];

        double min,max;
        rescale_to_0_255_and_copy((float *)data1d_Gf,pagesz,min,max,image2);
        if(data1d_Gf) {delete []data1d_Gf; data1d_Gf =0;}*/

        unsigned char *data_blended = 0;
        try {data_blended = new unsigned char [pagesz];}
        catch(...)  {v3d_msg("cannot allocate memory for data_blended."); return false;}
        V3DLONG i = 0;

        V3DLONG N = in_sz1[0];
        V3DLONG M = in_sz1[1];
        V3DLONG P = in_sz1[2];
        printf("%d,%d,%d,%d,%d\n\n",N,M,P,in_sz1[3],datatype1);

        V3DLONG offsetc = 1*pagesz;
        for(V3DLONG iz = 0; iz < P; iz++)
        {
            V3DLONG offsetk = iz*M*N;
            for(V3DLONG iy = 0; iy < M; iy++)
            {
                V3DLONG offsetj = iy*N;
                for(V3DLONG ix = 0; ix < N; ix++)
                {
                    data_blended[offsetk + offsetj + ix] = image1[offsetc+offsetk + offsetj + ix];
                    i++;
                }
            }
        }

       /* for(V3DLONG i = 0; i < pagesz; i++)
        {
           /* double f = 0;
            double f1 = 0;
            double f2 = 0;
            f1 = image1[i]/255.f;
            f2 = image2[i]/255.f;
            f = (f1 + f2) * (f1+f2);
            f = fabs(f);

            if (f>1) f = 1;

            data_blended[i] = f*255;
            data_blended[i] = 255 - image1[i];

        }*/

        simple_saveimage_wrapper(callback, outimg_file, (unsigned char *)data_blended, in_sz, datatype1);

        if (image1) {delete []image1; image1=0;}
     //   if (image2) {delete []image2; image2=0;}
        if (data_blended) {delete []data_blended; data_blended=0;}

    }
    else if (func_name == tr("image_align"))
    {

        if (output.size() != 1) return false;
        char * inimg_file = ((vector<char*> *)(input.at(0).p))->at(0);
       // char * inimg_enhanced = ((vector<char*> *)(input.at(1).p))->at(0);
        char * outimg_file = ((vector<char*> *)(output.at(0).p))->at(0);

        unsigned char * image1 = 0;
        V3DLONG in_sz1[4];
        int datatype1;
        if(!simple_loadimage_wrapper(callback, inimg_file, image1, in_sz1, datatype1))
        {
            if (image1) {delete []image1; image1=0;}
            return false;
        }

      /*  unsigned char * image2 = 0;
        V3DLONG in_sz2[4];
        int datatype2;
        if(!simple_loadimage_wrapper(callback, inimg_enhanced, image2, in_sz2, datatype2))
        {
            if (image2) {delete []image2; image2=0;}
            return false;
        }*/

        V3DLONG in_sz[4];
        in_sz[0] = 18671;
        in_sz[1] = 12079;
        in_sz[2] = 1;
        in_sz[3] = 1;

        V3DLONG pagesz = in_sz[0]*in_sz[1]*in_sz[2];

        unsigned char *data_blended = 0;
        try {data_blended = new unsigned char [pagesz];}
        catch(...)  {v3d_msg("cannot allocate memory for data_blended."); return false;}
        for(int i = 0; i < pagesz; i++)
            data_blended[i] = 0;
        V3DLONG N = in_sz[0];
        V3DLONG M = in_sz[1];
        V3DLONG P = in_sz[2];
        double a11 = 0.989488343532377;
        double a12 = -0.14461264817971808;
        double a21 = 0.14461264817971808;
        double a22 = 0.989488343532377;
        double xshift = -2747.242615559955;
        double yshift = -1505.0768840073147;

        double det = a11*a22 - a12*a21;

        for(V3DLONG iz = 0; iz < P; iz++)
        {
            V3DLONG offsetk = iz*M*N;
            for(V3DLONG iy = 0; iy < M; iy++)
            {
                V3DLONG offsetj = iy*N;
                for(V3DLONG ix = 0; ix < N; ix++)
                {

                    V3DLONG ix_old = int((a22*ix - a21*iy)/det) - xshift;
                    V3DLONG iy_old = int((-a12*ix + a11*iy)/det) - yshift;
                    if(ix_old >=0 && ix_old < in_sz1[0] && iy_old >=0 && iy_old < in_sz1[1])
                    {
                        V3DLONG offsetj_old = iy_old*in_sz1[0];
                        data_blended[offsetk + offsetj + ix] = image1[offsetk + offsetj_old + ix_old];
                    }
                }

            }
        }

        simple_saveimage_wrapper(callback, outimg_file, (unsigned char *)data_blended, in_sz, 1);

        if (image1) {delete []image1; image1=0;}
      //  if (image2) {delete []image2; image2=0;}
        if (data_blended) {delete []data_blended; data_blended=0;}
    }
    else if (func_name == tr("xy_yz_combine"))
    {

        if (output.size() != 1) return false;
        char * inimg_file_xy = ((vector<char*> *)(input.at(0).p))->at(0);
        char * inimg_file_yz = ((vector<char*> *)(input.at(1).p))->at(0);
        char * inswc_file = ((vector<char*> *)(input.at(1).p))->at(1);

        char * outimg_file = ((vector<char*> *)(output.at(0).p))->at(0);
        unsigned char * image_xy = 0;
        V3DLONG in_sz_xy[4];
        int datatype_xy;
        if(!simple_loadimage_wrapper(callback, inimg_file_xy, image_xy, in_sz_xy, datatype_xy))
        {
            if (image_xy) {delete []image_xy; image_xy=0;}
            return false;
        }
        V3DLONG pagesz_xy = in_sz_xy[0]*in_sz_xy[1]*in_sz_xy[2]*in_sz_xy[3];
        unsigned char* image_xy_apa = 0;
        image_xy_apa = new unsigned char [pagesz_xy];
        BinaryProcess(image_xy, image_xy_apa,in_sz_xy[0],in_sz_xy[1], in_sz_xy[2], 5, 3);
        double min,max;
        unsigned char* image_xy_apa_scaled = 0;
        image_xy_apa_scaled = new unsigned char [pagesz_xy];
        rescale_to_0_255_and_copy(image_xy_apa,pagesz_xy,min,max,image_xy_apa_scaled);
        for(V3DLONG i = 0; i <pagesz_xy; i++)
        {
            if(image_xy_apa_scaled[i] <=5)
                image_xy_apa_scaled[i] = 0;
        }

        unsigned char* image_xy_apa_scaled_filtered = 0;
        median_filter(image_xy_apa_scaled, in_sz_xy, 3, 3, 3, 1,(unsigned char* &)image_xy_apa_scaled_filtered);

        ColorMap *pc = new ColorMap(colorPseudoMaskColor, 256);
        int clen = pc->len;

        unsigned char * image_yz = 0;
        V3DLONG in_sz_yz[4];
        int datatype_yz;
        if(!simple_loadimage_wrapper(callback, inimg_file_yz, image_yz, in_sz_yz, datatype_yz))
        {
            if (image_yz) {delete []image_yz; image_yz=0;}
            return false;
        }
        V3DLONG pagesz_yz = in_sz_yz[0]*in_sz_yz[1]*in_sz_yz[2]*in_sz_yz[3];
        unsigned char* image_yz_apa = 0;
        image_yz_apa = new unsigned char [pagesz_yz];
        BinaryProcess(image_yz, image_yz_apa,in_sz_yz[0],in_sz_yz[1], in_sz_yz[2], 5, 3);

        unsigned char* image_yz_apa_scaled = 0;
        image_yz_apa_scaled = new unsigned char [pagesz_yz];
        rescale_to_0_255_and_copy(image_yz_apa,pagesz_yz,min,max,image_yz_apa_scaled);

        for(V3DLONG i = 0; i <pagesz_yz; i++)
        {
            if(image_yz_apa_scaled[i] <=5)
                image_yz_apa_scaled[i] = 0;
        }

        unsigned char* image_yz_apa_scaled_filtered = 0;
        median_filter(image_yz_apa_scaled, in_sz_yz, 3, 3, 3, 1,(unsigned char* &)image_yz_apa_scaled_filtered);

        unsigned char* image_yz_apa_scaled_4D = 0;
        image_yz_apa_scaled_4D = new unsigned char [pagesz_yz*3];
        for(V3DLONG i = 0; i<pagesz_yz;i++)
        {
            V3DLONG ind = image_yz_apa_scaled_filtered[i];
            if (ind>=clen) ind = ind % clen;
            image_yz_apa_scaled_4D[i] = 0;//pc->map2d[ind][0];
            image_yz_apa_scaled_4D[pagesz_yz+i] = pc->map2d[ind][1];
            image_yz_apa_scaled_4D[2*pagesz_yz+i] = pc->map2d[ind][2];

        }

        unsigned char * image_xy_yz = 0;
        V3DLONG in_sz_xy_yz[4];
        in_sz_xy_yz[0] = in_sz_xy[0] + in_sz_yz[2];
        in_sz_xy_yz[1] = in_sz_xy[1];
        in_sz_xy_yz[2] = 1;
        in_sz_xy_yz[3] = 3;
        V3DLONG pagesz_xy_yz = in_sz_xy_yz[0]*in_sz_xy_yz[1]*in_sz_xy_yz[2]*in_sz_xy_yz[3];
        V3DLONG pagesz_xy_yz_one = in_sz_xy_yz[0]*in_sz_xy_yz[1]*in_sz_xy_yz[2];

        image_xy_yz = new unsigned char [pagesz_xy_yz];
        for(V3DLONG i = 0; i < pagesz_xy_yz; i++)
            image_xy_yz[i] = 0;
        for(V3DLONG iy = 0; iy < in_sz_xy[1]; iy++)
        {
            V3DLONG offsetj = iy*in_sz_xy_yz[0];
            V3DLONG offsetj_old = iy*in_sz_xy[0];
            for(V3DLONG ix = 0; ix < in_sz_xy[0]; ix++)
            {
                V3DLONG ind = image_xy_apa_scaled_filtered[offsetj_old + ix];
                if (ind>=clen) ind = ind % clen;
                image_xy_yz[offsetj + ix]= 0;//pc->map2d[ind][0];
                image_xy_yz[pagesz_xy_yz_one+offsetj + ix] = pc->map2d[ind][1];
                image_xy_yz[pagesz_xy_yz_one*2+offsetj + ix] = pc->map2d[ind][2];
            }
        }

        NeuronTree nt = readSWC_file(inswc_file);
        for(V3DLONG ii = 0; ii < nt.listNeuron.size(); ii++)
        {
            nt.listNeuron[ii].z = 1;
            nt.listNeuron[ii].r = 2;
        }

        unsigned char* data1d_mask = 0;
        data1d_mask = new unsigned char [pagesz_xy];
        memset(data1d_mask,0,pagesz_xy*sizeof(unsigned char));
//        ComputemaskImage(nt, data1d_mask, in_sz_xy[0], in_sz_xy[1], in_sz_xy[2]);
        QList<int> empty_list;
        ComputemaskImage(nt, data1d_mask, in_sz_xy[0], in_sz_xy[1], in_sz_xy[2], empty_list, 0);

        for(V3DLONG iy = 0; iy < in_sz_xy[1]; iy++)
        {
            V3DLONG offsetj = iy*in_sz_xy_yz[0];
            V3DLONG offsetj_old = iy*in_sz_xy[0];
            for(V3DLONG ix = 0; ix < in_sz_xy[0]; ix++)
            {
                if(data1d_mask[offsetj_old + ix] > 0)
                {
                    image_xy_yz[offsetj + ix]= data1d_mask[offsetj_old + ix];
                    image_xy_yz[pagesz_xy_yz_one+offsetj + ix] = 0;//data1d_mask[offsetj_old + ix];
                    image_xy_yz[pagesz_xy_yz_one*2+offsetj + ix] = 0;//data1d_mask[offsetj_old + ix];
                }
            }
        }

        for(V3DLONG iy = 0; iy < in_sz_xy_yz[1]; iy++)
        {
            V3DLONG offsetj = iy*in_sz_xy_yz[0];
            for(V3DLONG ix = in_sz_xy[0]; ix < in_sz_xy_yz[0]; ix++)
            {
                   V3DLONG offsetz_old = (ix - in_sz_xy[0])*in_sz_xy[1];
                   V3DLONG ind = image_yz_apa_scaled_filtered[offsetz_old + iy];
                   if (ind>=clen) ind = ind % clen;
                   image_xy_yz[offsetj + ix]= 0;//pc->map2d[ind][0];
                   image_xy_yz[pagesz_xy_yz_one+offsetj + ix] = pc->map2d[ind][1];
                   image_xy_yz[pagesz_xy_yz_one*2+offsetj + ix] = pc->map2d[ind][2];
            }
        }

        NeuronTree nt2 = readSWC_file(inswc_file);
        for(V3DLONG ii = 0; ii < nt2.listNeuron.size(); ii++)
        {
            nt2.listNeuron[ii].x = 1;
            nt2.listNeuron[ii].r = 2;

        }

        unsigned char* data1d_mask2 = 0;
        data1d_mask2 = new unsigned char [pagesz_yz];
        memset(data1d_mask2,0,pagesz_yz*sizeof(unsigned char));
//        ComputemaskImage(nt2, data1d_mask2, in_sz_yz[0], in_sz_yz[1], in_sz_yz[2]);
        ComputemaskImage(nt, data1d_mask, in_sz_xy[0], in_sz_xy[1], in_sz_xy[2], empty_list, 0);

        for(V3DLONG iy = 0; iy < in_sz_xy_yz[1]; iy++)
        {
            V3DLONG offsetj = iy*in_sz_xy_yz[0];
            for(V3DLONG ix = in_sz_xy[0]; ix < in_sz_xy_yz[0]; ix++)
            {
                V3DLONG offsetz_old = (ix - in_sz_xy[0])*in_sz_xy[1];
                if(data1d_mask2[offsetz_old + iy] > 0)
                {
                    image_xy_yz[offsetj + ix]= data1d_mask2[offsetz_old + iy];
                    image_xy_yz[pagesz_xy_yz_one+offsetj + ix] = 0;//data1d_mask2[offsetz_old + iy];
                    image_xy_yz[pagesz_xy_yz_one*2+offsetj + ix] =0;// data1d_mask2[offsetz_old + iy];
                }
            }
        }

        simple_saveimage_wrapper(callback, outimg_file, (unsigned char *)image_xy_yz, in_sz_xy_yz, 1);
    }
    else if (func_name == tr("help"))
	{
        cout<<"Usage : v3d -x blend_two_image -f image_blend -o <outimg_file> -p \"^i1 <inputimg1_file> ^c1 <inputimg1_channel> ^o1 <inputimg1_output_channel> ^i2 <inputimg2_file> ^c2 <inputimg2_channel> ^o2 <inputimg2_output_channel>\""<<endl;
        cout<<endl;
        cout<<"inputimg1_channel          the input channel value for inputimg1_file, start from 1"<<endl;
        cout<<"inputimg1_output_channel   the output channel value for inputimg1_file, start from 1,shoule no bigger than 3"<<endl;
        cout<<"inputimg2_channel          the input channel value for inputimg2_file, start from 1"<<endl;
        cout<<"inputimg2_output_channel   the output channel value for inputimg2_file, start from 1,shoule no bigger than 3"<<endl;
        cout<<endl;
        return true;
	}
	else return false;

	return true;
}

template <class T> void median_filter(T* data1d,
                                      V3DLONG *in_sz,
                                      unsigned int Wx,
                                      unsigned int Wy,
                                      unsigned int Wz,
                                      unsigned int c,
                                      T* &outimg)
{

    V3DLONG N = in_sz[0];
    V3DLONG M = in_sz[1];
    V3DLONG P = in_sz[2];
    V3DLONG sc = in_sz[3];
    V3DLONG pagesz = N*M*P;

    T *arr,tmp;
    int ii,jj;
    int size = (2*Wx+1)*(2*Wy+1)*(2*Wz+1);
    arr = new T[size];

    //filtering
    V3DLONG offsetc = (c-1)*pagesz;

    //declare temporary pointer
    T *pImage = new T [pagesz];
    if (!pImage)
    {
        printf("Fail to allocate memory.\n");
        return;
    }
    else
    {
        for(V3DLONG i=0; i<pagesz; i++)
            pImage[i] = 0;
    }

    //Median Filtering
    for(V3DLONG iz = 0; iz < P; iz++)
    {
         printf("\r median filter : %d %% completed ", ((iz + 1)*100) / P);fflush(stdout);
        V3DLONG offsetk = iz*M*N;
        for(V3DLONG iy = 0; iy < M; iy++)
        {
            V3DLONG offsetj = iy*N;
            for(V3DLONG ix = 0; ix < N; ix++)
            {

                V3DLONG xb = ix-Wx; if(xb<0) xb = 0;
                V3DLONG xe = ix+Wx; if(xe>=N-1) xe = N-1;
                V3DLONG yb = iy-Wy; if(yb<0) yb = 0;
                V3DLONG ye = iy+Wy; if(ye>=M-1) ye = M-1;
                V3DLONG zb = iz-Wz; if(zb<0) zb = 0;
                V3DLONG ze = iz+Wz; if(ze>=P-1) ze = P-1;
                ii = 0;

                for(V3DLONG k=zb; k<=ze; k++)
                {
                    V3DLONG offsetkl = k*M*N;
                    for(V3DLONG j=yb; j<=ye; j++)
                    {
                        V3DLONG offsetjl = j*N;
                        for(V3DLONG i=xb; i<=xe; i++)
                        {
                            T dataval = data1d[ offsetc + offsetkl + offsetjl + i];
                            arr[ii] = dataval;
                            if (ii>0)
                            {
                                jj = ii;
                                while(jj > 0 && arr[jj-1]>arr[jj])
                                {
                                    tmp = arr[jj];
                                    arr[jj] = arr[jj-1];
                                    arr[jj-1] = tmp;
                                    jj--;
                                }
                            }
                            ii++;
                        }
                    }
                }

                //set value
                V3DLONG index_pim = offsetk + offsetj + ix;
                pImage[index_pim] = arr[int(0.5*ii)+1];
            }
        }
    }
printf("\n");

    outimg = pImage;
    delete [] arr;

}


/**
 * @file
 * This file is part of SeisSol.
 *
 * @author Carsten Uphoff (c.uphoff AT tum.de, http://www5.in.tum.de/wiki/index.php/Carsten_Uphoff,_M.Sc.)
 * @author Sebastian Wolf (wolf.sebastian AT in.tum.de, https://www5.in.tum.de/wiki/index.php/Sebastian_Wolf,_M.Sc.)
 *
 * @section LICENSE
 * Copyright (c) 2015 - 2020, SeisSol Group
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * @section DESCRIPTION
 **/

#ifndef MODEL_COMMON_HPP_
#define MODEL_COMMON_HPP_

#include <Eigen/Eigen>

#include "utils/logger.h"
#include "Initializer/typedefs.hpp"
#include "generated_code/init.h"
#include "Geometry/MeshTools.h"
#include "Numerical_aux/Transformation.h"

#include "Model/common_datastructures.hpp"

namespace seissol {
  namespace model {
    using Matrix99 = Eigen::Matrix<double, 9, 9>;

    bool testIfAcoustic(real mu);

    template<typename Tmaterial, typename Tmatrix>
    void getTransposedCoefficientMatrix( Tmaterial const& i_material,
                                         unsigned         i_dim,
                                         Tmatrix&         o_M )
    { o_M.setZero(); }

    template<typename Tmaterial, typename T>
    void getTransposedSourceCoefficientTensor(  Tmaterial const& material,
                                                T& E);

    template<typename Tmaterial, typename Tloc, typename Tneigh>
    void getTransposedGodunovState( Tmaterial const&  local,
                                    Tmaterial const&  neighbor,
                                    FaceType          faceType,
                                    Tloc&             QgodLocal,
                                    Tneigh&           QgodNeighbor );

    template<typename T>
    void getTransposedFreeSurfaceGodunovState( bool isAcoustic,
                                               T& QgodLocal,
                                               T& QgodNeighbor,
                                               Eigen::Matrix<double, 9, 9>& R);

    template<typename T>
    void getPlaneWaveOperator( T const& material,
                               double const n[3],
                               std::complex<double> Mdata[NUMBER_OF_QUANTITIES*NUMBER_OF_QUANTITIES] );

    template<typename T, typename S>
    void initializeSpecificLocalData( T const&,
                                      S* LocalData ) {}

    template<typename T, typename S>
    void initializeSpecificNeighborData(  T const&,
                                          S* NeighborData ) {}

    /*
     * Calculates the so called Bond matrix. Anisotropic materials are characterized by
     * 21 different material parameters. Due to the directional dependence of anisotropic
     * materials the parameters are not independet of the choice of the coordinate system.
     * The Bond matrix transforms materials from one orthogonal coordinate system to
     * another one.
     * c.f. 10.1111/j.1365-246X.2007.03381.x
     */
    void getBondMatrix( VrtxCoords const i_normal,
                        VrtxCoords const i_tangent1,
                        VrtxCoords const i_tangent2,
                        real* o_N );

    void getFaceRotationMatrix( VrtxCoords const i_normal,
                                VrtxCoords const i_tangent1,
                                VrtxCoords const i_tangent2,
                                init::T::view::type& o_T,
                                init::Tinv::view::type& o_Tinv );
  }
}

template<typename T>
void seissol::model::getPlaneWaveOperator(  T const& material,
                                            double const n[3],
                                            std::complex<double> Mdata[NUMBER_OF_QUANTITIES*NUMBER_OF_QUANTITIES] )
{
  yateto::DenseTensorView<2,std::complex<double>> M(Mdata, {NUMBER_OF_QUANTITIES, NUMBER_OF_QUANTITIES});
  M.setZero();

  double data[NUMBER_OF_QUANTITIES * NUMBER_OF_QUANTITIES];
  yateto::DenseTensorView<2,double> Coeff(data, {NUMBER_OF_QUANTITIES, NUMBER_OF_QUANTITIES});

  for (unsigned d = 0; d < 3; ++d) {
    Coeff.setZero();
    getTransposedCoefficientMatrix(material, d, Coeff);

    for (unsigned i = 0; i < NUMBER_OF_QUANTITIES; ++i) {
      for (unsigned j = 0; j < NUMBER_OF_QUANTITIES; ++j) {
        M(i,j) += n[d] * Coeff(j,i);
      }
    }
  }
}

template<typename T>
void seissol::model::getTransposedFreeSurfaceGodunovState( bool      isAcoustic,
                                                           T&        QgodLocal,
                                                           T&        QgodNeighbor,
                                                           Eigen::Matrix<double, 9, 9>& R)
{
  for (int i = 0; i < 9; i++) {
    for (int j = 0; j < 9; j++) {
      QgodNeighbor(i,j) = std::numeric_limits<double>::signaling_NaN();
    }
  }

  QgodLocal.setZero();
  if (isAcoustic) {
    // Acoustic material only has one traction (=pressure) and one velocity comp.
    // relevant to the Riemann problem
    QgodLocal(0, 6) = -1 * R(6,0) * 1/R(0,0); // S
    QgodLocal(6, 6) = 1.0;
  } else {
    std::array<int, 3> traction_indices = {0,3,5};
    std::array<int, 3> velocity_indices = {6,7,8};
    using Matrix33 = Eigen::Matrix<double, 3, 3>;
    Matrix33 R11 = R(traction_indices, {0,1,2});
    Matrix33 R21 = R(velocity_indices, {0,1,2});
    Matrix33 S = (-(R21 * R11.inverse())).eval();

    //set lower left block
    int row = 0;
    for (auto &t: traction_indices) {
      int col = 0;
      for (auto &v: velocity_indices) {
        QgodLocal(t, v) = S(row, col);
        col++;
      }
      row++;
    }

    //set lower right block
    for (auto &v : velocity_indices) {
      QgodLocal(v, v) = 1.0;
    }
  }
}

#endif


// Copyright 2021 Anthony Paul Astolfi
//
#include <batteries/state_machine_model.hpp>
//
#include <batteries/state_machine_model.hpp>

#include <boost/functional/hash.hpp>
#include <boost/operators.hpp>

#include <gmock/gmock.h>
#include <gtest/gtest.h>

#include <algorithm>

namespace {

using namespace batt::int_types;

enum struct Player { X, O };
enum struct Square { Empty, X, O };

inline std::ostream& operator<<(std::ostream& out, const Square& t)
{
    switch (t) {
    case Square::Empty:
        return out << " ";
    case Square::X:
        return out << "X";
    case Square::O:
        return out << "O";
    }
    return out << "?";
}

struct GameState : boost::equality_comparable<GameState> {
    std::array<std::array<Square, 3>, 3> board;
    Player to_move;

    GameState() noexcept
        : board{{
              {{Square::Empty, Square::Empty, Square::Empty}},
              {{Square::Empty, Square::Empty, Square::Empty}},
              {{Square::Empty, Square::Empty, Square::Empty}},
          }}
        , to_move{Player::X}
    {
    }

    explicit GameState(std::array<std::array<Square, 3>, 3> b, Player p) noexcept : board{b}, to_move{p}
    {
    }

    batt::RadixQueue<64> pack() const
    {
        batt::RadixQueue<64> p;
        for (const auto& row : this->board) {
            for (const Square sq : row) {
                p.push(3, (int)sq);
            }
        }
        p.push(2, (int)this->to_move);

        return p;
    }

    static GameState unpack(batt::RadixQueue<64> q)
    {
        GameState g;
        for (auto& row : g.board) {
            for (Square& sq : row) {
                sq = (Square)q.pop(3);
            }
        }
        g.to_move = (Player)q.pop(2);
        return g;
    }

    struct Hash {
        usize operator()(const GameState& s) const
        {
            usize seed = 0;
            boost::hash_combine(seed, (int)s.to_move);
            for (const auto& row : s.board) {
                for (const auto& col : row) {
                    boost::hash_combine(seed, (int)col);
                }
            }
            return seed;
        }
    };

    friend bool operator<(const GameState& l, const GameState& r)
    {
        return l.board < r.board || (l.board == r.board && (l.to_move < r.to_move));
    }

    BATT_MAYBE_UNUSED friend std::ostream& operator<<(std::ostream& out, const GameState& t)
    {
        if (t.to_move == Player::X) {
            out << "(X to move)" << std::endl;
        } else {
            out << "(O to move)" << std::endl;
        }
        return out << t.board[0][0] << "|" << t.board[0][1] << "|" << t.board[0][2] << std::endl
                   << "-+-+-" << std::endl
                   << t.board[1][0] << "|" << t.board[1][1] << "|" << t.board[1][2] << std::endl
                   << "-+-+-" << std::endl
                   << t.board[2][0] << "|" << t.board[2][1] << "|" << t.board[2][2];
    }

    friend bool operator==(const GameState& l, const GameState& r)
    {
        return l.board == r.board && l.to_move == r.to_move;
    }
};

usize count(const GameState& s, Square v)
{
    usize n = 0;
    for (const auto& row : s.board) {
        for (const auto& col : row) {
            if (col == v) {
                n += 1;
            }
        }
    }
    return n;
}

GameState rotate90(const GameState& s)
{
    GameState s2 = s;
    for (usize i = 0; i < 3; ++i) {
        for (usize j = 0; j < 3; ++j) {
            s2.board[i][j] = s.board[j][2 - i];
        }
    }
    return s2;
}

GameState flipH(const GameState& s)
{
    GameState s2 = s;
    for (usize i = 0; i < 3; ++i) {
        for (usize j = 0; j < 3; ++j) {
            s2.board[i][j] = s.board[2 - i][j];
        }
    }
    return s2;
}

GameState flipV(const GameState& s)
{
    GameState s2 = s;
    for (usize i = 0; i < 3; ++i) {
        for (usize j = 0; j < 3; ++j) {
            s2.board[i][j] = s.board[i][2 - j];
        }
    }
    return s2;
}

Player flip_player(Player p)
{
    return p == Player::X ? Player::O : Player::X;
}

Square square_from_player(Player p)
{
    return p == Player::X ? Square::X : Square::O;
}

void for_each_symmetry(GameState s, std::function<void(const GameState& s)> fn)
{
    for (int r = 0; r < 4; ++r) {
        fn(s);
        fn(flipH(s));
        fn(flipV(s));
        s = rotate90(s);
    }
}

BATT_MAYBE_UNUSED Square get_winner(const GameState& s)
{
    for (usize i = 0; i < 3; ++i) {
        if (s.board[i][0] != Square::Empty && s.board[i][0] == s.board[i][1] &&
            s.board[i][1] == s.board[i][2]) {
            return s.board[i][0];
        }
        if (s.board[0][i] != Square::Empty && s.board[0][i] == s.board[1][i] &&
            s.board[1][i] == s.board[2][i]) {
            return s.board[0][i];
        }
    }

    // Check diagonals
    //
    if (s.board[1][1] != Square::Empty &&
        ((s.board[0][0] == s.board[1][1] && s.board[1][1] == s.board[2][2]) ||
         (s.board[2][0] == s.board[1][1] && s.board[1][1] == s.board[0][2]))) {
        return s.board[1][1];
    }

    return Square::Empty;
}

class TicTacToeModel : public batt::StateMachineModel<GameState, GameState::Hash>
{
   public:
    bool prune_won_games = true;
    bool prune_symmetries = true;

    GameState initialize() override
    {
        return GameState{/*board*/ {{
                             {{Square::Empty, Square::Empty, Square::Empty}},
                             {{Square::Empty, Square::Empty, Square::Empty}},
                             {{Square::Empty, Square::Empty, Square::Empty}},
                         }},
                         /*to_move*/ Player::X};
    }

    void set_state(const GameState& s) override
    {
        BATT_STATE_MACHINE_VERBOSE() << std::endl
                                     << "Evaluating state: " << std::endl
                                     << s << std::endl
                                     << s.pack();
        this->state_ = s;
    }

    void step() override
    {
        if (this->prune_won_games && get_winner(this->state_) != Square::Empty) {
            return;
        }
        BATT_STATE_MACHINE_VERBOSE() << " No winner.  Generating moves...";

        usize i = this->pick_int(0, 2);
        usize j = 9999;
        this->do_one_of(
            [&] {
                j = this->pick_int(100, 102) - 100;
            },
            [&] {
                j = 0;
            },
            [&] {
                j = 1;
            },
            [&] {
                j = 2;
            });

        BATT_CHECK_IN_RANGE(0, j, 3);

        BATT_STATE_MACHINE_VERBOSE() << " Place at: (" << i << "," << j << ")";

        if (this->state_.board[i][j] == Square::Empty) {
            this->state_.board[i][j] = square_from_player(this->state_.to_move);
            this->state_.to_move = flip_player(this->state_.to_move);
            BATT_STATE_MACHINE_VERBOSE() << "-> " << this->state_;
        } else {
            BATT_STATE_MACHINE_VERBOSE() << " Illegal move.  Skipping...";
        }
    }

    GameState get_state() override
    {
        return this->state_;
    }

    bool check_invariants() override
    {
        usize ne = count(this->state_, Square::Empty);
        usize nx = count(this->state_, Square::X);
        usize no = count(this->state_, Square::O);

        return (ne + nx + no) == 9 && ((nx == no) || (nx == no + 1));
    }

    GameState normalize(const GameState& s) override
    {
        if (!this->prune_symmetries) {
            return s;
        }

        // All symmetries of a game state are equivalent, so pick the "least" one.
        //
        GameState s_norm = s;

        for_each_symmetry(s, [&](const GameState& t) {
            if (t < s_norm) {
                BATT_CHECK(!(s_norm < t) || s_norm == t);
                s_norm = t;
            }
        });
        if (s_norm != s) {
            BATT_STATE_MACHINE_VERBOSE() << " Normalized state: " << s_norm;
        }
        return s_norm;
    }

   private:
    GameState state_;
};

TEST(StateMachineModelTest, Basic)
{
    TicTacToeModel t;

    TicTacToeModel::Result r = t.check_model();

    std::vector<std::pair<int, int>> coords;
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            coords.emplace_back(i, j);
        }
    }
    EXPECT_EQ(coords.size(), 9u);
    usize positions = 0;
    do {
        GameState g;
        auto g_packed = g.pack();
        for (const auto& p : coords) {
            g.board[p.first][p.second] = square_from_player(g.to_move);
            g.to_move = flip_player(g.to_move);
            EXPECT_TRUE(t.state_visited(t.normalize(g)))
                << g << " " << g_packed << " (" << p.first << "," << p.second << ")";
            g_packed = g.pack();
            if (t.prune_won_games && get_winner(g) != Square::Empty) {
                break;
            }
        }

        positions += 1;
    } while (std::next_permutation(coords.begin(), coords.end()));

    EXPECT_EQ(positions, usize(9 * 8 * 7 * 6 * 5 * 4 * 3 * 2 * 1));

    EXPECT_TRUE(r.ok);
    EXPECT_EQ(r.state_count, 764u);
    EXPECT_EQ(r.branch_count, 11475u);
    EXPECT_EQ(r.self_branch_count, 6935u);
}

}  // namespace


#include "drake/solvers/fbstab/components/mpc_data.h"

#include <iostream>
#include <stdexcept>

#include <Eigen/Dense>

namespace drake {
namespace solvers {
namespace fbstab {

using MatrixXd = Eigen::MatrixXd;
using VectorXd = Eigen::VectorXd;
using Map = Eigen::Map<Eigen::MatrixXd>;
using ConstMap = Eigen::Map<const Eigen::MatrixXd>;

MpcData::MpcData(const std::vector<Eigen::MatrixXd>* Q,
                 const std::vector<Eigen::MatrixXd>* R,
                 const std::vector<Eigen::MatrixXd>* S,
                 const std::vector<Eigen::VectorXd>* q,
                 const std::vector<Eigen::VectorXd>* r,
                 const std::vector<Eigen::MatrixXd>* A,
                 const std::vector<Eigen::MatrixXd>* B,
                 const std::vector<Eigen::VectorXd>* c,
                 const std::vector<Eigen::MatrixXd>* E,
                 const std::vector<Eigen::MatrixXd>* L,
                 const std::vector<Eigen::VectorXd>* d,
                 const Eigen::VectorXd* x0) {
  if (Q == nullptr || R == nullptr || S == nullptr || q == nullptr ||
      r == nullptr || A == nullptr || B == nullptr || c == nullptr ||
      E == nullptr || L == nullptr || d == nullptr || x0 == nullptr) {
    throw std::runtime_error("A null pointer was passed to MpcData::MpcData.");
  }

  Q_ = Q;
  R_ = R;
  S_ = S;
  q_ = q;
  r_ = r;
  A_ = A;
  B_ = B;
  c_ = c;
  E_ = E;
  L_ = L;
  d_ = d;
  x0_ = x0;

  validate_length();
  validate_size();

  N_ = B_->size();
  nx_ = B_->at(0).rows();
  nu_ = B_->at(0).cols();
  nc_ = E_->at(0).rows();

  nz_ = (N_ + 1) * (nx_ + nu_);
  nl_ = (N_ + 1) * nx_;
  nv_ = (N_ + 1) * nc_;
}

void MpcData::gemvH(const Eigen::VectorXd& x, double a, double b,
                    Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::gemvH: y input is null.");
  }
  if (x.size() != nz_ || y->size() != nz_) {
    throw std::runtime_error("Size mismatch in MpcData::gemvH.");
  }
  if (b == 0.0) {
    y->fill(0.0);
  } else if (b != 1.0) {
    (*y) *= b;
  }

  // Create reshaped views of input and output vectors.
  Map w(y->data(), nx_ + nu_,
        N_ + 1);  // w = reshape(y, [nx + nu, N + 1]);
  ConstMap v(x.data(), nx_ + nu_,
             N_ + 1);  // v = reshape(x, [nx + nu, N + 1]);
  for (int i = 0; i < N_ + 1; i++) {
    const MatrixXd& Q = Q_->at(i);
    const MatrixXd& S = S_->at(i);
    const MatrixXd& R = R_->at(i);

    // These variables alias w.
    auto yx = w.block(0, i, nx_, 1);
    auto yu = w.block(nx_, i, nu_, 1);

    // These variables alias v.
    const auto vx = v.block(0, i, nx_, 1);
    const auto vu = v.block(nx_, i, nu_, 1);

    // [yx] += a * [Q(i) S(i)'] [vx]
    // [yu]        [S(i) R(i) ] [vu]
    // Using lazyProduct is inefficient so should be avoided when possible.
    if (a == 1.0) {
      yx.noalias() += Q * vx + S.transpose() * vu;
      yu.noalias() += S * vx + R * vu;
    } else if (a == -1.0) {
      yx.noalias() -= Q * vx + S.transpose() * vu;
      yu.noalias() -= S * vx + R * vu;
    } else {
      yx += a * Q.lazyProduct(vx);
      yx += a * S.transpose().lazyProduct(vu);
      yu += a * S.lazyProduct(vx);
      yu += a * R.lazyProduct(vu);
    }
  }
}

void MpcData::gemvA(const Eigen::VectorXd& x, double a, double b,
                    Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::gemvA: y input is null.");
  }
  if (x.size() != nz_ || y->size() != nv_) {
    throw std::runtime_error("Size mismatch in MpcData::gemvA.");
  }
  if (b == 0.0) {
    y->fill(0.0);
  } else if (b != 1.0) {
    (*y) *= b;
  }
  // Create reshaped views of input and output vectors.
  ConstMap z(x.data(), nx_ + nu_, N_ + 1);
  Map w(y->data(), nc_, N_ + 1);

  for (int i = 0; i < N_ + 1; i++) {
    const MatrixXd& E = E_->at(i);
    const MatrixXd& L = L_->at(i);

    // This aliases w.
    auto yi = w.col(i);

    // These alias z.
    const auto xi = z.block(0, i, nx_, 1);
    const auto ui = z.block(nx_, i, nu_, 1);

    // yi += a*(E*vx + L*vu)
    if (a == 1.0) {
      yi.noalias() += E * xi + L * ui;
    } else if (a == -1.0) {
      yi.noalias() -= E * xi + L * ui;
    } else {
      yi += a * E.lazyProduct(xi);
      yi += a * L.lazyProduct(ui);
    }
  }
}

void MpcData::gemvG(const Eigen::VectorXd& x, double a, double b,
                    Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::gemvG: y input is null.");
  }
  if (x.size() != nz_ || y->size() != nl_) {
    throw std::runtime_error("Size mismatch in MpcData::gemvG.");
  }
  if (b == 0.0) {
    y->fill(0.0);
  } else if (b != 1.0) {
    (*y) *= b;
  }
  // Create reshaped views of input and output vectors.
  ConstMap z(x.data(), nx_ + nu_, N_ + 1);
  Map w(y->data(), nx_, N_ + 1);

  w.col(0).noalias() += -a * z.block(0, 0, nx_, 1);

  for (int i = 1; i < N_ + 1; i++) {
    const MatrixXd& A = A_->at(i - 1);
    const MatrixXd& B = B_->at(i - 1);

    // Alias for the output at stage i.
    auto yi = w.col(i);
    // Aliases for the state and control at stage i - 1.
    const auto xm1 = z.block(0, i - 1, nx_, 1);
    const auto um1 = z.block(nx_, i - 1, nu_, 1);
    // Alias for the state at stage i.
    const auto xi = z.block(0, i, nx_, 1);

    // y(i) += a*(A(i-1)*x(i-1) + B(i-1)u(i-1) - x(i))
    if (a == 1.0) {
      yi.noalias() += A * xm1 + B * um1;
      yi.noalias() -= xi;
    } else if (a == -1.0) {
      yi.noalias() -= A * xm1 + B * um1;
      yi.noalias() += xi;
    } else {
      yi += a * A.lazyProduct(xm1);
      yi += a * B.lazyProduct(um1);
      yi -= a * xi;
    }
  }
}

void MpcData::gemvGT(const Eigen::VectorXd& x, double a, double b,
                     Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::gemvGT: y input is null.");
  }
  if (x.size() != nl_ || y->size() != nz_) {
    throw std::runtime_error("Size mismatch in MpcData::gemvGT.");
  }
  if (b == 0.0) {
    y->fill(0.0);
  } else if (b != 1.0) {
    (*y) *= b;
  }

  // Create reshaped views of input and output vectors.
  ConstMap v(x.data(), nx_, N_ + 1);
  Map w(y->data(), nx_ + nu_, N_ + 1);

  for (int i = 0; i < N_; i++) {
    const MatrixXd& A = A_->at(i);
    const MatrixXd& B = B_->at(i);

    // Aliases for the dual variables at stage i and i+1;
    const auto vi = v.col(i);
    const auto vp1 = v.col(i + 1);

    // Aliases for the state and control at stage i.
    auto xi = w.block(0, i, nx_, 1);
    auto ui = w.block(nx_, i, nu_, 1);

    // x(i) += a*(-v(i) + A(i)' * v(i+1))
    // u(i) += a*B(i)' * v(i+1)
    xi.noalias() += -a * vi;
    if (a == 1.0) {
      xi.noalias() += A.transpose() * vp1;
      ui.noalias() += B.transpose() * vp1;
    } else if (a == -1.0) {
      xi.noalias() -= A.transpose() * vp1;
      ui.noalias() -= B.transpose() * vp1;
    } else {
      xi.noalias() += a * A.transpose().lazyProduct(vp1);
    }
  }
  // The i = N step of the recursion.
  w.block(0, N_, nx_, 1).noalias() += -a * v.col(N_);
}

void MpcData::gemvAT(const Eigen::VectorXd& x, double a, double b,
                     Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::gemvAT: y input is null.");
  }
  if (x.size() != nv_ || y->size() != nz_) {
    throw std::runtime_error("Size mismatch in MpcData::gemvAT.");
  }
  if (b == 0.0) {
    y->fill(0.0);
  } else if (b != 1.0) {
    (*y) *= b;
  }
  // Create reshaped views of input and output vectors.
  ConstMap v(x.data(), nc_, N_ + 1);
  Map w(y->data(), nx_ + nu_, N_ + 1);

  for (int i = 0; i < N_ + 1; i++) {
    const MatrixXd& E = E_->at(i);
    const MatrixXd& L = L_->at(i);

    auto xi = w.block(0, i, nx_, 1);
    auto ui = w.block(nx_, i, nu_, 1);

    const auto vi = v.col(i);
    // x(i) += a*E(i)' * v(i)
    // u(i) += a*L(i)' * v(i)
    if (a == 1.0) {
      xi.noalias() += E.transpose() * vi;
      ui.noalias() += L.transpose() * vi;
    } else if (a == -1.0) {
      xi.noalias() -= E.transpose() * vi;
      ui.noalias() -= L.transpose() * vi;
    } else {
      ui.noalias() += a * L.transpose().lazyProduct(vi);
      xi.noalias() += a * E.transpose().lazyProduct(vi);
    }
  }
}

void MpcData::axpyf(double a, Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::axpyf: y input is null.");
  }
  if (y->size() != nz_) {
    throw std::runtime_error("Size mismatch in MpcData::axpyf.");
  }

  // Create reshaped view of the input vector.
  Map w(y->data(), nx_ + nu_, N_ + 1);

  for (int i = 0; i < N_ + 1; i++) {
    auto xi = w.block(0, i, nx_, 1);
    auto ui = w.block(nx_, i, nu_, 1);

    xi.noalias() += a * q_->at(i);
    ui.noalias() += a * r_->at(i);
  }
}

void MpcData::axpyh(double a, Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::axpyh: y input is null.");
  }
  if (y->size() != nl_) {
    throw std::runtime_error("Size mismatch in MpcData::axpyh.");
  }
  // Create reshaped view of the input vector.
  Map w(y->data(), nx_, N_ + 1);
  w.col(0) += -a * (*x0_);

  for (int i = 1; i < N_ + 1; i++) {
    w.col(i) += -a * c_->at(i - 1);
  }
}

void MpcData::axpyb(double a, Eigen::VectorXd* y) const {
  if (y == nullptr) {
    throw std::runtime_error("In MpcData::axpyb: y input is null.");
  }
  if (y->size() != nv_) {
    throw std::runtime_error("Size mismatch in MpcData::axpyb.");
  }
  // Create reshaped view of the input vector.
  Map w(y->data(), nc_, N_ + 1);

  for (int i = 0; i < N_ + 1; i++) {
    w.col(i).noalias() += -a * d_->at(i);
  }
}

void MpcData::validate_length() const {
  bool OK = true;

  const auto N = Q_->size();
  if (N <= 0) {
    throw std::runtime_error("Horizon length must be at least 1.");
  }

  OK = OK && N == R_->size();
  OK = OK && N == S_->size();
  OK = OK && N == q_->size();
  OK = OK && N == r_->size();
  OK = OK && (N - 1) == A_->size();
  OK = OK && (N - 1) == B_->size();
  OK = OK && (N - 1) == c_->size();
  OK = OK && N == E_->size();
  OK = OK && N == L_->size();
  OK = OK && N == d_->size();

  if (!OK) {
    throw std::runtime_error(
        "Sequence length mismatch in input data to MpcData.");
  }
}

void MpcData::validate_size() const {
  const int N = B_->size();

  const int nx = Q_->at(0).rows();
  if (x0_->size() != nx) {
    throw std::runtime_error("Size mismatch in input data to MpcData.");
  }
  for (int i = 0; i < N + 1; i++) {
    if (Q_->at(i).rows() != nx || Q_->at(i).cols() != nx) {
      throw std::runtime_error("Size mismatch in Q input to MpcData.");
    }
    if (S_->at(i).cols() != nx) {
      throw std::runtime_error("Size mismatch in S input to MpcData.");
    }
    if (q_->at(i).size() != nx) {
      throw std::runtime_error("Size mismatch in q input to MpcData.");
    }
    if (E_->at(i).cols() != nx) {
      throw std::runtime_error("Size mismatch in E input to MpcData.");
    }
  }
  for (int i = 0; i < N; i++) {
    if (A_->at(i).rows() != nx || A_->at(i).cols() != nx) {
      throw std::runtime_error("Size mismatch in A input to MpcData.");
    }
    if (B_->at(i).rows() != nx) {
      throw std::runtime_error("Size mismatch in B input to MpcData.");
    }
    if (c_->at(i).size() != nx) {
      throw std::runtime_error("Size mismatch in c input to MpcData.");
    }
  }

  const int nu = R_->at(0).rows();
  for (int i = 0; i < N + 1; i++) {
    if (R_->at(i).rows() != nu || R_->at(i).cols() != nu) {
      throw std::runtime_error("Size mismatch in R input to MpcData.");
    }
    if (S_->at(i).rows() != nu) {
      throw std::runtime_error("Size mismatch in S input to MpcData.");
    }
    if (r_->at(i).size() != nu) {
      throw std::runtime_error("Size mismatch in r input to MpcData.");
    }
    if (L_->at(i).cols() != nu) {
      throw std::runtime_error("Size mismatch in L input to MpcData.");
    }
  }
  for (int i = 0; i < N; i++) {
    if (B_->at(i).cols() != nu) {
      throw std::runtime_error("Size mismatch in B input to MpcData.");
    }
  }

  const int nc = E_->at(0).rows();
  for (int i = 0; i < N + 1; i++) {
    if (E_->at(i).rows() != nc) {
      throw std::runtime_error("Size mismatch in E input to MpcData.");
    }
    if (L_->at(i).rows() != nc) {
      throw std::runtime_error("Size mismatch in L input to MpcData.");
    }
    if (d_->at(i).size() != nc) {
      throw std::runtime_error("Size mismatch in d input to MpcData.");
    }
  }
}

}  // namespace fbstab
}  // namespace solvers
}  // namespace drake


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
// Copyright 2009 Trustees of Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek, Michael Hansen
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <iostream>
#include <vector>

#include <boost/foreach.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/pending/disjoint_sets.hpp>

using namespace boost;

int main(int argc, char* argv[])
{
  typedef adjacency_list <vecS, vecS, undirectedS> Graph;
  typedef graph_traits<Graph>::vertex_descriptor Vertex;
  typedef graph_traits<Graph>::edge_descriptor Edge;
  typedef graph_traits<Graph>::vertices_size_type VertexIndex;

  // Create a graph
  const int VERTEX_COUNT = 6;
  Graph graph(VERTEX_COUNT);

  add_edge(0, 1, graph);
  add_edge(1, 4, graph);

  // reate the disjoint-sets object, which requires rank and parent
  // vertex properties.
  std::vector<Vertex> rank(num_vertices(graph));
  std::vector<Vertex> parent(num_vertices(graph));

  typedef VertexIndex* Rank;
  typedef Vertex* Parent;
  disjoint_sets<Rank, Parent> ds(&rank[0], &parent[0]);

  // Determine the connected components, storing the results in the
  // disjoint-sets object.
  initialize_incremental_components(graph, ds);
  incremental_components(graph, ds);

  // Add a couple more edges and update the disjoint-sets
  add_edge(4, 0, graph);
  add_edge(2, 5, graph);

  ds.union_set(4, 0);
  ds.union_set(2, 5);

  BOOST_FOREACH(Vertex current_vertex, vertices(graph)) {
    std::cout << "representative[" << current_vertex << "] = " <<
      ds.find_set(current_vertex) << std::endl;
  }

  std::cout << std::endl;

  // Generate component index. NOTE: We would need to pass in a vertex
  // index map into the component_index constructor if our graph type
  // used listS instead of vecS (identity_property_map is used by
  // default).
  typedef component_index<VertexIndex> Components;
  Components components(parent.begin(), parent.end());

  // Iterate through the component indices
  BOOST_FOREACH(VertexIndex component_index, components) {
    std::cout << "component " << component_index << " contains: ";

    // Iterate through the child vertex indices for [component_index]
    BOOST_FOREACH(VertexIndex child_index,
                  components[component_index]) {
      std::cout << child_index << " ";
    }

    std::cout << std::endl;
  }

  return (0);
}


#ifndef FOOTSTEP_OBJECT_H
#define FOOTSTEP_OBJECT_H

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

#define LEFT_ROBOT_SIDE 0
#define RIGHT_ROBOT_SIDE 1
#define MIDFOOT_TYPE 2

// Data container for a footstep

class Footstep{
public:
  Footstep();
  Footstep(const Eigen::Vector3d & pos_in, const Eigen::Quaterniond & quat_in, const int & robot_side_in);

  ~Footstep();

  // Position and orientation of the sole of the footstep
  Eigen::Vector3d position;
  Eigen::Quaternion<double> orientation;
  Eigen::Matrix3d R_ori;

  // Setters
  void setPosOriSide(const Eigen::Vector3d & pos_in, const Eigen::Quaterniond & quat_in, const int & robot_side_in);
  void setPosOri(const Eigen::Vector3d & pos_in, const Eigen::Quaterniond & quat_in);
  void setRightSide();
  void setLeftSide();
  void setMidFoot();

  // Left or right side
  int robot_side;

  void computeMidfeet(const Footstep & footstep1, const Footstep & footstep2, Footstep & midfeet);

  int getSide();
  void printInfo();

private:
  void common_initialization();

};

#endif


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file voronoi.hpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-02-01
 */
#ifndef PAAL_VORONOI_HPP
#define PAAL_VORONOI_HPP

#include "voronoi_traits.hpp"

#include "paal/data_structures/metric/metric_traits.hpp"
#include "paal/utils/functors.hpp"

#include <boost/range/as_array.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/functional/hash.hpp>

#include <unordered_set>
#include <unordered_map>
#include <map>
#include <cassert>
#include <climits>

namespace paal {
namespace data_structures {

/**
 * @class voronoi
 * @brief simple implementation of the \ref voronoi concept.
 *
 * @tparam Metric
 */
template <typename Metric> class voronoi {
  public:
    typedef typename metric_traits<Metric>::VertexType VertexType;
    typedef std::multimap<VertexType, VertexType> GeneratorsToVertices;
    typedef std::unordered_set<VertexType, boost::hash<VertexType>>
        GeneratorsSet;
    typedef typename metric_traits<Metric>::DistanceType Dist;
    // TODO change to vector
    typedef GeneratorsSet Vertices;
  private:
    typedef std::unordered_map<VertexType,
                               typename GeneratorsToVertices::iterator,
                               boost::hash<VertexType>> VerticesToGenerators;

    VerticesToGenerators m_vertices_to_generators;
    GeneratorsToVertices m_generators_to_vertices;
    Vertices m_vertices;
    GeneratorsSet m_generators;

    const Metric &m_metric;
    const Dist m_cost_of_no_generator;
public:

    /**
     * @brief Constructor
     *
     * @param generators
     * @param vertices
     * @param m
     * @param costOfNoGenerator
     */
    voronoi(const GeneratorsSet &generators, Vertices vertices, const Metric &m,
            Dist costOfNoGenerator = std::numeric_limits<Dist>::max())
        : m_vertices(std::move(vertices)), m_metric(m),
          m_cost_of_no_generator(costOfNoGenerator) {
        for (VertexType f : generators) {
            add_generator(f);
        }
    }

    /**
     * @brief Copy constructor
     *
     * @param v
     */
    voronoi(const voronoi &v)
        : m_generators_to_vertices(v.m_generators_to_vertices),
          m_vertices(v.m_vertices), m_generators(v.m_generators),
          m_metric(v.m_metric),
          m_cost_of_no_generator(v.m_cost_of_no_generator) {
        auto b = m_generators_to_vertices.begin();
        auto e = m_generators_to_vertices.end();
        for (; b != e; ++b) {
            m_vertices_to_generators.insert(std::make_pair(b->second, b));
        }
    }

    /**
     * @brief Move constructor
     *
     * @param v
     */
    voronoi(voronoi &&v)
        : m_vertices_to_generators(std::move(v.m_vertices_to_generators)),
          m_generators_to_vertices(std::move(v.m_generators_to_vertices)),
          m_vertices(std::move(v.m_vertices)),
          m_generators(std::move(v.m_generators)), m_metric(v.m_metric),
          m_cost_of_no_generator(v.m_cost_of_no_generator) {}

    /// returns diff between new cost and old cost
    Dist add_generator(VertexType f) {
        Dist cost = Dist();
        m_generators.insert(f);

        // first generatorsility
        if (m_generators.size() == 1) {
            m_vertices_to_generators.clear();
            m_generators_to_vertices.clear();
            for (VertexType v : m_vertices) {
                m_vertices_to_generators[v] =
                    m_generators_to_vertices.insert(std::make_pair(f, v));
                cost += m_metric(v, f);
            }

            cost = cost - m_cost_of_no_generator;

        } else {
            for (VertexType v : m_vertices) {
                Dist d = m_metric(v, f) - dist(v);
                if (d < 0) {
                    cost += d;
                    assign(v, f);
                }
            }
        }
        return cost;
    }

    /// returns diff between new cost and old cost
    Dist rem_generator(VertexType f) {
        Dist cost = Dist();
        if (m_generators.size() == 1) {
            cost = m_cost_of_no_generator;
            for (VertexType v : m_vertices) {
                cost -= dist(v);
            }
            m_vertices_to_generators.clear();
            m_generators_to_vertices.clear();
        } else {
            auto op =
                std::bind(utils::not_equal_to(), f, std::placeholders::_1);
            auto begin = m_generators_to_vertices.lower_bound(f);
            auto end = m_generators_to_vertices.upper_bound(f);
            for (; begin != end;) {
                auto v = begin->second;
                // using the fact that generators is a map
                //(with other containers you have to be careful cause of iter
                // invalidation)
                ++begin;
                cost -= dist(v);
                cost += adjust_vertex(v, op);
            }
        }
        m_generators.erase(f);
        return cost;
    }

    /**
     * @brief getter for generators
     *
     * @return
     */
    const GeneratorsSet &get_generators() const { return m_generators; }

    /**
     * @brief getter for vertices
     *
     * @return
     */
    const Vertices &get_vertices() const { return m_vertices; }

    /**
     * @brief getter for vertices assigned to specific generator
     *
     * @param g
     */
    auto get_vertices_for_generator(VertexType g) const ->
    decltype(boost::as_array(m_generators_to_vertices.equal_range(g) |
             boost::adaptors::map_values))
    {
        return boost::as_array(m_generators_to_vertices.equal_range(g) |
               boost::adaptors::map_values);
    }

    ///operator==
    bool operator==(const voronoi & vor) const {
        return boost::equal(m_generators_to_vertices, vor.m_generators_to_vertices) &&
               m_cost_of_no_generator == vor.m_cost_of_no_generator &&
               m_metric == vor.m_metric;
    }

  private:

    /**
     * @brief distance of vertex to closest generator
     *
     * @param v
     *
     * @return
     */
    Dist dist(VertexType v) { return m_metric(v, vertex_to_generators(v)); }

    /**
     * @brief find new generator for vertex
     *             only generators satisfying filer condition are considered
     *
     * @tparam Filter
     * @param v
     * @param filter
     *
     * @return
     */
    template <typename Filter = utils::always_true>
    Dist adjust_vertex(VertexType v, Filter filter = Filter()) {
        bool init = true;
        Dist d = Dist();
        VertexType f_best = VertexType();
        for (VertexType f : m_generators) {
            if (filter(f)) {
                Dist td = m_metric(v, f);
                if (init || td < d) {
                    f_best = f;
                    d = td;
                    init = false;
                }
            }
        }
        assert(!init);
        assign(v, f_best);
        return d;
    }

    /**
     * @brief get generator for given vertex
     *
     * @param v
     *
     * @return
     */
    VertexType vertex_to_generators(VertexType v) const {
        auto i = m_vertices_to_generators.find(v);
        assert(i != m_vertices_to_generators.end());
        return i->second->first;
    }

    /**
     * @brief assign vertex to generator
     *
     * @param v
     * @param f
     */
    void assign(VertexType v, VertexType f) {
        auto prev = m_vertices_to_generators.at(v);
        m_generators_to_vertices.erase(prev);
        m_vertices_to_generators[v] =
            m_generators_to_vertices.insert(std::make_pair(f, v));
    }

};

namespace detail {
    template <typename Metric>
    using v_t = typename metric_traits<Metric>::VertexType;

    template <typename Metric>
    using generators_set_t = std::unordered_set<v_t<Metric>, boost::hash<v_t<Metric>>>;

    template <typename Metric>
    using dist_t = typename metric_traits<Metric>::DistanceType;
}

///make for voronoi
template <typename Metric>
voronoi<Metric> make_voronoi(
        const detail::generators_set_t<Metric> & generators,
        detail::generators_set_t<Metric> vertices,
        const Metric & metric,
        detail::dist_t<Metric> costOfNoGenerator = std::numeric_limits<detail::dist_t<Metric>>::max())
{
    return voronoi<Metric>(generators, std::move(vertices), metric, costOfNoGenerator);
}

/**
 * @brief specialization of voronoi_traits
 *
 * @tparam Metric
 */
template <typename Metric>
struct voronoi_traits<voronoi<Metric>> : public _voronoi_traits<
    voronoi<Metric>, typename metric_traits<Metric>::VertexType> {};
};
};

#endif // PAAL_VORONOI_HPP


/* ----------------------------------------------------------------------------

* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)

* See LICENSE for the license information
* -------------------------------------------------------------------------- */

/**
* @file    TimeTBB.cpp
* @brief   Measure task scheduling overhead in TBB
* @author  Richard Roberts
* @date    November 6, 2013
*/

#include <gtsam/global_includes.h>
#include <gtsam/base/Matrix.h>

#include <boost/assign/list_of.hpp>
#include <map>
#include <iostream>

using namespace std;
using namespace gtsam;
using boost::assign::list_of;

#ifdef GTSAM_USE_TBB

#include <tbb/tbb.h>
#undef max // TBB seems to include windows.h and we don't want these macros
#undef min

static const DenseIndex numberOfProblems = 1000000;
static const DenseIndex problemSize = 4;

typedef Eigen::Matrix<double, problemSize, problemSize> FixedMatrix;

/* ************************************************************************* */
struct ResultWithThreads
{
  typedef map<int, double>::value_type value_type;
  map<int, double> grainSizesWithoutAllocation;
  map<int, double> grainSizesWithAllocation;
};

typedef map<int, ResultWithThreads> Results;

/* ************************************************************************* */
struct WorkerWithoutAllocation
{
  vector<double>& results;

  WorkerWithoutAllocation(vector<double>& results) : results(results) {}

  void operator()(const tbb::blocked_range<size_t>& r) const
  {
    for(size_t i = r.begin(); i != r.end(); ++i)
    {
      FixedMatrix m1 = FixedMatrix::Random();
      FixedMatrix m2 = FixedMatrix::Random();
      FixedMatrix prod = m1 * m2;
      results[i] = prod.norm();
    }
  }
};

/* ************************************************************************* */
map<int, double> testWithoutMemoryAllocation()
{
  // A function to do some matrix operations without allocating any memory

  // Now call it
  vector<double> results(numberOfProblems);

  const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
  map<int, double> timingResults;
  for(size_t grainSize: grainSizes)
  {
    tbb::tick_count t0 = tbb::tick_count::now();
    tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
    tbb::tick_count t1 = tbb::tick_count::now();
    cout << "Without memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
    timingResults[(int)grainSize] = (t1 - t0).seconds();
  }

  return timingResults;
}

/* ************************************************************************* */
struct WorkerWithAllocation
{
  vector<double>& results;

  WorkerWithAllocation(vector<double>& results) : results(results) {}

  void operator()(const tbb::blocked_range<size_t>& r) const
  {
    tbb::cache_aligned_allocator<double> allocator;
    for(size_t i = r.begin(); i != r.end(); ++i)
    {
      double *m1data = allocator.allocate(problemSize * problemSize);
      Eigen::Map<Matrix> m1(m1data, problemSize, problemSize);
      double *m2data = allocator.allocate(problemSize * problemSize);
      Eigen::Map<Matrix> m2(m2data, problemSize, problemSize);
      double *proddata = allocator.allocate(problemSize * problemSize);
      Eigen::Map<Matrix> prod(proddata, problemSize, problemSize);

      m1 = Eigen::Matrix4d::Random(problemSize, problemSize);
      m2 = Eigen::Matrix4d::Random(problemSize, problemSize);
      prod = m1 * m2;
      results[i] = prod.norm();

      allocator.deallocate(m1data, problemSize * problemSize);
      allocator.deallocate(m2data, problemSize * problemSize);
      allocator.deallocate(proddata, problemSize * problemSize);
    }
  }
};

/* ************************************************************************* */
map<int, double> testWithMemoryAllocation()
{
  // A function to do some matrix operations with allocating memory

  // Now call it
  vector<double> results(numberOfProblems);

  const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
  map<int, double> timingResults;
  for(size_t grainSize: grainSizes)
  {
    tbb::tick_count t0 = tbb::tick_count::now();
    tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
    tbb::tick_count t1 = tbb::tick_count::now();
    cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
    timingResults[(int)grainSize] = (t1 - t0).seconds();
  }

  return timingResults;
}

/* ************************************************************************* */
int main(int argc, char* argv[])
{
  cout << "numberOfProblems = " << numberOfProblems << endl;
  cout << "problemSize = " << problemSize << endl;

  const vector<int> numThreads = list_of(1)(4)(8);
  Results results;

  for(size_t n: numThreads)
  {
    cout << "With " << n << " threads:" << endl;
    tbb::task_scheduler_init init((int)n);
    results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
    results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation();
    cout << endl;
  }

  cout << "Summary of results:" << endl;
  for(const Results::value_type& threads_result: results)
  {
    const int threads = threads_result.first;
    const ResultWithThreads& result = threads_result.second;
    if(threads != 1)
    {
      for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithoutAllocation)
      {
        const int grainsize = grainsize_time.first;
        const double speedup = results[1].grainSizesWithoutAllocation[grainsize] / grainsize_time.second;
        cout << threads << " threads, without allocation, grain size = " << grainsize << ", speedup = " << speedup << endl;
      }
      for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithAllocation)
      {
        const int grainsize = grainsize_time.first;
        const double speedup = results[1].grainSizesWithAllocation[grainsize] / grainsize_time.second;
        cout << threads << " threads, with allocation, grain size = " << grainsize << ", speedup = " << speedup << endl;
      }
    }
  }

  return 0;
}

#else

/* ************************************************************************* */
int main(int argc, char* argv [])
{
  cout << "GTSAM is compiled without TBB, please compile with TBB to use this program." << endl;
  return 0;
}

#endif