/*
   Copyright 2012 efalex <email>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#include "box_solver.hpp"
#include <algorithm>
#include <boost/numeric/ublas/io.hpp>
#include <boost/foreach.hpp>
#include <limits.h>
#include "box.hpp"
#include "figure.hpp"
#include "box_solution.hpp"
#include "glwidget.hpp"

using namespace boost::numeric::ublas;

void box_solver::init()
{
    count = 0;
    rotate_matrix.resize(0);
    matrix < int >r(3, 3);
    r(0, 0) =  1; r(0, 1) =  0; r(0, 2) =  0;
    r(1, 0) =  0; r(1, 1) =  0; r(1, 2) = -1;
    r(2, 0) =  0; r(2, 1) =  1; r(2, 2) =  0;
    rotate_matrix.push_back(r);
    r(0, 0) =  0; r(0, 1) =  0; r(0, 2) =  1;
    r(1, 0) =  0; r(1, 1) =  1; r(1, 2) =  0;
    r(2, 0) = -1; r(2, 1) =  0; r(2, 2) =  0;
    rotate_matrix.push_back(r);
    r(0, 0) =  0; r(0, 1) = -1; r(0, 2) =  0;
    r(1, 0) =  1; r(1, 1) =  0; r(1, 2) =  0;
    r(2, 0) =  0; r(2, 1) =  0; r(2, 2) =  1;
    rotate_matrix.push_back(r);
    depth.resize(8);
    for (unsigned int d = 0; d < depth.size(); ++d) {
        depth[d] = 0;
    }
}
box_solver::box_solver():check_count(1000000),stopComputingFlag(false),updateStatusFlag(false)
{
    init();
}

void box_solver::run()
{
#if 1
    initFigures();
    solve();
#else
    initSolution();
    std::cout << box_solution::isUnique(solution, solution_pos) << std::endl;
    GLWidget::gl_count = figures.size();
    GLWidget::gl_solution = solution;
    GLWidget::gl_solution_pos = solution_pos;
    emit drawBox();
    sleep(2);
    box_solution::addSolution(solution, solution_pos);
    std::cout << box_solution::isUnique(solution, solution_pos) << std::endl;
    std::cout << box_solution(solution, solution_pos);
    std::cout << "Depth: " << depth << std::endl;
    std::cout << std::endl;
    sleep(1);
    box_solution::addSolution(solution, solution_pos);
    sleep(1);
    box_solution::addSolution(solution, solution_pos);
    sleep(1);
    box_solution::addSolution(solution, solution_pos);
    sleep(1);
    box_solution::addSolution(solution, solution_pos);
    sleep(1);
    box_solution::addSolution(solution, solution_pos);
    sleep(1);
    box_solution::addSolution(solution, solution_pos);
#endif
}

int box_solver::iterateFigure(unsigned int i)
{
    if (stopComputingFlag) {
        return 0;
    }
    if (i >= figures.size()) {
        if (box_solution::isUnique(solution, solution_pos)) {
            // found solution!
            box_solution::addSolution(solution, solution_pos);
            GLWidget::gl_count = figures.size();
            GLWidget::gl_solution = solution;
            GLWidget::gl_solution_pos = solution_pos;
            GLWidget::gl_pos = box_solution::solution_list.size() - 1;
            std::cout << "Solution found!" << std::endl << " After " << count << " iterations" << std::endl;
            std::cout << "Solution: " << GLWidget::gl_pos << std::endl;
            std::cout << box_solution(solution, solution_pos);
            std::cout << "Depth: " << depth << std::endl;
            std::cout << std::endl;
            emit drawBox();
            usleep(100000);
            //stopComputingFlag = true;
        }
        return 0;
    }
    figure *f = &(figures[i]);
    for (int rx = 0; rx < 4; ++rx) {
        for (int ry = 0; ry < 4; ++ry) {
            for (int rz = 0; rz < 4; ++rz) {
                // find figure boundaries
                vector_int min_v(3);
                vector_int max_v(3);
                min_v[0] = min_v[1] = min_v[2] = INT_MAX;
                max_v[0] = max_v[1] = max_v[2] = INT_MIN;
                BOOST_FOREACH(figure_cube & c, f->cubes) {
                    min_v[0] = std::min(min_v[0], c.pos[0]);
                    max_v[0] = std::max(max_v[0], c.pos[0]);
                    min_v[1] = std::min(min_v[1], c.pos[1]);
                    max_v[1] = std::max(max_v[1], c.pos[1]);
                    min_v[2] = std::min(min_v[2], c.pos[2]);
                    max_v[2] = std::max(max_v[2], c.pos[2]);
                }
                //                std::cout << "min: " << min_v << " max: " << max_v << std::endl;
                //                std::cout << " x : "<< -min_v[0] << " <-> " << 4 - max_v[0] << std::endl;
                //                std::cout << " y : "<< -min_v[1] << " <-> " << 3 - max_v[1] << std::endl;
                //                std::cout << " z : "<< -min_v[2] << " <-> " << 2 - max_v[2] << std::endl;
                if (max_v[0] - min_v[0] > 4 || max_v[1] - min_v[1] > 3 || max_v[2] - min_v[2] > 2) {
                    continue;
                }
                // Move figure in the box
                vector_int v(3);
                for (int x = -min_v[0]; x < /*-min_v[0] + 1*/4 - max_v[0]; ++x) {
                    v[0] = x;
                    for (int y = -min_v[1]; y < /*-min_v[1] + 1 */3 - max_v[1]; ++y) {
                        v[1] = y;
                        for (int z = -min_v[2]; z < /*-min_v[2] + 1 */2 - max_v[2]; ++z) {
                            v[2] = z;
                            //b.dump();
#if 0
                            box *tmp_box = NULL;
                            if (i > 0 && i < 3) {
                                tmp_box = new box;
                                *tmp_box = b;
                            }
#endif
                            if (b.addFigure(*f, v)) {
                                //b.dump();
                                ++count;
                                ++depth[i];
                                solution[i] = *f;
                                solution_pos[i] = v;
#if 0
                                if (b.getVolume() != (i+1) * 6) {
                                    std::cerr << "Volume check error!!!" << std::endl;
                                    std::cerr << "Iteration: " << count << std::endl;
                                    for (int s = 0; s < i; ++s) {
                                        std::cout << "Item: " << s << " Position: " <<  solution_pos[s] << " Direction[0]: " << solution[s].direction[0] << " Direction[1]: " << solution[s].direction[1] << std::endl;
                                    }
                                    std::cerr << "Depth: " << depth << std::endl;
                                    std::cerr << std::endl;
                                }
#endif
                                if (count % check_count == 0 || updateStatusFlag /*|| i >= 6*/) {
                                    updateStatusFlag = false;
                                    std::cout << "Iteration: " << count << std::endl;
                                    for (unsigned int s = 0; s <= i; ++s) {
                                        std::cout << "Item: " << s << " Position: " <<  solution_pos[s] << " Direction[0]: " << solution[s].direction[0] << " Direction[1]: " << solution[s].direction[1] << std::endl;
                                    }
                                    std::cout << "Depth: " << depth << std::endl;
                                    std::cout << std::endl;
                                    GLWidget::gl_count = i + 1;
                                    GLWidget::gl_solution = solution;
                                    GLWidget::gl_solution_pos = solution_pos;
                                    emit drawBox();
                                    //usleep(10000);
                                    //stopComputingFlag = true;
                                }
                                // do recursive call
                                iterateFigure(i + 1);
                                if (stopComputingFlag) {
                                    return 0;
                                }
                                if (!b.delFigure(*f, v)) {
                                    std::cerr << "Delete error!" << std:: endl;
                                    exit(1);
                                }
                                //b.dump();
#if 0
                                if (tmp_box) {
                                    if (*tmp_box != b) {
                                        std::cerr << "Figure add/delete logic error!" << std:: endl;
                                        delete tmp_box;
                                        tmp_box = NULL;
                                        exit(1);
                                    }
                                    delete tmp_box;
                                    tmp_box = NULL;
                                }
#endif
#if 0
                                if (i == 0) {
                                    if (!b.isEmpty()) {
                                        std::cerr << "Figure add/delete logic error!" << std:: endl;
                                        exit(1);
                                    }
                                }
#endif
                            }
                        }
                    }
                }
                f->rotate(rotate_matrix[2]);
            }
            f->rotate(rotate_matrix[1]);
        }
        f->rotate(rotate_matrix[0]);
    }
    return 0;
}

int box_solver::solve()
{
    int res=iterateFigure(0);
    std::cout << "Total iterations: " << count << std::endl;
    std::cout << "Depth: " << depth << std::endl;
    //emit drawBox();
    return res;
}

void box_solver::initFigures()
{
    figures.resize(0);
    solution.resize(8);
    solution_pos.resize(8);
    for (unsigned int p = 0; p < solution_pos.size(); ++p) {
        solution_pos[p].resize(3);
    }
    // Figure 1
    {
        figure f;
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 0;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 1;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 0;
            fc.pos[1] = 0;
            fc.pos[2] = 1;
            int v[3];
            v[0] = 0;
            v[1] = -1;
            v[2] = 1;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 2;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            int v[3];
            v[0] = 1;
            v[1] = 0;
            v[2] = 1;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        figures.push_back(f);
    }
    // Figure 2
    {
        figure f;
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 0;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 1;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 0;
            fc.pos[1] = -1;
            fc.pos[2] = 0;
            int v[3];
            v[0] = -1;
            v[1] = -1;
            v[2] = 0;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 1;
            fc.pos[1] = -1;
            fc.pos[2] = 0;
            int v[3];
            v[0] = 0;
            v[1] = -1;
            v[2] = 1;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        figures.push_back(f);
    }
    // Figure 3
    {
        figure f;
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 0;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 1;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 0;
            fc.pos[1] = -1;
            fc.pos[2] = 0;
            int v[3];
            v[0] = 1;
            v[1] = -1;
            v[2] = 0;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 2;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            int v[3];
            v[0] = 1;
            v[1] = 0;
            v[2] = 1;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        figures.push_back(f);
    }
    // Figure 4
    {
        figure f;
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 0;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 1;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 0;
            fc.pos[1] = 1;
            fc.pos[2] = 0;
            int v[3];
            v[0] = 0;
            v[1] = 1;
            v[2] = 1;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 1;
            fc.pos[1] = -1;
            fc.pos[2] = 0;
            int v[3];
            v[0] = -1;
            v[1] = -1;
            v[2] = 0;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        figures.push_back(f);
    }
    // Figure 5
    {
        figure f;
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 0;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 0;
            fc.pos[1] = -1;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 0;
            fc.pos[1] = -1;
            fc.pos[2] = 1;
            int v[3];
            v[0] = 0;
            v[1] = 1;
            v[2] = 1;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 1;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            int v[3];
            v[0] = 1;
            v[1] = -1;
            v[2] = 0;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        figures.push_back(f);
    }
    // Figure 6
    {
        figure f;
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 0;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 0;
            fc.pos[1] = 0;
            fc.pos[2] = 1;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 1;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            int v[3];
            v[0] = 1;
            v[1] = 0;
            v[2] = 1;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 0;
            fc.pos[1] = -1;
            fc.pos[2] = 1;
            int v[3];
            v[0] = 1;
            v[1] = -1;
            v[2] = 0;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        figures.push_back(f);
    }
    //Figure 7
    {
        figure f;
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 0;
            fc.pos[1] = -1;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 1;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 0;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            int v[3];
            v[0] = -1;
            v[1] = 1;
            v[2] = 0;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 1;
            fc.pos[1] = -1;
            fc.pos[2] = 0;
            int v[3];
            v[0] = 1;
            v[1] = 0;
            v[2] = 1;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        figures.push_back(f);
    }
    //Figure 8
    {
        figure f;
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 1;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_full);
            fc.pos[0] = 0;
            fc.pos[1] = 0;
            fc.pos[2] = 1;
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 0;
            fc.pos[1] = 0;
            fc.pos[2] = 0;
            int v[3];
            v[0] = 0;
            v[1] = -1;
            v[2] = -1;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        {
            figure_cube fc;
            fc.c.setState(cell_half);
            fc.pos[0] = 1;
            fc.pos[1] = 0;
            fc.pos[2] = 1;
            int v[3];
            v[0] = 0;
            v[1] = 1;
            v[2] = 1;
            fc.c.setVector(v);
            f.addFigureCube(fc);
        }
        figures.push_back(f);
    }
}

void box_solver::stopComputing()
{
    stopComputingFlag = true;
}

void box_solver::updateStatus()
{
    updateStatusFlag = true;
}

void box_solver::setRotateVector(boost::numeric::ublas::matrix < int > & m, int x1, int y1, int z1, int x2, int y2 , int z2)
{
    m(0,0) = x1;
    m(1,0) = y1;
    m(2,0) = z1;

    m(0,1) = x2;
    m(1,1) = y2;
    m(2,1) = z2;

    m(0,2) = y1 * z2 - z1 * y2;
    m(1,2) = z1 * x2 - x1 * z2;
    m(2,2) = x1 * y2 - y1 * x2;
}

void box_solver::initSolution() {
    initFigures();
    matrix < int >r(3, 3);

    setRotateVector(r, 0,1,0,-1,0,0);
    solution[0] = figures[0];
    solution[0].rotate(r);
    solution_pos[0][0] = 0;
    solution_pos[0][1] = 0;
    solution_pos[0][2] = 0;

    setRotateVector(r, 1,0,0, 0,1,0);
    solution[1] = figures[1];
    solution[1].rotate(r);
    solution_pos[1][0] = 2;
    solution_pos[1][1] = 1;
    solution_pos[1][2] = 0;

    setRotateVector(r, -1,0,0, 0,0,-1);
    solution[2] = figures[2];
    solution[2].rotate(r);
    solution_pos[2][0] = 3;
    solution_pos[2][1] = 2;
    solution_pos[2][2] = 0;

    setRotateVector(r, 0,0,1, 1,0,0);
    solution[3] = figures[3];
    solution[3].rotate(r);
    solution_pos[3][0] = 1;
    solution_pos[3][1] = 0;
    solution_pos[3][2] = 0;

    setRotateVector(r, 0,0,-1, 0,1,0);
    solution[4] = figures[4];
    solution[4].rotate(r);
    solution_pos[4][0] = 0;
    solution_pos[4][1] = 2;
    solution_pos[4][2] = 1;

    setRotateVector(r, 0,1,0, 0,0,1);
    solution[5] = figures[5];
    solution[5].rotate(r);
    solution_pos[5][0] = 2;
    solution_pos[5][1] = 0;
    solution_pos[5][2] = 1;

    setRotateVector(r, 1,0,0, 0,-1,0);
    solution[6] = figures[6];
    solution[6].rotate(r);
    solution_pos[6][0] = 2;
    solution_pos[6][1] = 1;
    solution_pos[6][2] = 1;

    setRotateVector(r, 0,0,1, -1,0,0);
    solution[7] = figures[7];
    solution[7].rotate(r);
    solution_pos[7][0] = 1;
    solution_pos[7][1] = 2;
    solution_pos[7][2] = 0;

    GLWidget::gl_count = figures.size();
    GLWidget::gl_solution = solution;
    GLWidget::gl_solution_pos = solution_pos;
}


/**
 * $Id$
 *
 * Software License Agreement (GNU General Public License)
 *
 *  Copyright (C) 2015:
 *
 *    Johann Prankl, prankl@acin.tuwien.ac.at
 *    Aitor Aldoma, aldoma@acin.tuwien.ac.at
 *
 *      Automation and Control Institute
 *      Vienna University of Technology
 *      Gusshausstraße 25-29
 *      1170 Vienn, Austria
 *
 *  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 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * @author Johann Prankl
 *
 */

#ifndef KP_TSF_DATA_INTEGRATION_HH
#define KP_TSF_DATA_INTEGRATION_HH

#include <iostream>
#include <fstream>
#include <float.h>
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <boost/thread/mutex.hpp>
#include <boost/thread.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <boost/shared_ptr.hpp>
#include <v4r/common/impl/DataMatrix2D.hpp>
#include <v4r/camera_tracking_and_mapping/TSFData.h>
#include <v4r/camera_tracking_and_mapping/OcclusionClustering.hh>
#include <v4r/core/macros.h>

namespace v4r
{

/**
 * TSFDataIntegration
 */
class V4R_EXPORTS TSFDataIntegration
{
public:

  /**
   * Parameter
   */
  class Parameter
  {
  public:
    int max_integration_frames;
    float sigma_depth;
    bool filter_occlusions;
    double diff_cam_distance_map; // minimum distance the camera must move to select a keyframe
    double diff_delta_angle_map;  // or minimum angle a camera need to rotate to be selected
    int min_frames_integrated;
    Parameter()
      : max_integration_frames(20), sigma_depth(0.008), filter_occlusions(true), diff_cam_distance_map(0.5), diff_delta_angle_map(7.), min_frames_integrated(10) {}
  };

private:
  bool tsf_mapping;
  Parameter param;

  double sqr_diff_cam_distance_map;
  double cos_diff_delta_angle_map;

  static std::vector<cv::Vec4i> npat;

  cv::Mat_<double> intrinsic;

  bool run, have_thread;

  boost::thread th_obectmanagement;
  boost::thread th_init;

  std::vector<float> exp_error_lookup;

  TSFData *data;

  cv::Mat_<float> depth_norm;
  cv::Mat_<float> depth_weight;
  cv::Mat_<float> tmp_z;
  cv::Mat_<float> nan_z;

  Eigen::Matrix4f inv_pose0, inv_pose1;

  cv::Mat_<unsigned char> occ_mask;

  OcclusionClustering occ;

  void operate();

  bool selectFrame(const Eigen::Matrix4f &pose0, const Eigen::Matrix4f &pose1);
  void integrateData(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, const Eigen::Matrix4f &pose, const Eigen::Matrix4f &filt_pose, v4r::DataMatrix2D<Surfel> &filt_cloud);
  inline float sqr(const float &d) {return d*d;}

public:
  cv::Mat dbg;

  TSFDataIntegration(const Parameter &p=Parameter());
  ~TSFDataIntegration();

  void start();
  void stop();

  inline bool isStarted() {return have_thread;}

  inline void lock(){ data->lock(); }        // threaded object management, so we need to lock
  inline void unlock() { data->unlock(); }

  void reset();
  void setData(TSFData *_data) { data = _data; }

  void initCloud(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, v4r::DataMatrix2D<Surfel> &sf_cloud);

  static void computeRadius(v4r::DataMatrix2D<Surfel> &sf_cloud, const cv::Mat_<double> &intrinsic);
  static void computeNormals(v4r::DataMatrix2D<Surfel> &sf_cloud, int nb_dist=1);

  void setCameraParameter(const cv::Mat &_intrinsic);
  void setParameter(const Parameter &p);
  void setSelectMappingFrames(const int &_tsf_mapping) { tsf_mapping = _tsf_mapping; }

  typedef boost::shared_ptr< ::v4r::TSFDataIntegration> Ptr;
  typedef boost::shared_ptr< ::v4r::TSFDataIntegration const> ConstPtr;
};

/*************************** INLINE METHODES **************************/

} //--END--

#endif


#include <boost/python/class.hpp>
#include <boost/python/implicit.hpp>

#include <scitbx/boost_python/container_conversions.h>

#include <smtbx/refinement/constraints/geometrical_hydrogens.h>

#include <sstream>

namespace smtbx { namespace refinement { namespace constraints {
namespace boost_python {

  template <int n_hydrogens, bool staggered>
  struct terminal_tetrahedral_xhn_sites_wrapper
  {
    typedef terminal_tetrahedral_xhn_sites<n_hydrogens, staggered> wt;

    static void wrap() {
      using namespace boost::python;
      std::ostringstream sname;
      if (staggered) sname << "staggered_";
      sname << "terminal_tetrahedral_xh";
      if (n_hydrogens > 1) sname << n_hydrogens;
      sname << "_site";
      if (n_hydrogens > 1) sname << "s";
      std::string name = sname.str();
      if (staggered) {
        class_<wt,
               bases<asu_parameter>,
               std::auto_ptr<wt> >(name.c_str(), no_init)
          .def(init<site_parameter *,
                    site_parameter *,
                    site_parameter *,
                    independent_scalar_parameter *,
                    af::tiny<typename wt::scatterer_type *,
                             n_hydrogens> const &>
               ((arg("pivot"), arg("pivot_neighbour"), arg("stagger_on"),
                 arg("length"),
                 arg("hydrogen"))))
          ;
      }
      else {
        class_<wt,
               bases<asu_parameter>,
               std::auto_ptr<wt> >(name.c_str(), no_init)
          .def(init<site_parameter *,
                    site_parameter *,
                    independent_scalar_parameter *,
                    independent_scalar_parameter *,
                    cart_t const &,
                    af::tiny<typename wt::scatterer_type *,
                             n_hydrogens> const &>
               ((arg("pivot"), arg("pivot_neighbour"),
                 arg("azimuth"), arg("length"),
                 arg("e_zero_azimuth"),
                 arg("hydrogen"))))
          ;
      }
      implicitly_convertible<std::auto_ptr<wt>, std::auto_ptr<parameter> >();
    }
  };

  struct angle_parameter_wrapper {
    typedef angle_parameter wt;

    static void wrap() {
      using namespace boost::python;
      class_<wt,
             bases<scalar_parameter>,
             std::auto_ptr<wt> >("angle_parameter", no_init)
        .def(init<site_parameter *,
                site_parameter *,
                site_parameter *,
                double>
           ((arg("left"), arg("center"), arg("right"), arg("value"))));
      implicitly_convertible<std::auto_ptr<wt>, std::auto_ptr<parameter> >();
    }
  };

  struct secondary_xh2_sites_wrapper
  {
    typedef secondary_xh2_sites wt;

    static void wrap() {
      using namespace boost::python;
      class_<wt,
             bases<asu_parameter>,
             std::auto_ptr<wt> >("secondary_xh2_sites", no_init)
        .def(init<site_parameter *,
                site_parameter *,
                site_parameter *,
                independent_scalar_parameter *,
                scalar_parameter *,
                wt::scatterer_type *,
                wt::scatterer_type *>
           ((arg("pivot"), arg("pivot_neighbour_0"), arg("pivot_neighbour_1"),
             arg("length"), arg("h_c_h_angle"),
             arg("hydrogen_0"), arg("hydrogen_1"))));
      implicitly_convertible<std::auto_ptr<wt>, std::auto_ptr<parameter> >();
    }
  };

  struct tertiary_xh_site_wrapper
  {
    typedef tertiary_xh_site wt;

    static void wrap() {
      using namespace boost::python;
      class_<wt,
             bases<asu_parameter>,
             std::auto_ptr<wt> >("tertiary_xh_site", no_init)
        .def(init<site_parameter *,
                  site_parameter *,
                  site_parameter *,
                  site_parameter *,
                  independent_scalar_parameter *,
                  wt::scatterer_type *>
             ((arg("pivot"), arg("pivot_neighbour_0"), arg("pivot_neighbour_1"),
               arg("pivot_neighbour_2"), arg("length"),
               arg("hydrogen"))));
      implicitly_convertible<std::auto_ptr<wt>, std::auto_ptr<parameter> >();
    }
  };

  struct secondary_planar_xh_site_wrapper
  {
    typedef secondary_planar_xh_site wt;

    static void wrap() {
      using namespace boost::python;
      class_<wt,
             bases<asu_parameter>,
             std::auto_ptr<wt> >("secondary_planar_xh_site", no_init)
        .def(init<site_parameter *,
                  site_parameter *,
                  site_parameter *,
                  independent_scalar_parameter *,
                  wt::scatterer_type *>
             ((arg("pivot"), arg("pivot_neighbour_0"), arg("pivot_neighbour_1"),
               arg("length"),
               arg("hydrogen"))));
      implicitly_convertible<std::auto_ptr<wt>, std::auto_ptr<parameter> >();
    }
  };

  struct terminal_planar_xh2_sites_wrapper
  {
    typedef terminal_planar_xh2_sites wt;

    static void wrap() {
      using namespace boost::python;
      class_<wt,
             bases<asu_parameter>,
             std::auto_ptr<wt> >("terminal_planar_xh2_sites", no_init)
        .def(init<site_parameter *,
                  site_parameter *,
                  site_parameter *,
                  independent_scalar_parameter *,
                  wt::scatterer_type *, wt::scatterer_type *>
             ((arg("pivot"), arg("pivot_neighbour"),
               arg("pivot_neighbour_substituent"), arg("length"),
               arg("hydrogen_0"), arg("hydrogen_1"))));
      implicitly_convertible<std::auto_ptr<wt>, std::auto_ptr<parameter> >();
    }
  };

  struct terminal_linear_ch_site_wrapper
  {
    typedef terminal_linear_ch_site wt;

    static void wrap() {
      using namespace boost::python;
      class_<wt,
             bases<asu_parameter>,
             std::auto_ptr<wt> >("terminal_linear_ch_site", no_init)
        .def(init<site_parameter *,
                  site_parameter *,
                  independent_scalar_parameter *,
                  wt::scatterer_type *>
             ((arg("pivot"), arg("pivot_neighbour"), arg("length"),
               arg("hydrogen"))));
      implicitly_convertible<std::auto_ptr<wt>, std::auto_ptr<parameter> >();
    }
  };

  struct polyhedral_bh_site_wrapper
  {
    typedef polyhedral_bh_site wt;

    static void wrap() {
      using namespace boost::python;
      class_<wt,
             bases<asu_parameter>,
             std::auto_ptr<wt> >("polyhedral_bh_site", no_init)
        .def(init<site_parameter *,
                  af::shared<site_parameter *> const&,
                  independent_scalar_parameter *,
                  wt::scatterer_type *>
             ((arg("pivot"), arg("pivot_neighbours"), arg("length"),
               arg("hydrogen"))));
      implicitly_convertible<std::auto_ptr<wt>, std::auto_ptr<parameter> >();
    }
  };

  void wrap_geometrical_hydrogens() {
    {
      using namespace scitbx::boost_python::container_conversions;
      tuple_mapping_fixed_size<af::tiny<asu_parameter::scatterer_type *, 1> >();
      tuple_mapping_fixed_size<af::tiny<asu_parameter::scatterer_type *, 2> >();
      tuple_mapping_fixed_size<af::tiny<asu_parameter::scatterer_type *, 3> >();
      tuple_mapping_variable_capacity<af::shared<site_parameter *> >();
    }
    //                                    #H  #staggered?
    terminal_tetrahedral_xhn_sites_wrapper<1, false>::wrap();
    terminal_tetrahedral_xhn_sites_wrapper<2, false>::wrap();
    terminal_tetrahedral_xhn_sites_wrapper<3, false>::wrap();
    terminal_tetrahedral_xhn_sites_wrapper<1, true>::wrap();
    terminal_tetrahedral_xhn_sites_wrapper<2, true>::wrap();
    terminal_tetrahedral_xhn_sites_wrapper<3, true>::wrap();

    angle_parameter_wrapper::wrap();
    secondary_xh2_sites_wrapper::wrap();
    tertiary_xh_site_wrapper::wrap();
    secondary_planar_xh_site_wrapper::wrap();
    terminal_planar_xh2_sites_wrapper::wrap();
    terminal_linear_ch_site_wrapper::wrap();
    polyhedral_bh_site_wrapper::wrap();
  }

}}}}


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/miller/match_bijvoet_mates.h>
#include <boost/python/class.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>

namespace cctbx { namespace miller { namespace boost_python {

namespace {

  struct match_bijvoet_mates_wrappers
  {
    typedef match_bijvoet_mates w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t>("match_bijvoet_mates", no_init)
        .def(init<sgtbx::space_group_type const&,
                  af::shared<index<> > const&,
                  bool>((
                    arg("sg_type"), arg("indices"),
                    arg("assert_is_unique_set_under_symmetry")=true)))
        .def(init<sgtbx::reciprocal_space::asu const&,
                  af::shared<index<> > const&,
                  bool>((
                    arg("asu"), arg("indices"),
                    arg("assert_is_unique_set_under_symmetry")=true)))
        .def(init<af::shared<index<> > const&,
                  bool>((
                    arg("indices"),
                    arg("assert_is_unique_set_under_symmetry")=true)))
        .def("pairs", &w_t::pairs)
        .def("singles", &w_t::singles)
        .def("n_singles", &w_t::n_singles)
        .def("pairs_hemisphere_selection", &w_t::pairs_hemisphere_selection)
        .def("singles_hemisphere_selection",
          &w_t::singles_hemisphere_selection, ccr())
        .def("miller_indices_in_hemisphere",
          &w_t::miller_indices_in_hemisphere)
#define CCTBX_DEF(function_name) \
        .def(# function_name, \
          (af::shared<double>(w_t::*)(af::const_ref<double> const&) const) \
          &w_t::function_name)
        CCTBX_DEF(minus)
        CCTBX_DEF(additive_sigmas)
        CCTBX_DEF(average)
#undef CCTBX_DEF
      ;
    }
  };

} // namespace <anoymous>

  void wrap_match_bijvoet_mates()
  {
    match_bijvoet_mates_wrappers::wrap();
  }

}}} // namespace cctbx::miller::boost_python


#include "../Positive_Matrix_With_Prefactor.hxx"
#include "../../sdp_read.hxx"

#include <boost/filesystem.hpp>

void read_json(const boost::filesystem::path &input_path,
               std::vector<El::BigFloat> &objectives,
               std::vector<El::BigFloat> &normalization,
               std::vector<Positive_Matrix_With_Prefactor> &matrices);

void read_mathematica(const boost::filesystem::path &input_path,
                      std::vector<El::BigFloat> &objectives,
                      std::vector<El::BigFloat> &normalization,
                      std::vector<Positive_Matrix_With_Prefactor> &matrices);

void read_input(const boost::filesystem::path &input_file,
                std::vector<El::BigFloat> &objectives,
                std::vector<El::BigFloat> &normalization,
                std::vector<Positive_Matrix_With_Prefactor> &matrices)
{
  if(input_file.extension() == ".nsv")
    {
      for(auto &filename : read_file_list(input_file))
        {
          read_input(filename, objectives, normalization, matrices);
        }
    }
  else if(input_file.extension() == ".json")
    {
      read_json(input_file, objectives, normalization, matrices);
    }
  else
    {
      read_mathematica(input_file, objectives, normalization, matrices);
    }

  for(auto &matrix : matrices)
    {
      for(auto &pole : matrix.damped_rational.poles)
        {
          if(pole > 0)
            {
              throw std::runtime_error(
                "All poles must be negative, but found '" + to_string(pole)
                + "'");
            }
        }
    }
}


#include <boost/mpl/vector.hpp>
#include <boost/mpl/insert_range.hpp>
#include <boost/mpl/begin_end.hpp>
#include "tessellated_icosahedron.hpp"
#include "prog_interface_static_link.h"
#include "basic_voxel.hpp"
#include "image_model.hpp"
#include "odf_decomposition.hpp"
#include "odf_process.hpp"

#include "sample_model.hpp"
#include "space_mapping.hpp"

#include "dti_process.hpp"
#include "dsi_process.hpp"
#include "qbi_process.hpp"
#include "sh_process.hpp"
#include "gqi_process.hpp"
#include "gqi_mni_reconstruction.hpp"

#include "odf_deconvolusion.hpp"
#include "odf_decomposition.hpp"
#include "image_model.hpp"

extern std::string t1w_template_file_name;

typedef boost::mpl::vector<
    ReadDWIData,
    Dwi2Tensor
> dti_process;

typedef boost::mpl::vector<
    ReadDWIData,
    HQSpace2Odf,
    DetermineFiberDirections,
    ScaleZ0ToMinODF,
    SaveFA,
    SaveDirIndex,
    OutputODF
> hgqi_process;

template<class reco_type>
struct odf_reco_type{
    typedef boost::mpl::vector<
        ODFDeconvolusion,
        ODFDecomposition,
        DetermineFiberDirections,
        ScaleZ0ToMinODF,
        SaveFA,
        SaveDirIndex,
        OutputODF
    > common_odf_process;
    typedef typename boost::mpl::insert_range<common_odf_process,boost::mpl::begin<common_odf_process>::type,reco_type>::type type0;
    typedef typename boost::mpl::push_front<type0,ReadDWIData>::type type; // add ReadDWIData to the front
};

template<class reco_type>
struct estimation_type{
    typedef boost::mpl::vector<
        DetermineFiberDirections,
        EstimateResponseFunction
    > common_estimation_process;

    typedef typename boost::mpl::insert_range<common_estimation_process,boost::mpl::begin<common_estimation_process>::type,reco_type>::type type0;
    typedef typename boost::mpl::push_front<type0,ReadDWIData>::type type; // add ReadDWIData to the front
};

typedef odf_reco_type<boost::mpl::vector<
    QSpace2Pdf,
    Pdf2Odf
> >::type dsi_process;

const unsigned int equator_sample_count = 40;
typedef odf_reco_type<boost::mpl::vector<
    QBIReconstruction<equator_sample_count>
> >::type qbi_process;

typedef odf_reco_type<boost::mpl::vector<
    SHDecomposition
> >::type qbi_sh_process;

typedef odf_reco_type<boost::mpl::vector<
    BalanceScheme,
    QSpace2Odf,
    RestrictedDiffusionImaging
> >::type gqi_process;

typedef boost::mpl::vector<
    ReadDWIData,
    QSpaceSpectral
> gqi_spectral_process;

typedef boost::mpl::vector<
    ReadDWIData,
    BalanceScheme,
    SchemeConverter
> hardi_convert_process;

// for ODF deconvolution
typedef estimation_type<boost::mpl::vector<
    QSpace2Pdf,
    Pdf2Odf
> >::type dsi_estimate_response_function;

// for ODF deconvolution
typedef estimation_type<boost::mpl::vector<
    QBIReconstruction<equator_sample_count>
> >::type qbi_estimate_response_function;

// for ODF deconvolution
typedef estimation_type<boost::mpl::vector<
    SHDecomposition
> >::type qbi_sh_estimate_response_function;

// for ODF deconvolution
typedef boost::mpl::vector<
    ReadDWIData,
    BalanceScheme,
    QSpace2Odf,
    DetermineFiberDirections,
    RecordQA,
    EstimateResponseFunction

> gqi_estimate_response_function;

typedef boost::mpl::vector<
    DWINormalization,
    BalanceScheme,
    QSDR,
    RestrictedDiffusionImaging,
    ODFDeconvolusion,
    ODFDecomposition,
    EstimateZ0_MNI,
    DetermineFiberDirections,
    SaveFA,
    SaveDirIndex,
    OutputODF
> gqi_mni_process;

typedef boost::mpl::vector<
    ODFLoader,
    DetermineFiberDirections,
    SaveFA,
    SaveDirIndex
> reprocess_odf;

double base_function(double theta)
{
    if(std::abs(theta) < 0.000001)
        return 1.0/3.0;
    return (2*std::cos(theta)+(theta-2.0/theta)*std::sin(theta))/theta/theta;
}

std::pair<float,float> evaluate_fib(
        const image::geometry<3>& dim,
        const std::vector<std::vector<float> >& fib_fa,
        const std::vector<std::vector<float> >& fib_dir)
{
    unsigned char num_fib = fib_fa.size();
    char dx[13] = {1,0,0,1,1,0, 1, 1, 0, 1,-1, 1, 1};
    char dy[13] = {0,1,0,1,0,1,-1, 0, 1, 1, 1,-1, 1};
    char dz[13] = {0,0,1,0,1,1, 0,-1,-1, 1, 1, 1,-1};
    std::vector<image::vector<3> > dis(13);
    for(unsigned int i = 0;i < 13;++i)
    {
        dis[i] = image::vector<3>(dx[i],dy[i],dz[i]);
        dis[i].normalize();
    }
    float otsu = *std::max_element(fib_fa[0].begin(),fib_fa[0].end())*0.1;
    std::vector<std::vector<unsigned char> > connected(fib_fa.size());
    for(unsigned int index = 0;index < connected.size();++index)
        connected[index].resize(dim.size());
    float connection_count = 0;
    for(image::pixel_index<3> index(dim);index < dim.size();++index)
    {
        if(fib_fa[0][index.index()] <= otsu)
            continue;
        unsigned int index3 = index.index()+index.index()+index.index();
        for(unsigned char fib1 = 0;fib1 < num_fib;++fib1)
        {
            if(fib_fa[fib1][index.index()] <= otsu)
                break;
            for(unsigned int j = 0;j < 2;++j)
            for(unsigned int i = 0;i < 13;++i)
            {
                image::vector<3,int> pos;
                pos = j ? image::vector<3,int>(index[0] + dx[i],index[1] + dy[i],index[2] + dz[i])
                          :image::vector<3,int>(index[0] - dx[i],index[1] - dy[i],index[2] - dz[i]);
                if(!dim.is_valid(pos))
                    continue;
                image::pixel_index<3> other_index(pos[0],pos[1],pos[2],dim);
                unsigned int other_index3 = other_index.index()+other_index.index()+other_index.index();
                if(std::abs(image::vector<3>(&fib_dir[fib1][index3])*dis[i]) <= 0.8665)
                    continue;
                for(unsigned char fib2 = 0;fib2 < num_fib;++fib2)
                    if(fib_fa[fib2][other_index.index()] > otsu &&
                            std::abs(image::vector<3>(&fib_dir[fib2][other_index3])*dis[i]) > 0.8665)
                    {
                        connected[fib1][index.index()] = 1;
                        connected[fib2][other_index.index()] = 1;
                        connection_count += fib_fa[fib2][other_index.index()];
                    }
            }
        }
    }
    float no_connection_count = 0;
    for(image::pixel_index<3> index(dim);index < dim.size();++index)
    {
        for(unsigned int i = 0;i < num_fib;++i)
            if(fib_fa[i][index.index()] > otsu && !connected[i][index.index()])
            {
                no_connection_count += fib_fa[i][index.index()];
            }

    }

    return std::make_pair(connection_count,no_connection_count);
}

void flip_fib_dir(std::vector<float>& fib_dir,const unsigned char* order)
{
    for(unsigned int j = 0;j+2 < fib_dir.size();j += 3)
    {
        float x = fib_dir[j+order[0]];
        float y = fib_dir[j+order[1]];
        float z = fib_dir[j+order[2]];
        fib_dir[j] = x;
        fib_dir[j+1] = y;
        fib_dir[j+2] = z;
        if(order[3])
            fib_dir[j] = -fib_dir[j];
        if(order[4])
            fib_dir[j+1] = -fib_dir[j+1];
        if(order[5])
            fib_dir[j+2] = -fib_dir[j+2];
    }
}

const char* reconstruction(ImageModel* image_model,
                           unsigned int method_id,
                           const float* param_values,
                           bool check_b_table)
{
    static std::string output_name;
    try
    {
        if(!image_model->is_human_data())
            image_model->voxel.csf_calibration = false;
        image_model->voxel.recon_report.clear();
        image_model->voxel.recon_report.str("");
        image_model->voxel.param = param_values;
        std::ostringstream out;
        if(method_id != 4 && method_id != 7)
            image_model->voxel.output_rdi = 0;
        if(method_id == 1) // DTI
        {
            image_model->voxel.need_odf = 0;
            image_model->voxel.output_jacobian = 0;
            image_model->voxel.output_mapping = 0;
            image_model->voxel.scheme_balance = 0;
            image_model->voxel.half_sphere = 0;
            image_model->voxel.odf_deconvolusion = 0;
            image_model->voxel.odf_decomposition = 0;
        }
        else
        {
            out << ".odf" << image_model->voxel.ti.fold;// odf_order
            out << ".f" << image_model->voxel.max_fiber_number;
            if (image_model->voxel.need_odf)
                out << "rec";
            if (image_model->voxel.scheme_balance)
                out << ".bal";
            if (image_model->voxel.half_sphere)
                out << ".hs";
            if (image_model->voxel.csf_calibration &&(method_id == 4 || method_id == 7)) // GQI or QSDR
                out << ".csfc";
            else
                image_model->voxel.csf_calibration = false;
            if (image_model->voxel.odf_deconvolusion)
            {
                out << ".de" << param_values[2];
                if(image_model->voxel.odf_xyz[0] != 0 ||
                   image_model->voxel.odf_xyz[1] != 0 ||
                   image_model->voxel.odf_xyz[2] != 0)
                    out << ".at_" << image_model->voxel.odf_xyz[0]
                        << "_" << image_model->voxel.odf_xyz[1]
                        << "_" << image_model->voxel.odf_xyz[2];
            }
            if (image_model->voxel.odf_decomposition)
            {
                out << ".dec" << param_values[3] << "m" << (int)param_values[4];
                if(image_model->voxel.odf_xyz[0] != 0 ||
                   image_model->voxel.odf_xyz[1] != 0 ||
                   image_model->voxel.odf_xyz[2] != 0)
                    out << ".at_" << image_model->voxel.odf_xyz[0]
                        << "_" << image_model->voxel.odf_xyz[1]
                        << "_" << image_model->voxel.odf_xyz[2];
            }
        }

        // Copy SRC b-table to voxel b-table and sort it
        image_model->voxel.load_from_src(*image_model);

        // correct for b-table orientation
        if(check_b_table)
        {
            set_title("checking b-table");
            bool output_dif = image_model->voxel.output_diffusivity;
            bool output_tensor = image_model->voxel.output_tensor;
            image_model->voxel.output_diffusivity = false;
            image_model->voxel.output_tensor = false;
            image_model->reconstruct<dti_process>();
            image_model->voxel.output_diffusivity = output_dif;
            image_model->voxel.output_tensor = output_tensor;
            std::vector<std::vector<float> > fib_fa(1);
            std::vector<std::vector<float> > fib_dir(1);
            fib_fa[0].swap(image_model->voxel.fib_fa);
            fib_dir[0].swap(image_model->voxel.fib_dir);

            const unsigned char order[18][6] = {
                                    {0,1,2,1,0,0},
                                    {0,1,2,0,1,0},
                                    {0,1,2,0,0,1},
                                    {0,2,1,1,0,0},
                                    {0,2,1,0,1,0},
                                    {0,2,1,0,0,1},
                                    {1,0,2,1,0,0},
                                    {1,0,2,0,1,0},
                                    {1,0,2,0,0,1},
                                    {1,2,0,1,0,0},
                                    {1,2,0,0,1,0},
                                    {1,2,0,0,0,1},
                                    {2,1,0,1,0,0},
                                    {2,1,0,0,1,0},
                                    {2,1,0,0,0,1},
                                    {2,0,1,1,0,0},
                                    {2,0,1,0,1,0},
                                    {2,0,1,0,0,1}};
            const char txt[18][6] = {"012fx","012fy","012fz",
                                     "021fx","021fy","021fz",
                                     "102fx","102fy","102fz",
                                     "120fx","120fy","120fz",
                                     "210fx","210fy","210fz",
                                     "201fx","201fy","201fz"};

            float result[18] = {0};
            float cur_score = evaluate_fib(image_model->voxel.dim,fib_fa,fib_dir).first;
            for(int i = 0;i < 18;++i)
            {
                std::vector<std::vector<float> > new_dir(fib_dir);
                flip_fib_dir(new_dir[0],order[i]);
                result[i] = evaluate_fib(image_model->voxel.dim,fib_fa,new_dir).first;
            }
            int best = std::max_element(result,result+18)-result;

            if(result[best] > cur_score)
            {
                out << "." << txt[best];
                image_model->flip_b_table(order[best]);
                image_model->voxel.load_from_src(*image_model);
            }
        }

        switch (method_id)
        {
        case 0: //DSI local max
            image_model->voxel.recon_report <<
            " The diffusion data were reconstructed using diffusion spectrum imaging (Wedeen et al. MRM, 2005) with a Hanning filter of " << (int)param_values[0] << ".";
            if (image_model->voxel.odf_deconvolusion || image_model->voxel.odf_decomposition)
            {
                if (!image_model->reconstruct<dsi_estimate_response_function>())
                    return "reconstruction canceled";
            }
            out << ".dsi."<< (int)param_values[0] << ".fib.gz";
            if (!image_model->reconstruct<dsi_process>())
                return "reconstruction canceled";
            break;
        case 1://DTI
            image_model->voxel.recon_report << " The diffusion tensor was calculated.";
            out << ".dti.fib.gz";
            image_model->voxel.max_fiber_number = 1;
            if (!image_model->reconstruct<dti_process>())
                return "reconstruction canceled";
            break;

        case 2://QBI
            image_model->voxel.recon_report << " The diffusion data was reconstructed using q-ball imaging (Tuch, MRM 2004).";
            if (image_model->voxel.odf_deconvolusion || image_model->voxel.odf_decomposition)
            {
                if (!image_model->reconstruct<qbi_estimate_response_function>())
                    return "reconstruction canceled";
            }
            out << ".qbi."<< param_values[0] << "_" << param_values[1] << ".fib.gz";
            if (!image_model->reconstruct<qbi_process>())
                return "reconstruction canceled";
            break;
        case 3://QBI
            image_model->voxel.recon_report << " The diffusion data was reconstructed using spherical-harmonic-based q-ball imaging (Descoteaux et al., MRM 2007).";
            if (image_model->voxel.odf_deconvolusion || image_model->voxel.odf_decomposition)
            {
                if (!image_model->reconstruct<qbi_sh_estimate_response_function>())
                    return "reconstruction canceled";
            }
            out << ".qbi.sh"<< (int) param_values[1] << "." << param_values[0] << ".fib.gz";
            if (!image_model->reconstruct<qbi_sh_process>())
                return "reconstruction canceled";
            break;

        case 4://GQI
            if(param_values[0] == 0.0) // spectral analysis
            {
                image_model->voxel.recon_report <<
                " The diffusion data were reconstructed using generalized q-sampling imaging (Yeh et al., IEEE TMI, ;29(9):1626-35, 2010).";
                out << (image_model->voxel.r2_weighted ? ".gqi2.spec.fib.gz":".gqi.spec.fib.gz");
                if (!image_model->reconstruct<gqi_spectral_process>())
                    return "reconstruction canceled";
                break;
            }
            image_model->voxel.recon_report <<
            " The diffusion data were reconstructed using generalized q-sampling imaging (Yeh et al., IEEE TMI, ;29(9):1626-35, 2010) with a diffusion sampling length ratio of " << (float)param_values[0] << ".";
            if(image_model->voxel.output_rdi)
                image_model->voxel.recon_report <<
                    " The restricted diffusion was quantified using restricted diffusion imaging (Yeh et al., MRM, 77:603–612 (2017)).";

            if (image_model->voxel.odf_deconvolusion || image_model->voxel.odf_decomposition)
            {
                if (!image_model->reconstruct<gqi_estimate_response_function>())
                    return "reconstruction canceled";
            }
            if(image_model->voxel.r2_weighted)
                image_model->voxel.recon_report << " The ODF calculation was weighted by the square of the diffuion displacement.";
            if (image_model->voxel.output_rdi)
                out << ".rdi";
            out << (image_model->voxel.r2_weighted ? ".gqi2.":".gqi.") << param_values[0] << ".fib.gz";
            if(image_model->src_dwi_data.size() == 1)
            {
                if (!image_model->reconstruct<hgqi_process>())
                    return "reconstruction canceled";
                break;
            }

            if (!image_model->reconstruct<gqi_process>())
                return "reconstruction canceled";
            break;
        case 6:
            image_model->voxel.recon_report
                    << " The diffusion data were converted to HARDI using generalized q-sampling method with a regularization parameter of " << param_values[2] << ".";
            out << ".hardi."<< param_values[0]
                << ".b" << param_values[1]
                << ".reg" << param_values[2] << ".src.gz";
            if (!image_model->reconstruct<hardi_convert_process>())
                return "reconstruction canceled";
            break;
        case 7:

            if(image_model->voxel.reg_method == 4) // DMDM
            {
                {
                    gz_nifti in;
                    if(!in.load_from_file(t1w_template_file_name.c_str()) || !in.toLPS(image_model->voxel.t1wt))
                        return "Cannot load T1W template";
                    in.get_voxel_size(image_model->voxel.t1wt_vs.begin());
                    in.get_image_transformation(image_model->voxel.t1wt_tran);
                }
                {
                    gz_nifti in;
                    if(!in.load_from_file(image_model->voxel.t1w_file_name.c_str()) || !in.toLPS(image_model->voxel.t1w))
                        return "Cannot load T1W for DMDM normaliztion";
                    in.get_voxel_size(image_model->voxel.t1w_vs.begin());
                }
            }

            image_model->voxel.recon_report
            << " The diffusion data were reconstructed in the MNI space using q-space diffeomorphic reconstruction (Yeh et al., Neuroimage, 58(1):91-9, 2011) to obtain the spin distribution function (Yeh et al., IEEE TMI, ;29(9):1626-35, 2010). "
            << " A diffusion sampling length ratio of "
            << (float)param_values[0] << " was used, and the output resolution was " << param_values[1] << " mm.";
            // run gqi to get the spin quantity

            if(image_model->voxel.output_rdi)
                image_model->voxel.recon_report <<
                    " The restricted diffusion was quantified using restricted diffusion imaging (Yeh et al., MRM, 77:603–612 (2017)).";

            {
                std::vector<image::pointer_image<float,3> > tmp;
                tmp.swap(image_model->voxel.grad_dev);
                // clear mask to create whole volume QA map
                std::fill(image_model->voxel.mask.begin(),image_model->voxel.mask.end(),1.0);
                if (!image_model->reconstruct<gqi_estimate_response_function>())
                    return "reconstruction canceled";
                tmp.swap(image_model->voxel.grad_dev);
            }

            if(image_model->voxel.reg_method < 4) // 0,1,2 SPM norm, 3 CDM, 4 CDM-T1W
            {
                if(image_model->voxel.reg_method == 3)
                    out << ".cdm";
                else
                    out << ".reg" << (int)image_model->voxel.reg_method;
            }
            else
                out << ".cdmt1w";

            out << (image_model->voxel.r2_weighted ? ".qsdr2.":".qsdr.");
            out << param_values[0] << "." << param_values[1] << "mm";
            if(image_model->voxel.output_jacobian)
                out << ".jac";
            if(image_model->voxel.output_mapping)
                out << ".map";
            if (!image_model->reconstruct<gqi_mni_process>())
                return "reconstruction canceled";
            out << ".R" << (int)std::floor(image_model->voxel.R2*100.0) << ".fib.gz";
            break;
        }
        image_model->save_fib(out.str());
        output_name = image_model->file_name + out.str();
    }
    catch (std::exception& e)
    {
        output_name = e.what();
        return output_name.c_str();
    }
    catch (...)
    {
        return "unknown exception";
    }
    return output_name.c_str();
}

bool output_odfs(const image::basic_image<unsigned char,3>& mni_mask,
                 const char* out_name,
                 const char* ext,
                 std::vector<std::vector<float> >& odfs,
                 const tessellated_icosahedron& ti,
                 const float* vs,
                 const float* mni,
                 const std::string& report,
                 bool record_odf = true)
{
    begin_prog("output");
    ImageModel image_model;
    if(report.length())
        image_model.voxel.report = report.c_str();
    image_model.voxel.dim = mni_mask.geometry();
    image_model.voxel.ti = ti;
    image_model.voxel.max_fiber_number = 5;
    image_model.voxel.need_odf = record_odf;
    image_model.voxel.template_odfs.swap(odfs);
    image_model.voxel.param = mni;
    image_model.file_name = out_name;
    image_model.voxel.mask = mni_mask;
    std::copy(vs,vs+3,image_model.voxel.vs.begin());
    if (prog_aborted() || !image_model.reconstruct<reprocess_odf>())
        return false;
    image_model.save_fib(ext);
    image_model.voxel.template_odfs.swap(odfs);
    return true;
}

const char* odf_average(const char* out_name,std::vector<std::string>& file_names)
{
    static std::string error_msg,report;
    tessellated_icosahedron ti;
    float vs[3];
    image::basic_image<unsigned char,3> mask;
    std::vector<std::vector<float> > odfs;
    unsigned int half_vertex_count = 0;
    unsigned int row,col;
    float mni[16]={0};
    begin_prog("averaging");
    for (unsigned int index = 0;check_prog(index,file_names.size());++index)
    {
        const char* file_name = file_names[index].c_str();
        gz_mat_read reader;
        set_title(file_names[index].c_str());
        if(!reader.load_from_file(file_name))
        {
            error_msg = "Cannot open file ";
            error_msg += file_name;
            check_prog(0,0);
            return error_msg.c_str();
        }
        if(index == 0)
        {
            {
                const char* report_buf = 0;
                if(reader.read("report",row,col,report_buf))
                    report = std::string(report_buf,report_buf+row*col);
            }
            const float* odf_buffer;
            const short* face_buffer;
            const unsigned short* dimension;
            const float* vs_ptr;
            const float* fa0;
            const float* mni_ptr;
            unsigned int face_num,odf_num;
            error_msg = "";
            if(!reader.read("dimension",row,col,dimension))
                error_msg = "dimension";
            if(!reader.read("fa0",row,col,fa0))
                error_msg = "fa0";
            if(!reader.read("voxel_size",row,col,vs_ptr))
                error_msg = "voxel_size";
            if(!reader.read("odf_faces",row,face_num,face_buffer))
                error_msg = "odf_faces";
            if(!reader.read("odf_vertices",row,odf_num,odf_buffer))
                error_msg = "odf_vertices";
            if(!reader.read("trans",row,col,mni_ptr))
                error_msg = "trans";
            if(error_msg.length())
            {
                error_msg += " missing in ";
                error_msg += file_name;
                check_prog(0,0);
                return error_msg.c_str();
            }
            mask.resize(image::geometry<3>(dimension));
            for(unsigned int index = 0;index < mask.size();++index)
                if(fa0[index] != 0.0)
                    mask[index] = 1;
            std::copy(vs_ptr,vs_ptr+3,vs);
            ti.init(odf_num,odf_buffer,face_num,face_buffer);
            half_vertex_count = odf_num >> 1;
            std::copy(mni_ptr,mni_ptr+16,mni);
        }
        else
        // check odf consistency
        {
            const float* odf_buffer;
            const unsigned short* dimension;
            unsigned int odf_num;
            error_msg = "";
            if(!reader.read("dimension",row,col,dimension))
                error_msg = "dimension";
            if(!reader.read("odf_vertices",row,odf_num,odf_buffer))
                error_msg = "odf_vertices";
            if(error_msg.length())
            {
                error_msg += " missing in ";
                error_msg += file_name;
                check_prog(0,0);
                return error_msg.c_str();
            }

            if(odf_num != ti.vertices_count || dimension[0] != mask.width() ||
                    dimension[1] != mask.height() || dimension[2] != mask.depth())
            {
                error_msg = "Inconsistent dimension in ";
                error_msg += file_name;
                check_prog(0,0);
                return error_msg.c_str();
            }
            for (unsigned int index = 0;index < col;++index,odf_buffer += 3)
            {
                if(ti.vertices[index][0] != odf_buffer[0] ||
                   ti.vertices[index][1] != odf_buffer[1] ||
                   ti.vertices[index][2] != odf_buffer[2])
                {
                    error_msg = "Inconsistent ODF orientations in ";
                    error_msg += file_name;
                    return error_msg.c_str();
                }
            }
        }

        {
            const float* fa0;
            if(!reader.read("fa0",row,col,fa0))
            {
                error_msg = "Cannot find image information in ";
                error_msg += file_name;
                check_prog(0,0);
                return error_msg.c_str();
            }
            for(unsigned int index = 0;index < mask.size();++index)
                if(fa0[index] != 0.0)
                    mask[index] = 1;
        }

        std::vector<const float*> odf_bufs;
        std::vector<unsigned int> odf_bufs_size;
        //get_odf_bufs(reader,odf_bufs,odf_bufs_size);
        {
            odf_bufs.clear();
            odf_bufs_size.clear();
            for (unsigned int odf_index = 0;1;++odf_index)
            {
                std::ostringstream out;
                out << "odf" << odf_index;
                const float* odf_buf = 0;
                if (!reader.read(out.str().c_str(),row,col,odf_buf))
                    break;
                odf_bufs.push_back(odf_buf);
                odf_bufs_size.push_back(row*col);
            }
        }
        if(odf_bufs.empty())
        {
            error_msg += "No ODF data found in ";
            error_msg += file_name;
            check_prog(0,0);
            return error_msg.c_str();
        }
        if(index == 0)
        {
            odfs.resize(odf_bufs.size());
            for(unsigned int i = 0;i < odf_bufs.size();++i)
                odfs[i].resize(odf_bufs_size[i]);
        }
        else
        {
            bool inconsistence = false;
            if(odfs.size() != odf_bufs.size())
                inconsistence = true;
            for(unsigned int i = 0;i < odf_bufs.size();++i)
                if(odfs[i].size() != odf_bufs_size[i])
                    inconsistence = true;
            if(inconsistence)
            {
                error_msg = "Inconsistent mask coverage in ";
                error_msg += file_name;
                check_prog(0,0);
                return error_msg.c_str();
            }
        }
        for(unsigned int i = 0;i < odf_bufs.size();++i)
            image::add(odfs[i].begin(),odfs[i].end(),odf_bufs[i]);
    }
    if (prog_aborted())
        return 0;

    set_title("averaging odfs");
    for (unsigned int odf_index = 0;odf_index < odfs.size();++odf_index)
        for (unsigned int j = 0;j < odfs[odf_index].size();++j)
            odfs[odf_index][j] /= (double)file_names.size();

    std::ostringstream out;
    out << "A group average template was constructed from a total of " << file_names.size() << " subjects." << report.c_str();
    report = out.str();
    set_title("output files");
    output_odfs(mask,out_name,".mean.odf.fib.gz",odfs,ti,vs,mni,report);
    output_odfs(mask,out_name,".mean.fib.gz",odfs,ti,vs,mni,report,false);
    return 0;
}