/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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 "SiconosConfig.h"

#include "SiconosAlgebraTypeDef.hpp"

#include <boost/numeric/ublas/io.hpp>            // for >>
//#include <boost/numeric/ublas/vector_proxy.hpp>  // for project
#include <boost/numeric/ublas/vector_sparse.hpp>

#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/blas.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
namespace siconosBindings = boost::numeric::bindings::blas;

#include "SimpleMatrix.hpp"
#include "BlockVector.hpp"
#include "ioVector.hpp"
#include "SiconosVector.hpp"
#include "SiconosAlgebra.hpp"

// Do not document
/// @cond
#include "Question.hpp"

struct IsDense : public Question<bool>
{
  using SiconosVisitor::visit;

  void visit(const SiconosVector& v)
  {
    answer = v._dense;
  }

  void visit(const BlockVector& v)
  {
    answer = false;
  }
};

struct IsSparse : public Question<bool>
{

  using SiconosVisitor::visit;

  void visit(const SiconosVector& v)
  {
    answer = !v._dense;
  }

  void visit(const BlockVector& v)
  {
    answer = false;
  }
};

struct IsBlock : public Question<bool>
{
  using SiconosVisitor::visit;

  void visit(const SiconosVector& v)
  {
    answer = false;
  }

  void visit(const BlockVector& v)
  {
    answer = true;
  }
};

/// @endcond

// =================================================
//                CONSTRUCTORS
// =================================================

// Default
SiconosVector::SiconosVector()
{
  _dense = true;
  vect.Dense = new DenseVect(ublas::zero_vector<double>());
}

// parameters: dimension and type.
SiconosVector::SiconosVector(unsigned row, Siconos::UBLAS_TYPE type)
{
  if (type == Siconos::SPARSE)
  {
    _dense = false;
    vect.Sparse = new SparseVect(ublas::zero_vector<double>(row));
  }
  else if (type == Siconos::DENSE)
  {
    _dense = true;
    vect.Dense = new DenseVect(ublas::zero_vector<double>(row));
  }
  else
  {
    SiconosVectorException::selfThrow("SiconosVector::constructor(Siconos::UBLAS_TYPE, unsigned int) failed, invalid type given");
  }
}

// parameters: dimension, default value for all components and type.
SiconosVector::SiconosVector(unsigned row, double val, Siconos::UBLAS_TYPE type)
{
  if (type == Siconos::SPARSE)
  {
    _dense = false;
    vect.Sparse = new SparseVect(row);
    fill(val);
  }
  else if (type == Siconos::DENSE)
  {
    _dense = true;
    vect.Dense = new DenseVect(ublas::scalar_vector<double>(row, val));
  }
  else
  {
    SiconosVectorException::selfThrow("SiconosVector::constructor(Siconos::UBLAS_TYPE, unsigned int) : invalid type given");
  }
}

// parameters: a vector (stl) of double and the type.
SiconosVector::SiconosVector(const std::vector<double>& v, Siconos::UBLAS_TYPE typ)
{
  if (typ != Siconos::DENSE)
    SiconosVectorException::selfThrow("SiconosVector::constructor(Siconos::UBLAS_TYPE, std::vector<double>, unsigned int) : invalid type given");

  _dense = true;
  vect.Dense = new DenseVect(v.size());
  std::copy(v.begin(), v.end(), (vect.Dense)->begin());
}

// Copy
SiconosVector::SiconosVector(const SiconosVector &svect) : std11::enable_shared_from_this<SiconosVector>()
{
  if (ask<IsDense>(svect)) // dense
  {
    _dense = true;
    vect.Dense = new DenseVect(svect.size());
    noalias(*vect.Dense) = (*svect.dense());
    // std::copy((vect.Dense)->begin(), (vect.Dense)->end(), (svect.dense())->begin());
  }
  else //sparse
  {
    _dense = false;
    vect.Sparse = new SparseVect(svect.size());
    noalias(*vect.Sparse) = (*svect.sparse());
    //std::copy((vect.Sparse)->begin(), (vect.Sparse)->end(), (svect.sparse())->begin());
  }

  // Note FP: using constructor + noalias = (or std::copy) is more
  // efficient than a call to ublas::vector copy constructor, this for
  // large or small vectors.
}

// Copy from BlockVector
SiconosVector::SiconosVector(const BlockVector & vIn) : std11::enable_shared_from_this<SiconosVector>()
{
  if (ask<IsDense>(**(vIn.begin()))) // dense
  {
    _dense = true;
    vect.Dense = new DenseVect(vIn.size());
  }
  else
  {
    _dense = false;
    vect.Sparse = new SparseVect(vIn.size());
  }

  VectorOfVectors::const_iterator it;
  unsigned int pos = 0;
  for (it = vIn.begin(); it != vIn.end(); ++it)
  {
    setBlock(pos, **it);
    pos += (*it)->size();
  }

}

SiconosVector::SiconosVector(const DenseVect& m)
{
  _dense = true;
  vect.Dense = new DenseVect(m.size());
  noalias(*vect.Dense) = m;

}

SiconosVector::SiconosVector(const SparseVect& m)
{
  _dense = false;
  vect.Sparse = new SparseVect(m.size());
  noalias(*vect.Sparse) = m;
}

SiconosVector::SiconosVector(const std::string &file, bool ascii)
{
  _dense = true;
  vect.Dense = new DenseVect();
  if (ascii)
  {
    ioVector::read(file, *this, ioVector::ASCII_IN);
   }
  else
  {
    ioVector::read(file, *this, ioVector::BINARY_IN);
  }
}

SiconosVector::SiconosVector(const SiconosVector& v1, const SiconosVector& v2)
{
  unsigned int size1 = v1.size();
  if (ask<IsDense>(v1) && ask<IsDense>(v2))
  {
    _dense = true;
    vect.Dense = new DenseVect(size1 + v2.size());
  }
  else if (ask<IsSparse>(v1) && ask<IsSparse>(v2))
  {
    _dense = false;
    vect.Sparse = new SparseVect(size1 + v2.size());
  }
  else
  {
    SiconosVectorException::selfThrow("SiconosVector::SiconosVector :: mixed dense and sparse vector detected");
  }
  setBlock(0, v1);
  setBlock(size1, v2);
}

SiconosVector::~SiconosVector()
{
  if (_dense)
    delete(vect.Dense);
  else delete(vect.Sparse);
}

// =================================================
//        get Ublas component (dense or sparse)
// =================================================

const DenseVect SiconosVector::getDense(unsigned int) const
{
  if (!_dense)
    SiconosVectorException::selfThrow("SiconosVector::getDense(unsigned int row, unsigned int col) : the current vector is not a Dense vector");

  return *vect.Dense;
}

const SparseVect SiconosVector::getSparse(unsigned int)const
{

  if (_dense)
    SiconosVectorException::selfThrow("SiconosVector::getSparse(unsigned int row, unsigned int col) : the current vector is not a Sparse vector");

  return *vect.Sparse;
}

SparseVect* SiconosVector::sparse(unsigned int)const
{

  if (_dense)
    SiconosVectorException::selfThrow("SiconosVector::sparse(unsigned int row, unsigned int col) : the current vector is not a Sparse vector");

  return vect.Sparse;
}

double* SiconosVector::getArray() const
{
  assert(vect.Dense && "SiconosVector::getArray() : not yet implemented for sparse vector.");

  return &(((*vect.Dense).data())[0]);
}

// ===========================
//       fill vector
// ===========================

void SiconosVector::zero()
{
  if (_dense)
    siconosBindings::scal(0.0, *vect.Dense);

  else
  {
    assert(vect.Sparse);
    *vect.Sparse *= 0.0;
  }

}

void SiconosVector::setVector(unsigned int , const SiconosVector& newV)
{
  if (newV.size() != size())
    SiconosVectorException::selfThrow("SiconosVector::setVector(num,v), unconsistent sizes.");

  *this = newV ;
}

void SiconosVector::fill(double value)
{
  if (!_dense)
  {
    for (unsigned int i = 0; i < (vect.Sparse)->size(); ++i)
      (vect.Sparse)->push_back(i, value);
  }
  else
    siconosBindings::set(value, *vect.Dense);

}

//=======================
// set vector dimension
//=======================

void SiconosVector::resize(unsigned int n, bool preserve)
{
  if (_dense)
    (vect.Dense)->resize(n, preserve);
  else
    (vect.Sparse)->resize(n, preserve);
}

//=======================
//       get norm
//=======================

double SiconosVector::normInf() const
{
  if (_dense)
    return norm_inf(*vect.Dense);
  else //if(num==4)
    return norm_inf(*vect.Sparse);
}

double SiconosVector::norm2() const
{
  if (_dense)
    return ublas::norm_2(*vect.Dense);
  else //if(num==4)
    return ublas::norm_2(*vect.Sparse);
}
//======================================
// get sum of all elements of the vector
//=====================================
double SiconosVector::vector_sum() const
{
  if (_dense)
    return ublas::sum(*vect.Dense);
  else
    return ublas::sum(*vect.Sparse);
}

//=====================
// screen display
//=====================

void SiconosVector::display()const
{
  std::cout.setf(std::ios::scientific);
  std::cout.precision(6);
  if (_dense)
    std::cout << *vect.Dense << std::endl;
  else if (vect.Sparse)
    std::cout << *vect.Sparse << std::endl;
}

//============================
// Convert vector to a std::string
//============================

const std::string SiconosVector::toString() const
{
  std::stringstream sstr;
  std::string s;
  if (_dense)
    sstr << *vect.Dense;
  else
    sstr << *vect.Sparse;
  sstr >> s;
  s = s.substr(4, s.size() - 5); // Remove "[size](" at the beginning of the std::string
  std::string::size_type pos;
  while ((pos = s.find(",")) != std::string::npos) // Replace "," by " " in the std::string
    s[pos] = ' ';
  return s;
}

//=============================
// Elements access (get or set)
//=============================

double SiconosVector::getValue(unsigned int row) const
{
  assert(row < size() && "SiconosVector::getValue(index) : Index out of range");

  if (_dense)
    return (*vect.Dense)(row);
  else
    return (*vect.Sparse)(row);
}

void SiconosVector::setValue(unsigned int row, double value)
{
  assert(row < size() && "SiconosVector::setValue(index, value) : Index out of range");
  if (_dense)
    (*vect.Dense)(row) = value ;
  else
    (*vect.Sparse)(row) = value;
}

double& SiconosVector::operator()(unsigned int row)
{
  assert(row < size() && "SiconosVector::operator ( index ): Index out of range");

  if (_dense)
    return (*vect.Dense)(row);
  else
    return (*vect.Sparse)(row).ref();
}

double SiconosVector::operator()(unsigned int row) const
{
  assert(row < size() && "SiconosVector::operator ( index ): Index out of range");

  if (_dense)
    return (*vect.Dense)(row);
  else
    return ((*vect.Sparse)(row)).ref();
}

//============================================
// Access (get or set) to blocks of elements
//============================================

void SiconosVector::setBlock(unsigned int index, const SiconosVector& vIn)
{
  // Set current vector elements, starting from position "index", to the values of vector vIn

  // Exceptions ...
  assert(&vIn != this && "SiconosVector::this->setBlock(pos,vIn): vIn = this.");

  assert(index < size() && "SiconosVector::setBlock : invalid ranges");

  unsigned int end = vIn.size() + index;
  assert(end <= size() && "SiconosVector::setBlock : invalid ranges");

  assert (vIn.num() == num() && "SiconosVector::setBlock: inconsistent types.");

  if (_dense)
    noalias(ublas::subrange(*vect.Dense, index, end)) = *vIn.dense();
  else
    noalias(ublas::subrange(*vect.Sparse, index, end)) = *vIn.sparse();
}

void SiconosVector::toBlock(SiconosVector& vOut, unsigned int sizeB, unsigned int startIn, unsigned int startOut) const
{
  // To copy a subBlock of the vector (from position startIn to startIn+sizeB) into vOut (from pos. startOut to startOut+sizeB).
  // Check dim ...
  assert(startIn < size() && "vector toBlock(v1,v2,...): start position in input vector is out of range.");

  assert(startOut < vOut.size() && "vector toBlock(v1,v2,...): start position in output vector is out of range.");

  assert(startIn + sizeB <= size() && "vector toBlock(v1,v2,...): end position in input vector is out of range.");
  assert(startOut + sizeB <= vOut.size() && "vector toBlock(v1,v2,...): end position in output vector is out of range.");

  unsigned int endOut = startOut + sizeB;
  unsigned int numIn = num();
  unsigned int numOut = vOut.num();

  if (numIn == numOut)
  {
    if (numIn == 1) // vIn / vOut are Dense
      noalias(ublas::subrange(*vOut.dense(), startOut, endOut)) = ublas::subrange(*vect.Dense, startIn, startIn + sizeB);
    else // if(numIn == 4)// vIn / vOut are Sparse
      noalias(ublas::subrange(*vOut.sparse(), startOut, endOut)) = ublas::subrange(*vect.Sparse, startIn, startIn + sizeB);
  }
  else // vIn and vout of different types ...
  {
    if (numIn == 1) // vIn Dense
      noalias(ublas::subrange(*vOut.sparse(), startOut, endOut)) = ublas::subrange(*vect.Dense, startIn, startIn + sizeB);
    else // if(numIn == 4)// vIn Sparse
      noalias(ublas::subrange(*vOut.dense(), startOut, endOut)) = ublas::subrange(*vect.Sparse, startIn, startIn + sizeB);
  }
}

void SiconosVector::addBlock(unsigned int index, const SiconosVector& vIn)
{
  // Add vIn to the current vector, starting from position "index".
  // vIn may be a BlockVector.

  //if ( num != 1 ) SiconosVectorException::selfThrow("SiconosVector::addBlock : vector should be dense");

  if (&vIn == this)
    SiconosVectorException::selfThrow("SiconosVector::this->addBlock(pos,vIn): vIn = this.");

  unsigned int end = vIn.size();
  if ((index + end) > size()) SiconosVectorException::selfThrow("SiconosVector::addBlock : invalid ranges");

  unsigned int numVin = vIn.num();

  if (numVin != num()) SiconosVectorException::selfThrow("SiconosVector::addBlock : inconsistent types.");

  if (_dense)
    noalias(ublas::subrange(*vect.Dense, index, index + end)) += *vIn.dense();
  else
    noalias(ublas::subrange(*vect.Sparse, index, index + end)) += *vIn.sparse();
}

void SiconosVector::subBlock(unsigned int index, const SiconosVector& vIn)
{
  // Add vIn from the current vector, starting from position "index".
  // vIn may be a BlockVector.

  //  if ( num != 1 ) SiconosVectorException::selfThrow("SiconosVector::subBlock : vector should be dense");

  unsigned int end = vIn.size();
  if ((index + end) > size()) SiconosVectorException::selfThrow("SiconosVector::subBlock : invalid ranges");

  unsigned int numVin = vIn.num();
  if (numVin != num()) SiconosVectorException::selfThrow("SiconosVector::subBlock : inconsistent types.");

  if (_dense)
    noalias(ublas::subrange(*vect.Dense, index, index + end)) -= *vIn.dense();
  else
    noalias(ublas::subrange(*vect.Sparse, index, index + end)) -= *vIn.sparse();
}

//===============
//  Assignment
//===============

SiconosVector& SiconosVector::operator = (const SiconosVector& vIn)
{
  if (&vIn == this) return *this; // auto-assignment.

  assert(size() == vIn.size() && "SiconosVector::operator = failed: inconsistent sizes.");

  unsigned int vInNum = vIn.num();
  {
    switch (num())
    {
    case 1:
      switch (vInNum)
      {
      case 1:
        //siconosBindings::copy(*vIn.dense(),*vect.Dense);
        noalias(*vect.Dense) = *vIn.dense();
        break;
      case 4:
        noalias(*vect.Dense) = *vIn.sparse();
        break;
      default:
        SiconosVectorException::selfThrow("SiconosVector::operator = : invalid type given");
        break;
      }
      break;
    case 4:
      if (vInNum == 4)
        noalias(*vect.Sparse) = *vIn.sparse();
      else
        SiconosVectorException::selfThrow("SiconosVector::operator = : can not set sparse = dense.");
      break;
    default:
      SiconosVectorException::selfThrow("SiconosVector::operator = : invalid type given");
      break;
    }
  }
  return *this;
}

SiconosVector& SiconosVector::operator = (const BlockVector& vIn)
{
  VectorOfVectors::const_iterator it;
  unsigned int pos = 0;
  for (it = vIn.begin(); it != vIn.end(); ++it)
  {
    setBlock(pos, **it);
    pos += (*it)->size();
  }
  return *this;
}

SiconosVector& SiconosVector::operator = (const DenseVect& d)
{
  if (!_dense)
    SiconosVectorException::selfThrow("SiconosVector::operator = DenseVect : forbidden: the current vector is not dense.");
  if (d.size() != size())
    SiconosVectorException::selfThrow("SiconosVector::operator = DenseVect : inconsistent size.");

  siconosBindings::copy(d, *vect.Dense);
  return *this;
}

SiconosVector& SiconosVector::operator = (const SparseVect& sp)
{
  if (_dense)
    SiconosVectorException::selfThrow("SiconosVector::operator = SparseVect : current vector is not sparse.");
  if (sp.size() != size())
    SiconosVectorException::selfThrow("SiconosVector::operator = SparseVect : inconsistent size.");

  noalias(*vect.Sparse) = sp;

  return *this;
}

SiconosVector& SiconosVector::operator = (const double* d)
{
  assert(_dense && "SiconosVector::operator = double* : forbidden: the current vector is not dense.");

  siconosBindings::detail::copy(vect.Dense->size(), d, 1, getArray(), 1);
  return *this;
}

unsigned SiconosVector::copyData(double* data) const
{
  assert(_dense && "SiconosVector::copyData : forbidden: the current vector is not dense.");

  unsigned size = vect.Dense->size();
  siconosBindings::detail::copy(vect.Dense->size(), getArray(), 1, data, 1);
  return size;
}

//=================================
// Op. and assignment (+=, -= ... )
//=================================

SiconosVector& SiconosVector::operator += (const SiconosVector& vIn)
{
  if (&vIn == this) // alias
  {
    // Note: using this *= 2.0 is much more time-consuming.
    switch (num())
    {
    case 1:
      *vect.Dense += *vect.Dense;
      break;
    case 4:
      *vect.Sparse += *vect.Sparse;
      break;
    default:
      SiconosVectorException::selfThrow("SiconosVector::operator += : invalid type given");
      break;
    }
    return *this;
  }

  unsigned int vInNum = vIn.num();
  {
    switch (num())
    {
    case 1:
      switch (vInNum)
      {
      case 1:
        noalias(*vect.Dense) += *vIn.dense();
        break;
      case 4:
        noalias(*vect.Dense) += *vIn.sparse();
        break;
      default:
        SiconosVectorException::selfThrow("SiconosVector::operator += : invalid type given");
        break;
      }
      break;
    case 4:
      if (vInNum == 4)
        noalias(*vect.Sparse) += *vIn.sparse();
      else SiconosVectorException::selfThrow("SiconosVector::operator += : can not add a dense to a sparse.");
      break;
    default:
      SiconosVectorException::selfThrow("SiconosVector::operator += : invalid type given");
      break;
    }
  }
  return *this;
}
SiconosVector& SiconosVector::operator += (const BlockVector& vIn)
{
  VectorOfVectors::const_iterator it;
  unsigned int pos = 0;
  for (it = vIn.begin(); it != vIn.end(); ++it)
  {
    addBlock(pos, **it);
    pos += (*it)->size();
  }
  return *this;
}

SiconosVector& SiconosVector::operator -= (const SiconosVector& vIn)
{
  if (&vIn == this)
  {
    this->zero();
    return *this;
  }

  unsigned int vInNum = vIn.num();
  {
    switch (num())
    {
    case 1:
      switch (vInNum)
      {
      case 1:
        noalias(*vect.Dense) -= *vIn.dense();
        break;
      case 4:
        noalias(*vect.Dense) -= *vIn.sparse();
        break;
      default:
        SiconosVectorException::selfThrow("SiconosVector::operator -= : invalid type given");
        break;
      }
      break;
    case 4:
      if (vInNum == 4)
        noalias(*vect.Sparse) -= *vIn.sparse();
      else SiconosVectorException::selfThrow("SiconosVector::operator -= : can not sub a dense to a sparse.");
      break;
    default:
      SiconosVectorException::selfThrow("SiconosVector::operator -= : invalid type given");
      break;
    }
  }
  return *this;
}

SiconosVector& SiconosVector::operator -= (const BlockVector& vIn)
{
  VectorOfVectors::const_iterator it;
  unsigned int pos = 0;
  for (it = vIn.begin(); it != vIn.end(); ++it)
  {
    subBlock(pos, **it);
    pos += (*it)->size();
  }
  return *this;
}

//===============
// Comparison
//===============

bool operator == (const SiconosVector &m, const SiconosVector &x)
{
  return ((m - x).norm2() < std::numeric_limits<double>::epsilon());
}

//==================
// y = scalar * x
//==================

SiconosVector operator * (const  SiconosVector&m, double d)
{
  unsigned int numM = m.num();

  if (numM == 1)
  {
    // Copy m into p and call siconosBindings::scal(d,p), p = d*p.
    DenseVect p = *m.dense();
    siconosBindings::scal(d, p);
    return p;
  }
  else// if(numM==4)
  {
    return (SparseVect)(*m.sparse() * d);
  }
}

SiconosVector operator * (double d, const  SiconosVector&m)
{
  unsigned int numM = m.num();

  if (numM == 1)
  {
    // Copy m into p and call siconosBindings::scal(d,p), p = d*p.
    DenseVect p = *m.dense();
    siconosBindings::scal(d, p);
    return p;
  }
  else// if(numM==4)
  {
    return (SparseVect)(*m.sparse() * d);
  }
}

SiconosVector operator / (const SiconosVector &m, double d)
{
  unsigned int numM = m.num();

  if (numM == 1)
  {
    DenseVect p = *m.dense();
    siconosBindings::scal((1.0 / d), p);
    return p;
  }

  else// if(numM==4){
    return (SparseVect)(*m.sparse() / d);
}

//====================
//  Vectors addition
//====================

SiconosVector operator + (const  SiconosVector& x, const  SiconosVector& y)
{
  if (x.size() != y.size())
    SiconosVectorException::selfThrow("SiconosVector, x + y: inconsistent sizes");

  unsigned int numX = x.num();
  unsigned int numY = y.num();

  if (numX == numY) // x, y SiconosVector of the same type
  {
    if (numX == 1)
    {
      //    siconosBindings::xpy(*x.dense(),p);
      //    return p;
      return (DenseVect)(*x.dense() + *y.dense());
    }
    else
      return (SparseVect)(*x.sparse() + *y.sparse());
  }

  else // x, y SiconosVector with y and x of different types
  {
    if (numX == 1)
      return (DenseVect)(*x.dense() + *y.sparse());
    else
      return (DenseVect)(*x.sparse() + *y.dense());
  }

}

void add(const SiconosVector& x, const SiconosVector& y, SiconosVector& z)
{
  // Computes z = x + y in an "optimized" way (in comparison with operator +)

  if (x.size() != y.size() || x.size() != z.size())
    SiconosVectorException::selfThrow("add(x,y,z): inconsistent sizes");

  unsigned int numX = x.num();
  unsigned int numY = y.num();
  unsigned int numZ = z.num();

  if (&z == &x) // x, and z are the same object.
  {
    z += y;
  }
  else if (&z == &y) // y and z are the same object, different from x
  {
    z += x;
  }
  else // No common memory between x,y and z
  {

    if (numZ != 0) // z is a SiconosVector
    {
      if (numX == numY && numX != 0) // x, y SiconosVector of the same type
      {
        if (numX == 1)
        {
          if (numZ != 1)
            SiconosVectorException::selfThrow("SiconosVector addition, add(x,y,z) failed - Addition of two dense vectors into a sparse.");
          noalias(*z.dense()) = *x.dense() + *y.dense() ;
        }
        else
        {
          if (numZ == 1)
            noalias(*z.dense()) = *x.sparse() + *y.sparse() ;
          else
            noalias(*z.sparse()) = *x.sparse() + *y.sparse() ;
        }
      }
      else if (numX != 0 && numY != 0) // x and y of different types => z must be dense.
      {
        if (numZ != 1)
          SiconosVectorException::selfThrow("SiconosVector addition, add(x,y,z) failed - z can not be sparse.");
        if (numX == 1)
          noalias(*z.dense()) = *x.dense() + *y.sparse();
        else
          noalias(*z.dense()) = *x.sparse() + *y.dense() ;
      }
    }
  }
}

//======================
//  Vectors subtraction
//======================

SiconosVector operator - (const  SiconosVector& x, const  SiconosVector& y)
{
  if (x.size() != y.size())
    SiconosVectorException::selfThrow("SiconosVector, x - y: inconsistent sizes");

  unsigned int numX = x.num();
  unsigned int numY = y.num();

  if (numX == numY) // x, y SiconosVector of the same type
  {
    if (numX == 1)
    {
      //    siconosBindings::xpy(*x.dense(),p);
      //    return p;
      return (DenseVect)(*x.dense() - *y.dense());
    }
    else
      return (SparseVect)(*x.sparse() - *y.sparse());
  }
  else // x, y SiconosVector with y and x of different types
  {
    if (numX == 1)
      return (DenseVect)(*x.dense() - *y.sparse());
    else
      return (DenseVect)(*x.sparse() - *y.dense());
  }
}

void sub(const SiconosVector& x, const SiconosVector& y, SiconosVector& z)
{
  // Computes z = x - y in an "optimized" way (in comparison with operator +)

  if (x.size() != y.size() || x.size() != z.size())
    SiconosVectorException::selfThrow("sub(x,y,z): inconsistent sizes");

  unsigned int numX = x.num();
  unsigned int numY = y.num();
  unsigned int numZ = z.num();

  if (&z == &x) // x and z are the same object.
  {
    z -= y;
  }
  else if (&z == &y) // y and z are the same object
  {
    {
      if (numX == 1)
      {
        if (numZ != 1)
          SiconosVectorException::selfThrow("SiconosVector subtraction, sub(x,y,z) failed - Subtraction of two dense vectors into a sparse.");
        *z.dense() = *x.dense() - *y.dense() ;
      }
      else
      {
        if (numZ == 1)
          *z.dense() = *x.sparse() - *y.dense() ;
        else
          *z.sparse() = *x.sparse() - *y.sparse() ;
      }
    }
  }
  else // No common memory between x or y and z
  {

    if (numZ != 0) // z is a SiconosVector
    {
      if (numX == numY && numX != 0) // x, y SiconosVector of the same type
      {
        if (numX == 1)
        {
          if (numZ != 1)
            SiconosVectorException::selfThrow("SiconosVector addition, sub(x,y,z) failed - Addition of two dense vectors into a sparse.");
          noalias(*z.dense()) = *x.dense() - *y.dense() ;
        }
        else
        {
          if (numZ == 1)
            noalias(*z.dense()) = *x.sparse() - *y.sparse() ;
          else
            noalias(*z.sparse()) = *x.sparse() - *y.sparse() ;
        }
      }
      else if (numX != 0 && numY != 0) // x and y of different types => z must be dense.
      {
        if (numZ != 1)
          SiconosVectorException::selfThrow("SiconosVector addition, sub(x,y,z) failed - z can not be sparse.");
        if (numX == 1)
          noalias(*z.dense()) = *x.dense() - *y.sparse();
        else
          noalias(*z.dense()) = *x.sparse() - *y.dense() ;
      }
    }
  }
}

void axpby(double a, const SiconosVector& x, double b, SiconosVector& y)
{
  // Computes y = ax + by

  if (x.size() != y.size())
    SiconosVectorException::selfThrow("axpby(x,y,z): inconsistent sizes");

  unsigned int numX = x.num();
  unsigned int numY = y.num();

  if (numX == numY) // x and y of the same type
  {
    if (numX == 1) // all dense
    {
      siconosBindings::scal(b, *y.dense());
      siconosBindings::axpy(a, *x.dense(), *y.dense());
    }
    else // all sparse
    {
      *y.sparse() *= b;
      if (&y != &x)
        noalias(*y.sparse()) += a**x.sparse();
      else
        *y.sparse() += a**x.sparse();
    }
  }

  else // x and y of different types
  {
    y *= b;
    {
      if (numX == 1)
        *y.sparse() += a**x.dense();
      else
        *y.dense() +=  a**x.sparse();
    }
  }
}

void axpy(double a, const SiconosVector& x, SiconosVector& y)
{
  // Computes y = ax + y

  if (x.size() != y.size())
    SiconosVectorException::selfThrow("axpy(x,y,z): inconsistent sizes");

  unsigned int numX = x.num();
  unsigned int numY = y.num();

  if (numX == numY) // x and y of the same type
  {
    if (numX == 1) // all dense
      siconosBindings::axpy(a, *x.dense(), *y.dense());

    else // all sparse
    {
      if (&y != &x)
        noalias(*y.sparse()) += a**x.sparse();
      else
        *y.sparse() += a**x.sparse();
    }
  }

  else // x and y of different types
  {
    {
      if (numX == 1)
        *y.sparse() += a**x.dense();
      else
        *y.dense() +=  a**x.sparse();
    }
  }
}

double inner_prod(const SiconosVector &x, const SiconosVector &m)
{
  if (x.size() != m.size())
    SiconosVectorException::selfThrow("inner_prod: inconsistent sizes");

  unsigned int numM = m.num();
  unsigned int numX = x.num();

  if (numX == numM)
  {
    if (numM == 1)
      return siconosBindings::dot(*x.dense(), *m.dense());
    else
      return inner_prod(*x.sparse(), *m.sparse());
  }
  else if (numM == 1)
    return inner_prod(*x.sparse(), *m.dense());
  else
    return inner_prod(*x.dense(), *m.sparse());
}

// outer_prod(v,w) = trans(v)*w
SimpleMatrix outer_prod(const SiconosVector &x, const SiconosVector& m)
{
  unsigned int numM = m.num();
  unsigned int numX = x.num();

  if (numM == 1)
  {
    if (numX == 1)
      return (DenseMat)(outer_prod(*x.dense(), *m.dense()));

    else// if(numX == 4)
      return (DenseMat)(outer_prod(*x.sparse(), *m.dense()));
  }
  else // if(numM == 4)
  {
    if (numX == 1)
      return (DenseMat)(outer_prod(*x.dense(), *m.sparse()));

    else //if(numX == 4)
      return (DenseMat)(outer_prod(*x.sparse(), *m.sparse()));
  }
}

void scal(double a, const SiconosVector & x, SiconosVector & y, bool init)
{
  // To compute y = a *x (init = true) or y += a*x (init = false)

  if (&x == &y)
  {
    if (init)
      y *= a;
    else
    {
      y *= (1.0 + a);
    }
  }
  else
  {
    unsigned int sizeX = x.size();
    unsigned int sizeY = y.size();

    if (sizeX != sizeY)
      SiconosVectorException::selfThrow("scal(a,SiconosVector,SiconosVector) failed, sizes are not consistent.");

    unsigned int numY = y.num();
    unsigned int numX = x.num();
    if (numX == numY)
    {

      if (numX == 1) // ie if both are Dense
      {
        if (init)
          //siconosBindings::axpby(a,*x.dense(),0.0,*y.dense());
          noalias(*y.dense()) = a * *x.dense();
        else
          noalias(*y.dense()) += a * *x.dense();
      }
      else  // if both are sparse
      {
        if (init)
          noalias(*y.sparse()) = a**x.sparse();
        else
          noalias(*y.sparse()) += a**x.sparse();
      }
    }
    else
    {
      if (numY == 0 || numX == 0) // if y or x is block
      {
        if (init)
        {
          y = x;
          y *= a;
        }
        else
        {
          SiconosVector tmp(x);
          tmp *= a;
          y += tmp;
        }
      }
      else
      {
        if (numY == 1) // if y is dense
        {
          if (init)
            noalias(*y.dense()) = a**x.sparse();
          else
            noalias(*y.dense()) += a**x.sparse();

        }
        else
          SiconosVectorException::selfThrow("SiconosVector::scal(a,dense,sparse) not allowed.");
      }
    }
  }
}

void subscal(double a, const SiconosVector & x, SiconosVector & y, const Index& coord, bool init)
{
  // To compute sub_y = a *sub_x (init = true) or sub_y += a*sub_x (init = false)
  // Coord  = [r0x r1x r0y r1y];
  // subX is the sub-vector of x, for row numbers between r0x and r1x-1.
  // The same for y with riy.

  // Check dimensions
  unsigned int dimX = coord[1] - coord[0];
  unsigned int dimY = coord[3] - coord[2];
  if (dimY != dimX)
    SiconosVectorException::selfThrow("subscal(a,x,y,...) error: inconsistent sizes between (sub)x and (sub)y.");
  if (dimY > y.size() || dimX > x.size())
    SiconosVectorException::selfThrow("subscal(a,x,y,...) error: input index too large.");

  unsigned int numY = y.num();
  unsigned int numX = x.num();

  if (&x == &y) // if x and y are the same object
  {
    if (numX == 1) // Dense
    {
      ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[2], coord[3]));
      if (coord[0] == coord[2])
      {
        if (init)
          subY *= a;
        else
          subY *= (1.0 + a);
      }
      else
      {
        ublas::vector_range<DenseVect> subX(*x.dense(), ublas::range(coord[0], coord[1]));
        if (init)
          subY = a * subX;
        else
          subY += a * subX;
      }
    }
    else //if (numX == 4) // Sparse
    {
      ublas::vector_range<SparseVect> subY(*y.sparse(), ublas::range(coord[2], coord[3]));
      if (coord[0] == coord[2])
      {
        if (init)
          subY *= a;
        else
          subY *= (1.0 + a);
      }
      else
      {
        ublas::vector_range<SparseVect> subX(*x.sparse(), ublas::range(coord[0], coord[1]));
        if (init)
          subY = a * subX;
        else
          subY += a * subX;
      }
    }
  }
  else
  {
    if (numX == numY)
    {
      if (numX == 1) // ie if both are Dense
      {
        ublas::vector_range<DenseVect> subX(*x.dense(), ublas::range(coord[0], coord[1]));
        ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[2], coord[3]));

        if (init)
          noalias(subY) = a * subX;
        else
          noalias(subY) += a * subX;
      }
      else  // if both are sparse
      {
        ublas::vector_range<SparseVect> subX(*x.sparse(), ublas::range(coord[0], coord[1]));
        ublas::vector_range<SparseVect> subY(*y.sparse(), ublas::range(coord[2], coord[3]));

        if (init)
          noalias(subY) = a * subX;
        else
          noalias(subY) += a * subX;
      }
    }
    else // x and y of different types ...
    {
      if (numY == 1) // y dense, x sparse
      {
        ublas::vector_range<DenseVect> subY(*y.dense(), ublas::range(coord[2], coord[3]));
        ublas::vector_range<SparseVect> subX(*x.sparse(), ublas::range(coord[0], coord[1]));

        if (init)
          noalias(subY) = a * subX;
        else
          noalias(subY) += a * subX;
      }
      else // y sparse, x dense => fails
        SiconosVectorException::selfThrow("SiconosVector::subscal(a,dense,sparse) not allowed.");
    }
  }
}
void cross_product(const SiconosVector& V1, const SiconosVector& V2, SiconosVector& VOUT)
{
  if (V1.size() != 3 || V2.size() != 3 || VOUT.size() != 3)
    SiconosVectorException::selfThrow("SiconosVector::cross_product allowed only with dim 3.");

  double aux = V1.getValue(1) * V2.getValue(2) - V1.getValue(2) * V2.getValue(1);
  VOUT.setValue(0, aux);

  aux = V1.getValue(2) * V2.getValue(0) - V1.getValue(0) * V2.getValue(2);
  VOUT.setValue(1, aux);

  aux = V1.getValue(0) * V2.getValue(1) - V1.getValue(1) * V2.getValue(0);
  VOUT.setValue(2, aux);

}

//

void abs_wise(const SiconosVector& V, SiconosVector& Vabs)
{
  for (unsigned int it = 0; it < V.size(); ++it)
  {
    Vabs.setValue(it, std::abs(V.getValue(it)));
  };
}

//

void getMax(const SiconosVector& V, double& maxvalue, unsigned int& idmax)
{
  maxvalue = V.getValue(0);
  idmax = 0;
  for (unsigned int it = 1; it < V.size(); ++it)
  {
    if (V.getValue(it) > maxvalue)
    {
      maxvalue = V.getValue(it);
      idmax = it;
    };
  };
}

//

void getMin(const SiconosVector& V, double& minvalue, unsigned int& idmin)
{
  minvalue = V.getValue(0);
  idmin = 0;
  for (unsigned int it = 1; it < V.size(); ++it)
  {
    if (V.getValue(it) < minvalue)
    {
      minvalue = V.getValue(it);
      idmin = it;
    };
  };
}

//
/*
SiconosVector abs_wise(const SiconosVector& V){
  SiconosVector Vabs(V.size());
  for (int it = 0; it < V.size(); ++it){
    Vabs.setValue(it,std::abs(V.getValue(it)));
  };
  return Vabs;
}
//
void getMin(const SiconosVector& V, double& minvalue, unsigned int& idmin){
  minvalue = V.getValue(0);
  idmin = 0;
  for (unsigned int it = 1; it < V.size(); ++it){
    if (V.getValue(it) < minvalue){
      minvalue = V.getValue(it);
      idmin = it;
    };
  };
}
*/
void setBlock(const SiconosVector& vIn, SP::SiconosVector vOut, unsigned int sizeB,
              unsigned int startIn, unsigned int startOut)
{
  unsigned int endOut = startOut + sizeB;
  unsigned int numIn = vIn.num();
  unsigned int numOut = vOut->num();
  assert(vOut->size() >= endOut && "The output vector is too small");
  if (numIn == numOut)
  {
    if (numIn == 1) // vIn / vOut are Dense
      noalias(ublas::subrange(*vOut->dense(), startOut, endOut)) = ublas::subrange(*vIn.dense(), startIn, startIn + sizeB);
    else // if(numIn == 4)// vIn / vOut are Sparse
      noalias(ublas::subrange(*vOut->sparse(), startOut, endOut)) = ublas::subrange(*vIn.sparse(), startIn, startIn + sizeB);
  }
  else // vIn and vout of different types ...
  {
    if (numIn == 1) // vIn Dense
      noalias(ublas::subrange(*vOut->sparse(), startOut, endOut)) = ublas::subrange(*vIn.dense(), startIn, startIn + sizeB);
    else // if(numIn == 4)// vIn Sparse
      noalias(ublas::subrange(*vOut->dense(), startOut, endOut)) = ublas::subrange(*vIn.sparse(), startIn, startIn + sizeB);
  }
}

unsigned int SiconosVector::size(void) const
{
  if (!_dense)
  {
    return (vect.Sparse->size());
  }
  else
  {
    return (vect.Dense->size());
  }
}

SiconosVector& operator *= (SiconosVector& v, const double& s)
{
  if (v._dense)
    *v.dense() *= s;
  else
    *v.sparse() *= s;
  return v;
}

SiconosVector& operator /= (SiconosVector& v, const double& s)
{
  if (v._dense)
    *v.dense() /= s;
  else
    *v.sparse() /= s;
  return v;
}


/* Copyright (C) 2018-2019 Thomas Jespersen, TKJ Electronics. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the MIT License
 *
 * 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 MIT License for further details.
 *
 * Contact information
 * ------------------------------------------
 * Thomas Jespersen, TKJ Electronics
 * Web      :  http://www.tkjelectronics.dk
 * e-mail   :  thomasj@tkjelectronics.dk
 * ------------------------------------------
 */

#include "Path.h"

#include <string>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>

#include <cmath>
#include <algorithm>

#include <Eigen/SVD>

/* For visualization/plotting only */
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#define PATH_DEBUG  0

namespace MPC
{

    Polynomial::Polynomial()
    {

    }

    // Copy constructor
    Polynomial::Polynomial(const Polynomial& poly) : coeffs_(poly.coeffs_)
    {

    }

    Polynomial::~Polynomial()
    {

    }

    Polynomial::Polynomial(unsigned int order) : coeffs_(order+1, 0.0)
    {

    }

    /* Coefficients are stored such that c[0] is the coefficient for the lowest order element, such that:
     * y = c[n]*x^n + c[n-1]*x^(n-1) + ... c[2]*x^2 + c[1]*x + c[0]
     */
    Polynomial::Polynomial(std::vector<double> coeffs)
    {
        coeffs_ = coeffs;
    }

    Polynomial::Polynomial(const double * coeffs, int num_coeffs)
    {
        coeffs_.insert(coeffs_.end(), coeffs, coeffs + num_coeffs);
    }

    // Assignment operator
    Polynomial& Polynomial::operator=(const Polynomial& other)
    {
        coeffs_ = other.coeffs_;
        return *this;
    }

    Polynomial Polynomial::operator+(const Polynomial& other) const
    {
        Polynomial out;

        if (order() >= other.order()) {
            out.coeffs_ = coeffs_;
            for (size_t i = 0; i < other.coeffs_.size(); i++)
                out.coeffs_.at(i) += other.coeffs_.at(i);
        }
        else {
            out.coeffs_ = other.coeffs_;
            for (size_t i = 0; i < coeffs_.size(); i++)
                out.coeffs_.at(i) += coeffs_.at(i);
        }

        return out;
    }

    Polynomial Polynomial::operator-(const Polynomial& other) const
    {
        Polynomial out;

        if (order() >= other.order()) {
            out.coeffs_ = coeffs_;
            for (size_t i = 0; i < other.coeffs_.size(); i++)
                out.coeffs_.at(i) -= other.coeffs_.at(i);
        }
        else {
            out.coeffs_ = other.coeffs_;
            for (size_t i = 0; i < coeffs_.size(); i++)
                out.coeffs_.at(i) -= coeffs_.at(i);
        }

        return out;
    }

    Polynomial Polynomial::operator+(const double& offset) const
    {
        Polynomial out;
        if (coeffs_.size() == 0) return out;

        out.coeffs_ = coeffs_;
        out.coeffs_[0] += offset;
        return out;
    }

    Polynomial Polynomial::operator-(const double& offset) const
    {
        Polynomial out;
        if (coeffs_.size() == 0) return out;

        out.coeffs_ = coeffs_;
        out.coeffs_[0] -= offset;
        return out;
    }

    void Polynomial::print()
    {
        std::cout << "Polynomial:" << std::endl;
        std::cout << "   y = ";
        if (order() < 0) {
            std::cout << "0" << std::endl << std::endl;
            return;
        }

        for (int i = order(); i >= 1; i--) {
            if (i > 1)
                std::cout << coeffs_.at(i) << "*t^" << i << " + ";
            else
                std::cout << coeffs_.at(i) << "*t + ";
        }
        std::cout << coeffs_.at(0) << std::endl;
        std::cout << std::endl;
    }

    /* Coefficients are stored such that c[0] is the coefficient for the lowest order element, such that:
     * out = c[n]*t^n + c[n-1]*t^(n-1) + ... c[2]*t^2 + c[1]*t + c[0]
     */
    double Polynomial::evaluate(double t)
    {
        // Polynomial evaluation
        double pow_t = 1;
        double out = 0;
        for (unsigned int i = 0; i < coeffs_.size(); i++) {
            out += coeffs_[i] * pow_t;
            pow_t *= t;
        }

        return out;
    }

    double Polynomial::operator()(double t)
    {
        return evaluate(t);
    }

    std::vector<double> Polynomial::evaluate(std::vector<double> tVec)
    {
        std::vector<double> evaluatedPoints;

        // Polynomial evaluation
        for (auto& t : tVec) {
            double pow_t = 1;
            double out = 0;
            for (unsigned int i = 0; i < coeffs_.size(); i++) {
                pow_t *= t;
                out += coeffs_[i] * pow_t;
            }

            evaluatedPoints.push_back(out);
        }

        return evaluatedPoints;
    }

    std::vector<double> Polynomial::operator()(std::vector<double> tVec)
    {
        return evaluate(tVec);
    }

    int Polynomial::order() const
    {
        return (int)coeffs_.size() - 1;
    }

    Polynomial Polynomial::squared() const
    {
        // Given polynomial coefficients for:
        // f(x) = c_n*x^n + c_n-1*x^(n-1) + ... c_1*x + c_0
        // This function computes the coefficients of the polynomial f^2(x)
        // Coefficients are ordered such that coeffs[0] = c_0  (lowest order)

        // f^2(x) = sum(sum(c_i * c_j * x^(i+j))
        // The resulting coefficients, coeff_k = c_i*c_j | i+j=k
        int n = order(); // input polynomial order
        int m = 2*n; // squared (output) polynomial order

        if (m < 0) {
            Polynomial empty;
            return empty;
        }

        Polynomial squared(m);
        for (int k = 0; k <= m; k++) {
            for (int i = 0; i <= n; i++) {
                for (int j = 0; j <= n; j++) {
                    if (i + j == k) {
                        squared.coeffs_.at(k) = squared.coeffs_.at(k) + coeffs_.at(i) * coeffs_.at(j);
                    }
                }
            }
        }
        return squared;
    }

    Polynomial Polynomial::derivative() const
    {
        // Given polynomial coefficients for:
        // f(x) = c_n*x^n + c_n-1*x^(n-1) + ... c_1*x + c_0
        // This function computes the coefficients of the polynomial f'(x)
        // f'(x) = df/dx = n*c_n*x^(n-1) + (n-1)*c_n-1*x^(n-2) + ... +
        // f'(x) = df/dx = n*c_n*x^(n-1) + (n-1)*c_n-1*x^(n-2) + ... + 2*c_2*x + 1*c_1
        // Coefficients are ordered such that coeff[0] = c_0  (lowest order)
        int n = order();

        if (n < 0) {
            Polynomial empty;
            return empty;
        }

        Polynomial derivative(n-1);
        for (int i = 0; i <= (n-1); i++) {
            derivative.coeffs_.at(i) = (i+1) * coeffs_.at(i+1);
        }
        return derivative;
    }

    double Polynomial::getCoefficient(unsigned int index)
    {
        if (int(index) > order())
            return 0;
        else
            return coeffs_.at(index);
    }

    double Polynomial::findMinimum(double s_init, double s_lower, double s_upper, double stoppingCriteria, unsigned int maxIterations)
    {
        // Find the minimum by Newton minimization
        if (order() < 0) return 0;

        Polynomial dPoly = derivative();
        Polynomial ddPoly = dPoly.derivative();

        // Newton's method - https://en.wikipedia.org/wiki/Newton%27s_method_in_optimization
        bool converged = false;
        unsigned int iterations = 0;
        double s = s_init;
        double delta_s;

        while (!converged && iterations < maxIterations) {
            double FirstDerivative = dPoly.evaluate(s); // first derivative, for higher dimensions it would be the Gradient
            double SecondDerivative = ddPoly.evaluate(s); // second derivative, for higher dimensions it would be the Hessian

            delta_s = -FirstDerivative / SecondDerivative;
            s = s + delta_s;

            if (s < s_lower)
                s = s_lower;

            if (s > s_upper)
                s = s_upper;

            if (std::abs(delta_s) < stoppingCriteria)
                converged = true;

            iterations = iterations + 1;
        }

        return s;
    }

    void Polynomial::FitPoints(unsigned int order, std::vector<double>& tVec, std::vector<double>& values, bool EnforceBeginEndConstraint, bool EnforceBeginEndAngleConstraint)
    {
        if (order < 1) {
            std::cout << "Polynomial order needs to be at least 1" << std::endl; throw;
        }
        else if (order < 3 && EnforceBeginEndConstraint && EnforceBeginEndAngleConstraint) {
            std::cout << "Polynomial order too low to enforce both type of constraints" << std::endl; throw;
        }
        if (tVec.size() != values.size()) {
            std::cout << "Number of evaluation points (tVec) and corresponding values needs to be consistent" << std::endl; throw;
        }

        unsigned int n = tVec.size(); // should be the same length as values

        Eigen::VectorXd t = Eigen::VectorXd::Map(tVec.data(), tVec.size());
        Eigen::VectorXd tPow = Eigen::VectorXd::Ones(n, 1);

        Eigen::MatrixXd A(n, order+1);
        // Fill the matrix using the evaluation points, t
        for (unsigned int i = 0; i <= order; i++) {
            A.block(0, i, n, 1) = tPow;
            tPow = tPow.cwiseProduct(t); // tPow = tPow .* tVec
        }

        Eigen::VectorXd b = Eigen::VectorXd::Map(values.data(), values.size());

        // Create matrix and vector of equality constraints
        Eigen::MatrixXd Aeq;
        Eigen::VectorXd beq;
        if (EnforceBeginEndConstraint || EnforceBeginEndAngleConstraint) { // compute the constraint matrices
            Aeq.resize(4, order+1); // OBS. Note that resizing does not initialize values!
            beq.resize(4, 1);

            // First two rows enforces begin and end constraints
            Aeq(0,0) = 1;
            Aeq(1,0) = 1;
            // Last two rows enforces begin and end angle (derivative) constraints
            Aeq(2,0) = 0;
            Aeq(3,0) = 0;

            for (unsigned int j = 1; j <= order; j++) {
                Aeq(0, j) = Aeq(0, j-1) * t(0);
                Aeq(1, j) = Aeq(1, j-1) * t(n-1);
                Aeq(2, j) = Aeq(0, j-1) * j;
                Aeq(3, j) = Aeq(1, j-1) * j;
            }

            beq(0) = b(0); // first value
            beq(1) = b(n-1); // last value
            beq(2) = (b(1)-b(0)) / (t(1)-t(0)); // begin angle
            beq(3) = (b(n-1)-b(n-2)) / (t(n-1)-t(n-2)); // end angle
        }

#if PATH_DEBUG
        std::cout << "Aeq = " << std::endl << Aeq << std::endl;
        std::cout << "beq = " << std::endl << beq << std::endl;
#endif

        if (order < 3) { // can only enforce one type of the constraints
            // limit constraints due to reduced order such that we only require start and end point to be fulfilled
            if (EnforceBeginEndConstraint) {
                // Extract the first two rows
                Aeq = Aeq.block(0, 0, 2, order+1);
                beq = beq.block(0, 0, 2, 2);
            } else if (EnforceBeginEndAngleConstraint) {
                // Extract the last two rows
                Aeq = Aeq.block(2, 0, 2, order+1);
                beq = beq.block(2, 0, 2, 2);
            }
        }

        Eigen::VectorXd polyCoeffs;
        if (EnforceBeginEndConstraint || EnforceBeginEndAngleConstraint)
            polyCoeffs = ConstrainedLeastSquares(A, b, Aeq, beq, n*10000);
        else
            polyCoeffs = ConstrainedLeastSquares(A, b, Eigen::MatrixXd(), Eigen::VectorXd(), 1);

#if PATH_DEBUG
        std::cout << "polyCoeffs = " << std::endl << polyCoeffs << std::endl;
#endif

        coeffs_.clear();
        coeffs_.insert(coeffs_.begin(), polyCoeffs.data(), polyCoeffs.data() + polyCoeffs.rows());
    }

    Eigen::VectorXd Polynomial::ConstrainedLeastSquares(const Eigen::MatrixXd& A, const Eigen::VectorXd& b, const Eigen::MatrixXd& Aeq, const Eigen::VectorXd& beq, double lambda)
    {
        Eigen::MatrixXd A_;
        Eigen::VectorXd b_;

        if (Aeq.rows() == beq.rows() && Aeq.cols() == A.cols()) {
            A_.resize(A.rows() + Aeq.rows(), A.cols()); // OBS. Note that resizing does not initialize values!
            b_.resize(b.rows() + beq.rows());
            A_ << A,
                  Aeq*lambda;
            b_ << b,
                  beq*lambda;
        } else {
            A_ = A;
            b_ = b;
        }

#if PATH_DEBUG
        std::cout << "A_ = " << std::endl << A_ << std::endl;
#endif

        // Pseudo-inverse through numerically unstable way
        //   A_invpseudo = inv(A_'*A_) * A_'
        // Pseudo-inverse through MATLAB
        //   A_invpseudo2 = pinv(A_)  % Moore-Penrose Pseudoinverse of matrix of A
        // Pseudo-inverse through SVD
        //   [U,S,V] = svd(A_);
        //   Sinv = [diag(1./diag(S)), zeros(size(S,2), size(S,1)-size(S,2))];
        //   A_invpseudo3 = V * Sinv * U';
        //   x = A_invpseudo3 * b_;

        Eigen::JacobiSVD<Eigen::MatrixXd> svd( A_, Eigen::ComputeFullV | Eigen::ComputeFullU );

        // create a matrix of just zeros and fill top part with inverse singular values in a diagonal matrix
        Eigen::MatrixXd Sinv = Eigen::MatrixXd::Zero(svd.singularValues().rows(), A_.rows());
        Sinv.block(0, 0, svd.singularValues().rows(), svd.singularValues().rows()) = svd.singularValues().asDiagonal().inverse().toDenseMatrix();
#if PATH_DEBUG
        std::cout << "Sinv = " << std::endl << Sinv << std::endl;
#endif

        Eigen::MatrixXd A_PseudoInverse = svd.matrixV() * Sinv * svd.matrixU().transpose();
#if PATH_DEBUG
        std::cout << "A_PseudoInverse = " << std::endl << A_PseudoInverse << std::endl;
#endif

        // Now get the solution (least squares minimization) by using the pseudo-inverse
        Eigen::VectorXd x = A_PseudoInverse * b_;
#if PATH_DEBUG
        std::cout << "x = " << std::endl << x << std::endl;
#endif

        return x;
    }

    Path::Path() : computed_dsquared_(false), s_end_(0)
    {

    }

    Path::Path(Polynomial& poly_x, Polynomial& poly_y, double s_end) : poly_x_(poly_x), poly_y_(poly_y), s_end_(s_end), computed_dsquared_(false)
    {

    }

    Path::Path(Trajectory& trajectory, unsigned int approximationOrder, bool StopAtEnd, bool EnforceBeginEndConstraint, bool EnforceBeginEndAngleConstraint) : computed_dsquared_(false)
    {
        FitTrajectory(trajectory, approximationOrder, StopAtEnd, EnforceBeginEndConstraint, EnforceBeginEndAngleConstraint);
    }

    Path::~Path()
    {

    }

    // Assignment operator
    Path& Path::operator=(const Path& other)
    {
        poly_x_ = other.poly_x_;
        poly_y_ = other.poly_y_;
        s_end_ = other.s_end_;
        computed_dsquared_ = other.computed_dsquared_;
        dsquared_ = other.dsquared_;
        return *this;
    }

    Eigen::Vector2d Path::get(double s)
    {
        return Eigen::Vector2d(poly_x_.evaluate(s), poly_y_.evaluate(s));
    }

    Eigen::Vector2d Path::operator()(double s)
    {
        return get(s);
    }

    std::vector<Eigen::Vector2d> Path::get(std::vector<double> sVec)
    {
        std::vector<Eigen::Vector2d> evaluatedPoints;

        // Polynomial evaluation
        for (auto& s : sVec) {
            evaluatedPoints.push_back(Eigen::Vector2d(poly_x_.evaluate(s), poly_y_.evaluate(s)));
        }

        return evaluatedPoints;
    }

    std::vector<Eigen::Vector2d> Path::operator()(std::vector<double> sVec)
    {
        return get(sVec);
    }

    int Path::order()
    {
        if (poly_x_.order() >= poly_y_.order())
            return poly_x_.order();
        else
            return poly_y_.order();
    }

    double Path::getXcoefficient(unsigned int index)
    {
        return poly_x_.getCoefficient(index);
    }

    double Path::getYcoefficient(unsigned int index)
    {
        return poly_y_.getCoefficient(index);
    }

    double Path::ArcCurveLength(double t)
    {
        // Arc curve length of a polynomial path whose position is defined as:
        //   p(t) = [x(t), y(t)]
        // Is given by the integral:
        // integral( sqrt( dxdt^2 + dydt^2 ) ) dt
        // This can however be approximated by using the velocity polynomial
        //   Q = sqrt( (df_x/dx)^2 + (df_dy)^2 );

        // First we compute the coefficients of the inner polynomial, f
        //   f = (df_x/dx)^2 + (df_dy)^2
        if (!computed_dsquared_) {
            Polynomial dx = poly_x_.derivative(); // taking the difference of a polynomial, moves the coefficients
            Polynomial dy = poly_y_.derivative();
            Polynomial dx_squared = dx.squared();
            Polynomial dy_squared = dy.squared();
            dsquared_ = dx_squared + dy_squared;
            computed_dsquared_ = true;
        }
        // Such that
        // Q = sqrt(EvaluatePolynomial(f_coeff, t))

        // Return the approximated arc curve length at point t - hence curve length from f(0) to f(t)
        // Based on http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.5.2912&rep=rep1&type=pdf
        // The approximation (see paper above) is given by
        // s(t) = t/2 * (5/9 * Q(1.774597*t/2) + 8/9 * Q(t/2) + 5/9 * Q(0.225403*t/2))
        double s = t/2.0 * (
                            5.0/9.0 * sqrt(dsquared_.evaluate(1.774597*t/2.0)) +
                            8.0/9.0 * sqrt(dsquared_.evaluate(t/2.0)) +
                            5.0/9.0 * sqrt(dsquared_.evaluate(0.225403*t/2.0))
                           );
        return s;
    }

    double Path::ApproximateArcCurveLength(double t, double discretizationStepSize)
    {
        double distance = 0;
        double t_current = 0;

        double x_prev = poly_x_.evaluate(t_current);
        double y_prev = poly_y_.evaluate(t_current);
        double x, y;
        double x_diff, y_diff;

        while (t_current < t) {
            t_current += discretizationStepSize;
            x = poly_x_.evaluate(t_current);
            y = poly_y_.evaluate(t_current);

            x_diff = x - x_prev;
            y_diff = y - y_prev;

            distance += sqrtf(x_diff*x_diff + y_diff*y_diff); // Euclidean distance

            x_prev = x;
            y_prev = y;
        }

        return distance;
    }

    void Path::plot(bool drawXup, double x_min, double y_min, double x_max, double y_max)
    {
        double aspect_ratio = (x_max - x_min) / (y_max - y_min);
        double xres, yres;
        if (aspect_ratio >= 1) {
            yres = 500;
            xres = yres * aspect_ratio;
        } else {
            xres = 500;
            yres = xres / aspect_ratio;
        }

        // Create black empty images
        cv::Mat image;
        if (drawXup)
            image = cv::Mat( xres, yres, CV_8UC3, cv::Scalar( 255, 255, 255 ) );
        else
            image = cv::Mat( yres, xres, CV_8UC3, cv::Scalar( 255, 255, 255 ) );

        // Scale range (x_min:x_max) and (y_min:y_max) to (0:499)
        double scale_x = xres / (x_max - x_min);
        double scale_y = yres / (y_max - y_min);
        double center_x = (x_min + x_max) / 2.0;
        double center_y = (y_min + y_max) / 2.0;

        if (s_end_ <= 0) return; // can not plot if length is unknown
        double s_spacing = s_end_ / 99.0;

        Eigen::Vector2d p_prev = get(0);
        Eigen::Vector2d p;

        for (unsigned int s_idx = 0; s_idx < 100; s_idx++) {
            double s = s_spacing * s_idx;
            p = get(s);

            float x = (p[0]-x_min) * scale_x;
            float y = (p[1]-y_min) * scale_y;

            if (x >= 0 && x < xres && y >= 0 && y < yres) {
                cv::Point point;
                if (drawXup) // draw with robot x-axis pointing up in plot
                    point = cv::Point(yres-y,xres-x);
                else
                    point = cv::Point(x,yres-y);

                // Draw a line
                //cv::line(image, cv::Point(p_prev[0], p_prev[1]), point, cv::Scalar( 0, 0, 0 ), 1, 8 );
                cv::drawMarker(image, point, cv::Scalar( 255, 0, 0 ), cv::MARKER_CROSS, 3, 1, 8 );

                p_prev = p;
            }
        }

        cv::imshow("Path", image);

        cv::waitKey( 5 );
    }

    void Path::plot(cv::Mat& image, cv::Scalar color, bool drawXup, double x_min, double y_min, double x_max, double y_max)
    {
        double xres = image.cols;
        double yres = image.rows;

        cv::line(image, cv::Point(0, yres/2), cv::Point(xres-1, yres/2), cv::Scalar(128,128,128), 1, 8, 0);
        cv::line(image, cv::Point(xres/2, 0), cv::Point(xres/2, yres-1), cv::Scalar(128,128,128), 1, 8, 0);

        // Scale range (x_min:x_max) and (y_min:y_max) to (0:499)
        double scale_x = xres / (x_max - x_min);
        double scale_y = yres / (y_max - y_min);
        double center_x = (x_min + x_max) / 2.0;
        double center_y = (y_min + y_max) / 2.0;

        if (s_end_ <= 0) return; // can not plot if length is unknown
        double s_spacing = s_end_ / 99.0;

        Eigen::Vector2d p_prev = get(0);
        Eigen::Vector2d p;

        for (unsigned int s_idx = 0; s_idx < 100; s_idx++) {
            double s = s_spacing * s_idx;
            p = get(s);

            float x = (p[0]-x_min) * scale_x;
            float y = (p[1]-y_min) * scale_y;

            if (x >= 0 && x < xres && y >= 0 && y < yres) {
                cv::Point point;
                if (drawXup) // draw with robot x-axis pointing up in plot
                    point = cv::Point(yres-y,xres-x);
                else
                    point = cv::Point(x,yres-y);

                // Draw a line
                //cv::line(image, cv::Point(p_prev[0], p_prev[1]), point, cv::Scalar( 0, 0, 0 ), 1, 8 );
                cv::drawMarker(image, point, color, cv::MARKER_CROSS, 3, 1, 8 );

                p_prev = p;
            }
        }
    }

    void Path::PlotPoint(double sValue, cv::Mat& image, cv::Scalar color, bool drawXup, double x_min, double y_min, double x_max, double y_max)
    {
        double xres = image.cols;
        double yres = image.rows;
        Eigen::Vector2d p = get(sValue);

        // Scale range (x_min:x_max) and (y_min:y_max) to (0:499)
        double scale_x = xres / (x_max - x_min);
        double scale_y = yres / (y_max - y_min);
        double center_x = (x_min + x_max) / 2.0;
        double center_y = (y_min + y_max) / 2.0;

        float x = (p[0]-x_min) * scale_x;
        float y = (p[1]-y_min) * scale_y;

        if (x >= 0 && x < xres && y >= 0 && y < yres) {
            cv::Point point;
            if (drawXup) // draw with robot x-axis pointing up in plot
                point = cv::Point(yres-y,xres-x);
            else
                point = cv::Point(x,yres-y);

            cv::drawMarker(image, point, color, cv::MARKER_STAR, 6, 2, 8);
        }
    }

    void Path::FitTrajectory(Trajectory& trajectory, unsigned int approximationOrder, bool StopAtEnd, bool EnforceBeginEndConstraint, bool EnforceBeginEndAngleConstraint)
    {
        // approximationOrder is for x(t) and y(t) fitting
        unsigned order_t2s = approximationOrder + 1; // order_t2s is for t(s) fitting
        unsigned order_f2s = approximationOrder + 1; // order_f2s is the order for the final fitted path: x(s) and y(s)

        std::vector<double> tVec = trajectory.GetDistanceList(); // get approximated distance vector for the individual points in the trajectory
        std::vector<double> xValues = trajectory.GetX();
        std::vector<double> yValues = trajectory.GetY();

        if (tVec.size() <= order_t2s) { // we do not have sufficient points for fitting - therefore just do a path that corresponds to holding a static position (end point)
            s_end_ = 99;
            poly_x_ = Polynomial({trajectory.back().point[0], 0.001});
            poly_y_ = Polynomial({trajectory.back().point[1], 0.001});
            return;
        }

        /* Fit window points to two polynomials, x(t) and y(t), using parameters t starting with t0=0 and spaced with the distance between each point (chordal parameterization) */
        Polynomial xt, yt;
        xt.FitPoints(approximationOrder, tVec, xValues, EnforceBeginEndConstraint, EnforceBeginEndAngleConstraint);
        yt.FitPoints(approximationOrder, tVec, yValues, EnforceBeginEndConstraint, EnforceBeginEndAngleConstraint);

        // Create temporary path holding the x(t) and y(t) approximation
        Path tmpPath(xt, yt);

        /* Compute numerical approximation of arc length at the points, t, using the fitted polynomial */
        // Given the now approximated x(t), y(t) path, we approximate the arc curve length for the different points
        // Hence to be able to create a mapping between s to t : t(s)
        std::vector<double> sVec;
        for (double& t : tVec) {
            sVec.push_back(tmpPath.ArcCurveLength(t));
        }

        double sTotal = sVec.back();

        /* Fit t(s) polynomial */
        Polynomial ts;
        ts.FitPoints(order_t2s, sVec, tVec, true, false);

        /* Create vector of evenly spaced distances, s, and get corresponding t values */
        // Make 100 linearly (evenly) seperated distance points
        double spacing = sTotal / 99;
        std::vector<double> sEven;
        std::vector<double> tEven;
        for (int i = 0; i < 100; i++) {
            double s = i * spacing;
            sEven.push_back(s);
            tEven.push_back(ts.evaluate(s));
        }

        /* Use vector of the corresponding t values (matching the evenly spaced s distances) to get corresponding x-y value pairs from the initial approximation */
        std::vector<double> xEven;
        std::vector<double> yEven;
        for (double& t : tEven) {
            xEven.push_back(xt.evaluate(t));
            yEven.push_back(yt.evaluate(t));
        }

        /* Hack to make the fitted trajectory keep the same position after reaching the distance value */
        /*if (StopAtEnd) {
            for (int i = 1; i < 100; i++) {
                double s = sTotal + i * spacing;
                sEven.push_back(s);
                xEven.push_back(xEven.back() + spacing/1000);
                yEven.push_back(yEven.back());
            }
        }*/

        /* Use the s to x-y pairs to create final approximation: x(s) and y(s) */
        // Fit two new polynomials on the new generated points, x_0,...,x_n  and y_0,...,y_n  using the evenly spaced distance parameters, s_0,...,s_n, as the parameter
        poly_x_.FitPoints(order_f2s, sEven, xEven, EnforceBeginEndConstraint, EnforceBeginEndAngleConstraint);
        poly_y_.FitPoints(order_f2s, sEven, yEven, EnforceBeginEndConstraint, EnforceBeginEndAngleConstraint);
        s_end_ = sEven.back(); // sTotal
    }

    void Path::print()
    {
        std::cout << "x(s) ";
        poly_x_.print();
        std::cout << "y(s) ";
        poly_y_.print();
    }

    double Path::length()
    {
        return s_end_;
    }

    double Path::FindClosestPoint(const Eigen::Vector2d& position)
    {
        // Find point on path being closest to the input position and return the corresponding path parameter (s-value) at this point
        // First we create a centered path around the input position, by taking the two polynomial and subtracting the position value
        Polynomial xs_centered = poly_x_ - position[0];
        Polynomial ys_centered = poly_y_ - position[1];
        // Given this centered polynomial, the distance to any point along the original path is defined by the function:
        //   dist(s) = sqrt( f_x(s)^2 + f_y(s)^2 )
        // Since this is the function we want to minimize we create this function as a distance polynomial such that we can take the derivative and find the minimum point
        Polynomial xs_squared = xs_centered.squared();
        Polynomial ys_squared = ys_centered.squared();
        // And since we want to find the closest distance, we can also just
        // minimize the squared distance: dist(s)^2
        //   dist(s)^2 = f_x(s)^2 + f_y(s)^2
        Polynomial dist_squared = xs_squared + ys_squared;

        // Finally we find the minimum distance, s, through a Newton-based minimization
        return dist_squared.findMinimum(0, 0, s_end_, 0.001, 100);
    }

}


/*

RPBA - Robust Parallel Bundle Adjustment

File rpbacore.cpp

Description: Parallel bundle adjustment core

Copyright 2019 Helmut Mayer, Bundeswehr University Munich, Germany, Helmut.Mayer@unibw.de

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

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

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

*/

#include "rpba.h"
#include "system.h"
#include <boost/math/special_functions/round.hpp>

void CalcPLSA(Eigen::MatrixXf &XX,
		std::vector<Eigen::MatrixXf> &xg,
		const std::vector<std::vector<float> > &wx, const std::vector<std::vector<float> > &wy,
		const std::vector<std::vector<float> > &wxy,
		std::vector<std::vector<IMAPNR> > &pointXXvio,
		std::vector<PMat> &PMatrices,
		std::vector<Camera> &cameras,
		const std::vector<int> &iwidth, const std::vector<int> &iheight,
		const bool robustflag,
		const std::vector<bool> &addflagsin,
		std::ostream& os, const int mt,
		const bool extevalflag) {

	std::vector<bool> addflags,addflagsold;
	addflags = addflagsin;

	int nraddp = 0;
	for (int i = 0; i < (int) addflags.size(); ++i)
		if (addflags[i])
			++nraddp;

	addflagsold = addflags;

	Eigen::Matrix3f Ksi;

	std::vector<std::vector<IMAPNR> > pointXXv,pointXXvold;
	pointXXv = pointXXvio;

	const int pxs = (int) pointXXv.size();
	int nrimages = (int) xg.size();

	bool gcpflag = false;

	Eigen::MatrixXd XXd(4,pxs), XXdold(4,pxs), XXdt(4,pxs), XXt(4,pxs);
	std::vector<Eigen::MatrixXd> Zs; // XXs;

	std::vector<int> imgind, imgindbv(nrimages);
	std::vector<Camera> cameraps(nrimages);
	std::vector<int> gcpinfo;
	std::vector<std::vector<float> > W3iji(6);
	std::vector<PMatd> PMatricesd(nrimages),PMatricesdold(nrimages),PMatricesdt(nrimages);
	std::vector<std::vector<double> > gcpdata,gcpdatait;

	for (int i = 0; i < nrimages; ++i) {
		cameraps[i] = cameras[i];
		PMatricesd[i] = PMatrices[i].cast <double> ();
		PMatricesdold[i] = PMatrices[i].cast <double> ();
	}

	XXd = XX.cast <double> ();

	int minnrimages = 70;
	const int minnrthreads = boost::math::iround((float) nrimages / (float) minnrimages);

	int nrthreads = mt;

	if (minnrthreads < nrthreads)
		nrthreads = minnrthreads;

	float ihw2 = 0.f;
	for(int i = 0; i < nrimages; ++i)
		ihw2 += (float) ((iheight[i] + iwidth[i]) * (iheight[i] + iwidth[i])) * 0.25f;
	ihw2 /= (float) nrimages;

	bool parflag = nrthreads > 1 && nrimages > 5;

	if (extevalflag) {
		EvalLSA(nraddp, cameraps, XXd, xg, wx, wy, wxy, pointXXv, PMatricesd, ihw2);
		return;
	}

	if (parflag) {

		std::vector<float> xiout;
		Eigen::MatrixXf wap;
		std::vector<float> sig0vio;
		double sig0g = 0.;
		long redundancy = 0;

		gcpflag = true;

		std::cout << "#threads: " << nrthreads << " " << minnrthreads << '\n';

		std::vector<std::vector<bool> > pitflagsv(pxs);
		std::vector<LSA*> LSAv(nrthreads);
		std::vector<Eigen::MatrixXd> XXdd(nrthreads);
		std::vector<std::vector<PMatd> > PMatricesdd(nrthreads);
		std::vector<std::vector<Camera> > camerasoit(nrthreads);
		std::vector<std::vector<std::vector<IMAPNR> > > pointXXvv(nrthreads);
		std::vector<IMAPNR> pxvv;
		std::vector<std::vector<IMAPNR> > pxvvv(nrthreads);
		std::vector<int> nriv(nrthreads+1);
		std::vector<std::vector<int> > imgindv(nrthreads),gcpinfov(nrthreads),pindv(nrthreads);
		std::vector<std::vector<std::vector<std::vector<float> > > > w3dvivv(nrthreads);
		std::vector<std::vector<std::vector<float> > > sig0vvv(nrthreads);
		std::vector<std::vector<std::vector<float> > > W3ijiv(nrthreads);
		std::vector<std::vector<float> > sig0vvs(nrimages);
		std::vector<long> redundancyv(nrthreads);
		std::vector<float> sig0wv(nrimages);
		std::vector<double> sig0gv(nrthreads);
		std::vector<std::vector<std::vector<double> > > gcpdatav;
		std::vector<int> beginv(nrthreads),endv(nrthreads);
		std::vector<Camera> camerasnew(nrimages),camerasold(nrimages);

		std::vector<std::vector<int> > imagesperpart;
		std::vector<idx_t> part(nrimages);

		const System::Time startp = System::getTickCount();
		imagesperpart.resize(nrthreads);
		PartitionpointXXv(pointXXv, nrimages, nrthreads, part, imagesperpart);
		std::cout << "\n\nPartitioning runtime: " << (float) (System::getTickCount()-startp) / 1000.f << "s\n\n";

		nriv[0] = 0;
		for (int it = 0; it < nrthreads; ++it) {
			nriv[it+1] = nriv[it] + (int) imagesperpart[it].size();
			w3dvivv[it].resize(6);
		}
		nriv[nrthreads] = nrimages;

		std::vector<int> imageind(nrimages);

		int itj = -1;
		for (int it = 0; it < nrthreads; ++it)
			for (int j = 0; j < (int) imagesperpart[it].size(); ++j)
				imageind[++itj] = imagesperpart[it][j];

		for (int it = 0; it < nrthreads; ++it) {
			camerasoit[it] = cameras;
			PMatricesdd[it] = std::vector<PMatd>(nrimages);
			for (int i = 0; i < nrimages; ++i) {
				PMatricesdd[it][i] = PMatrices[i].cast <double> ();
			} // for i
			imgindv[it].resize(nriv[it+1] - nriv[it]);
			std::cout << "Thread: " << it << " #images " << nriv[it+1] - nriv[it] << " indices " << nriv[it] << ' ' << nriv[it+1] << '\n';
			for (int i = 0; i < nriv[it+1] - nriv[it]; ++i) {
				imgindv[it][i] = imageind[i + nriv[it]];
				imgindbv[imageind[i + nriv[it]]] = it;
			}
			std::sort(imgindv[it].begin(),imgindv[it].end());
			sig0vvv[it].resize(nrimages);
		} // for it

		gcpinfo.resize(pxs);
		std::vector<int>::iterator pxi;
		for (pxi = gcpinfo.begin(); pxi != gcpinfo.end(); ++pxi)
			(*pxi) = -1;

		sig0g = 0.;
		redundancy = 0;
		int nrparimages = (nrimages - 2) * 6 + 5 + nrimages * nraddp;

		int nrobs;
		double sig0opt = 1.e20;

		int iflag = 1, iter = 1;
		if (robustflag)
			iflag = 2;
		bool robustflagit;
		bool iniflag = true;

		gcpflag = true;

		for (int i = 0; i < pxs; ++i) {
			if ((int) pointXXv[i].size() > 0) {
				pitflagsv[i].resize(nrthreads);
				for (int j = 0; j < nrthreads; ++j)
					pitflagsv[i][j] = false;
				for (int j = 0; j < (int) pointXXv[i].size(); ++j)
					pitflagsv[i][imgindbv[pointXXv[i][j].image]] = true;
			} // if pointXXv
		} // for i

		gcpdatav.resize(nrthreads);

		float sig0frac = 1.01f;

		while (iflag > 0 && iter < 51) {
			robustflagit = false;

			if (iter > 1) {
				if (iter < 3)
					addflags[0] = addflags[1] = addflags[2] = false;
				else
					addflags = addflagsold;

				std::vector<std::ostringstream> osv(nrthreads);

				#pragma omp parallel for num_threads(nrthreads)
				for (int it = 0; it < nrthreads; ++it) {
					if (iter == 2)
						LSAv[it] = new LSA( camerasoit[it], addflags,
								true, imgindv[it], PMatricesdd[it],
								pointXXvv[it], XXdd[it], (int)pindv[it].size(), xg,
								wx, wy, wxy, W3iji,
								true, gcpinfov[it], gcpdatait,
								ihw2, false);
					else
						LSAv[it]->InputLSAP(addflags, camerasoit[it], imgindv[it], pointXXvv[it], PMatricesdd[it], XXdd[it], (int) pointXXvv[it].size(),
								gcpinfov[it]);

					LSAv[it]->Adjust(false, osv[it], true, 0, W3ijiv[it], true, gcpdatav[it]);

					LSAv[it]->OutputLSAP(PMatricesdd[it], pointXXvv[it], XXdd[it], gcpinfov[it], camerasoit[it]);
				} // for it

				for (int it = 0; it < nrthreads; ++it) {
					os << osv[it].str();
					for (int i = 0; i < (int) imgindv[it].size(); ++i) {
						PMatricesd[imgindv[it][i]] = PMatricesdd[it][imgindv[it][i]];
						const int iii = imgindv[it][i];
						cameraps[iii] = camerasoit[it][imgindv[it][i]];
						camerasnew[iii] = camerasoit[it][imgindv[it][i]];
					} // for i
				}
			} // it iter > 1

			if (iflag == 1) {
				robustflagit = robustflag;
				if (robustflag)
					sig0frac = 1.02f;
			}

			const int nrppt = pxs / nrthreads;
			if (iniflag) {
				for (int itt = 0; itt < nrthreads; ++itt) {
					beginv[itt] = nrppt * itt;
					endv[itt] = nrppt * (itt + 1);
					if (itt == nrthreads - 1)
						endv[itt] = pxs;

					const int eb = endv[itt] - beginv[itt];
					for (int vv = 0; vv < 6; ++vv) {
						w3dvivv[itt][vv].resize(eb);
						for (int ebb = 0; ebb < eb; ++ebb)
							w3dvivv[itt][vv][ebb].resize(nrthreads);
					}
					W3ijiv[itt].resize(6);

				} // for itt
			} // if iniflag

			if (iter > 1) {
				for (int i = 0; i < pxs; ++i)
					pointXXv[i].resize(0);

				for (int it= 0; it < nrthreads; ++it)
					for (int i = 0; i < (int) pindv[it].size(); ++i)
						for (size_t j = 0; j < pointXXvv[it][i].size(); ++j)
							pointXXv[pindv[it][i]].push_back(pointXXvv[it][i][j]);

				for (int i = 0; i < pxs; ++i)
					if ((int) pointXXv[i].size() < 2)
						pointXXv[i].clear();
			}
			else {
				for (int it = 0; it < nrthreads; ++it) {
					for (int i = 0; i < (int) imgindv[it].size(); ++i) {
						const int iii = imgindv[it][i];
						cameraps[iii] = cameras[iii];
						camerasnew[iii] =  cameras[iii];
					} // for i
				} // for it
			} // else

			for (int itt = 0; itt < nrthreads; ++itt)
				for (int i = 0; i < 6; ++i)
					W3ijiv[itt][i].resize(0);
			for (int i = 0; i < pxs; ++i) {
				if ((int) pointXXv[i].size() > 0) {
					for (int it = 0; it < nrthreads; ++it)
						pitflagsv[i][it] = false;
					for (int j = 0; j < (int) pointXXv[i].size(); ++j)
						pitflagsv[i][imgindbv[pointXXv[i][j].image]] = true;
				} // if pointXXv
			} // for i

			if (robustflagit) {
				#pragma omp parallel for num_threads(nrthreads)
				for (int itt = 0; itt < nrthreads; ++itt) {
					for (int iii = 0; iii < nrimages; ++iii)
						sig0vvv[itt][iii].resize(0);
					sig0gv[itt] = 0.;
					redundancyv[itt] = 0;
					for (int i = beginv[itt], ii = 0; i < endv[itt]; ++i, ++ii) {
						if ((int) pointXXv[i].size() > 0) {
							CalcLSAXMulti(cameraps,i,XXd,xg,wx,wy,wxy,pointXXv,PMatricesd,
									imgindbv, nrthreads, pitflagsv[i],
									w3dvivv[itt][0][ii],w3dvivv[itt][1][ii],w3dvivv[itt][2][ii],
									w3dvivv[itt][3][ii],w3dvivv[itt][4][ii],w3dvivv[itt][5][ii],
									sig0gv[itt],redundancyv[itt],sig0vvv[itt],sig0wv,1,false,false);
						} // if pointXX
					} // for i
				} // for itt

				sig0g = 0.;
				redundancy = 0;
				for (int itt = 0; itt < nrthreads; ++itt) {
					sig0g += sig0gv[itt];
					redundancy += redundancyv[itt];
				}

				for (int i = 0; i < nrimages; ++i)
					sig0vvs[i].resize(0);

				for (int itt = 0; itt < nrthreads; ++itt)
					for (int i = 0; i < nrimages; ++i)
						sig0vvs[i].insert(sig0vvs[i].begin(),sig0vvv[itt][i].begin(),sig0vvv[itt][i].end());
				for (int i = 0; i < nrimages; ++i) {
					if (sig0vvs[i].size() == 0) {
						std::cout << "Image " << i << " without observations!!!!!!!!!!!!!!!!!!!!!\n\n";
						sig0wv[i] = 0.f;
					}
					else {
						std::sort(sig0vvs[i].begin(),sig0vvs[i].end());
						sig0wv[i] = 1.f / (sig0vvs[i][(int) sig0vvs[i].size() / 2] * 2.1981f); // 1.4826 // 1.706f - 1.306^2
					}
				} // for i
			} // if robustflagit

			int robustmode = 0;
			if (robustflagit)
				robustmode = 2;

			bool weightonlyflag = false;
			if (iter == 1)
				weightonlyflag = true;

			#pragma omp parallel for num_threads(nrthreads)
			for (int itt = 0; itt < nrthreads; ++itt) {
				sig0gv[itt] = 0.;
				redundancyv[itt] = 0;
				for (int i = beginv[itt], ii = 0; i < endv[itt]; ++i, ++ii) {
					if ((int) pointXXv[i].size() > 0) {
						CalcLSAXMulti(cameraps,i,XXd,xg,wx,wy,wxy,pointXXv,PMatricesd,
								imgindbv, nrthreads, pitflagsv[i],
								w3dvivv[itt][0][ii],w3dvivv[itt][1][ii],w3dvivv[itt][2][ii],
								w3dvivv[itt][3][ii],w3dvivv[itt][4][ii],w3dvivv[itt][5][ii],
								sig0gv[itt],redundancyv[itt],sig0vvv[itt],sig0wv,robustmode,weightonlyflag,true);
					} // if pointxxv
				} // for i
			} // for itt

			sig0g = 0.;
			redundancy = 0;
			for (int itt = 0; itt < nrthreads; ++itt) {
				sig0g += sig0gv[itt];
				redundancy += redundancyv[itt];
			}

			nrobs = 0;
			for (int i = 0; i < pxs; ++i)
				nrobs += (int) pointXXv[i].size();
			nrobs *= 2;

			sig0g /= (double) (redundancy - nrparimages);

			if (iter > 0) {
				std::cout << "IT: " << iter << " " << iflag << "  " << sqrt(sig0g * ihw2) << ' ' << sig0opt / sig0g << "  ";
				std::cout << sig0g << " " << redundancy << " " << nrobs << " " << nrparimages << " " << nraddp << "  " << redundancy - nrparimages << '\n';
			}

			if (sig0g <= sig0opt) {
				XXdold = XXd;
				for (int i = 0; i < nrimages; ++i)
					PMatricesdold[i] = PMatricesd[i];
				pointXXvold = pointXXv;
				camerasold = camerasnew;
			}

			for (int itt = 0; itt < nrthreads; ++itt) {
				camerasoit[itt] = camerasnew;
				for (int i = 0; i < (int) imgindv[itt].size(); ++i)
					PMatricesdd[itt][imgindv[itt][i]] = PMatricesd[imgindv[itt][i]];
				pointXXvv[itt].resize(0);
				pindv[itt].resize(0);
			} // for itt

			int nrgcpnew = 0;
			for (int itt = 0; itt < nrthreads; ++itt)
				gcpdatav[itt].resize(0);

			for (int itt = 0; itt < nrthreads; ++itt) {
				for (int i = beginv[itt], ii = 0; i < endv[itt]; ++i, ++ii) {
					if ((int) pointXXv[i].size() < 2)
						continue;
					int gnum = 0;
					for (int it = 0; it < nrthreads; ++it) {
						if (pitflagsv[i][it])
							++gnum;
						if (gnum > 1)
							break;
					}
					if (gnum > 1) {
						gcpinfo[i] = nrgcpnew;
						for (int ittt = 0; ittt < nrthreads; ++ittt) {
							gcpdatav[ittt].push_back(std::vector<double>(3));
							gcpdatav[ittt][nrgcpnew][0] = XXd(0,i);
							gcpdatav[ittt][nrgcpnew][1] = XXd(1,i);
							gcpdatav[ittt][nrgcpnew][2] = XXd(2,i);
						} // for ittt
						for (int ittt = 0; ittt < nrthreads; ++ittt)
							if (pitflagsv[i][ittt]) {
								for (int vv = 0; vv < 6; ++vv)
									W3ijiv[ittt][vv].push_back(w3dvivv[itt][vv][ii][ittt]);
							}
						++nrgcpnew;
					}
					else {
						gcpinfo[i] = -1;
					} // else
				} // for i
			} // for itt

			for (int i = 0; i < pxs; ++i) {
				for (int it = 0; it < nrthreads; ++it) {
					pxvvv[it].resize(0);
				}
				for (int j = 0; j < (int) pointXXv[i].size(); ++j) {
					const int iipi = imgindbv[pointXXv[i][j].image];
					pxvvv[iipi].push_back(pointXXv[i][j]);
				}
				for (int it = 0; it < nrthreads; ++it) {
					if (pxvvv[it].size() > 0) {
						pointXXvv[it].push_back(pxvvv[it]);
						pindv[it].push_back(i);
					} // if
				} // for
			} // for i

			for (int it = 0; it < nrthreads; ++it)
				gcpinfov[it].resize((int) pindv[it].size());

			#pragma omp parallel for num_threads(nrthreads)
			for (int it = 0; it < nrthreads; ++it) {
				XXdd[it].resize(4,(int) pindv[it].size());
				for (int j = 0; j < (int) pindv[it].size(); ++j) {
					XXdd[it].col(j) = XXd.col(pindv[it][j]);
					gcpinfov[it][j] = gcpinfo[pindv[it][j]];
				} // for i
			} // for it

			if (iter > 1) {
				if (sig0g <= sig0opt) {
					if (sig0opt < sig0g * sig0frac && iter > 3) { // 1.05
						--iflag;
					}
					else {
						sig0opt = sig0g;
					}
				}
				else {
					if (iter > 3)
						--iflag;
				}
			}
			else
				iniflag = false;

			++iter;

		} // while iflag

		pointXXvio = pointXXvold;

		for (int i = 0; i < nrimages; ++i)
			PMatrices[i] = PMatricesdold[i].cast <float> ();

		XX = XXdold.cast <float> ();

		cameras = camerasold;

		for (int it = 0; it < nrthreads; ++it)
			delete LSAv[it];

	} // if nrimages >
	else {

		gcpflag = false;

		LSA *LSA1 = new LSA(cameras, addflags,
				false, imgind, PMatricesd, pointXXv, XXd, pxs, xg,
				wx, wy, wxy, W3iji,
				gcpflag, gcpinfo, gcpdata,
				ihw2, robustflag);

		LSA1->Adjust(robustflag, os, false, -1, W3iji, false, gcpdata);

		LSA1->OutputLSAc(PMatrices, pointXXvio, XX, cameras);

		delete LSA1;

	} // else

	return;
} //void CalcPLSAc

// Bundle adjustment for points for multiple images
bool CalcLSAXMulti(const std::vector<Camera> &cameraps,
		const int pi, Eigen::MatrixXd &XX,
		const std::vector<Eigen::MatrixXf> &xg,
		const std::vector<std::vector<float> > &wx, const std::vector<std::vector<float> > &wy,
		const std::vector<std::vector<float> > &wxy,
		std::vector<std::vector<IMAPNR> > &pointXXvio,
		const std::vector<PMatd> &PMatrices,
		const std::vector<int> &imgindbv, const int nrthreads, const std::vector<bool> &pitflags,
		std::vector<float> &wXXv, std::vector<float> &wYYv, std::vector<float> &wZZv,
		std::vector<float> &wXYv, std::vector<float> &wXZv, std::vector<float> &wYZv,
		double &sig0io, long &redundancy, std::vector<std::vector<float> > &sig0vv,
		const std::vector<float> &sig0wv, const int robustmode, const bool weightonlyflag, const bool wxyflag) {

	int js;
	int iter = 1;
	int iflag = 1;
	if (robustmode > 0)
		iflag = 2;
	float sig0 = 1.e20f;
	float X0,X1,X2,Xold0,Xold1,Xold2,X0in,X1in,X2in;
	float sig0opt = 1.e20f;

	X0 = Xold0 = X0in = (float) XX(0,pi);
	X1 = Xold1 = X1in = (float) XX(1,pi);
	X2 = Xold2 = X2in = (float) XX(2,pi);

	bool convflag = false;

	const int pxs = (int) pointXXvio[pi].size();
	int pxsd = pxs;
	int pxs2 = pxs * 2;

	std::vector<float> p1v(pxs), p2v(pxs), p3v(pxs);
	std::vector<float> wxi(pxs),wyi(pxs),wxyi(pxs),wxio(pxs),wyio(pxs),wxyio(pxs),sig0v(pxs);

	Eigen::Matrix3d eNpp, eNppi;
	Eigen::Vector3d exip;
	Eigen::Vector2d ecorr2;
	Eigen::Matrix<double, 2, 3> eBpp2;
	Eigen::Matrix<double, 3, 2> eNpp12;
	Eigen::MatrixXd eBpp2wv(2 * pxs, 3);
	Eigen::MatrixXd eNppv;

	std::vector<IMAPNR>::iterator pxvi;
	std::vector<double> corrv(pxs2);
	std::vector<bool> delf(pxs,false);
	float wsum = 0.f;

	if (wxyflag)
		eNppv = Eigen::MatrixXd::Constant(nrthreads * 3, 3, -1.);

	for(pxvi = pointXXvio[pi].begin(), js = 0; pxvi != pointXXvio[pi].end(); ++pxvi, ++js){
		const int j = pxvi->image;
		const int pxx = pxvi->pointnr;
		wxi[js] = wxio[js] = wx[j][pxx];
		wyi[js] = wyio[js] = wy[j][pxx];
		wxyi[js] = wxyio[js] = wxy[j][pxx];

		wsum += wxi[js] + wyi[js];
	} // for pxvi

	CalcCorrPointMulti(cameraps, X0, X1, X2, pointXXvio[pi],corrv,PMatrices,xg,
			wxi,wyi,wxyi,wxio,wyio,wxyio,sig0opt,sig0v,p1v,p2v,p3v);

	if (weightonlyflag) {
		wsum /= (float) pxs2;
		sig0io += sig0opt / wsum;
		redundancy += pxs2 - 3;
	}

	while((iflag > -1) && (iter < 101)){ // 101

		eNpp.setZero();
		if (wxyflag)
			eNppv.setZero();
		for(pxvi = pointXXvio[pi].begin(), js = 0; pxvi != pointXXvio[pi].end(); ++pxvi, ++js) {
			const int j = pxvi->image;

			const double x = p1v[js];
			const double y = p2v[js];

			const double P3 = p3v[js];
			const double P1 = p1v[js] * p3v[js];
			const double P2 = p2v[js] * p3v[js];

			eBpp2(0,0) = PMatrices[j](0,0) * P3 - PMatrices[j](2,0) * P1;
			eBpp2(0,1) = PMatrices[j](0,1) * P3 - PMatrices[j](2,1) * P1;
			eBpp2(0,2) = PMatrices[j](0,2) * P3 - PMatrices[j](2,2) * P1;
			eBpp2(1,0) = PMatrices[j](1,0) * P3 - PMatrices[j](2,0) * P2;
			eBpp2(1,1) = PMatrices[j](1,1) * P3 - PMatrices[j](2,1) * P2;
			eBpp2(1,2) = PMatrices[j](1,2) * P3 - PMatrices[j](2,2) * P2;

			CalcCalibratedDerivatives(eBpp2(0,0), eBpp2(1,0), eBpp2(0,1), eBpp2(1,1), eBpp2(0,2), eBpp2(1,2),
					x, y, cameraps[j].distpara[0], cameraps[j].distpara[1],
					cameraps[j].K(0,0), cameraps[j].K(1,1), cameraps[j].K(0,1));

			const int js2 = 2 * js;
			const int js21 = js2 + 1;
			eBpp2wv(js2,0) = eBpp2(0,0) * wxi[js] + eBpp2(1,0) * wxyi[js];
			eBpp2wv(js21,0) = eBpp2(1,0) * wyi[js] + eBpp2(0,0) * wxyi[js];
			eBpp2wv(js2,1) = eBpp2(0,1) * wxi[js] + eBpp2(1,1) * wxyi[js];
			eBpp2wv(js21,1) = eBpp2(1,1) * wyi[js] + eBpp2(0,1) * wxyi[js];
			eBpp2wv(js2,2) = eBpp2(0,2) * wxi[js] + eBpp2(1,2) * wxyi[js];
			eBpp2wv(js21,2) = eBpp2(1,2) * wyi[js] + eBpp2(0,2) * wxyi[js];

			eNpp.noalias() += eBpp2.transpose() * eBpp2wv.block<2,3>(js2,0);

			if (wxyflag) {
				if (nrthreads == 1)
					continue;
				else { // if (nn0flag) {
					const int ij = imgindbv[j];
					for (int i = 0; i < nrthreads; ++i)
						if (pitflags[i] && i != ij) {
							eNppv.block<3,3>(3 * i,0).noalias() += eBpp2.transpose() * eBpp2wv.block<2,3>(js * 2,0);
						}
				} // else
			}

		}

		if (wxyflag) {
			if (nrthreads == 1) {
				eNppv = eNpp;
			}
		}

		const double m1 = 1.001;

		eNpp(0,0) *= m1;
		eNpp(1,1) *= m1;
		eNpp(2,2) *= m1;

		eNppi = eNpp.inverse();

		exip.setZero();
		for (int i = 0; i < pxs; ++i) {
			ecorr2(0) = corrv[i * 2];
			ecorr2(1) = corrv[i * 2 + 1];

			exip.noalias() += eNppi * eBpp2wv.block<2,3>(i * 2,0).transpose() * ecorr2;
		}

		X0 = Xold0 + (float) exip(0);
		X1 = Xold1 + (float) exip(1);
		X2 = Xold2 + (float) exip(2);

		CalcCorrPointMulti(cameraps, X0, X1, X2, pointXXvio[pi],corrv,PMatrices,xg,
				wxi,wyi,wxyi,wxio,wyio,wxyio,sig0,sig0v,p1v,p2v,p3v);

		if (iflag == 1 && robustmode == 2) {
			for(pxvi = pointXXvio[pi].begin(), js = 0; pxvi != pointXXvio[pi].end(); ++pxvi, ++js) {
				if (sig0v[js] * sig0wv[pxvi->image] > 16.f) {
					wxi[js] *= 1.e-4f;
					wyi[js] *= 1.e-4f;
					wxyi[js] *= 1.e-4f;
					if (wxi[js] + wyi[js] < 2.e-8f) {
						wxi[js] = wyi[js] = 1.e-8f;
						wxyi[js] = 0.f;
					}
					--pxsd;
					delf[js] = true;
				} // if sig0v
			} // for pxvi
			if (pxsd < 2)
				break;
		} // if robustflag

		if (sig0 < sig0opt * 1.001) {
			if (sig0opt < sig0 * 1.05f) // 1.01f
				--iflag;
			if (!weightonlyflag) {
				XX(0,pi) = Xold0 = X0;
				XX(1,pi) = Xold1 = X1;
				XX(2,pi) = Xold2 = X2;
			}
			sig0opt = sig0;
			convflag = true;
			if (wxyflag) {
				for (int i = 0; i < nrthreads; ++i) {
					const int i3 = 3 * i;
					const int i31 = i3 + 1;
					wXXv[i] = (float) eNppv(i3,0);
					wYYv[i] = (float) eNppv(i31,1);
					wZZv[i] = (float) eNppv(i3 + 2,2);
					wXYv[i] = (float) eNppv(i3,1);
					wXZv[i] = (float) eNppv(i3,2);
					wYZv[i] = (float) eNppv(i31,2);
				}
			}
		} //if sig0
		else {
			--iflag;
		}

		if (weightonlyflag)
			break;

		++iter;
	} //while iflag

	if (robustmode == 1) {
		for(pxvi = pointXXvio[pi].begin(), js = 0; pxvi != pointXXvio[pi].end(); ++pxvi, ++js)
			sig0vv[pxvi->image].push_back(sig0v[js]);
	} // if robustmode

	if (robustmode == 2) {
		std::vector<int> delv;
		for (int i = 0; i < pxs; ++i)
			if (delf[i])
				delv.push_back(i);
		if ((int) delv.size() > 0) {
			pxs2 -= (int) delv.size() * 2;
			for (int i = (int)(delv.size()) - 1; i > -1; --i) {
				pointXXvio[pi].erase(pointXXvio[pi].begin() + delv[i]);
			}
		}
	} // if robustmode

	if (pointXXvio[pi].size() < 2 || !convflag) {
		pointXXvio[pi].clear();
		convflag = false;
		XX(0,pi) = X0in;
		XX(1,pi) = X1in;
		XX(2,pi) = X2in;
	}
	else {
		if (!weightonlyflag) {
			wsum /= (float) pxs2;
			sig0io += sig0opt / wsum;
			redundancy += pxs2 - 3;
		}
	}

	return(convflag);
} // bool CalcLSAXMulti

// Computation of corrections for bundle adjustment for points only for multi image case
void CalcCorrPointMulti(const std::vector<Camera> &cameraps,
		const double X0, const double X1, const double X2,
		std::vector<IMAPNR> &pxv,
		std::vector<double> &corrv,
		const std::vector<PMatd> &PM,
		const std::vector<Eigen::MatrixXf> &xg,
		const std::vector<float> &wx, const std::vector<float> &wy, const std::vector<float> &wxy,
		const std::vector<float> &wxo, const std::vector<float> &wyo, const std::vector<float> &wxyo,
		float &sig0, std::vector<float> &sig0v,
		std::vector<float> &p1v, std::vector<float> &p2v, std::vector<float> &p3v){

	int js;

	sig0 = 0;
	std::vector<IMAPNR>::iterator pxvi;
	for(pxvi = pxv.begin(), js = 0; pxvi != pxv.end(); ++pxvi, ++js){
		const int j = pxvi->image;
		const int pxx = pxvi->pointnr;
		const int js2 = js * 2;

//		if (fabs(PM[j](0,3)) > 1.e5 || fabs(PM[j](1,3)) > 1.e5 || fabs(PM[j](2,3)) > 1.e5)
//			continue;

		double p1 = PM[j](0,0) * X0 + PM[j](0,1) * X1 + PM[j](0,2) * X2 + PM[j](0,3);
		double p2 = PM[j](1,0) * X0 + PM[j](1,1) * X1 + PM[j](1,2) * X2 + PM[j](1,3);
		double p3 = PM[j](2,0) * X0 + PM[j](2,1) * X1 + PM[j](2,2) * X2 + PM[j](2,3);

		if (fabs(p3) < 1.e-3) {
			if (p3 > 0.)
				p3 = 1.e3;
			else
				p3 = -1.e3;
		}
		else
			p3 = 1. / p3;

		p1 *= p3;
		p2 *= p3;

		p1v[js] = (float) p1;
		p2v[js] = (float) p2;
		p3v[js] = (float) p3;

		RadialReconstructionImage(p1, p2, cameraps[j]);

		p1 *= cameraps[j].K(0,0);
		p2 *= cameraps[j].K(0,0);

		const double dx = xg[j](0,pxx) - p1;
		const double dy = xg[j](1,pxx) - p2;

		corrv[js2] = dx;
		corrv[js2 + 1] = dy;
		const float sig0i = (float) (dx * dx * wx[js] + dy * dy * wy[js] + 2 * dx * dy * wxy[js]);
		sig0 += sig0i;
		sig0v[js] = (float) (dx * dx * wxo[js] + dy * dy * wyo[js] + 2 * dx * dy * wxyo[js]);
	}

	return;
} //void CalcCorrPointMulti

void CalcCalibratedDerivatives(double &xp0, double &yp0, double &xp1, double &yp1, double &xp2, double &yp2,
		const double x, const double y, const double k2, const double k4,
		const double fx, const double fy, const double s) {

	const double xpo0 = xp0;
	const double ypo0 = yp0;
	const double xpo1 = xp1;
	const double ypo1 = yp1;
	const double xpo2 = xp2;
	const double ypo2 = yp2;

	const double xy2 = x * x + y * y;
	const double xy4 = xy2 * xy2;

	const double xxpc = k4 * xy4 + x * (2 * k2 * x + 4 * k4 * x * xy2) + k2 * xy2 + 1.;
	const double xypc = x * (4 * k4 * xy2 * y + 2 * k2 * y);
	const double yxpc = y * (4 * k4 * x * xy2 + 2 * k2 * x);
	const double yypc = y * (2 * k2 * y + 4 * k4 * xy2 * y) + k4 * xy4 + k2 * xy2 + 1.;

	xp0 = (xxpc * xpo0 + xypc * ypo0);
	yp0 = (yypc * ypo0 + yxpc * xpo0);
	xp0 = xp0 * fx + yp0 * s;
	yp0 *= fy;

	xp1 = (xxpc * xpo1 + xypc * ypo1);
	yp1 = (yypc * ypo1 + yxpc * xpo1);
	xp1 = xp1 * fx + yp1 * s;
	yp1 *= fy;

	xp2 = (xxpc * xpo2 + xypc * ypo2);
	yp2 = (yypc * ypo2 + yxpc * xpo2);
	xp2 = xp2 * fx + yp2 * s;
	yp2 *= fy;

	return;
} // CalcCalibratedDerivatives

// Evaluation of bundle adjustment for calibrated images
void EvalLSA(const int nraddp, const std::vector<Camera> &cameraps,
		const Eigen::MatrixXd &XXd,
		const std::vector<Eigen::MatrixXf> &xg,
		const std::vector<std::vector<float> > &wxi, const std::vector<std::vector<float> > &wyi,
		const std::vector<std::vector<float> > &wxyi,
		std::vector<std::vector<IMAPNR> > &pointXXv,
		const std::vector<PMatd> &PMatricesd,
		const float ihw2) {

	long redundancy = 0;
	long nrobs = 0;
	float sig0g = 0.f;
	int ii,jj;
	const int nrimages =  (int) PMatricesd.size();
	std::vector<std::vector<IMAPNR> >::iterator pxv;
	std::vector<IMAPNR>::iterator pxvi;
	std::vector<float> p1v,p2v,p3v;

	for (pxv = pointXXv.begin(), ii = 0; pxv != pointXXv.end(); ++pxv, ++ii) {
		const int pxss = (int) pxv->size();
		const int pxss2 = pxss * 2;

		p1v.resize(pxss);
		p2v.resize(pxss);
		p3v.resize(pxss);

		if (pxss > 1) {
			std::vector<float> wxii(pxss),wyii(pxss),wxyii(pxss),sig0v(pxss);
			std::vector<double> corrv(pxss2);
			float wsum = 0.f;
			float sig0optf;
			int js,pxx;
			for(pxvi = pxv->begin(), js = 0; pxvi != pxv->end(); ++pxvi, ++js) {
				jj = pxvi->image;
				pxx = pxvi->pointnr;
				wxii[js] = wxi[jj][pxx];
				wyii[js] = wyi[jj][pxx];
				wxyii[js] = wxyi[jj][pxx];

				wsum += wxii[js] + wyii[js];
			} // for pxvi

			wsum = wsum / (float) pxss2;

			std::vector<bool> pitflags(1);
			std::vector<int> imgindbv;
			CalcCorrPointMulti(cameraps, XXd(0,ii), XXd(1,ii), XXd(2,ii), (*pxv),corrv,PMatricesd,xg,
					wxii,wyii,wxyii,wxii,wyii,wxyii,sig0optf,sig0v,p1v,p2v,p3v);
			sig0g += sig0optf / wsum;
			redundancy += pxss2 - 3;
			nrobs += pxss2;
		} // if pxss
	} // for pxv

	sig0g *= 1.f / (float) (redundancy - (nrimages - 2) * 6 - 5 - nrimages * nraddp);

	std::cout << "ITTTT: " << sqrtf(sig0g * ihw2) << ' ' << nrobs << " " << redundancy - (nrimages - 2) * 6 - 5 - nrimages * nraddp << '\n';

	return;
} // EvalLSA

void PartitionpointXXv(std::vector<std::vector<IMAPNR> > &pointXXv, const int nrimages,
		const int nrthreads, std::vector<idx_t> &part,
		std::vector<std::vector<int> > &imagesperpart)
{
  const int minpointnr = 0; // minimum number of points used for the graph

  const System::Time startp = System::getTickCount();

  std::vector<std::vector<int> > AM(nrimages);
  std::vector<int> BM(nrimages,0);
  for (int i = 0; i < nrimages; ++i) {
	  AM[i].resize(nrimages);
	  for (int j = 0; j < nrimages; ++j)
	  	  AM[i][j] = 0;
	  } // for i

  std::vector<std::vector<IMAPNR> >::iterator pxv;
  std::vector<IMAPNR>::iterator pxvi,pxvj;

  for (pxv = pointXXv.begin(); pxv != pointXXv.end(); ++pxv) {
	  if ((int) pxv->size() < 2)
		  continue;
	  for (pxvi = pxv->begin(); pxvi != pxv->end(); ++pxvi) {
		  const int fi = pxvi->image;
		  BM[fi] += (int) pxv->size();
		  for (pxvj = pxvi + 1; pxvj != pxv->end(); ++pxvj) {
			  const int si = pxvj->image;
			  ++AM[fi][si];
			  ++AM[si][fi];
		  }
	  }
  }

  int nredges = 0;
  for (int i = 0; i < nrimages; ++i) {
	  for (int j = 0; j < nrimages; ++j) {
		  if (AM[i][j] > minpointnr)
			  ++nredges;
	  }
  }

  std::vector<idx_t> xadj(nrimages + 1), adjncy(nredges), vwgt(nrimages);
  xadj[0] = 0;

  int indexadj = -1;
  for (int i = 0; i < nrimages; ++i) {

    int nrconn = 0;
    for (int j = 0; j < nrimages; ++j)
    	if (AM[i][j] > minpointnr) {
    		adjncy[++indexadj] = j;
    		++nrconn;
    	}

    xadj[i+1] = xadj[i] + nrconn;
    vwgt[i] = (idx_t) pow(BM[i],0.3333);
  } // for

  std::cout << "Preparation time: " << (float) (System::getTickCount()-startp) / 1000.f << "s\n";

  idx_t nrparts = nrthreads;
  idx_t inrimages = nrimages;
  idx_t edgecut;

  idx_t options[METIS_NOPTIONS];
  METIS_SetDefaultOptions(options);
  options[METIS_OPTION_CONTIG] = 1;

  // One constraint
  idx_t ncon = 1;

  METIS_PartGraphRecursive(&inrimages, &ncon, &xadj[0], &adjncy[0],
		  &vwgt[0], nullptr, nullptr, &nrparts, nullptr,
		  nullptr, &options[0], &edgecut, &part[0]);

  std::cout << "After partitioning: " << (float) (System::getTickCount()-startp) / 1000.f << "s\n\n";

  for (int i = 0; i < nrimages; ++i)
	  imagesperpart[part[i]].push_back(i);

  std::cout << "Images per partition: ";
  for (int it = 0; it < nrthreads; ++it)
	  std::cout << imagesperpart[it].size() << ' ';
  std::cout << '\n';

  std::cout << "After analysis of parts: " << (float) (System::getTickCount()-startp) / 1000.f << "s\n";

  return;
} // PartitionpointXXv