/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "NDDODipoleMomentCalculator.h"
#include "NDDODipoleMatrixCalculator.h"
#include <Sparrow/Implementations/Nddo/Am1/AM1Method.h>
#include <Sparrow/Implementations/Nddo/Mndo/MNDOMethod.h>
#include <Sparrow/Implementations/Nddo/Pm6/PM6Method.h>
#include <Sparrow/Implementations/Nddo/Utils/IntegralsEvaluationUtils/multipoleTypes.h>
#include <Sparrow/Implementations/Nddo/Utils/NDDOInitializer.h>
#include <Sparrow/Implementations/Nddo/Utils/ParameterUtils/PrincipalQuantumNumbers.h>
#include <Utils/DataStructures/AtomsOrbitalsIndexes.h>
#include <Utils/Geometry.h>
#include <Eigen/Eigenvalues>

namespace Scine {
namespace Sparrow {

template<class NDDOMethod>
NDDODipoleMomentCalculator<NDDOMethod>::~NDDODipoleMomentCalculator() = default;

template<class NDDOMethod>
NDDODipoleMomentCalculator<NDDOMethod>::NDDODipoleMomentCalculator(NDDOMethod& method, DipoleMatrixCalculator& dipoleMatrixCalculator)
  : method_(method), dipoleMatrixCalculator_(dipoleMatrixCalculator) {
}

template<class NDDOMethod>
std::unique_ptr<NDDODipoleMomentCalculator<NDDOMethod>>
NDDODipoleMomentCalculator<NDDOMethod>::create(NDDOMethod& method, DipoleMatrixCalculator& dipoleMatrixCalculator) {
  NDDODipoleMomentCalculator<NDDOMethod> instance(method, dipoleMatrixCalculator);
  return std::make_unique<NDDODipoleMomentCalculator<NDDOMethod>>(std::move(instance));
}

template<class NDDOMethod>
Eigen::RowVector3d NDDODipoleMomentCalculator<NDDOMethod>::calculate() const {
  auto atomicCharges = method_.getAtomicCharges();
  auto coreCharges = method_.getInitializer().getCoreCharges();
  auto densityMatrix = method_.getDensityMatrix().restrictedMatrix();
  auto positions = method_.getPositions();

  if (useNDDOApproximation_) {
    auto elements = method_.getElementTypes();
    std::vector<int> nrAOs(positions.rows());
    std::vector<double> chargeSeparationSP(nrAOs.size());
    std::vector<double> chargeSeparationPD(nrAOs.size());

    for (int el = 0; el < nrAOs.size(); ++el) {
      nrAOs[el] = method_.getAtomsOrbitalsIndexesHolder().getNOrbitals(el);
      chargeSeparationSP[el] =
          method_.getInitializer().getElementParameters().get(elements[el]).chargeSeparations().get(nddo::multipole::sp1);
      chargeSeparationPD[el] =
          method_.getInitializer().getElementParameters().get(elements[el]).chargeSeparations().get(nddo::multipole::pd1);
    }
    return calculateWithNDDOApproximation(std::move(atomicCharges), std::move(positions), std::move(densityMatrix),
                                          std::move(elements), std::move(nrAOs), std::move(chargeSeparationSP),
                                          std::move(chargeSeparationPD));
  }
  else {
    auto overlapMatrix = method_.getOverlapMatrix();
    Eigen::RowVector3d evaluationCoordinate = Utils::Position::Zero();
    if (!dipoleMatrixCalculator_.isValid()) {
      dipoleMatrixCalculator_.fillDipoleMatrix(evaluationCoordinate);
    }
    return calculateWithDipoleMatrix(std::move(coreCharges), std::move(positions), std::move(densityMatrix),
                                     dipoleMatrixCalculator_.getAODipoleMatrix(), std::move(overlapMatrix),
                                     std::move(evaluationCoordinate));
  }
}

template<class NDDOMethod>
Eigen::RowVector3d NDDODipoleMomentCalculator<NDDOMethod>::calculateWithNDDOApproximation(
    std::vector<double> atomicCharges, Utils::PositionCollection positions, Eigen::MatrixXd densityMatrix,
    Utils::ElementTypeCollection elements, std::vector<int> nrAOs, std::vector<double> chargeSeparationSP,
    std::vector<double> chargeSeparationPD) const {
  Eigen::MatrixX3d atomicDipoles(positions.rows(), 3);

  Eigen::RowVector3d totalDipole = Utils::Dipole::Zero(3);

  int currentdensityMatrixOrbital = 0;
  // Put center of mass at the origin
  auto centerOfMass = Utils::Geometry::getCenterOfMass(positions, Utils::Geometry::getMasses(elements));
  positions.rowwise() -= centerOfMass;
  for (unsigned atom = 0; atom < atomicDipoles.rows(); ++atom) {
    // Mullikan charges dipole contribution
    atomicDipoles.row(atom) = atomicCharges[atom] * positions.row(atom);

    // SP hybridization
    if (chargeSeparationSP[atom] != 0) {
      const double HybrSP = 2.0 * chargeSeparationSP[atom];
      for (int pOrbComponent = 0; pOrbComponent < 3; ++pOrbComponent) {
        atomicDipoles.row(atom)(pOrbComponent) -=
            HybrSP * densityMatrix(currentdensityMatrixOrbital, currentdensityMatrixOrbital + 1 + pOrbComponent);
      }
    }
    // PD hybridization
    //                                  0  1   2   3   4         5      6      7      8
    // d orbitals are ordered this way: s, px, py, pz, d(x2-y2), d(xz), d(z2), d(yz), d(xy)
    // where 0 corresponds to currentdensityMatrixOrbital
    // See generalTypes.h for details.
    if (chargeSeparationPD[atom] != 0) {
      const double HybrPD = 2.0 * chargeSeparationPD[atom];
      const double OneOverSqrt3 = 1.0 / std::sqrt(3);

      const double DensMatComponentsX =
          densityMatrix(currentdensityMatrixOrbital + 1, currentdensityMatrixOrbital + 4) -
          densityMatrix(currentdensityMatrixOrbital + 1, currentdensityMatrixOrbital + 6) * OneOverSqrt3 +
          densityMatrix(currentdensityMatrixOrbital + 2, currentdensityMatrixOrbital + 8) +
          densityMatrix(currentdensityMatrixOrbital + 3, currentdensityMatrixOrbital + 5);

      atomicDipoles.row(atom)(0) -= DensMatComponentsX * HybrPD;

      const double DensMatComponentsY =
          densityMatrix(currentdensityMatrixOrbital + 1, currentdensityMatrixOrbital + 8) -
          densityMatrix(currentdensityMatrixOrbital + 2, currentdensityMatrixOrbital + 4) -
          densityMatrix(currentdensityMatrixOrbital + 2, currentdensityMatrixOrbital + 6) * OneOverSqrt3 +
          densityMatrix(currentdensityMatrixOrbital + 3, currentdensityMatrixOrbital + 7);

      atomicDipoles.row(atom)(1) -= DensMatComponentsY * HybrPD;

      const double DensMatComponentsZ =
          densityMatrix(currentdensityMatrixOrbital + 1, currentdensityMatrixOrbital + 5) +
          densityMatrix(currentdensityMatrixOrbital + 2, currentdensityMatrixOrbital + 7) +
          densityMatrix(currentdensityMatrixOrbital + 3, currentdensityMatrixOrbital + 6) * 2.0 * OneOverSqrt3;

      atomicDipoles.row(atom)(2) -= DensMatComponentsZ * HybrPD;
    }

    currentdensityMatrixOrbital += nrAOs[atom];
  }

  totalDipole += atomicDipoles.colwise().sum();
  return totalDipole;
}

template<class NDDOMethod>
void NDDODipoleMomentCalculator<NDDOMethod>::useNDDOApproximation(bool useNDDOApprox) {
  useNDDOApproximation_ = useNDDOApprox;
}

template<class NDDOMethod>
Eigen::RowVector3d NDDODipoleMomentCalculator<NDDOMethod>::calculateWithDipoleMatrix(
    std::vector<double> coreCharges, Utils::PositionCollection positions, Eigen::MatrixXd densityMatrix,
    Utils::DipoleMatrix dipoleMatrix, Eigen::MatrixXd overlapMatrix, Eigen::RowVector3d dipoleEvaluationCoordinate) const {
  auto const nAtoms = coreCharges.size();
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(overlapMatrix);
  Eigen::MatrixXd sqrtS = es.operatorInverseSqrt();
  Eigen::MatrixXd nonOrthogonalDensityMatrix =
      sqrtS.selfadjointView<Eigen::Upper>() * densityMatrix * sqrtS.selfadjointView<Eigen::Upper>();
  Eigen::RowVector3d dipole;
  dipole.setZero();

  positions.rowwise() -= dipoleEvaluationCoordinate;

  const auto atomicOrbitalsNumber = dipoleMatrix.x().get<Utils::derivOrder::zero>().cols();
  // Classical Core charge component
  for (int atom = 0; atom < nAtoms; ++atom) {
    dipole += coreCharges[atom] * positions.row(atom);
  }

  // electronic component (diagonal)
  for (int AO = 0; AO < atomicOrbitalsNumber; ++AO) {
    dipole.x() -= nonOrthogonalDensityMatrix(AO, AO) * dipoleMatrix.x().get<Utils::derivOrder::zero>()(AO, AO);
    dipole.y() -= nonOrthogonalDensityMatrix(AO, AO) * dipoleMatrix.y().get<Utils::derivOrder::zero>()(AO, AO);
    dipole.z() -= nonOrthogonalDensityMatrix(AO, AO) * dipoleMatrix.z().get<Utils::derivOrder::zero>()(AO, AO);
  }
  // electronic component(off-diagonal)
  for (int nu = 0; nu < atomicOrbitalsNumber; ++nu) {
    for (int mu = nu + 1; mu < atomicOrbitalsNumber; ++mu) {
      dipole.x() -= 2 * nonOrthogonalDensityMatrix(nu, mu) * dipoleMatrix.x().get<Utils::derivOrder::zero>()(nu, mu);
      dipole.y() -= 2 * nonOrthogonalDensityMatrix(nu, mu) * dipoleMatrix.y().get<Utils::derivOrder::zero>()(nu, mu);
      dipole.z() -= 2 * nonOrthogonalDensityMatrix(nu, mu) * dipoleMatrix.z().get<Utils::derivOrder::zero>()(nu, mu);
    }
  }
  return dipole;
}

template class NDDODipoleMomentCalculator<nddo::PM6Method>;
template class NDDODipoleMomentCalculator<nddo::AM1Method>;
template class NDDODipoleMomentCalculator<nddo::MNDOMethod>;
} // namespace Sparrow
} // namespace Scine


#include "decon.h"
#include "defines.h"
#include "utils.h"
#include "reader.h"
#include "resampler.h"
#include "math_local.h"
#include "writer.h"
#include <algorithm>
#include <boost/program_options.hpp>

namespace po = boost::program_options;

int main(int argc, char** argv) {
  // parameters
  float xy_res = UNSET_FLOAT;
  float kernel_zstep = UNSET_FLOAT;
  float img_zstep = UNSET_FLOAT;
  float subtract_constant = UNSET_FLOAT;
  unsigned int iterations = UNSET_UNSIGNED_INT;
  unsigned int bit_depth = UNSET_UNSIGNED_INT;
  bool overwrite = UNSET_BOOL;
  bool verbose = UNSET_BOOL;

  // declare the supported options
  po::options_description visible_opts("usage: decon [options] path\n\nAllowed options");
  visible_opts.add_options()
      ("help,h", "display this help message")
      ("kernel,k", po::value<std::string>()->required(),"kernel file path")
      ("iterations,n", po::value<unsigned int>(&iterations)->required(),"deconvolution iterations")
      ("xy-rez,x", po::value<float>(&xy_res)->default_value(0.104f), "x/y resolution (um/px)")
      ("kernel-spacing,p", po::value<float>(&kernel_zstep)->default_value(1.0f),"z-step size of kernel")
      ("image-spacing,q", po::value<float>(&img_zstep)->default_value(1.0f),"z-step size of input image")
      ("subtract-constant,s", po::value<float>(&subtract_constant)->default_value(0.0f),"constant intensity value to subtract from input image")
      ("output,o", po::value<std::string>()->required(),"output file path")
      ("bit-depth,b", po::value<unsigned int>(&bit_depth)->default_value(16),"bit depth (8, 16, or 32) of output image")
      ("overwrite,w", po::value<bool>(&overwrite)->default_value(false)->implicit_value(true)->zero_tokens(), "overwrite output if it exists")
      ("verbose,v", po::value<bool>(&verbose)->default_value(false)->implicit_value(true)->zero_tokens(), "display progress and debug information")
      ("version", "display the version number")
  ;

  po::options_description hidden_opts;
  hidden_opts.add_options()
    ("input", po::value<std::string>()->required(), "input file path")
  ;

  po::positional_options_description positional_opts;
  positional_opts.add("input", 1);

  po::options_description all_opts;
  all_opts.add(visible_opts).add(hidden_opts);

  // parse options
  po::variables_map varsmap;
  try {
    po::store(po::command_line_parser(argc, argv).options(all_opts).positional(positional_opts).run(), varsmap);

    // print help message
    if (varsmap.count("help") || (argc == 1)) {
      std::cerr << "decon: deconvolves an image with a PSF or PSF parameters\n";
      std::cerr << visible_opts << std::endl;
      return EXIT_FAILURE;
    }

    // print version number
    if (varsmap.count("version")) {
      std::cerr << DECON_VERSION << std::endl;
      return EXIT_FAILURE;
    }

    // check options
    po::notify(varsmap);

  } catch (po::error& e) {
    std::cerr << "decon: " << e.what() << "\n\n";
    std::cerr << visible_opts << std::endl;
    return EXIT_FAILURE;
  } catch (...) {
    std::cerr << "decon: unknown error during command line parsing\n\n";
    std::cerr << visible_opts << std::endl;
    return EXIT_FAILURE;
  }

  // check files
  const char* in_path = varsmap["input"].as<std::string>().c_str();
  if (!IsFile(in_path)) {
    std::cerr << "decon: input path is not a file" << std::endl;
    return EXIT_FAILURE;
  }
  const char* kernel_path = varsmap["kernel"].as<std::string>().c_str();
  if (!IsFile(kernel_path)) {
    std::cerr << "decon: kernel path is not a file" << std::endl;
    return EXIT_FAILURE;
  }
  const char* out_path = varsmap["output"].as<std::string>().c_str();
  if (IsFile(out_path)) {
    if (!overwrite) {
      std::cerr << "decon: output path already exists" << std::endl;
      return EXIT_FAILURE;
    } else if (verbose) {
        std::cout << "overwriting: " << out_path << std::endl;
    }
  }

  // check bit depth
  unsigned int bits[] = {8, 16, 32};
  unsigned int* p = std::find(std::begin(bits), std::end(bits), bit_depth);
  if (p == std::end(bits)) {
    std::cerr << "decon: bit depth must be 8, 16, or 32" << std::endl;
    return EXIT_FAILURE;
  }

  // print parameters
  if (verbose) {
    std::cout << "\nInput Parameters\n";
    std::cout << "Iterations = " << iterations << "\n";
    std::cout << "Input Path = " << in_path << "\n";
    std::cout << "Kernel Path = " << kernel_path << "\n";
    std::cout << "Output Path = " << out_path << "\n";
    std::cout << "Overwrite = " << overwrite << "\n";
    std::cout << "Bit Depth = " << bit_depth << std::endl;
  }

  // read data
  kImageType::Pointer img = ReadImageFile<kImageType>(in_path);
  kImageType::Pointer kernel = ReadImageFile<kImageType>(kernel_path);

  // set spacing
  kImageType::SpacingType img_spacing;

  img_spacing[0] = xy_res;
  img_spacing[1] = xy_res;
  img_spacing[2] = img_zstep;
  img->SetSpacing(img_spacing);

  kImageType::SpacingType kernel_spacing;

  kernel_spacing[0] = xy_res;
  kernel_spacing[1] = xy_res;
  kernel_spacing[2] = kernel_zstep;
  kernel->SetSpacing(kernel_spacing);

  // resample kernel
  if (img_zstep != kernel_zstep)
  {
    kernel = Resampler(kernel, img_spacing, verbose);
  }

  // subtract constant
  if (subtract_constant != 0.0)
  {
    img = SubtractConstantClamped(img, (kPixelType) subtract_constant/std::numeric_limits<unsigned short>::max()); // TODO: scale subtraction by input type

  }

  // decon
  kImageType::Pointer decon_img = RichardsonLucy(img, kernel, iterations, verbose);

  // write file
  if (bit_depth == 8) {
    using PixelTypeOut = unsigned char;
    using ImageTypeOut = itk::Image<PixelTypeOut, kDimensions>;
    WriteImageFile<kImageType,ImageTypeOut>(decon_img, out_path);
  } else if (bit_depth == 16) {
    using PixelTypeOut = unsigned short;
    using ImageTypeOut = itk::Image<PixelTypeOut, kDimensions>;
    WriteImageFile<kImageType,ImageTypeOut>(decon_img, out_path);
  } else if (bit_depth == 32) {
    using PixelTypeOut = float;
    using ImageTypeOut = itk::Image<PixelTypeOut, kDimensions>;
    WriteImageFile<kImageType,ImageTypeOut>(decon_img, out_path);
  } else {
    std::cerr << "decon: unknown bit depth" << std::endl;
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}


/*
    Copyright (c) 2011-2014 University of Zurich

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in
    all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
    THE SOFTWARE.
*/

#ifndef PLLL_INCLUDE_GUARD__ENUMIMPL_PARALLELSERIAL_ENUM_CPP
#define PLLL_INCLUDE_GUARD__ENUMIMPL_PARALLELSERIAL_ENUM_CPP

#include <boost/thread.hpp>

namespace plll
{
    template<class RealTypeContext, class IntTypeContext, class JobManager>
    /*
      The JobManager class must have a method
          void addJob(unsigned dim,
                      const linalg::math_rowvector<typename IntTypeContext::Integer> & x_end,
                      const typename RealTypeContext::Real & ell,
                      const typename RealTypeContext::Real & bound,
                      bool iszero);
    */
    class JobData;

    template<class RealTypeContext, class IntTypeContext, class JobManager>
    class ThreadData
    // Stores per thread data, like certain arrays
    {
        friend class JobData<RealTypeContext, IntTypeContext, JobManager>;

    private:
        Lattice<RealTypeContext, IntTypeContext> * d_lattice;
        JobManager & d_jm;
        Updater<RealTypeContext, IntTypeContext> & d_update;
        unsigned d_begin;
        unsigned d_end;
        unsigned d_dim;

        volatile bool d_need_to_setup;
        volatile unsigned d_need_to_setup_begin;
        volatile unsigned d_need_to_setup_end;
        volatile Lattice<RealTypeContext, IntTypeContext> * d_need_to_setup_lattice;

        linalg::math_rowvector<typename IntTypeContext::Integer> d_result;
        linalg::math_rowvector<typename IntTypeContext::Integer> d_x;
        linalg::math_rowvector<typename RealTypeContext::Real> d_x_real;
        linalg::base_rowvector<long> d_delta;
        linalg::base_rowvector<int> d_delta2;
        linalg::math_rowvector<int> d_r; /* Used for improvement sketched in Appendix B of Gama,
        Nguyen, Regev: "Lattice Enumeration Using Extreme Pruning", EUROCRYPT 2010. Stores
        information on how much of the cache in d_sigma can be used. */
        linalg::math_matrix<typename RealTypeContext::Real> d_sigma; /* used for improvement
        sketched in Appendix B of Gama, Nguyen, Regev: "Lattice Enumeration Using Extreme Pruning",
        EUROCRYPT 2010. The i-th row caches intermediate values used during the computation of
        d_c[i]. */
        linalg::math_rowvector<typename RealTypeContext::Real> d_c;
        linalg::math_rowvector<typename RealTypeContext::Real> d_ell;
        linalg::math_rowvector<typename RealTypeContext::Real> d_bounds; /* For pruning. This is
        updated by JobData, not by ThreadData, and any relation to d_bound is managed by JobData. */
        std::vector<bool> d_iszero;

        std::list<JobData<RealTypeContext, IntTypeContext, JobManager> > d_job_list;

        boost::mutex d_bound_update_mutex;
        volatile bool d_bound_updated;
        typename RealTypeContext::Real d_bound;

        volatile bool d_split;
        volatile bool d_stop;

    public:
        ThreadData(JobManager & jm, Updater<RealTypeContext, IntTypeContext> & update)
            : d_lattice(NULL), d_jm(jm), d_update(update), d_begin(0), d_end(0), d_dim(0),
              d_need_to_setup(false), d_need_to_setup_begin(0), d_need_to_setup_end(0),
              d_need_to_setup_lattice(NULL), d_bound_updated(false), d_split(false),
              d_stop(false)
        {
        }

        void setup(Lattice<RealTypeContext, IntTypeContext> & lattice, unsigned begin, unsigned end)
        // Called at beginning of specific enumerate
        {
            d_need_to_setup = false;
            d_lattice = &lattice;
            d_begin = begin;
            d_end = end;
            d_dim = d_end - d_begin + 1;
            // The updater functions rely on the size of d_result and d_x, hence always resize them!
            d_result.resize(d_dim);
            d_x.resize(d_dim);
            // Only resize the others when necessary
            if (d_x_real.size() < d_dim)
            {
                d_x_real.resize(d_dim, linalg::Initialize(lattice.rc()));
                d_delta.resize(d_dim);
                d_delta2.resize(d_dim);
                d_r.resize(d_dim + 1);
                d_sigma.resize(d_dim, d_dim, linalg::Initialize(lattice.rc()));
                d_c.resize(d_dim, linalg::Initialize(lattice.rc()));
                d_ell.resize(d_dim + 1, linalg::Initialize(lattice.rc()));
                d_bounds.resize(d_dim, linalg::Initialize(lattice.rc()));
                d_iszero.resize(d_dim + 1);
            }
            for (unsigned i = 0; i < d_dim; ++i)
            {
                setZero(d_x_real[i]);
                setZero(d_c[i]);
                setZero(d_ell[i]);
                setZero(d_bounds[i]);
//            for (unsigned j = 0; j < d_dim; ++j)
//                setZero(d_sigma(i, j));
            }
            setZero(d_ell[d_dim]);
            d_bound_updated = false;
            setZero(d_bound);
            d_split = false;
            d_stop = false;
            updateContext();
        }

        void updateContext()
        {
            RealTypeContext & rc = d_lattice->rc();
            for (unsigned i = 0; i < d_sigma.rows(); ++i)
                for (unsigned j = 0; j < d_sigma.cols(); ++j)
                    d_sigma(i, j).setContext(rc);
            for (unsigned i = 0; i < d_x_real.size(); ++i)
                d_x_real[i].setContext(rc);
            for (unsigned i = 0; i < d_c.size(); ++i)
                d_c[i].setContext(rc);
            for (unsigned i = 0; i < d_ell.size(); ++i)
                d_ell[i].setContext(rc);
            for (unsigned i = 0; i < d_bounds.size(); ++i)
                d_bounds[i].setContext(rc);
            d_bound.setContext(rc);
        }

        inline void flagToSetup(Lattice<RealTypeContext, IntTypeContext> & lattice, unsigned begin, unsigned end)
        {
            d_need_to_setup_lattice = &lattice;
            d_need_to_setup_begin = begin;
            d_need_to_setup_end = end;
            d_need_to_setup = true;
        }

        inline void doSetup()
        {
            if (d_need_to_setup)
            {
                setup((Lattice<RealTypeContext, IntTypeContext>&)*d_need_to_setup_lattice, d_need_to_setup_begin, d_need_to_setup_end);
                d_need_to_setup = false;
            }
        }

        inline void setUpdatedBound(const typename RealTypeContext::Real & bound)
        {
            boost::lock_guard<boost::mutex> guard(d_bound_update_mutex);
            d_bound_updated = true;
            d_bound = bound;
        }

        inline bool hasUpdatedBound() const
        {
            return d_bound_updated;
        }

        inline bool updateBound(typename RealTypeContext::Real & bound)
        {
            bool updated = false;
            if (d_bound_updated)
            {
                boost::lock_guard<boost::mutex> guard(d_bound_update_mutex);
                if (d_bound_updated)
                {
                    d_bound_updated = false;
                    bound = d_bound;
                    updated = true;
                }
            }
            return updated;
        }

        inline void flagToStop()
        {
            d_stop = true;
        }

        inline bool shouldIStop()
        {
            return d_stop;
        }

        inline void flagToSplit()
        {
            d_split = true;
        }

        inline bool shouldISplit()
        {
            if (d_split)
            {
                d_split = false;
                return true;
            }
            else
                return false;
        }
    };

    template<class RealTypeContext, class IntTypeContext, class JobManager>
    class JobData
    // The job itself. Contains info on job parameters, local parameters while running, as well as contains result
    {
    private:
        linalg::math_rowvector<typename IntTypeContext::Integer> d_x_end; // td.d_dim - d_x_end.size() is the dimension which is
        // still to be enumerated
        typename RealTypeContext::Real d_ell;
        bool d_iszero;

    public:
        linalg::math_rowvector<typename IntTypeContext::Integer> d_result; // is zero vector (of no length) until a solution was found
        typename RealTypeContext::Real d_bound;

    private:
        void propagate_new_bound(ThreadData<RealTypeContext, IntTypeContext, JobManager> & td)
        {
            td.d_jm.d_pruninghelper.computeBounds(td.d_bounds, td.d_dim, d_bound);
        }

        inline void run_init(ThreadData<RealTypeContext, IntTypeContext, JobManager> & td, unsigned realdim, unsigned enumbegin, unsigned enumdim)
        {
            // Fill in x, delta, delta2, ell
            for (unsigned i = 0; i < d_x_end.size(); ++i)
            {
                td.d_x[enumdim + i] = d_x_end[i];
                arithmetic::convert(td.d_x_real[enumdim + i], d_x_end[i], td.d_lattice->rc());
            }
            td.d_ell[enumdim] = d_ell;
            td.d_iszero[enumdim] = d_iszero;

            // Prepare enumeration
            for (unsigned i = 0; i < enumdim; ++i)
            {
                td.d_r[i] = realdim - 1;
                setZero(td.d_sigma(i, realdim - 1));
            }
            typename RealTypeContext::Real tmp(td.d_lattice->rc());

            for (unsigned j = td.d_r[enumdim - 1]; j >= enumdim; --j) // this is the summation order considered in Pujol-Stehle
            {
                tmp = td.d_x_real[j] * td.d_lattice->getCoeff(enumbegin + j, enumbegin + enumdim - 1);
                td.d_sigma(enumdim - 1, j - 1) = td.d_sigma(enumdim - 1, j) + tmp;
            }
            td.d_c[enumdim - 1] = -td.d_sigma(enumdim - 1, enumdim - 1);
            bool ru;
            arithmetic::convert_round(td.d_x[enumdim - 1], td.d_c[enumdim - 1], ru, td.d_lattice->ic());
            arithmetic::convert(td.d_x_real[enumdim - 1], td.d_x[enumdim - 1], td.d_lattice->rc());
            td.d_iszero[enumdim - 1] = td.d_iszero[enumdim] && isZero(td.d_x[enumdim - 1]);
            td.d_delta[enumdim - 1] = 0;
            td.d_delta2[enumdim - 1] = ru ? 1 : -1;
            propagate_new_bound(td);
        }

        class CallUpdater
        {
        public:
            inline bool operator() (ThreadData<RealTypeContext, IntTypeContext, JobManager> & td, JobData & jd,
                                    unsigned realdim, unsigned enumbegin, unsigned enumdim, unsigned stage, bool & noSolutionYet,
                                    typename RealTypeContext::Real & tmp) const
            {
                if (td.d_update(*td.d_lattice, td.d_begin, td.d_end, jd.d_result, jd.d_bound, td.d_x, td.d_ell[0], noSolutionYet))
                    jd.propagate_new_bound(td);
                return true;
            }
        };

        class CallUpdaterNotify
        {
        private:
            mutable bool d_was_updated;

        public:
            inline CallUpdaterNotify()
                : d_was_updated(false)
            {
            }

            inline bool wasUpdated() const
            {
                return d_was_updated;
            }

            inline bool operator() (ThreadData<RealTypeContext, IntTypeContext, JobManager> & td, JobData & jd,
                                    unsigned realdim, unsigned enumbegin, unsigned enumdim, unsigned stage, bool & noSolutionYet,
                                    typename RealTypeContext::Real & tmp) const
            {
                if (td.d_update(*td.d_lattice, td.d_begin, td.d_end, jd.d_result, jd.d_bound, td.d_x, td.d_ell[0], noSolutionYet))
                {
                    jd.propagate_new_bound(td);
                    d_was_updated = true;
                }
                return true;
            }
        };

        class CallRefiner
        {
        public:
            inline bool operator() (ThreadData<RealTypeContext, IntTypeContext, JobManager> & td, JobData & jd,
                                    unsigned realdim, unsigned enumbegin, unsigned enumdim, unsigned stage, bool & noSolutionYet,
                                    typename RealTypeContext::Real & tmp) const
            {
                td.d_jm.addJob(realdim, td.d_x, td.d_ell[0], jd.d_bound, td.d_iszero[0]);
                // update bound (improves pruning)
                if (td.d_jm.isBoundUpdated())
                {
                    jd.d_bound = td.d_jm.getUpdatedBound();
                    jd.propagate_new_bound(td);
                }
                return true;
            }
        };

        class CallRunner;
        friend class CallRunner;

        class CallRunner
        {
        private:
            static void enqueue(ThreadData<RealTypeContext, IntTypeContext, JobManager> & td, JobData & jd,
                                unsigned realdim, unsigned enumbegin, unsigned enumdim, unsigned stage,
                                typename RealTypeContext::Real & tmp)
            {
                while (true)
                {
                    tmp = td.d_x_real[stage] - td.d_c[stage];
                    square(td.d_ell[stage], tmp);
                    td.d_ell[stage] *= td.d_lattice->getNormSq(enumbegin + stage);
                    td.d_ell[stage] += td.d_ell[stage + 1];
                    if (td.d_ell[stage] <= td.d_bounds[stage])
                        td.d_job_list.push_back(JobData(td.d_dim - stage, td.d_x, td.d_ell[stage], jd.d_bound, td.d_iszero[stage], stage));
                    else
                    {
                        if (++stage >= enumdim)
                            break;
                    }
                    // Modify d_x[stage]
                    if (td.d_iszero[stage + 1])
                    {
                        ++td.d_x[stage];
                    }
                    else
                    {
                        td.d_delta2[stage] = -td.d_delta2[stage];
                        td.d_delta[stage] = -td.d_delta[stage] + td.d_delta2[stage];
                        td.d_x[stage] += arithmetic::convert(td.d_delta[stage], td.d_lattice->ic());
                    }
                    arithmetic::convert(td.d_x_real[stage], td.d_x[stage], td.d_lattice->rc());
                    td.d_iszero[stage] = false;
                }

                td.d_jm.addJobs(td.d_job_list.begin(), td.d_job_list.end());
                td.d_job_list.clear();
            }

        public:
            inline bool operator() (ThreadData<RealTypeContext, IntTypeContext, JobManager> & td, JobData & jd,
                                    unsigned realdim, unsigned enumbegin, unsigned enumdim, unsigned stage, bool & noSolutionYet,
                                    typename RealTypeContext::Real & tmp) const
            {
                if (td.shouldIStop())
                    return false;
                if (td.shouldISplit())
                {
                    enqueue(td, jd, realdim, enumbegin, enumdim, stage, tmp);
                    return false;
                }
                if (td.updateBound(jd.d_bound)) // update bound
                    jd.propagate_new_bound(td);
                jd.go_stage_down<false>(td, realdim, enumbegin, enumdim, stage - 1, 0, tmp);
                CallUpdaterNotify caller;
                jd.do_the_run<true, true>(td, realdim, enumbegin, stage, stage - 1, noSolutionYet, caller, 0);
                if (caller.wasUpdated())
                {
                    // Inform other threads about update
                    td.d_jm.tryToUpdateBound(jd.d_result, jd.d_bound);
                }
                return true;
            }
        };

        template<bool avoid_bottom>
        static inline void go_stage_down(ThreadData<RealTypeContext, IntTypeContext, JobManager> & td,
                                  unsigned realdim, unsigned enumbegin, unsigned enumdim, unsigned stage, unsigned zerostage,
                                  typename RealTypeContext::Real & tmp)
        {
            if (stage > 0)
            {
                if (td.d_r[stage - 1] < td.d_r[stage])
                    td.d_r[stage - 1] = td.d_r[stage];
            }
            for (unsigned j = td.d_r[stage]; j > stage; --j) // this is the summation order considered in Pujol-Stehle
            {
                tmp = td.d_x_real[j] * td.d_lattice->getCoeff(enumbegin + j, enumbegin + stage);
                td.d_sigma(stage, j - 1) = td.d_sigma(stage, j) + tmp;
            }
            td.d_c[stage] = -td.d_sigma(stage, stage);
            bool ru;
            arithmetic::convert_round(td.d_x[stage], td.d_c[stage], ru, td.d_lattice->ic());
            td.d_iszero[stage] = td.d_iszero[stage + 1] && isZero(td.d_x[stage]);
            if (avoid_bottom && (stage == zerostage) && td.d_iszero[stage])
                // fuck it. this should _not_ be zero! (in case avoidbottom is set!)
                ++td.d_x[stage];
            arithmetic::convert(td.d_x_real[stage], td.d_x[stage], td.d_lattice->rc());
            td.d_delta[stage] = 0;
            td.d_delta2[stage] = ru ? 1 : -1;
        }

        template<bool avoid_bottom, bool inc_stage_after_call, class Caller>
        inline bool do_the_run(ThreadData<RealTypeContext, IntTypeContext, JobManager> & td,
                               unsigned realdim, unsigned enumbegin, unsigned enumdim, unsigned stage,
                               bool & noSolutionYet, const Caller & caller, unsigned zerostage = 0)
        {
            typename RealTypeContext::Real tmp(td.d_lattice->rc());

            while (true)
            {
                tmp = td.d_x_real[stage] - td.d_c[stage];
                square(td.d_ell[stage], tmp);
                td.d_ell[stage] *= td.d_lattice->getNormSq(enumbegin + stage);
                td.d_ell[stage] += td.d_ell[stage + 1];
                if (td.d_ell[stage] <= td.d_bounds[stage])
                {
                    if (stage == zerostage)
                    {
                        if (!caller(td, *this, realdim, enumbegin, enumdim, stage, noSolutionYet, tmp))
                            return false;
                        if (inc_stage_after_call)
                        {
                            if (++stage >= enumdim)
                                break;
                            // Update r for the lower stage
                            td.d_r[stage - 1] = stage;
                        }
                    }
                    else
                    {
                        --stage;
                        go_stage_down<avoid_bottom>(td, realdim, enumbegin, enumdim, stage, zerostage, tmp);
                        continue;
                    }
                }
                else
                {
                    if (++stage >= enumdim)
                        break;
                    // Update r for the lower stage
                    td.d_r[stage - 1] = stage;
                }
                // Modify d_x[stage]
                if (td.d_iszero[stage + 1])
                {
                    ++td.d_x[stage];
                }
                else
                {
                    td.d_delta2[stage] = -td.d_delta2[stage];
                    td.d_delta[stage] = -td.d_delta[stage] + td.d_delta2[stage];
                    td.d_x[stage] += arithmetic::convert(td.d_delta[stage], td.d_lattice->ic());
                }
                arithmetic::convert(td.d_x_real[stage], td.d_x[stage], td.d_lattice->rc());
                td.d_iszero[stage] = false;
            }
            return true;
        }

        bool d_done;

    public:
        JobData(unsigned dim, const linalg::math_rowvector<typename IntTypeContext::Integer> & x_end,
                const typename RealTypeContext::Real & ell,
                const typename RealTypeContext::Real & bound,
                bool iszero, unsigned ofs = 0)
            : d_x_end(dim), d_ell(ell), d_iszero(iszero), d_bound(bound), d_done(false)
        {
            for (unsigned i = 0; i < dim; ++i)
                d_x_end[i] = x_end[ofs + i];
        }

        void markDone()
        {
            d_done = true;
        }

        bool isDone() const
        {
            return d_done;
        }

        const typename RealTypeContext::Real & ell() const
        {
            return d_ell;
        }

        unsigned dimension() const
        {
            return d_x_end.size();
        }

        bool update(typename RealTypeContext::Real & bound)
        // Returns false if job is superfluous
        {
            d_bound = bound;
            return d_ell <= d_bound;
        }

        unsigned leftoverDims(const ThreadData<RealTypeContext, IntTypeContext, JobManager> & td) const
        {
            return td.d_dim - d_x_end.size();
        }

        void print()
        {
            for (unsigned i = 0; i < d_x_end.size(); ++i)
                std::cout << " " << d_x_end[i];
            std::cout << " -- " << d_ell << " -- " << d_bound << " -- " << (d_iszero ? "true" : "false") << "\n";
        }

        void run(ThreadData<RealTypeContext, IntTypeContext, JobManager> & td)
        {
            // Prepare enumeration
            td.doSetup();
            unsigned enumdim = td.d_dim - d_x_end.size(); // td.d_enumdim;
            bool noSolutionYet = true;
            run_init(td, td.d_dim, td.d_begin, enumdim);
            // Do enumeration
            CallUpdater caller;
            do_the_run<true, true>(td, td.d_dim, td.d_begin, enumdim, enumdim - 1, noSolutionYet, caller, 0);
        }

        void run2(ThreadData<RealTypeContext, IntTypeContext, JobManager> & td)
        {
            enum { THRESHOLD = 40 };

            // Prepare enumeration
            td.doSetup();
            unsigned enumdim = td.d_dim - d_x_end.size(); // td.d_enumdim;
            bool noSolutionYet = true;
            run_init(td, td.d_dim, td.d_begin, enumdim);
            if (enumdim > THRESHOLD)
            {
                // Do enumeration in two (main) stages
                CallRunner caller;
                do_the_run<false, false>(td, td.d_dim, td.d_begin, enumdim, enumdim - 1, noSolutionYet, caller, THRESHOLD);
            }
            else
            {
                // Do enumeration
                CallUpdater caller;
                do_the_run<true, true>(td, td.d_dim, td.d_begin, enumdim, enumdim - 1, noSolutionYet, caller, 0);
            }
        }

        void refine(ThreadData<RealTypeContext, IntTypeContext, JobManager> & td, unsigned dim)
        // Computes the next dim dimensions. We must have dim < leftoverDims(td).
        {
            // Prepare refinement
            td.doSetup();
            assert(dim < leftoverDims(td));
            unsigned realdim = dim + d_x_end.size();
            bool noSolutionYet; // not needed
            unsigned enumbegin = td.d_begin + td.d_dim - realdim;
            run_init(td, realdim, enumbegin, dim);

            // Do refinement
            CallRefiner caller;
            do_the_run<false, false>(td, realdim, enumbegin, dim, dim - 1, noSolutionYet, caller, 0);
        }
    };
}

#endif


#include <fstream>
#include <cmath>
#include <iostream>
#include <sstream>
#include <cstdlib>
#include <vector>
#include <string.h>
#include <netcdfcpp.h>
#include <gsl/gsl_cdf.h>
#include <boost/random.hpp>
#include <boost/math/distributions/inverse_gaussian.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/program_options.hpp>
#include <WilcoxonTest.h>
namespace po = boost::program_options;
using namespace std;

const string help = "help";
const string dataFileName = "dataFile";
const string configFileName = "configFile";

int dataYsize;
int dataXsize;

void commandConfigMessage()
{
        cout << "Alternatively, instead of config file, you can write configuration arguments directly to the command line, ";
        cout << "overwriting some or all the arguments. " << endl;
        cout << "Note! testIndexes and ontrolIndexes must be a Comma separated integer list _WITHOUT_ spaces!" << endl << endl;
        cout << "Example: WilcoxonTest --" << dataFileName << " data.nc ";
        cout << "--DataName data " << "--xDimension  n " << " --yDimension m ";
        cout << "--testIndexes  0,2,4,6 " << " --controlIndexes 1,3,5,7" << endl<< endl;
        cout << "Note that if both Command line config parameters and Config file is given, then the command line parameters will be used" << endl<< endl;
}

bool validateCommandLine(po::variables_map * vm, po::options_description * commandArgs)
{
    if(!vm->count(dataFileName)){
        cout << "Invalid parameters." << endl;
        cout << *commandArgs << endl;
        cout << "Example: WilcoxonTest --" << dataFileName << " data.nc --" << configFileName << " conf.cfg" << endl<< endl;

        commandConfigMessage();
        return false;
    }
    return true;
}

bool validateConfiguration(po::variables_map * vm, po::options_description * configuration)
{
    if(!vm->count("DataName") || !vm->count("xDimension") || !vm->count("yDimension")
            || !vm->count("testIndexes") || !vm->count("controlIndexes")){

        cout << "Configuration file has missing options" << endl;
        cout << *configuration << endl;
        cout << "Example: conf.cfg:" << endl;
        cout << "DataName = data" << endl;
        cout << "xDimension = n" << endl;
        cout << "yDimension = m" << endl;
        cout << "testIndexes = 0, 2, 4, 6" << endl;
        cout << "controlIndexes = 1, 3, 5, 7" << endl<< endl;

        commandConfigMessage();
        return false;
    }
    return true;
}

float * readNetCdfFile(string fileName, string dataName, string xDimension, string yDimension)
{
    NcFile file(fileName.c_str(), NcFile::ReadOnly);
    if (!file.is_valid())
    {
        cout << "Couldn't open file!\n";
        throw;
    }

    NcVar *experimentData = file.get_var(dataName.c_str());

    dataYsize = file.get_dim(yDimension.c_str())->size();
    dataXsize = file.get_dim(xDimension.c_str())->size();

    float * data = new float[dataYsize * dataXsize];;

    experimentData->get(&data[0], dataYsize, dataXsize);
    return data;
}

//TODO optimize this thing
void writeResultsFile(string dataFileName, string yDimension, vector<double> * pValues)
{
    NcFile file(dataFileName.c_str(), NcFile::ReadOnly);
    NcVar *geneNames = file.get_var("gene");
    int dataYsize = file.get_dim(yDimension.c_str())->size();

    FILE * pFile;
    pFile = fopen ("results.txt","w");

    cout << "Writing results to file.." << endl;

    int percentage = dataYsize / 10;
    int percentageCount = 10;
    for(int i = 0; i < dataYsize; i++){
        char * c = geneNames->as_string(i*28);
        std::string str;
        if(pValues->at(i) > 0.05){
            std::ostringstream strs;
            strs << pValues->at(i);
            str = strs.str();
        }else{
            str = "NA";
        }
        fprintf (pFile, "%-20s-%20s", c, str.c_str());
        fprintf (pFile, "\n");
        if(i == percentage)
        {
            cout <<  percentageCount << "% complete" << endl;
            percentageCount += 10;
            percentage += dataYsize / 10;
        }
        delete c;
    }
    fclose(pFile);
}

int main(int argc, char* argv[])
{
    po::options_description commandArgs("Command line arguments");
    commandArgs.add_options()
        (help.c_str(), "produce help message")
        (dataFileName.c_str(), po::value< string > (), "A NetCDF file containing the data that tests will be run on")
        (configFileName.c_str(), po::value< string > (), "A configuration INI file containing all the specified options under 'Configuration'")
    ;

    po::options_description config("Configuration");
    config.add_options()
        ("DataName", po::value<string>(), "Name of the data variable in NetCDF file - string")
        ("xDimension", po::value<string>(), "Name of the Y dimension of the data matrix - string")
        ("yDimension", po::value<string>(), "Name of the X dimension of the data matrix - string")
        ("testIndexes", po::value<string>(), "Test group column indexes - comma separated int array")
        ("controlIndexes", po::value<string>(), "Control group column indexes - comma separated int array")
        ;
    commandArgs.add(config);

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, commandArgs), vm);
    po::notify(vm);

    if (!vm[help].empty()) {
        cout << commandArgs << endl;
        cout << config << endl;
        return 1;
    }
    else if (validateCommandLine(&vm, &commandArgs)) {

        //This doesnt overwrite command line parameters
        if(vm.count(configFileName)){
            std::ifstream file;
            file.open(vm[configFileName].as<string>().c_str());
            if (!file.is_open())
            {
                cerr << "Config file does not exist! " << endl;
                throw;
            }

            po::store(po::parse_config_file(file, config), vm);
            po::notify(vm);
        }

        if(validateConfiguration(&vm, &config)){
            string dataFileName = vm.at("dataFile").as<string>();
            string testIndexes = vm.at("testIndexes").as<string>();
            string controlIndexes = vm.at("controlIndexes").as<string>();
            string dataName = vm.at("DataName").as<string>();
            string xDimension = vm.at("xDimension").as<string>();
            string yDimension = vm.at("yDimension").as<string>();
            float * data = readNetCdfFile(dataFileName, dataName, xDimension, yDimension);
            WilcoxonTest wilx(data, dataXsize, dataYsize, testIndexes, controlIndexes);
            vector<double> * pValues = wilx.test();
            writeResultsFile(dataFileName, yDimension, pValues);
        }
    }
    return EXIT_SUCCESS;
}


// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * 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.
//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye 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 ETH Zurich, Wyss Zurich, Zurich Eye 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.
//
// Copyright (c) 2011-2013, Paul Furgale and others.
// All rights reserved.
//
// Unlike otherwise stated in source files, the code in this repository is
// published under the Revised BSD (New BSD) license.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * 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.
//     * Neither the name of the <organization> 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 <COPYRIGHT HOLDER> 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.

#pragma once

#include <vector>
#include <Eigen/Core>
#include <ze/common/types.hpp>
#include <ze/common/matrix.hpp>

namespace ze {
// imported from Schweizer-Messer Kinematics. Not intended to be used outside
// of the spline package.
namespace sm {

inline Matrix4 boxPlus(const Vector6 & dt)
{
  Matrix4 Bp;
  Bp <<   0.0,  -dt[5],   dt[4],  -dt[0],
          dt[5],     0.0,  -dt[3],  -dt[1],
         -dt[4],   dt[3],     0.0,  -dt[2],
          0.0,     0.0,     0.0,     0.0;
  return Bp;
}

inline Matrix46 boxMinus(const Vector4 & p)
{
  Matrix46 Bm;
  Bm <<    p[3],     0.0,     0.0,     0.0,     -p[2],     p[1],
           0.0,    p[3],     0.0,    p[2],       0.0,    -p[0],
           0.0,     0.0,    p[3],   -p[1],      p[0],      0.0,
           0.0,     0.0,     0.0,     0.0,       0.0,      0.0;
  return Bm;
}

inline Matrix6 boxTimes(Matrix4 const & T_ba)
{
  Matrix6 Tbp = Matrix6::Zero();

  Tbp.topLeftCorner<3, 3>() = T_ba.topLeftCorner<3, 3>();
  Tbp.bottomRightCorner<3, 3>() = T_ba.topLeftCorner<3, 3>();
  Tbp.topRightCorner<3, 3>() = -skewSymmetric(T_ba(0, 3), T_ba(1, 3), T_ba(2, 3))
                               * T_ba.topLeftCorner<3, 3>();

  return Tbp;
}

} // namespace kinematics
} // namespace ze


/**
 *
 *
 *                               Heu-MCHC
 *
 * A fast and accurate heuristic algorithm for the haplotype inference
 * problem on pedigree data with recombinations and mutations
 *
 * Copyright (C) 2009,2010,2011  Yuri PIROLA
 *
 * Distributed under the terms of the GNU General Public License (GPL)
 *
 *
 * This file is part of Heu-MCHC.
 *
 * Heu-MCHC 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.
 *
 * Heu-MCHC 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 Heu-MCHC.  If not, see <http://www.gnu.org/licenses/>.
 *
 **/
#include <string>
#include <iostream>
#include <fstream>

#include <map>

#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/graphviz.hpp>

#include <cstdio>

#include "locus-graph.hpp"
#include "irregular_mat.hpp"
#include "belief-propagation.hpp"
#include "util.h"
#include "log.h"
#include <m4ri/m4ri.h>

using namespace boost;
using namespace ped;

#define MZD_INIT( r, c ) mzd_init( r<32?32:r, c<32?32:c )

template <typename T>
class TLN_t {
public:
  TLN_t<T>* const pred;
  T data;
  TLN_t(TLN_t<T>* const _pred, T _data)
		:pred(_pred), data(_data) {}

};

typedef TLN_t<ped::h_variable_t> tln_h_t;
typedef TLN_t<ped::e_variable_t> tln_e_t;

static bool
is_indiv_homoz_at_locus(const pindiv i, const int l) {
  return i!=NULL && i->g[l]!=GEN2;
}

class ped_lg_label_writer {
private:
  LocusGraph& g;
  int l;
public:
  ped_lg_label_writer(LocusGraph& _lg, int _l)
		:g(_lg), l(_l)
  {}

  void operator()(std::ostream& out, const LG_Vertex& v) {
	 out << "[label=\"I" << g[v].individ->id <<
		"^"<< l <<" (ls=" << ped::locus_abbrv_status_names[g[v].ls] << ", p~=" << g[v].ptilde <<")\"]";
  }

  void operator()(std::ostream& out, const LG_Edge& e) {
	 out << "[label=\"d=" << g[e].d << "\"]";
  }
};

class general_locus_graph_visitor: public boost::default_dfs_visitor {
private:
  LocusGraph& lg;

public:
  general_locus_graph_visitor(LocusGraph& _lg)
		:lg(_lg)
  {}

  void initialize_vertex(LG_Vertex v, const LocusGraph& g) {
	 lg[v].pred_vertex= v;
  }

  void tree_edge(LG_Edge e, const LocusGraph& g) {
	 DEBUG("Edge " ST_FMT "-" ST_FMT " is in the spanning forest.",
			 source(e, lg), target(e, lg));
	 lg[e].in_spanning_forest= true;
	 lg[target(e, lg)].pred_vertex= source(e, lg);
	 lg[target(e, lg)].pred_edge= e;
  }
};

static LocusGraph *
build_locus_graph_from_pedigree(pgenped gp,
										  const unsigned int l) {
  my_assert(gp!=NULL);
  assert_ulim(l, gp->n_loci);
  DEBUG("Building locus graph of locus %d", l);

  LocusGraph * plg= new LocusGraph(gp->n_indiv, l);
  LocusGraph& lg= *plg;

  int index= 0;
  BGL_FORALL_VERTICES(v, lg, LocusGraph) {
	 pindiv i= gp->individuals[index];
	 ++index;
	 lg[v].individ= i;
	 lg[v].ptilde= -1;
	 lg[v].ls= ped::LS_UNDETERMINED;
// Set the status of the locus
	 if (is_indiv_homoz_at_locus(i, l)) {
		lg[v].ls= ped::LS_IMMUTABLE;
		lg[v].ptilde= i->g[l];
	 } else {
// Individual heterozygous
		if (is_indiv_homoz_at_locus(i->f, l)) {
// Father homozygous
		  if (is_indiv_homoz_at_locus(i->m, l)) {
// Both parents homozygous
			 if (i->f->g[l]==i->m->g[l]) {
// Same homozygousity -> Ambiguous
				lg[v].ls= ped::LS_PRED_AMBIGUOUS;
				lg[v].ptilde= i->f->g[l];
			 } else {
// Different homozygousity -> Doubly predetermined
				lg[v].ls= ped::LS_PRED_DOUBLE;
				lg[v].ptilde= i->f->g[l];
			 }
		  } else {
// Only father homozygous
			 lg[v].ls= ped::LS_PRED_SINGLE_FATHER;
			 lg[v].ptilde= i->f->g[l];
		  }
		} else if (is_indiv_homoz_at_locus(i->m, l)) {
// Only mother homozygous
		  lg[v].ls= ped::LS_PRED_SINGLE_MOTHER;
		  lg[v].ptilde= (i->m->g[l]+1)%2;
		}
	 }

	 my_assert(lg[v].ptilde!=-1 || lg[v].ls==ped::LS_UNDETERMINED);

  }

  index= 0;
  BGL_FORALL_VERTICES(v, lg, LocusGraph) {
	 pindiv i= gp->individuals[index];
	 if (i->f!=NULL && i->f->g[l]==GEN2) {
		DEBUG("Adding edge %d %d", i->id, i->fi);
		LocusGraph::edge_descriptor ed= add_edge(i->id, i->fi, lg).first;
		lg[ed].d= 0;
	 }
	 if (i->m!=NULL && i->m->g[l]==GEN2) {
		DEBUG("Adding edge %d %d", i->id, i->mi);
		LocusGraph::edge_descriptor ed= add_edge(i->id, i->mi, lg).first;
		lg[ed].d= (gp->individuals[index]->g[l]==GEN2)?1:0;
	 }
	 ++index;
  }
  general_locus_graph_visitor vis(lg);
  depth_first_search(lg, visitor(vis));

#ifdef LOG_GRAPHS
  write_graphviz(std::cout, lg,
					  ped_lg_label_writer(lg, l),
					  ped_lg_label_writer(lg, l));
#endif

  return plg;
}

static LocusGraph**
build_all_locus_graphs_from_pedigree(pgenped gp) {
  my_assert(gp!=NULL);
  LocusGraph** lgs= NPALLOC(LocusGraph*, gp->n_loci);

  for(unsigned int i= 0; i<gp->n_loci; ++i) {
	 lgs[i]= build_locus_graph_from_pedigree(gp, i);
  }

  return lgs;
}

static LocusGraph*
build_general_locus_graph_from_pedigree(pgenped gp) {
// Build a general locus graph (w/o checking homozygousity)
  my_assert(gp!=NULL);
  DEBUG("Building general locus graph");
  LocusGraph* const plg= new LocusGraph(gp->n_indiv, -1);
  LocusGraph& lg= *plg;
  int index= 0;
  BGL_FORALL_VERTICES(v, lg, LocusGraph) {
	 pindiv i= gp->individuals[index];
	 ++index;
	 lg[v].individ= i;
  }

  index= 0;
  BGL_FORALL_VERTICES(v, lg, LocusGraph) {
	 pindiv i= gp->individuals[index];
	 if (i->f!=NULL) {
		LocusGraph::edge_descriptor ed= add_edge(i->id, i->fi, lg).first;
	 }
	 if (i->m!=NULL) {
		LocusGraph::edge_descriptor ed= add_edge(i->id, i->mi, lg).first;
	 }
	 ++index;
  }

  general_locus_graph_visitor vis(lg);
  depth_first_search(lg, visitor(vis));

#ifdef LOG_GRAPHS
  write_graphviz(std::cout, lg,
					  ped_lg_label_writer(lg, -1),
					  ped_lg_label_writer(lg, -1));
#endif

  return plg;
}

static ped::e_variable_t
get_m_variable_from_edge(const LocusGraph& lg,
								 const int u,
								 const int v,
								 const int l) {
// Determino chi e' il genitore
  bool u_child_of_v= (lg[u].individ->fi==v) ||
	 (lg[u].individ->mi==v);

  my_assert(u_child_of_v || (lg[v].individ->fi==u) ||
				(lg[v].individ->mi==u));

  if (u_child_of_v)
	 return ped::e_variable_t(ped::e_variable_t::MUT,
									  u, l, lg[u].individ->fi==v?0:1);
  else
	 return ped::e_variable_t(ped::e_variable_t::MUT,
									  v, l, lg[v].individ->fi==u?0:1);
}

static ped::h_variable_t
get_h_variable_from_edge(const LocusGraph& lg,
								 const int u,
								 const int v,
								 const int l) {
// Determino chi e' il genitore
  bool u_child_of_v= (lg[u].individ->fi==v) ||
	 (lg[u].individ->mi==v);

  my_assert(u_child_of_v || (lg[v].individ->fi==u) ||
				(lg[v].individ->mi==u));

  if (u_child_of_v)
	 return ped::h_variable_t(u, lg[u].individ->fi==v?0:1);
  else
	 return ped::h_variable_t(v, lg[v].individ->fi==u?0:1);
}

static void
get_rec_variables_from_edge_to_tln(const LocusGraph& lg,
											  const int u,
											  const int v,
											  const int l,
											  tln_e_t*& evars,
											  int & cont) {
#ifndef NO_RECOMBINATIONS
// Determino chi e' il genitore
  bool u_child_of_v= (lg[u].individ->fi==v) ||
	 (lg[u].individ->mi==v);

  my_assert(u_child_of_v || (lg[v].individ->fi==u) ||
				(lg[v].individ->mi==u));
  const int p= (u_child_of_v)?v:u;
  const int c= (u_child_of_v)?u:v;
  const int parenthood= lg[c].individ->fi==p?0:1;

  for (int i= 1; i<=l; ++i) {
	 if (lg[p].individ->g[i]==GEN2) {
		evars= new tln_e_t(evars,
								 ped::e_variable_t(ped::e_variable_t::REC,
														 c, i, parenthood));
		++cont;
	 }
  }
#endif
}

static void
get_rec_variables_from_edge_to_set(const LocusGraph& lg,
											  const int u,
											  const int v,
											  const int l,
											  ped::e_vars_t& evars) {
#ifndef NO_RECOMBINATIONS
// Determino chi e' il genitore
  bool u_child_of_v= (lg[u].individ->fi==v) ||
	 (lg[u].individ->mi==v);

  my_assert(u_child_of_v || (lg[v].individ->fi==u) ||
				(lg[v].individ->mi==u));
  const int p= (u_child_of_v)?v:u;
  const int c= (u_child_of_v)?u:v;
  const int parenthood= lg[c].individ->fi==p?0:1;

  for (int i= 1; i<=l; ++i) {
	 if (lg[p].individ->g[i]==GEN2) {
		evars.insert(ped::e_variable_t(ped::e_variable_t::REC,
												 c, i, parenthood));
	 }
  }
#endif
}

static ped::e_variable_t
get_extremal_m_var(const ped::locus_status ls,
						 const int v,
						 const int locus) {
  switch (ls) {
	 case ped::LS_UNDETERMINED:
		fail();
		break;
	 case ped::LS_IMMUTABLE:
// none to add
		return ped::e_variable_t();
		break;
	 case ped::LS_PRED_SINGLE_FATHER:
		return ped::e_variable_t(ped::e_variable_t::MUT, v, locus, 0);
		break;
	 case ped::LS_PRED_SINGLE_MOTHER:
		return ped::e_variable_t(ped::e_variable_t::MUT, v, locus, 1);
		break;
	 case ped::LS_PRED_DOUBLE:
		return ped::e_variable_t(ped::e_variable_t::MUT, v, locus, 0);
		break;
	 case ped::LS_PRED_AMBIGUOUS:
		return ped::e_variable_t(ped::e_variable_t::MUT, v, locus, 0);
		break;
  }
  fail();
  return ped::e_variable_t();
}

template <typename T>
class to_visit_t {
private:
  std::list<T> datas;

public:
  void push(const T& el) {
	 datas.push_front(el);
  }

  T pop(void) {
	 T el= datas.front();
	 datas.pop_front();
	 return el;
  }

  bool empty(void) const {
	 return datas.empty();
  }
};

template <typename SET>
static void
read_tree_path_events(tln_e_t* const n, SET& set) {
  tln_e_t* tmp= n;
  while (tmp->pred!=NULL) {
#ifdef NO_RECOMBINATIONS
	 if (tmp->data.kind() != ped::e_variable_t::REC)
#endif
#ifdef NO_MUTATIONS
	 if (tmp->data.kind() != ped::e_variable_t::MUT)
#endif
		set.insert(tmp->data);
	 tmp= tmp->pred;
  }
}

template <typename SET>
static void
read_tree_path_hvars(tln_h_t* const n, SET& set) {
  tln_h_t* tmp= n;
  while (tmp->pred!=NULL) {
	 set.insert(tmp->data);
	 tmp= tmp->pred;
  }
}

static void
add_constraints_from_v_in_locus(const LocusGraph& lg,
										  const LG_Vertex root,
										  cycle_constraints_t& ccv,
										  const int locus,
										  const int n_indiv,
										  std::vector<bool>& visited) {
  my_assert(lg[root].ls != ped::LS_UNDETERMINED);
  FINETRACE("Adding constraints from predetermined vertex %d.", root);

  to_visit_t< LG_Vertex > to_visit;
  to_visit_t< tln_h_t* > tv_h;
  to_visit_t< tln_e_t* > tv_e;
  to_visit_t< int > tv_ne;
  std::vector< int > sum(n_indiv, -1);

  std::list< tln_h_t* > td_h;
  std::list< tln_e_t* > td_e;
  std::list< int > td_ne;

  ped::e_vars_t::iterator it;
  ped::e_variable_t m;
  ped::h_variable_t h;

  tln_e_t* const root_e= new tln_e_t(NULL, e_variable_t());
  tln_h_t* const root_h= new tln_h_t(NULL, h_variable_t());

  sum[root]= lg[root].ptilde;

  int cont= 1;
  m= get_extremal_m_var(lg[root].ls, root, locus);
  if (m.i>=0) {
	 tln_e_t* n= new tln_e_t(root_e, m);
	 tv_e.push(n);
	 cont= 2;
  } else {
	 tv_e.push(root_e);
  }
  tv_ne.push(cont);
  tv_h.push(root_h);
  to_visit.push(root);

  while (!to_visit.empty()) {
	 LG_Vertex v= to_visit.pop();
	 tln_h_t* hvars= tv_h.pop();
	 tln_e_t* evars= tv_e.pop();
	 int ne= tv_ne.pop();
	 td_h.push_front(hvars);
	 td_e.push_front(evars);
	 td_ne.push_front(ne);
// Iterates over its out edges
	 graph_traits<LocusGraph>::out_edge_iterator e, e_end, next;
	 tie(e, e_end) = out_edges(v, lg);
	 for (; e != e_end; ++e) {
		if (!lg[*e].in_spanning_forest)
		  continue;
		const LG_Vertex u= target(*e, lg);
		if (sum[u]==-1) {
		  FINETRACE("Considering edge %d-%d", v, u);
		  sum[u]= sum[v]+lg[*e].d;
		  m= get_m_variable_from_edge(lg, u, v, locus);
		  tln_e_t* new_evars= new tln_e_t(evars, m);
		  cont= 1;
		  get_rec_variables_from_edge_to_tln(lg, u, v, locus, new_evars, cont);
		  h= get_h_variable_from_edge(lg, u, v, locus);
		  tln_h_t* new_hvars= new tln_h_t(hvars, h);
		  to_visit.push(u);
		  tv_e.push(new_evars);
		  tv_ne.push(cont);
		  tv_h.push(new_hvars);
		  if (lg[u].ls != ped::LS_UNDETERMINED) {
			 DEBUG("Constraint detected between vertex " ST_FMT " and vertex " ST_FMT ".", root, u);
			 visited[u]= true;
			 cycle_constraint_t* cc= new cycle_constraint_t;
			 ccv.push_back(cc);
			 cc->constant= (sum[u]+lg[u].ptilde)%2==1;
			 m= get_extremal_m_var(lg[u].ls, u, locus);
			 if (m.i>=0) {
				new_evars= new tln_e_t(new_evars, m);
				++cont;
			 }
			 read_tree_path_hvars(new_hvars, cc->h_vars);
			 read_tree_path_events(new_evars, cc->events);
		  }
// Free memory
//			 delete new_hvars;
//			 for (int i= 0; i<cont; ++i) {
//				tln_e_t* tmp= new_evars;
//				new_evars= new_evars->pred;
//				delete tmp;
//			 }
//		  }
		}
	 }
  }

  while (!td_h.empty()) {
	 delete td_h.front();
	 td_h.pop_front();
  }
  while (!td_e.empty()) {
	 int ne= td_ne.front();
	 tln_e_t* evars= td_e.front();
	 for (int i= 0; i<ne; ++i) {
		tln_e_t* tmp= evars;
		evars= evars->pred;
		delete tmp;
	 }
	 td_ne.pop_front();
	 td_e.pop_front();
  }
}

static void
add_immutable_immutable_constraints
  (pgenped pg,
	ped::e_vars_t& m_vars_univ,
	ped::cycle_constraints_t& cycle_constraints)
{
  for (unsigned int i= 0; i<pg->n_indiv; ++i) {
	 if (pg->individuals[i]->fi==-1) {
		my_assert(pg->individuals[i]->mi==-1);
	 } else {
		pindiv const ii= pg->individuals[i];
		for (unsigned int j= 0; j<pg->n_loci; ++j) {
		  if (ii->g[j]!=GEN2 && ii->f->g[j]!=GEN2) {
			 if (ii->g[j]!=ii->f->g[j]) {
				ped::e_variable_t mv(ped::e_variable_t::MUT, i, j, 0);
				m_vars_univ.insert(mv);
				ped::cycle_constraint_t* cc= new ped::cycle_constraint_t;
				cc->events.insert(mv);
				cc->constant= true;
				cycle_constraints.push_back(cc);
				DEBUG("Added immutable constraint for indiv. %d at locus %d"
						" because he and his father are homozygous and they have"
						" different alleles.",
						mv.i, mv.l);
			 }
		  }
		  if (ii->g[j]!=GEN2 && ii->m->g[j]!=GEN2) {
			 if (ii->g[j]!=ii->m->g[j]) {
				ped::e_variable_t mv(ped::e_variable_t::MUT, i, j, 1);
				m_vars_univ.insert(mv);
				ped::cycle_constraint_t* cc= new ped::cycle_constraint_t;
				cc->events.insert(mv);
				cc->constant= true;
				cycle_constraints.push_back(cc);
				DEBUG("Added immutable constraint for indiv. %d at locus %d"
						" because he and his mother are homozygous and they have"
						" different alleles.",
						mv.i, mv.l);
			 }
		  }
		  if ((ii->g[j]==GEN2) &&
				(ii->f->g[j] == ii->m->g[j]) &&
				(ii->f->g[j]!=GEN2)) {
			 ped::e_variable_t mv0(ped::e_variable_t::MUT, i, j, 0);
			 if (m_vars_univ.find(mv0)==m_vars_univ.end()) {
				m_vars_univ.insert(mv0);
				ped::e_variable_t mv1(ped::e_variable_t::MUT, i, j, 1);
				m_vars_univ.insert(mv1);
				ped::cycle_constraint_t* cc= new ped::cycle_constraint_t;
				cc->events.insert(mv0);
				cc->events.insert(mv1);
				cc->constant= true;
				cycle_constraints.push_back(cc);
				DEBUG("Added special constraint for indiv. %d at locus %d"
						" because he is ambiguously predetermined.",
						i, j);
			 }
		  }
		}
	 }
  }
}

// Compute the tree path between u and v on lg
static void
tree_path(const LocusGraph& lg,
			 const LG_Vertex& u,
			 const LG_Vertex& v,
			 std::list< LG_Vertex >& path_u_v) {
  list<LG_Edge> path_v_root;
  LG_Vertex tmp= v;
  while (lg[tmp].pred_vertex != tmp) {
	 path_v_root.push_front(lg[tmp].pred_edge);
	 tmp= lg[tmp].pred_vertex;
  }
  list<LG_Edge> path_u_root;
  tmp= u;
  while (lg[tmp].pred_vertex != tmp) {
	 path_u_root.push_front(lg[tmp].pred_edge);
	 tmp= lg[tmp].pred_vertex;
  }
  while (!path_u_root.empty() && !path_v_root.empty()
			&& (path_u_root.front()==path_v_root.front())) {
	 path_u_root.pop_front();
	 path_v_root.pop_front();
  }
  tmp= u;
  path_u_v.push_front(u);
  while (!path_u_root.empty()) {
	 LG_Edge e= path_u_root.back();
	 if (source(e, lg)==tmp) {
		tmp= target(e, lg);
	 } else if (target(e, lg)==tmp) {
		tmp= source(e, lg);
	 } else {
		ERROR("Error in determining the tree path from " ST_FMT " to " ST_FMT ".", u, v);
		fail();
	 }
	 path_u_v.push_back(tmp);
	 path_u_root.pop_back();
  }
  tmp= path_u_v.back();
  while (!path_v_root.empty()) {
	 LG_Edge e= path_v_root.front();
	 if (source(e, lg)==tmp) {
		tmp= target(e, lg);
	 } else if (target(e, lg)==tmp) {
		tmp= source(e, lg);
	 } else {
		ERROR("Error in determining the tree path from " ST_FMT " to " ST_FMT ".", u, v);
		fail();
	 }
	 path_u_v.push_back(tmp);
	 path_v_root.pop_front();
  }
#ifdef LOG_DEBUG_ENABLED
  DEBUG_NOTN("The foundamental cycle of edge (" ST_FMTL(4) " -" ST_FMTL(4) " ) "
				 "contains the path {", u, v);
  for (std::list< LG_Vertex >::const_iterator it= path_u_v.begin();
		 it != path_u_v.end(); ++it) {
	 if (it!=path_u_v.begin())
		DEBUG_MSG("%s",", ");
	 DEBUG_MSG("" ST_FMTL(4) " ", *it);
  }
  DEBUG_MSG("%s","}\n");
#endif
}

static void
add_non_tree_constraints(const LocusGraph& glg,
								 const LocusGraph& lg,
								 cycle_constraints_t& ccv,
								 const int locus) {
  BGL_FORALL_EDGES(e, lg, LocusGraph) {
	 if (lg[e].in_spanning_forest)
		continue;
	 const LG_Vertex u= source(e, lg);
	 const LG_Vertex v= target(e, lg);
	 DEBUG("Adding constraints for non-tree edge " ST_FMT "-" ST_FMT ".", u, v);
	 std::list< LG_Vertex > cycle_e;
	 tree_path(lg, u, v, cycle_e);

	 unsigned int predet_vertices= 0;
	 for (std::list< LG_Vertex >::const_iterator it= cycle_e.begin();
			it != cycle_e.end(); ++it) {
		if (lg[*it].ls!=ped::LS_UNDETERMINED)
		  ++predet_vertices;
	 }
	 DEBUG("The foundamental cycle of (" ST_FMT "-" ST_FMT ") has %u predetermined vertices.",
			 u, v, predet_vertices);
	 if (predet_vertices<=1) {
		DEBUG("The foundamental cycle exists in the locus graph %d.", locus);
		DEBUG("Adding NON-tree constraint.");
		ped::cycle_constraint_t* c= new cycle_constraint_t;
		ccv.push_back(c);
		LG_Vertex prev= v;
		c->constant= false;
		for (std::list< LG_Vertex >::const_iterator it= cycle_e.begin();
			  it != cycle_e.end(); ++it) {
		  bool found= false;
		  LG_Edge lge;
		  tie(lge, found)= edge(prev, *it, lg);
		  my_assert(found);
		  c->constant ^= (lg[lge].d==1);
#ifndef NO_MUTATIONS
		  c->events.insert(get_m_variable_from_edge(lg, prev, *it, locus));
#endif
#ifndef NO_RECOMBINATIONS
		  get_rec_variables_from_edge_to_set(lg, prev, *it, locus, c->events);
#endif
		  c->h_vars.insert(get_h_variable_from_edge(lg, prev, *it, locus));
		  prev= *it;
		}
	 } else {
// Compute the predetermined endpoints of the path
		DEBUG("The foundamental cycle does not exist in the locus graph %d.",
				locus);
		DEBUG("Adding tree constraint.");
		LG_Vertex x= u;
		for (std::list< LG_Vertex >::const_iterator it= cycle_e.begin();
			  (it != cycle_e.end()) && (lg[x].ls==ped::LS_UNDETERMINED);
			  ++it) {
		  x= *it;
		}
		my_assert(lg[x].ls!=ped::LS_UNDETERMINED);
		LG_Vertex y= v;
		for (std::list< LG_Vertex >::const_reverse_iterator it= cycle_e.rbegin();
			  (it != cycle_e.rend()) && (lg[y].ls==ped::LS_UNDETERMINED);
			  ++it) {
		  y= *it;
		}
		my_assert(lg[y].ls!=ped::LS_UNDETERMINED);
		my_assert(x!=y);
		DEBUG("Endpoints of the constraint " ST_FMT "-" ST_FMT ".", x, y);
		ped::cycle_constraint_t* c= new cycle_constraint_t;
		ccv.push_back(c);
		LG_Vertex prev= v;
		c->constant= false;
		bool avoid= false;
		for (std::list< LG_Vertex >::const_iterator it= cycle_e.begin();
			  it != cycle_e.end(); ++it) {
		  if (!avoid) {
			 bool found= false;
			 LG_Edge lge;
			 DEBUG("" ST_FMT " " ST_FMT "", prev, *it);
			 tie(lge, found)= edge(prev, *it, lg);
			 my_assert(found);
			 c->constant ^= (lg[lge].d==1);
#ifndef NO_MUTATIONS
			 c->events.insert(get_m_variable_from_edge(lg, prev, *it, locus));
#endif
#ifndef NO_RECOMBINATIONS
			 get_rec_variables_from_edge_to_set(lg, prev, *it, locus, c->events);
#endif
			 c->h_vars.insert(get_h_variable_from_edge(lg, prev, *it, locus));
		  }
		  prev= *it;
		  if (*it==y)
			 avoid= false;
		  if (*it==x)
			 avoid= true;
		}
#ifndef NO_MUTATIONS
// Add extremal m variables
		ped::e_variable_t m= get_extremal_m_var(lg[x].ls, x, locus);
		if (m.i>=0) {
		  c->events.insert(m);
		}
		m= get_extremal_m_var(lg[y].ls, y, locus);
		if (m.i>=0) {
		  c->events.insert(m);
		}
#endif
// Add predetermined phases
		c->constant ^= ((lg[x].ptilde+lg[y].ptilde)%2)==1;
	 }
  }
}

static void
add_special_constraints(ped::e_vars_t& m_vars_univ,
								LocusGraph** lgs,
								ped::cycle_constraints_t& cycle_constraints)
{
  ped::e_vars_t new_vars;
  ped::e_vars_t::iterator iMVU= m_vars_univ.begin();
  const ped::e_vars_t::iterator& iMVUend= m_vars_univ.end();
  for (; iMVU!=iMVUend; ++iMVU) {
	 const ped::e_variable_t& mv= *iMVU;
	 if (mv.kind()==ped::e_variable_t::MUT && mv.p==0 && (*lgs[mv.l])[mv.i].ls>=ped::LS_PRED_DOUBLE) {
		ped::cycle_constraint_t* cc= new ped::cycle_constraint_t;
		cc->events.insert(mv);
		ped::e_variable_t mv1= mv;
		mv1.p= 1;
		cc->events.insert(mv1);
		cc->constant= (*lgs[mv.l])[mv.i].ls==ped::LS_PRED_AMBIGUOUS;
		cycle_constraints.push_back(cc);
		m_vars_univ.insert(iMVU, mv1);
		new_vars.insert(mv1);
		DEBUG("Added special constraint for indiv. %d at locus %d because he is %s.",
				mv.i, mv.l, ped::locus_status_names[(*lgs[mv.l])[mv.i].ls]);
	 }
  }
}

static void
build_basic_constraints(LocusGraph* const glg,
								LocusGraph* const * const lgs,
								pgenped gp,
								ped::cycle_constraints_t& constraints) {
/*********************************************************
 *
 * Per ogni locus graph Gl
 *   Per ogni vertice v
 *     if v is predetermined then
 *       visita il grafo a partire da v e aggiungi i vincoli
 *
 *********************************************************/
  for (unsigned int l= 0; l<gp->n_loci; ++l) {
	 DEBUG("Starting analysis of constraints of locus %d.", l);
	 LocusGraph& lg= *(lgs[l]);
	 std::vector<bool> visited(gp->n_indiv, false);
	 for (unsigned int v= 0; v<gp->n_indiv; ++v) {
		if (lg[v].ls!=ped::LS_UNDETERMINED && !visited[v]) {
		  visited[v]= true;
		  TRACE("Vertex %d is predetermined.", v);
		  add_constraints_from_v_in_locus(lg, v, constraints,
													 l, gp->n_indiv, visited);
		}
	 }
	 add_non_tree_constraints(*glg, lg, constraints, l);
  }
}

void
build_constraints_from_pedigree(pgenped gp,
										  ped::cycle_constraints_t& final_ct,
										  ped::e_vars_t& e_vars_univ) {

#if (defined NO_RECOMBINATIONS) && (defined NO_MUTATIONS)
#error "Both Recombinations and Mutations are excluded. Impossible to continue."
#endif
#ifdef NO_MUTATIONS
#warning "Only RECOMBINATIONS are considered."
  WARN("Only RECOMBINATIONS are considered.");
#endif
#ifdef NO_RECOMBINATIONS
#warning "Only MUTATIONS are considered."
  WARN("Only MUTATIONS are considered.");
#endif
  my_assert(gp!=NULL);
// Build the locus graphs
  LocusGraph* glg= build_general_locus_graph_from_pedigree(gp);
  LocusGraph** lgs= build_all_locus_graphs_from_pedigree(gp);

// Build the basic constraints
  ped::cycle_constraints_t basic_ct;
  build_basic_constraints(glg, lgs, gp, basic_ct);
#ifdef LOG_DEBUG_ENABLED
  {
	 DEBUG("Basic constraints:");
	 int _cont= 0;
	 for (cycle_constraints_t::iterator it= basic_ct.begin();
			it!=basic_ct.end();
			++it, ++_cont) {
		DEBUG("Constraint %4d.", _cont);
		DEBUG("h_vars: %s", to_c_str((*it)->h_vars));
		DEBUG("events: %s", to_c_str((*it)->events));
		DEBUG("constant: %d", ((*it)->constant)?1:0);
	 }
  }
#endif

// Build the basis of the null-space of the vector space spanned by
// the set of h-variable subsets

// 1- build the map h-variable <->position
  ped::h_vars_t h_vars;
  unsigned int i= 0;
  for (ped::cycle_constraints_t::const_iterator cit= basic_ct.begin();
		 cit!=basic_ct.end(); ++cit) {
	 for (ped::h_vars_t::const_iterator hit= (*cit)->h_vars.begin();
			hit!= (*cit)->h_vars.end(); ++hit){
		h_vars.insert(*hit);
	 }
	 ++i;
  }

  const size_t N_BASIC_CT= i;
  const size_t N_H_VARS= h_vars.size();
#ifdef LOG_STATS_ENABLED
  static bool first= true;
  STATS_IF(first, "# basic constraints " ST_FMT "", N_BASIC_CT);
  STATS_IF(first, "# h-variables " ST_FMT "", N_H_VARS);
#endif

  if (N_BASIC_CT>0) {
	 DEBUG("There are " ST_FMTL(4) "  basic constraints and " ST_FMTL(4) "  h-variables.",
			 N_BASIC_CT, N_H_VARS);

	 std::map<h_variable_t, unsigned int> h_to_pos;
	 std::vector<h_variable_t> pos_to_h(N_H_VARS);
	 i= 0;
	 for (ped::h_vars_t::const_iterator hit= h_vars.begin();
			hit!= h_vars.end(); ++hit, ++i) {
		h_to_pos[*hit]= i;
		pos_to_h.push_back(*hit);
	 }

// 2- build the binary matrix
	 mzd_t *bm= MZD_INIT(N_H_VARS, N_BASIC_CT);
	 i= 0;
	 for (ped::cycle_constraints_t::const_iterator cit= basic_ct.begin();
			cit!=basic_ct.end(); ++cit) {
		for (size_t j= 0; j<N_H_VARS; ++j)
		  mzd_write_bit(bm, j, i, 0);
		for (ped::h_vars_t::const_iterator hit= (*cit)->h_vars.begin();
			  hit!= (*cit)->h_vars.end(); ++hit){
		  mzd_write_bit(bm, h_to_pos[*hit], i, 1);
		}
		++i;
	 }
	 const size_t rank_bm= mzd_echelonize_m4ri(bm, 0, 0);
	 STATS_IF(first, "rank basic constraints " ST_FMT "", rank_bm);
//	 DEBUG("The rank of the basic constraint matrix is %4d.", rank_bm);
/**
 * 3- reorder column so that the first rank_bm are independent
 * */
	 std::vector<int> col_perm;
	 mzd_t *bm_re= MZD_INIT(rank_bm, N_BASIC_CT);
	 size_t pos= 0;
	 for (i= 0; i<rank_bm; ++i) {
		while (mzd_read_bit(bm, i, pos)==0) {
		  ++pos;
		}
// Independent column
		for (size_t j= 0; j<=i; ++j)
		  mzd_write_bit(bm_re, j, i, mzd_read_bit(bm, j, pos));
		col_perm.push_back(pos);
		++pos;
	 }
	 size_t second_part= rank_bm;
	 i= 0;
	 pos= 0;
	 while ((second_part<N_BASIC_CT) && (i<N_H_VARS)) {
		while ((pos<N_BASIC_CT) && (mzd_read_bit(bm, i, pos) == 0)) {
// Move column pos to second_part
		  for (size_t j= 0; j<i; ++j)
			 mzd_write_bit(bm_re, j, second_part, mzd_read_bit(bm, j, pos));
		  col_perm.push_back(pos);
		  ++second_part;
		  ++pos;
		}
		++i;
		++pos;
	 }
// Move remaining columns to second_part
	 while (second_part<N_BASIC_CT) {
		for (size_t j= 0; j<N_H_VARS; ++j)
		  mzd_write_bit(bm_re, j, second_part, mzd_read_bit(bm, j, pos));
		col_perm.push_back(pos);
		++second_part;
		++pos;
	 }

// Transform the independent columns into a identity matrix
	 for (i= rank_bm-1; i>0; --i) {
		for (size_t j= 0; j<i; ++j) {
		  if (mzd_read_bit(bm_re, j, i)==1) {
			 mzd_row_add(bm_re, i, j);
		  }
		}
	 }

// The last N_BASIC_CT - rank_bm columns contain the null-space

// Generate cycle-constraints
	 for (i= rank_bm; i<N_BASIC_CT; ++i) {
		DEBUG("Generating cycle constraint " ST_FMTL(4) " .", i+1-rank_bm);
		ped::cycle_constraint_t* cc= new ped::cycle_constraint_t;
		cc->events= basic_ct[col_perm[i]]->events;
		cc->constant= basic_ct[col_perm[i]]->constant;
		DEBUG("Dependent constraint %d", col_perm[i]);
		for (size_t j= 0; j<rank_bm; ++j) {
		  if (mzd_read_bit(bm_re, j, i)==1) {
			 DEBUG("+ constraint %d", col_perm[j]);
			 inplace_symmetric_difference(cc->events, basic_ct[col_perm[j]]->events);
			 cc->constant= cc->constant != basic_ct[col_perm[j]]->constant;
		  }
		}
		inplace_union(e_vars_univ, cc->events);
		final_ct.push_back(cc);
	 }
	 mzd_free(bm);
	 mzd_free(bm_re);
  } else {
	 INFO("There are no basic constraints.");
  }

  add_special_constraints(e_vars_univ, lgs, final_ct);

  add_immutable_immutable_constraints(gp, e_vars_univ, final_ct);

  for (ped::cycle_constraints_t::const_iterator cit= basic_ct.begin();
		 cit!=basic_ct.end(); ++cit) {
	 delete *cit;
  }
  for (unsigned int l= 0; l<gp->n_loci; ++l)
	 delete (lgs[l]);
  delete glg;
  pfree(lgs);
#ifdef LOG_STATS_ENABLED
  STATS_IF(first, "# initial cycle constraints " ST_FMT "", final_ct.size());
  STATS_IF(first, "# initial events variables " ST_FMT "", e_vars_univ.size());
  first= false;
#endif
}