/* 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 <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
// Note Franck : sounds useless. It seems it's defined in bindings
// (to be checked, especially on windows)

// #define BIND_FORTRAN_LOWERCASE_UNDERSCORE

// needed for blas3
#include <assert.h>

namespace siconosBindings = boost::numeric::bindings;

// for ublas::axpy_prod, ...
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

// require for matrix stuff like value_type
//#include <boost/numeric/bindings/traits/ublas_matrix.hpp>

#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"

#include "SiconosAlgebra.hpp"

using namespace Siconos;

//======================
// Product of matrices
//======================

const SimpleMatrix prod(const SiconosMatrix &A, const SiconosMatrix& B)
{
  // To compute C = A * B
  assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!");
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  if ((A.size(1) != B.size(0)))
    SiconosMatrixException::selfThrow("Matrix function C=prod(A,B): inconsistent sizes");

  unsigned int numA = A.num();
  unsigned int numB = B.num();

  // == TODO: implement block product ==
  if (numA == 0 || numB == 0)
    SiconosMatrixException::selfThrow("Matrix product ( C=prod(A,B) ): not yet implemented for BlockMatrix objects.");

  if (numA == 7 || numB == 6) // A = identity or B = 0
    return SimpleMatrix(B);

  else if (numB == 7 || numA == 6) // B = identity or A = 0
    return SimpleMatrix(A);

  else // neither A or B is equal to identity or zero.
  {
    if (numB == 1)
    {
      if (numA == 1)
      {
        DenseMat p(A.size(0), B.size(1));
        siconosBindings::blas::gemm(1.0, *A.dense(), *B.dense(), 1.0, p);
        //      return (DenseMat)(prod(*A.dense(),*B.dense()));
        return p;
      }
      else if (numA == 2)
        return (DenseMat)(prod(*A.triang(), *B.dense()));
      else if (numA == 3)
        return (DenseMat)(prod(*A.sym(), *B.dense()));
      else if (numA == 4)
        return (DenseMat)(prod(*A.sparse(), *B.dense()));
      else// if(numA==5)
        return (DenseMat)(prod(*A.banded(), *B.dense()));
    }
    else if (numB == 2)
    {
      if (numA == 1)
        return (DenseMat)(prod(*A.dense(), *B.triang()));
      else if (numA == 2)
        return (TriangMat)(prod(*A.triang(), *B.triang()));
      else if (numA == 3)
        return (DenseMat)(prod(*A.sym(), *B.triang()));
      else if (numA == 4)
        return (DenseMat)(prod(*A.sparse(), *B.triang()));
      else //if(numA==5)
        return (DenseMat)(prod(*A.banded(), *B.triang()));
    }
    else if (numB == 3)
    {
      if (numA == 1)
        return (DenseMat)(prod(*A.dense(), *B.sym()));
      else if (numA == 2)
        return (DenseMat)(prod(*A.triang(), *B.sym()));
      else if (numA == 3)
        return (SymMat)(prod(*A.sym(), *B.sym()));
      else if (numA == 4)
        return (DenseMat)(prod(*A.sparse(), *B.sym()));
      else // if (numA == 5)
        return (DenseMat)(prod(*A.banded(), *B.sym()));
    }
    else if (numB == 4)
    {
      if (numA == 1)
        return (DenseMat)(prod(*A.dense(), *B.sparse()));
      else if (numA == 2)
        return (DenseMat)(prod(*A.triang(), *B.sparse()));
      else if (numA == 3)
        return (DenseMat)(prod(*A.sym(), *B.sparse()));
      else if (numA == 4)
        return (SparseMat)(prod(*A.sparse(), *B.sparse()));
      else //if(numA==5){
        return (DenseMat)(prod(*A.banded(), *B.sparse()));
    }
    else //if(numB==5)
    {
      if (numA == 1)
        return (DenseMat)(prod(*A.dense(), *B.banded()));
      else if (numA == 2)
        return (DenseMat)(prod(*A.triang(), *B.banded()));
      else if (numA == 3)
        return (DenseMat)(prod(*A.sym(), *B.banded()));
      else if (numA == 4)
        return (DenseMat)(prod(*A.sparse(), *B.banded()));
      else //if(numA==5)
        return (DenseMat)(prod(*A.banded(), *B.banded()));
    }
  }
}
/**

indexStart : indexStart[0] is the first raw, indexStart[1] is the first col
dim : dim[0] number of raw, dim[1] number of col
*/
// void zeroBlock(const SiconosMatrix& A, index indexStart, index dim){
//   ;
// }
// void prod(const SiconosMatrix& A, const SiconosMatrix& B, SiconosMatrix& C, int indexACol, bool init){
//   // To compute C[indexAcol::] = A * B

//   unsigned int numA = A.num();
//   unsigned int numB = B.num();
//   unsigned int numC = C.num();
//   if (numA == 0 || numB == 0 || numC == 0)
//     SiconosMatrixException::selfThrow("Matrix function prod(A,B,C,index): inconsistent sizes");
//   // === if C is zero or identity => read-only ===
//   if (numC == 6 || numC == 7)
//     SiconosMatrixException::selfThrow("Matrix product ( prod(A,B,C,index) ): wrong type for resulting matrix C (read-only: zero or identity).");

//   if (numA == 7 || numC == 6) // A = identity or 0
//     SiconosMatrixException::selfThrow("Matrix function prod(A,B,C,index): numA == 7 || numC == 6 not yet implemented");

//   int rawB = B.size(0);
//   int colB = B.size(1);

// }
void prod(const SiconosMatrix& A, const SiconosMatrix& B, SiconosMatrix& C, bool init)
{
  // To compute C = A * B
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!" );
  assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!" );
  if(!C.isBlock())
    C.resetLU();

  if ((A.size(1) != B.size(0)))
    SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): inconsistent sizes");

  if (A.size(0) != C.size(0) || B.size(1) != C.size(1))
    SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): inconsistent sizes");

  unsigned int numA = A.num();
  unsigned int numB = B.num();
  unsigned int numC = C.num();

  // == TODO: implement block product ==
  if (numA == 0 || numB == 0)
    SiconosMatrixException::selfThrow("Matrix product ( prod(A,B,C) ): not yet implemented for BlockMatrix objects.");

  // === if C is zero or identity => read-only ===
  if (numC == 6 || numC == 7)
    SiconosMatrixException::selfThrow("Matrix product ( prod(A,B,C) ): wrong type for resulting matrix C (read-only: zero or identity).");

  if (numA == 7) // A = identity ...
  {
    if (init)
    {
      if (&C != &B) C = B; // if C and B are two different objects.
      // else nothing
    }
    else
      C += B;
  }

  else if (numB == 7) // B = identity
  {
    if (init)
    {
      if (&C != &A) C = A; // if C and A are two different objects.
      // else nothing
    }
    else
      C += A;
  }

  else if (numA == 6 || numB == 6) // if A or B = 0
  {
    if (init)
      C.zero();
    //else nothing
  }
  else if (numC == 0) // if C is Block - Temp. solution
  {
    SimpleMatrix tmp(C);
    prod(A, B, tmp, init);
    C = tmp;
  }
  else // neither A or B is equal to identity or zero.
  {
    if (init)
    {
      if (&C == &A) // if common memory between A and C
      {
        switch (numA)
        {
        case 1:
          if (numB == 1)
          {
            *C.dense()  = prod(*A.dense(), *B.dense());
            //siconosBindings::blas::gemm(1.0, *A.dense(), *B.dense(), 0.0, *C.dense());
          }
          else if (numB == 2)
            *C.dense()  = prod(*A.dense(), *B.triang());
          else if (numB == 3)
            *C.dense()  = prod(*A.dense(), *B.sym());
          else if (numB == 4)
            *C.dense()  = prod(*A.dense(), *B.sparse());
          else //if(numB==5)
            *C.dense() = prod(*A.dense(), *B.banded());
          break;
        case 2:
          if (numB != 2)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.triang() = prod(*A.triang(), *B.triang());
          break;
        case 3:
          if (numB != 3)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sym() = prod(*A.sym(), *B.sym());
          break;
        case 4:
          if (numB != 4)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sparse() = prod(*A.sparse(), *B.sparse());
          break;
        default:
          SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
        }
      }
      else if (&C == &B)
      {
        switch (numB)
        {
        case 1:
          if (numA == 1)
            *C.dense() = prod(*A.dense(), *B.dense());
          else if (numA == 2)
            *C.dense()  = prod(*A.triang(), *B.dense());
          else if (numA == 3)
            *C.dense()  = prod(*A.sym(), *B.dense());
          else if (numA == 4)
            *C.dense()  = prod(*A.sparse(), *B.dense());
          else //if(numB==5)
            *C.dense() = prod(*A.banded(), *B.dense());
          break;
        case 2:
          if (numA != 2)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.triang() = prod(*A.triang(), *B.triang());
          break;
        case 3:
          if (numA != 3)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sym() = prod(*A.sym(), *B.sym());
          break;
        case 4:
          if (numA != 4)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sparse() = prod(*A.sparse(), *B.sparse());
          break;
        default:
          SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
        }
      }
      else // if no alias between C and A or B.
      {
        switch (numC)
        {
        case 1:
          if (numB == 1)
          {
            if (numA == 1)
              noalias(*C.dense()) = prod(*A.dense(), *B.dense());
            else if (numA == 2)
              noalias(*C.dense()) = prod(*A.triang(), *B.dense());
            else if (numA == 3)
              noalias(*C.dense())  = prod(*A.sym(), *B.dense());
            else if (numA == 4)
              noalias(*C.dense()) = prod(*A.sparse(), *B.dense());
            else// if(numA==5)
              noalias(*C.dense())  = prod(*A.banded(), *B.dense());
          }
          else if (numB == 2)
          {
            if (numA == 1)
              noalias(*C.dense())  = prod(*A.dense(), *B.triang());
            else if (numA == 2)
              noalias(*C.dense())  = prod(*A.triang(), *B.triang());
            else if (numA == 3)
              noalias(*C.dense())  = prod(*A.sym(), *B.triang());
            else if (numA == 4)
              noalias(*C.dense())  = prod(*A.sparse(), *B.triang());
            else //if(numA==5)
              noalias(*C.dense())  = prod(*A.banded(), *B.triang());
          }
          else if (numB == 3)
          {
            if (numA == 1)
              noalias(*C.dense())  = prod(*A.dense(), *B.sym());
            else if (numA == 2)
              noalias(*C.dense())  = prod(*A.triang(), *B.sym());
            else if (numA == 3)
              noalias(*C.dense())  = prod(*A.sym(), *B.sym());
            else if (numA == 4)
              noalias(*C.dense())  = prod(*A.sparse(), *B.sym());
            else // if (numA == 5)
              noalias(*C.dense())  = prod(*A.banded(), *B.sym());
          }
          else if (numB == 4)
          {
            if (numA == 1)
              noalias(*C.dense()) = prod(*A.dense(), *B.sparse());
            else if (numA == 2)
              noalias(*C.dense()) = prod(*A.triang(), *B.sparse());
            else if (numA == 3)
              noalias(*C.dense()) = prod(*A.sym(), *B.sparse());
            else if (numA == 4)
              noalias(*C.dense()) = prod(*A.sparse(), *B.sparse());
            else //if(numA==5){
              noalias(*C.dense()) = prod(*A.banded(), *B.sparse());
          }
          else //if(numB==5)
          {
            if (numA == 1)
              noalias(*C.dense()) = prod(*A.dense(), *B.banded());
            else if (numA == 2)
              noalias(*C.dense()) = prod(*A.triang(), *B.banded());
            else if (numA == 3)
              noalias(*C.dense()) = prod(*A.sym(), *B.banded());
            else if (numA == 4)
              noalias(*C.dense()) = prod(*A.sparse(), *B.banded());
            else //if(numA==5)
              noalias(*C.dense()) = prod(*A.banded(), *B.banded());
          }
          break;
        case 2:
          if (numA != 2 || numB != 2)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          noalias(*C.triang()) = prod(*A.triang(), *B.triang());
          break;
        case 3:
          if (numA != 3 || numB != 3)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          noalias(*C.sym()) = prod(*A.sym(), *B.sym());
          break;
        case 4:
          if (numA != 4 || numB != 4)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          noalias(*C.sparse()) = prod(*A.sparse(), *B.sparse());
          break;
        default:
          SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
        }
      }
    }
    else // += case
    {
      if (&C == &A) // if common memory between A and C
      {
        switch (numA)
        {
        case 1:
          if (numB == 1)
            *C.dense() += prod(*A.dense(), *B.dense());
          else if (numB == 2)
            *C.dense()  += prod(*A.dense(), *B.triang());
          else if (numB == 3)
            *C.dense()  += prod(*A.dense(), *B.sym());
          else if (numB == 4)
            *C.dense()  += prod(*A.dense(), *B.sparse());
          else //if(numB==5)
            *C.dense() += prod(*A.dense(), *B.banded());
          break;
        case 2:
          if (numB != 2)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.triang() += prod(*A.triang(), *B.triang());
          break;
        case 3:
          if (numB != 3)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sym() += prod(*A.sym(), *B.sym());
          break;
        case 4:
          if (numB != 4)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sparse() += prod(*A.sparse(), *B.sparse());
          break;
        default:
          SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
        }
      }
      else if (&C == &B)
      {
        switch (numB)
        {
        case 1:
          if (numA == 1)
            *C.dense() += prod(*A.dense(), *B.dense());
          else if (numA == 2)
            *C.dense()  += prod(*A.triang(), *B.dense());
          else if (numA == 3)
            *C.dense()  += prod(*A.sym(), *B.dense());
          else if (numA == 4)
            *C.dense()  += prod(*A.sparse(), *B.dense());
          else //if(numB==5)
            *C.dense() += prod(*A.banded(), *B.dense());
          break;
        case 2:
          if (numA != 2)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.triang() += prod(*A.triang(), *B.triang());
          break;
        case 3:
          if (numA != 3)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sym() += prod(*A.sym(), *B.sym());
          break;
        case 4:
          if (numA != 4)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          *C.sparse() += prod(*A.sparse(), *B.sparse());
          break;
        default:
          SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
        }
      }
      else // if no alias between C and A or B.
      {
        switch (numC)
        {
        case 1:
          if (numB == 1)
          {
            if (numA == 1)
              noalias(*C.dense()) += prod(*A.dense(), *B.dense());
            else if (numA == 2)
              noalias(*C.dense()) += prod(*A.triang(), *B.dense());
            else if (numA == 3)
              noalias(*C.dense())  += prod(*A.sym(), *B.dense());
            else if (numA == 4)
              noalias(*C.dense()) += prod(*A.sparse(), *B.dense());
            else// if(numA==5)
              noalias(*C.dense())  += prod(*A.banded(), *B.dense());
          }
          else if (numB == 2)
          {
            if (numA == 1)
              noalias(*C.dense())  += prod(*A.dense(), *B.triang());
            else if (numA == 2)
              noalias(*C.dense())  += prod(*A.triang(), *B.triang());
            else if (numA == 3)
              noalias(*C.dense())  += prod(*A.sym(), *B.triang());
            else if (numA == 4)
              noalias(*C.dense())  += prod(*A.sparse(), *B.triang());
            else //if(numA==5)
              noalias(*C.dense())  += prod(*A.banded(), *B.triang());
          }
          else if (numB == 3)
          {
            if (numA == 1)
              noalias(*C.dense())  += prod(*A.dense(), *B.sym());
            else if (numA == 2)
              noalias(*C.dense())  += prod(*A.triang(), *B.sym());
            else if (numA == 3)
              noalias(*C.dense())  += prod(*A.sym(), *B.sym());
            else if (numA == 4)
              noalias(*C.dense())  += prod(*A.sparse(), *B.sym());
            else // if (numA == 5)
              noalias(*C.dense())  += prod(*A.banded(), *B.sym());
          }
          else if (numB == 4)
          {
            if (numA == 1)
              noalias(*C.dense()) += prod(*A.dense(), *B.sparse());
            else if (numA == 2)
              noalias(*C.dense()) += prod(*A.triang(), *B.sparse());
            else if (numA == 3)
              noalias(*C.dense()) += prod(*A.sym(), *B.sparse());
            else if (numA == 4)
              noalias(*C.dense()) += prod(*A.sparse(), *B.sparse());
            else //if(numA==5){
              noalias(*C.dense()) += prod(*A.banded(), *B.sparse());
          }
          else //if(numB==5)
          {
            if (numA == 1)
              noalias(*C.dense()) += prod(*A.dense(), *B.banded());
            else if (numA == 2)
              noalias(*C.dense()) += prod(*A.triang(), *B.banded());
            else if (numA == 3)
              noalias(*C.dense()) += prod(*A.sym(), *B.banded());
            else if (numA == 4)
              noalias(*C.dense()) += prod(*A.sparse(), *B.banded());
            else //if(numA==5)
              noalias(*C.dense()) += prod(*A.banded(), *B.banded());
          }
          break;
        case 2:
          if (numA != 2 || numB != 2)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          noalias(*C.triang()) += prod(*A.triang(), *B.triang());
          break;
        case 3:
          if (numA != 3 || numB != 3)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          noalias(*C.sym()) += prod(*A.sym(), *B.sym());
          break;
        case 4:
          if (numA != 4 || numB != 4)
            SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
          noalias(*C.sparse()) += prod(*A.sparse(), *B.sparse());
          break;
        default:
          SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): wrong type for C (according to A and B types).");
        }
      }
    }
  if(!C.isBlock())
    C.resetLU();
  }
}

void axpy_prod(const SiconosMatrix& A, const SiconosMatrix& B, SiconosMatrix& C, bool init)
{
  // To compute C = A * B (init = true) or C += A * B (init = false) using ublas axpy_prod.
  if ((A.size(1) != B.size(0)))
    SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): inconsistent sizes");

  if (A.size(0) != C.size(0) || B.size(1) != C.size(1))
    SiconosMatrixException::selfThrow("Matrix function prod(A,B,C): inconsistent sizes");

  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!" );
  assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!" );
  if(!C.isBlock())
    C.resetLU();
  unsigned int numA = A.num();
  unsigned int numB = B.num();
  unsigned int numC = C.num();
  // == TODO: implement block product ==
  if (numA == 0 || numB == 0)
    SiconosMatrixException::selfThrow("Matrix product ( prod(A,B,C) ): not yet implemented for BlockMatrix objects.");

  // === if C is zero or identity => read-only ===
  if (numC == 6 || numC == 7)
    SiconosMatrixException::selfThrow("Matrix product ( prod(A,B,C) ): wrong type for resulting matrix C (read-only: zero or identity).");

  if (numA == 7) // A = identity ...
  {
    if (!init) C += B;
    else
    {
      if (&C != &B)
        C = B; // if C and B are two different objects.
      // else nothing
    }
  }

  else if (numB == 7) // B = identity
  {
    if (!init) C += A;
    else
    {
      if (&C != &A) C = A; // if C and A are two different objects.
      // else nothing
    }
  }

  else if (numA == 6 || numB == 6) // if A or B = 0
  {
    if (init) C.zero(); // else nothing
  }
  else if (numC == 0) // if C is Block - Temp. solution
  {
    SimpleMatrix tmp(C);
    axpy_prod(A, B, tmp, init);
    C = tmp;
  }
  else // neither A or B is equal to identity or zero.
  {
    if (&C == &A) // if common memory between A and C
    {
      switch (numA)
      {
      case 1:
        if (numB == 1)
          ublas::axpy_prod(*A.dense(), *B.dense(), *A.dense(), init);
        else if (numB == 2)
          ublas::axpy_prod(*A.dense(), *B.triang(), *A.dense(), init);
        else if (numB == 3)
          ublas::axpy_prod(*A.dense(), *B.sym(), *A.dense(), init);
        else if (numB == 4)
          ublas::axpy_prod(*A.dense(), *B.sparse(), *A.dense(), init);
        else //if(numB==5)
          ublas::axpy_prod(*A.dense(), *B.banded(), *A.dense(), init);
        break;
      case 2:
        //        if(numB != 2)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //        ublas::axpy_prod(*A.triang(), *B.triang(), *A.triang(), init);
        break;
      case 3:
        //if(numB != 3)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //ublas::axpy_prod(*A.sym(), *B.sym(), *A.sym(), init);
        break;
      case 4:
        //        if(numB != 4)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //ublas::axpy_prod(*A.sparse(), *B.sparse(), *A.sparse(),init);
        break;
      default:
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
      }
    }
    else if (&C == &B)
    {
      switch (numB)
      {
      case 1:
        if (numA == 1)
          ublas::axpy_prod(*A.dense(), *B.dense(), *B.dense(), init);
        else if (numA == 2)
          ublas::axpy_prod(*A.triang(), *B.dense(), *B.dense(), init);
        else if (numA == 3)
          ublas::axpy_prod(*A.sym(), *B.dense(), *B.dense(), init);
        else if (numA == 4)
          ublas::axpy_prod(*A.sparse(), *B.dense(), *B.dense(), init);
        else //if(numB==5)
          ublas::axpy_prod(*A.banded(), *B.dense(), *B.dense(), init);
        break;
      case 2:
        //if(numA != 2)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //        ublas::axpy_prod(*A.triang(), *B.triang(),*B.triang(), init);
        break;
      case 3:
        //        if(numA != 3)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //ublas::axpy_prod(*A.sym(), *B.sym(), *B.sym(), init);
        break;
      case 4:
        //        if(numA != 4)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //ublas::axpy_prod(*A.sparse(), *B.sparse(), *B.sparse(), init);
        break;
      default:
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
      }
    }
    else // if no alias between C and A or B.
    {
      switch (numC)
      {
      case 1:
        if (numB == 1)
        {
          if (numA == 1)
            ublas::axpy_prod(*A.dense(), *B.dense(), *C.dense(), init);
          else if (numA == 2)
            ublas::axpy_prod(*A.triang(), *B.dense(), *C.dense(), init);
          else if (numA == 3)
            ublas::axpy_prod(*A.sym(), *B.dense(), *C.dense(), init);
          else if (numA == 4)
            ublas::axpy_prod(*A.sparse(), *B.dense(), *C.dense(), init);
          else// if(numA==5)
            ublas::axpy_prod(*A.banded(), *B.dense(), *C.dense(), init);
        }
        else if (numB == 2)
        {
          if (numA == 1)
            ublas::axpy_prod(*A.dense(), *B.triang(), *C.dense(), init);
          else if (numA == 2)
            ublas::axpy_prod(*A.triang(), *B.triang(), *C.dense(), init);
          else if (numA == 3)
            ublas::axpy_prod(*A.sym(), *B.triang(), *C.dense(), init);
          else if (numA == 4)
            ublas::axpy_prod(*A.sparse(), *B.triang(), *C.dense(), init);
          else //if(numA==5)
            ublas::axpy_prod(*A.banded(), *B.triang(), *C.dense(), init);
        }
        else if (numB == 3)
        {
          if (numA == 1)
            ublas::axpy_prod(*A.dense(), *B.sym(), *C.dense(), init);
          else if (numA == 2)
            ublas::axpy_prod(*A.triang(), *B.sym(), *C.dense(), init);
          else if (numA == 3)
            ublas::axpy_prod(*A.sym(), *B.sym(), *C.dense(), init);
          else if (numA == 4)
            ublas::axpy_prod(*A.sparse(), *B.sym(), *C.dense(), init);
          else // if (numA == 5)
            ublas::axpy_prod(*A.banded(), *B.sym(), *C.dense(), init);
        }
        else if (numB == 4)
        {
          if (numA == 1)
            ublas::axpy_prod(*A.dense(), *B.sparse(), *C.dense(), init);
          else if (numA == 2)
            ublas::axpy_prod(*A.triang(), *B.sparse(), *C.dense(), init);
          else if (numA == 3)
            ublas::axpy_prod(*A.sym(), *B.sparse(), *C.dense(), init);
          else if (numA == 4)
            ublas::axpy_prod(*A.sparse(), *B.sparse(), *C.dense(), init);
          else //if(numA==5){
            ublas::axpy_prod(*A.banded(), *B.sparse(), *C.dense(), init);
        }
        else //if(numB==5)
        {
          if (numA == 1)
            ublas::axpy_prod(*A.dense(), *B.banded(), *C.dense(), init);
          else if (numA == 2)
            ublas::axpy_prod(*A.triang(), *B.banded(), *C.dense(), init);
          else if (numA == 3)
            ublas::axpy_prod(*A.sym(), *B.banded(), *C.dense(), init);
          else if (numA == 4)
            ublas::axpy_prod(*A.sparse(), *B.banded(), *C.dense(), init);
          else //if(numA==5)
            ublas::axpy_prod(*A.banded(), *B.banded(), *C.dense(), init);
        }
        break;
      case 2:
        // if(numA!= 2 || numB != 2)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //ublas::axpy_prod(*A.triang(), *B.triang(),*C.triang(), init);
        break;
      case 3:
        //        if(numA!= 3 || numB != 3)
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        //ublas::axpy_prod(*A.sym(), *B.sym(),*C.sym(),init);
        break;
      case 4:
        if (numA != 4 || numB != 4)
          SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
        ublas::sparse_prod(*A.sparse(), *B.sparse(), *C.sparse(), init);
        break;
      default:
        SiconosMatrixException::selfThrow("Matrix function axpy_prod(A,B,C): wrong type for C (according to A and B types).");
      }
    }
  if(!C.isBlock())
    C.resetLU();
  }
}

void gemmtranspose(double a, const SiconosMatrix& A, const SiconosMatrix& B, double b, SiconosMatrix& C)
{
  if (A.isBlock() || B.isBlock() || C.isBlock())
    SiconosMatrixException::selfThrow("gemm(...) not yet implemented for block matrices.");
  unsigned int numA = A.num();
  unsigned int numB = B.num();
  unsigned int numC = C.num();
  if (numA != 1 || numB != 1 || numC != 1)
    SiconosMatrixException::selfThrow("gemm(...) failed: reserved to dense matrices.");

  assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!");
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  siconosBindings::blas::gemm(a, siconosBindings::trans(*A.dense()), siconosBindings::trans(*B.dense()), b, *C.dense());

  C.resetLU();
}

void gemm(double a, const SiconosMatrix& A, const SiconosMatrix& B, double b, SiconosMatrix& C)
{
  unsigned int numA = A.num();
  unsigned int numB = B.num();
  unsigned int numC = C.num();
  assert(!(B.isPLUFactorized()) && "B is PLUFactorized in prod !!");
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");
  C.resetLU();

  // At the time, only dense output allowed
  DenseMat * tmpC = NULL;
  if (numA == 0 || numB == 0 || numC == 0)
    SiconosMatrixException::selfThrow("gemm(...) not yet implemented for block matrices.");

  if (numA == 1 && numB == 1 && numC == 1)
    siconosBindings::blas::gemm(a, *A.dense(), *B.dense(), b, *C.dense());
  else if (numA == 1 && numB == 1 && numC != 1)
  {
    // Copy C into tmpC ...
    tmpC = new DenseMat(*C.dense());
    siconosBindings::blas::gemm(a, *A.dense(), *B.dense(), b, *tmpC);
    std::cout << *tmpC << std::endl;
    noalias(*C.dense()) = *tmpC;
    delete tmpC;
  }
  else
    SiconosMatrixException::selfThrow("gemm(...) not yet implemented for these kinds of matrices.");
  C.resetLU();
}

void scal(double a, const SiconosMatrix& A, SiconosMatrix& B, bool init)
{
  // To compute B = a * A (init = true) or B += a*A (init = false).
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!" );
  if(!B.isBlock())
    B.resetLU();

  if (&A == &B)
  {
    if (init) B *= a;
    else B *= (1.0 + a);
  }
  else
  {
    unsigned int numA = A.num();
    unsigned int numB = B.num();

    if (numB == 6 || numB == 7) // B = 0 or identity.
      SiconosMatrixException::selfThrow("scal(a,A,B) : forbidden for B being a zero or identity matrix.");

    if (numA == 6)
    {
      if (init) B.zero(); // else nothing
    }
    else if (numA == 7)
    {
      if (init)
      {
        B.eye();
        B *= a;
      }
      else
      {
        // Assuming B is square ...
        for (unsigned int i = 0; i < B.size(0); ++i)
          B(i, i) += a;
      }
    }
    else
    {
      if (numA == numB) // if A and B are of the same type ...
      {
        switch (numA)
        {

        case 0: // A and B are block
          if (isComparableTo(A, B))
          {
            const BlockMatrix& Aref = static_cast<const BlockMatrix&>(A);
            BlockMatrix& Bref = static_cast<BlockMatrix&>(B);
            BlocksIterator1 itB1;
            BlocksIterator2 itB2;
            ConstBlocksIterator1 itA1 = Aref._mat->begin1();
            ConstBlocksIterator2 itA2;
            for (itB1 = Bref._mat->begin1(); itB1 != Bref._mat->end1(); ++itB1)
            {
              itA2 = itA1.begin();
              for (itB2 = itB1.begin(); itB2 != itB1.end(); ++itB2)
              {
                scal(a, **itA2++, **itB2, init);
              }
              itA1++;
            }
          }
          else // if A and B are not "block-consistent"
          {
            if (init)
            {
              for (unsigned int i = 0; i < A.size(0); ++i)
                for (unsigned int j = 0; j < A.size(1); ++j)
                  B(i, j) = a * A(i, j);
            }
            else
            {
              for (unsigned int i = 0; i < A.size(0); ++i)
                for (unsigned int j = 0; j < A.size(1); ++j)
                  B(i, j) += a * A(i, j);
            }
          }
          break;

        case 1: // if both are dense
          if (init)
            noalias(*B.dense()) = a ** A.dense();
          else
            noalias(*B.dense()) += a ** A.dense();
          break;
        case 2:
          if (init)
            noalias(*B.triang()) = a ** A.triang();
          else
            noalias(*B.triang()) += a ** A.triang();
          break;
        case 3:
          if (init)
            noalias(*B.sym()) = a ** A.sym();
          else
            noalias(*B.sym()) += a ** A.sym();
          break;
        case 4:
          if (init)
            noalias(*B.sparse()) = a ** A.sparse();
          else
            noalias(*B.sparse()) += a ** A.sparse();
          break;
        case 5:
          if (init)
            noalias(*B.banded()) = a ** A.banded();
          else
            noalias(*B.banded()) += a ** A.banded();
          break;
        }
      }
      else // if A and B are of different types.
      {
        if (numA == 0 || numB == 0) // if A or B is block
        {
          if (init)
          {
            B = A;
            B *= a;
          }
          else
          {
            SimpleMatrix tmp(A);
            tmp *= a;
            B += tmp; // bof bof ...
          }
        }
        else
        {
          if (numB != 1)
            SiconosMatrixException::selfThrow("scal(a,A,B) failed. A and B types do not fit together.");

          if (init)
          {
            switch (numB)
            {
            case 1:
              noalias(*B.dense()) = a ** A.dense();
              break;
            case 2:
              noalias(*B.dense()) = a ** A.triang();
              break;
            case 3:
              noalias(*B.dense()) = a ** A.sym();
              break;
            case 4:
              noalias(*B.dense()) = a ** A.sparse();
              break;
            case 5:
              noalias(*B.dense()) = a ** A.banded();
              break;
            }
          }
          else

          {
            switch (numB)
            {
            case 1:
              noalias(*B.dense()) += a ** A.dense();
              break;
            case 2:
              noalias(*B.dense()) += a ** A.triang();
              break;
            case 3:
              noalias(*B.dense()) += a ** A.sym();
              break;
            case 4:
              noalias(*B.dense()) += a ** A.sparse();
              break;
            case 5:
              noalias(*B.dense()) += a ** A.banded();
              break;
            }
          }
        }
      }
    }
  }
}


#include <boost/format.hpp>
#include <iostream>
#include <cmath>

struct show_resolution_save
{
  bool first;
  float ass, bss, css;
  show_resolution_save() : first(true) {}
};

struct common
{
  float a, b, c;
  show_resolution_save show_resolution_sve;
  std::ostream & out_stream;
  common(std::ostream& out_stream_) : out_stream(out_stream_) {}
};

void
show_resolution(common& cmn, int h, int k, int l)
{
  show_resolution_save& sve = cmn.show_resolution_sve;
  if (sve.first) {
    sve.first = false;
    if (cmn.a <= 0 || cmn.b <= 0 || cmn.c <= 0) {
      throw std::runtime_error(
        "invalid unit cell constants.");
    }
    sve.ass = 1/(cmn.a*cmn.a);
    sve.bss = 1/(cmn.b*cmn.b);
    sve.css = 1/(cmn.c*cmn.c);
  }
  float dss = h*h*sve.ass + k*k*sve.bss + l*l*sve.css;
  std::ostream& cout = cmn.out_stream;
  if (dss == 0) {
    cout << boost::format(" %3d %3d %3d     infinity\n")
      % h % k % l;
  }
  else {
    cout << boost::format(" %3d %3d %3d %12.6f\n")
      % h % k % l % std::sqrt(1/dss);
  }
}

void
conv_recipe(common& cmn)
{
  cmn.a = 11.0;
  cmn.b = 12.0;
  cmn.c = 13.0;
  show_resolution(cmn, 0, 0, 0);
  show_resolution(cmn, 1, 2, 3);
}

int
main()
{
  common cmn(std::cout);
  conv_recipe(cmn);
  return 0;
}


#ifndef STAN_MATH_PRIM_MAT_PROB_BERNOULLI_LOGIT_GLM_LPMF_HPP
#define STAN_MATH_PRIM_MAT_PROB_BERNOULLI_LOGIT_GLM_LPMF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/broadcast_array.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_consistent_size.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/mat/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/mat/meta/is_vector.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
namespace math {

/**
 * Returns the log PMF of the Generalized Linear Model (GLM)
 * with Bernoulli distribution and logit link function.
 * The idea is that bernoulli_logit_glm_lpmf(y, x, alpha, beta) should
 * compute a more efficient version of bernoulli_logit_lpmf(y, alpha + x * beta)
 * by using analytically simplified gradients.
 * If containers are supplied, returns the log sum of the probabilities.
 * @tparam T_y type of binary vector of dependent variables (labels);
 * this can also be a single binary value;
 * @tparam T_x type of the matrix of independent variables (features); this
 * should be an Eigen::Matrix type whose number of rows should match the
 * length of y and whose number of columns should match the length of beta
 * @tparam T_alpha type of the intercept(s);
 * this can be a vector (of the same length as y) of intercepts or a single
 * value (for models with constant intercept);
 * @tparam T_beta type of the weight vector;
 * this can also be a single value;
 * @param y binary vector parameter
 * @param x design matrix
 * @param alpha intercept (in log odds)
 * @param beta weight vector
 * @return log probability or log sum of probabilities
 * @throw std::domain_error if x, beta or alpha is infinite.
 * @throw std::domain_error if y is not binary.
 * @throw std::invalid_argument if container sizes mismatch.
 */

template <bool propto, typename T_y, typename T_x, typename T_alpha,
          typename T_beta>
typename return_type<T_x, T_alpha, T_beta>::type bernoulli_logit_glm_lpmf(
    const T_y &y, const T_x &x, const T_alpha &alpha, const T_beta &beta) {
  static const char *function = "bernoulli_logit_glm_lpmf";
  typedef typename stan::partials_return_type<T_y, T_x, T_alpha, T_beta>::type
      T_partials_return;

  using Eigen::Dynamic;
  using Eigen::Matrix;
  using std::exp;

  if (size_zero(y, x, beta))
    return 0.0;

  T_partials_return logp(0.0);

  const size_t N = x.col(0).size();
  const size_t M = x.row(0).size();

  check_bounded(function, "Vector of dependent variables", y, 0, 1);
  check_finite(function, "Matrix of independent variables", x);
  check_finite(function, "Weight vector", beta);
  check_finite(function, "Intercept", alpha);
  check_consistent_size(function, "Vector of dependent variables", y, N);
  check_consistent_size(function, "Weight vector", beta, M);
  if (is_vector<T_alpha>::value)
    check_consistent_sizes(function, "Vector of intercepts", alpha,
                           "Vector of dependent variables", y);

  if (!include_summand<propto, T_x, T_alpha, T_beta>::value)
    return 0.0;

  Matrix<T_partials_return, Dynamic, 1> signs(N, 1);
  {
    scalar_seq_view<T_y> y_vec(y);
    for (size_t n = 0; n < N; ++n) {
      signs[n] = 2 * y_vec[n] - 1;
    }
  }
  Matrix<T_partials_return, Dynamic, 1> beta_dbl(M, 1);
  {
    scalar_seq_view<T_beta> beta_vec(beta);
    for (size_t m = 0; m < M; ++m) {
      beta_dbl[m] = value_of(beta_vec[m]);
    }
  }
  Eigen::Array<T_partials_return, Dynamic, 1> ytheta
      = signs.array() * (value_of(x) * beta_dbl).array();
  scalar_seq_view<T_alpha> alpha_vec(alpha);

  // Compute the log-density and handle extreme values gracefully
  // using Taylor approximations.
  // And compute the derivatives wrt theta.
  static const double cutoff = 20.0;
  Matrix<T_partials_return, Dynamic, 1> theta_derivative(N, 1);
  T_partials_return theta_derivative_sum = 0;
  T_partials_return exp_m_ythetan;
  for (size_t n = 0; n < N; ++n) {
    ytheta[n] += signs[n] * value_of(alpha_vec[n]);
    exp_m_ythetan = exp(-ytheta[n]);
    if (ytheta[n] > cutoff) {
      logp -= exp_m_ythetan;
      theta_derivative[n] = -exp_m_ythetan;
    } else if (ytheta[n] < -cutoff) {
      logp += ytheta[n];
      theta_derivative[n] = signs[n];
    } else {
      logp -= log1p(exp_m_ythetan);
      theta_derivative[n] = signs[n] * exp_m_ythetan / (exp_m_ythetan + 1);
    }
    if (!is_vector<T_alpha>::value)
      theta_derivative_sum += theta_derivative[n];
  }

  // Compute the necessary derivatives.
  operands_and_partials<T_x, T_alpha, T_beta> ops_partials(x, alpha, beta);
  if (!is_constant_struct<T_beta>::value) {
    ops_partials.edge3_.partials_ = value_of(x).transpose() * theta_derivative;
  }
  if (!is_constant_struct<T_x>::value) {
    ops_partials.edge1_.partials_ = theta_derivative * beta_dbl.transpose();
  }
  if (!is_constant_struct<T_alpha>::value) {
    if (is_vector<T_alpha>::value)
      ops_partials.edge2_.partials_ = theta_derivative;
    else
      ops_partials.edge2_.partials_[0] = theta_derivative_sum;
  }

  return ops_partials.build(logp);
}

template <typename T_y, typename T_x, typename T_alpha, typename T_beta>
inline typename return_type<T_x, T_beta, T_alpha>::type
bernoulli_logit_glm_lpmf(const T_y &y, const T_x &x, const T_alpha &alpha,
                         const T_beta &beta) {
  return bernoulli_logit_glm_lpmf<false>(y, x, alpha, beta);
}
}  // namespace math
}  // namespace stan
#endif


//  ratio.hpp  ---------------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes
//  Copyright 2009 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*

This code was derived by Beman Dawes from Howard Hinnant's time2_demo prototype.
Many thanks to Howard for making his code available under the Boost license.
The original code was modified to conform to Boost conventions and to section
20.4 Compile-time rational arithmetic [ratio], of the C++ committee working
paper N2798.
See http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2008/n2798.pdf.

time2_demo contained this comment:

    Much thanks to Andrei Alexandrescu,
                   Walter Brown,
                   Peter Dimov,
                   Jeff Garland,
                   Terry Golubiewski,
                   Daniel Krugler,
                   Anthony Williams.
*/

// The way overflow is managed for ratio_less is taken from llvm/libcxx/include/ratio

#ifndef BOOST_RATIO_DETAIL_RATIO_OPERATIONS_HPP
#define BOOST_RATIO_DETAIL_RATIO_OPERATIONS_HPP

#include <boost/ratio/config.hpp>
#include <boost/ratio/detail/mpl/abs.hpp>
#include <boost/ratio/detail/mpl/sign.hpp>
#include <cstdlib>
#include <climits>
#include <limits>
#include <boost/cstdint.hpp>
#include <boost/type_traits/integral_constant.hpp>
#include <boost/core/enable_if.hpp>
#include <boost/integer_traits.hpp>

//
// We simply cannot include this header on gcc without getting copious warnings of the kind:
//
// boost/integer.hpp:77:30: warning: use of C99 long long integer constant
//
// And yet there is no other reasonable implementation, so we declare this a system header
// to suppress these warnings.
//
#if defined(__GNUC__) && (__GNUC__ >= 4)
#pragma GCC system_header
#endif

namespace mars_boost {} namespace boost = mars_boost; namespace mars_boost
{

//----------------------------------------------------------------------------//
//                                 helpers                                    //
//----------------------------------------------------------------------------//

namespace ratio_detail
{

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y, mars_boost::intmax_t = mpl::sign_c<mars_boost::intmax_t, Y>::value>
  class br_add;

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_add<X, Y, 1>
  {
      static const mars_boost::intmax_t min = mars_boost::integer_traits<mars_boost::intmax_t>::const_min;
      static const mars_boost::intmax_t max = mars_boost::integer_traits<mars_boost::intmax_t>::const_max;

      BOOST_RATIO_STATIC_ASSERT(X <= max - Y , BOOST_RATIO_OVERFLOW_IN_ADD, ());
  public:
      static const mars_boost::intmax_t value = X + Y;
  };

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_add<X, Y, 0>
  {
  public:
      static const mars_boost::intmax_t value = X;
  };

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_add<X, Y, -1>
  {
      static const mars_boost::intmax_t min = mars_boost::integer_traits<mars_boost::intmax_t>::const_min;
      static const mars_boost::intmax_t max = mars_boost::integer_traits<mars_boost::intmax_t>::const_max;

      BOOST_RATIO_STATIC_ASSERT(min - Y <= X, BOOST_RATIO_OVERFLOW_IN_ADD, ());
  public:
      static const mars_boost::intmax_t value = X + Y;
  };

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y, mars_boost::intmax_t = mpl::sign_c<mars_boost::intmax_t, Y>::value>
  class br_sub;

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_sub<X, Y, 1>
  {
      static const mars_boost::intmax_t min = mars_boost::integer_traits<mars_boost::intmax_t>::const_min;
      static const mars_boost::intmax_t max = mars_boost::integer_traits<mars_boost::intmax_t>::const_max;

      BOOST_RATIO_STATIC_ASSERT(min + Y <= X, BOOST_RATIO_OVERFLOW_IN_SUB, ());
  public:
      static const mars_boost::intmax_t value = X - Y;
  };

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_sub<X, Y, 0>
  {
  public:
      static const mars_boost::intmax_t value = X;
  };

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_sub<X, Y, -1>
  {
      static const mars_boost::intmax_t min = mars_boost::integer_traits<mars_boost::intmax_t>::const_min;
      static const mars_boost::intmax_t max = mars_boost::integer_traits<mars_boost::intmax_t>::const_max;

      BOOST_RATIO_STATIC_ASSERT(X <= max + Y, BOOST_RATIO_OVERFLOW_IN_SUB, ());
  public:
      static const mars_boost::intmax_t value = X - Y;
  };

  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_mul
  {
      static const mars_boost::intmax_t nan =
          mars_boost::intmax_t(BOOST_RATIO_UINTMAX_C(1) << (sizeof(mars_boost::intmax_t) * CHAR_BIT - 1));
      static const mars_boost::intmax_t min = mars_boost::integer_traits<mars_boost::intmax_t>::const_min;
      static const mars_boost::intmax_t max = mars_boost::integer_traits<mars_boost::intmax_t>::const_max;

      static const mars_boost::intmax_t a_x = mpl::abs_c<mars_boost::intmax_t, X>::value;
      static const mars_boost::intmax_t a_y = mpl::abs_c<mars_boost::intmax_t, Y>::value;

      BOOST_RATIO_STATIC_ASSERT(X != nan, BOOST_RATIO_OVERFLOW_IN_MUL, ());
      BOOST_RATIO_STATIC_ASSERT(Y != nan, BOOST_RATIO_OVERFLOW_IN_MUL, ());
      BOOST_RATIO_STATIC_ASSERT(a_x <= max / a_y, BOOST_RATIO_OVERFLOW_IN_MUL, ());
  public:
      static const mars_boost::intmax_t value = X * Y;
  };

  template <mars_boost::intmax_t Y>
  class br_mul<0, Y>
  {
  public:
      static const mars_boost::intmax_t value = 0;
  };

  template <mars_boost::intmax_t X>
  class br_mul<X, 0>
  {
  public:
      static const mars_boost::intmax_t value = 0;
  };

  template <>
  class br_mul<0, 0>
  {
  public:
      static const mars_boost::intmax_t value = 0;
  };

  // Not actually used but left here in case needed in future maintenance
  template <mars_boost::intmax_t X, mars_boost::intmax_t Y>
  class br_div
  {
      static const mars_boost::intmax_t nan = mars_boost::intmax_t(BOOST_RATIO_UINTMAX_C(1) << (sizeof(mars_boost::intmax_t) * CHAR_BIT - 1));
      static const mars_boost::intmax_t min = mars_boost::integer_traits<mars_boost::intmax_t>::const_min;
      static const mars_boost::intmax_t max = mars_boost::integer_traits<mars_boost::intmax_t>::const_max;

      BOOST_RATIO_STATIC_ASSERT(X != nan, BOOST_RATIO_OVERFLOW_IN_DIV, ());
      BOOST_RATIO_STATIC_ASSERT(Y != nan, BOOST_RATIO_OVERFLOW_IN_DIV, ());
      BOOST_RATIO_STATIC_ASSERT(Y != 0, BOOST_RATIO_DIVIDE_BY_0, ());
  public:
      static const mars_boost::intmax_t value = X / Y;
  };

  // ratio arithmetic
  template <class R1, class R2> struct ratio_add;
  template <class R1, class R2> struct ratio_subtract;
  template <class R1, class R2> struct ratio_multiply;
  template <class R1, class R2> struct ratio_divide;

  template <class R1, class R2>
  struct ratio_add
  {
      //The nested typedef type shall be a synonym for ratio<T1, T2>::type where T1 has the value R1::num *
      //R2::den + R2::num * R1::den and T2 has the value R1::den * R2::den.
      // As the preceding doesn't works because of overflow on mars_boost::intmax_t we need something more elaborated.
  private:
      static const mars_boost::intmax_t gcd_n1_n2 = mpl::gcd_c<mars_boost::intmax_t, R1::num, R2::num>::value;
      static const mars_boost::intmax_t gcd_d1_d2 = mpl::gcd_c<mars_boost::intmax_t, R1::den, R2::den>::value;
  public:
      // No need to normalize as ratio_multiply is already normalized
      typedef typename ratio_multiply
         <
             ratio<gcd_n1_n2, R1::den / gcd_d1_d2>,
             ratio
             <
                 mars_boost::ratio_detail::br_add
                 <
                     mars_boost::ratio_detail::br_mul<R1::num / gcd_n1_n2, R2::den / gcd_d1_d2>::value,
                     mars_boost::ratio_detail::br_mul<R2::num / gcd_n1_n2, R1::den / gcd_d1_d2>::value
                 >::value,
                 R2::den
             >
         >::type type;
  };
  template <class R, mars_boost::intmax_t D>
  struct ratio_add<R, ratio<0,D> >
  {
    typedef R type;
  };

  template <class R1, class R2>
  struct ratio_subtract
  {
      //The nested typedef type shall be a synonym for ratio<T1, T2>::type where T1 has the value
      // R1::num *R2::den - R2::num * R1::den and T2 has the value R1::den * R2::den.
      // As the preceding doesn't works because of overflow on mars_boost::intmax_t we need something more elaborated.
  private:
      static const mars_boost::intmax_t gcd_n1_n2 = mpl::gcd_c<mars_boost::intmax_t, R1::num, R2::num>::value;
      static const mars_boost::intmax_t gcd_d1_d2 = mpl::gcd_c<mars_boost::intmax_t, R1::den, R2::den>::value;
  public:
      // No need to normalize as ratio_multiply is already normalized
      typedef typename ratio_multiply
         <
             ratio<gcd_n1_n2, R1::den / gcd_d1_d2>,
             ratio
             <
                 mars_boost::ratio_detail::br_sub
                 <
                     mars_boost::ratio_detail::br_mul<R1::num / gcd_n1_n2, R2::den / gcd_d1_d2>::value,
                     mars_boost::ratio_detail::br_mul<R2::num / gcd_n1_n2, R1::den / gcd_d1_d2>::value
                 >::value,
                 R2::den
             >
         >::type type;
  };

  template <class R, mars_boost::intmax_t D>
  struct ratio_subtract<R, ratio<0,D> >
  {
    typedef R type;
  };

  template <class R1, class R2>
  struct ratio_multiply
  {
      // The nested typedef type  shall be a synonym for ratio<R1::num * R2::den - R2::num * R1::den, R1::den * R2::den>::type.
      // As the preceding doesn't works because of overflow on mars_boost::intmax_t we need something more elaborated.
  private:
     static const mars_boost::intmax_t gcd_n1_d2 = mpl::gcd_c<mars_boost::intmax_t, R1::num, R2::den>::value;
     static const mars_boost::intmax_t gcd_d1_n2 = mpl::gcd_c<mars_boost::intmax_t, R1::den, R2::num>::value;
  public:
      typedef typename ratio
         <
             mars_boost::ratio_detail::br_mul<R1::num / gcd_n1_d2, R2::num / gcd_d1_n2>::value,
             mars_boost::ratio_detail::br_mul<R2::den / gcd_n1_d2, R1::den / gcd_d1_n2>::value
         >::type type;
  };

  template <class R1, class R2>
  struct ratio_divide
  {
      // The nested typedef type  shall be a synonym for ratio<R1::num * R2::den, R2::num * R1::den>::type.
      // As the preceding doesn't works because of overflow on mars_boost::intmax_t we need something more elaborated.
  private:
      static const mars_boost::intmax_t gcd_n1_n2 = mpl::gcd_c<mars_boost::intmax_t, R1::num, R2::num>::value;
      static const mars_boost::intmax_t gcd_d1_d2 = mpl::gcd_c<mars_boost::intmax_t, R1::den, R2::den>::value;
  public:
      typedef typename ratio
         <
             mars_boost::ratio_detail::br_mul<R1::num / gcd_n1_n2, R2::den / gcd_d1_d2>::value,
             mars_boost::ratio_detail::br_mul<R2::num / gcd_n1_n2, R1::den / gcd_d1_d2>::value
         >::type type;
  };
  template <class R1, class R2>
  struct is_evenly_divisible_by
  {
  private:
      static const mars_boost::intmax_t gcd_n1_n2 = mpl::gcd_c<mars_boost::intmax_t, R1::num, R2::num>::value;
      static const mars_boost::intmax_t gcd_d1_d2 = mpl::gcd_c<mars_boost::intmax_t, R1::den, R2::den>::value;
  public:
      typedef integral_constant<bool,
             ((R2::num / gcd_n1_n2 ==1) && (R1::den / gcd_d1_d2)==1)
      > type;
  };

  template <class T>
  struct is_ratio : public mars_boost::false_type
  {};
  template <mars_boost::intmax_t N, mars_boost::intmax_t D>
  struct is_ratio<ratio<N, D> > : public mars_boost::true_type
  {};

  template <class R1, class R2,
            mars_boost::intmax_t Q1 = R1::num / R1::den, mars_boost::intmax_t M1 = R1::num % R1::den,
            mars_boost::intmax_t Q2 = R2::num / R2::den, mars_boost::intmax_t M2 = R2::num % R2::den>
  struct ratio_less1
  {
    static const bool value = Q1 < Q2;
  };

  template <class R1, class R2, mars_boost::intmax_t Q>
  struct ratio_less1<R1, R2, Q, 0, Q, 0>
  {
    static const bool value = false;
  };

  template <class R1, class R2, mars_boost::intmax_t Q, mars_boost::intmax_t M2>
  struct ratio_less1<R1, R2, Q, 0, Q, M2>
  {
    static const bool value = true;
  };

  template <class R1, class R2, mars_boost::intmax_t Q, mars_boost::intmax_t M1>
  struct ratio_less1<R1, R2, Q, M1, Q, 0>
  {
    static const bool value = false;
  };

  template <class R1, class R2, mars_boost::intmax_t Q, mars_boost::intmax_t M1, mars_boost::intmax_t M2>
  struct ratio_less1<R1, R2, Q, M1, Q, M2>
  {
    static const bool value = ratio_less1<ratio<R2::den, M2>, ratio<R1::den, M1>
                                            >::value;
  };

  template <
      class R1,
      class R2,
      mars_boost::intmax_t S1 = mpl::sign_c<mars_boost::intmax_t, R1::num>::value,
    mars_boost::intmax_t S2 = mpl::sign_c<mars_boost::intmax_t, R2::num>::value
>
  struct ratio_less
  {
      static const bool value = S1 < S2;
  };

  template <class R1, class R2>
  struct ratio_less<R1, R2, 1LL, 1LL>
  {
      static const bool value = ratio_less1<R1, R2>::value;
  };

  template <class R1, class R2>
  struct ratio_less<R1, R2, -1LL, -1LL>
  {
      static const bool value = ratio_less1<ratio<-R2::num, R2::den>,
                                            ratio<-R1::num, R1::den> >::value;
  };

}  // namespace ratio_detail

}  // namespace mars_boost

#endif  // BOOST_RATIO_HPP


#include <Rcpp.h>

//[[Rcpp::depends(RcppEigen)]]
#include <RcppEigen.h>

#include "experimentalSetup.hpp"
#include "individual.hpp"
#include "encodingScheme.hpp"
#include "logLikelihoods.hpp"

#include "AuxiliaryFunctions.hpp"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>

//[[Rcpp::export(.oneStepApproximationCpp)]]
Rcpp::List oneStepApproximationCpp(const Eigen::VectorXd & encodedProfiles, const Eigen::VectorXd & sampleParameters,
                                   const Eigen::VectorXd & noiseParameters, const Eigen::VectorXd & mixtureParameters,
                                   const Eigen::VectorXd & coverage, const Eigen::VectorXd markerImbalances,
                                   const std::vector< std::vector< Eigen::MatrixXd > > potentialParents,
                                   const Eigen::MatrixXd & knownProfiles, const Eigen::MatrixXd & allKnownProfiles,
                                   const Eigen::VectorXd & alleleFrequencies, const double & theta,
                                   const std::size_t & numberOfContributors, const std::size_t & numberOfMarkers,
                                   const Eigen::VectorXd & numberOfAlleles, const std::size_t & levelsOfStutterRecursion,
                                   const bool & dualEstimation)
{
    const std::size_t numberOfKnownContributors = knownProfiles.cols();

    const Eigen::VectorXd tolerance = Eigen::VectorXd::Zero(4);
    const ExperimentalSetup ES(numberOfMarkers, numberOfAlleles, numberOfContributors, numberOfKnownContributors,
                               knownProfiles, allKnownProfiles, coverage, potentialParents, markerImbalances, 0.8,
                               noiseParameters, tolerance, theta, alleleFrequencies, levelsOfStutterRecursion, dualEstimation);

    const std::size_t & N = numberOfAlleles[0] * (numberOfAlleles[0] + 1) / 2.0;
    const std::size_t & numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;

    std::vector<Eigen::MatrixXd> genotypeMatrices(N);
    Eigen::VectorXd estimatedProbability = Eigen::VectorXd::Zero(N);
    double estimatedProbabilitySum = 0.0;
    double maxValue = -HUGE_VAL;

    std::size_t n = 0;
    for (std::size_t u = 0; u < numberOfUnknownContributors; u++)
    {
        const std::size_t & a = 2 * u;
        Eigen::VectorXd encodedProfiles_um = encodedProfiles;
        for (std::size_t i = 0; i < numberOfAlleles[0]; i++)
        {
            Eigen::VectorXd encodedProfiles_um = encodedProfiles;
            double tt = encodedProfiles_um[a] + 1;
            encodedProfiles_um[a] = i;

            for (std::size_t j = i; j < numberOfAlleles[0]; j++)
            {

                encodedProfiles_um[a + 1] = j;
                Individual I_umij(encodedProfiles_um, sampleParameters, noiseParameters, mixtureParameters, markerImbalances, ES);

                genotypeMatrices[n] = decoding(I_umij.EncodedProfile, ES.NumberOfAlleles, 1, numberOfUnknownContributors);
                estimatedProbability[n] = I_umij.Fitness;
                estimatedProbabilitySum += std::exp(I_umij.Fitness);

                if (I_umij.Fitness > maxValue)
                    maxValue = I_umij.Fitness;

                n++;
            }
        }
    }

    Eigen::VectorXd normalisedProbability = (estimatedProbability - maxValue * Eigen::VectorXd::Ones(N)).array().exp();
    normalisedProbability = normalisedProbability / normalisedProbability.sum();

    return Rcpp::List::create(Rcpp::Named("GenotypeMatrix") = genotypeMatrices,
                              Rcpp::Named("LogUnnormalisedProbabilitySum") = std::log(estimatedProbabilitySum),
                              Rcpp::Named("LogUnnormalisedProbability") = estimatedProbability,
                              Rcpp::Named("NormalisedProbabilities") = normalisedProbability);
}

//[[Rcpp::export(.EAApproximationCpp)]]
Rcpp::List EAApproximationCpp(const std::vector<Eigen::VectorXd> & encodedProfiles, const Eigen::VectorXd & sampleParameters,
                              const Eigen::VectorXd & noiseParameters, const Eigen::VectorXd & mixtureParameters,
                              const Eigen::VectorXd & coverage, const Eigen::VectorXd markerImbalances,
                              const std::vector< std::vector< Eigen::MatrixXd > > potentialParents,
                              const Eigen::MatrixXd & knownProfiles, const Eigen::MatrixXd & allKnownProfiles,
                              const Eigen::VectorXd & alleleFrequencies, const double & theta,
                              const std::size_t & numberOfContributors, const std::size_t & numberOfMarkers,
                              const Eigen::VectorXd & numberOfAlleles, const std::size_t & levelsOfStutterRecursion,
                              const bool & type1, const bool & dualEstimation)
{
    const std::size_t numberOfKnownContributors = knownProfiles.cols();

    const Eigen::VectorXd tolerance = Eigen::VectorXd::Zero(4);
    const ExperimentalSetup ES(numberOfMarkers, numberOfAlleles, numberOfContributors, numberOfKnownContributors,
                               knownProfiles, allKnownProfiles, coverage, potentialParents, markerImbalances, 0.8,
                               noiseParameters, tolerance, theta, alleleFrequencies, levelsOfStutterRecursion, dualEstimation);

    const std::size_t & numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;
    const std::size_t & N = encodedProfiles.size();

    std::vector<Eigen::MatrixXd> genotypeMatrices(N);
    Eigen::VectorXd estimatedProbability = Eigen::VectorXd::Zero(N);
    double estimatedProbabilitySum = 0.0;
    double maxValue = -HUGE_VAL;
    for (std::size_t n = 0; n < N; n++)
    {
        const Eigen::VectorXd & encodedProfiles_n = encodedProfiles[n];
        Individual I(encodedProfiles_n, sampleParameters, noiseParameters, mixtureParameters, markerImbalances, ES);

        genotypeMatrices[n] = decoding(I.EncodedProfile, ES.NumberOfAlleles, 1, numberOfUnknownContributors);

        if (type1) {
            estimatedProbability[n] = I.Fitness;
            estimatedProbabilitySum += std::exp(I.Fitness - estimatedProbability[0]);
        }
        else {
            estimatedProbability[n] = I.Fitness;
            estimatedProbabilitySum += std::exp(I.Fitness);
        }

        if (I.Fitness > maxValue)
            maxValue = I.Fitness;
    }

    Eigen::VectorXd normalisedProbability = (estimatedProbability - maxValue * Eigen::VectorXd::Ones(N)).array().exp();
    normalisedProbability = normalisedProbability / normalisedProbability.sum();

    double logEstimatedProbabilitySum = std::log(estimatedProbabilitySum);
    if (type1) {
        logEstimatedProbabilitySum += estimatedProbability[0];
    }

    return Rcpp::List::create(Rcpp::Named("GenotypeMatrix") = genotypeMatrices,
                              Rcpp::Named("LogUnnormalisedProbabilitySum") = logEstimatedProbabilitySum,
                              Rcpp::Named("LogUnnormalisedProbability") = estimatedProbability,
                              Rcpp::Named("NormalisedProbabilities") = normalisedProbability);
}

////
struct sampledPriorGenotype {
    Eigen::VectorXd encodedGenotype;
    double priorProbability;

    sampledPriorGenotype(Eigen::VectorXd encodedGenotype_, double priorProbability_) :
        encodedGenotype(encodedGenotype_), priorProbability(priorProbability_) {};
};

Eigen::MatrixXd expectedContributionMarker(const Eigen::MatrixXd & genotype, const std::vector<Eigen::MatrixXd> &potentialParents_m,
                                           const double & levelsOfStutterRecursion)
{
    const std::size_t & N = genotype.rows();
    const std::size_t & M = genotype.cols();

    Eigen::MatrixXd expectedContributionProfile_m = Eigen::MatrixXd::Zero(N, M);
    for (std::size_t u = 0; u < M; u++)
    {
        Eigen::VectorXd genotype_u = genotype.col(u);
        Eigen::VectorXd stutterContribution = Eigen::VectorXd::Zero(N);
        for (std::size_t n = 0; n < N; n++)
        {
            stutterContribution[n] = ParentStutterContribution(n, levelsOfStutterRecursion, 1, genotype_u, potentialParents_m, N);
        }

        expectedContributionProfile_m.col(u) = genotype_u + stutterContribution;
    }

    return expectedContributionProfile_m;
}

void adjustCoverage(Eigen::VectorXd & adjustedCoverage, const double & sampleParameter, const Eigen::VectorXd & mixtureParamters,
                    const double & markerParameter, const Eigen::MatrixXd & genotype, const std::vector< Eigen::MatrixXd > & potentialParents_m,
                    const double & levelsOfStutterRecursion)
{
    const std::size_t & N = adjustedCoverage.size();
    const Eigen::VectorXd & E_c = expectedContributionMarker(genotype, potentialParents_m, levelsOfStutterRecursion);
    const Eigen::VectorXd & mu_ma = sampleParameter * markerParameter * (E_c * mixtureParamters);

    adjustedCoverage = adjustedCoverage - mu_ma;
    for (std::size_t n = 0; n < N; n++)
    {
        if (adjustedCoverage[n] <= 0.0)
        {
            adjustedCoverage[n] = std::exp(adjustedCoverage[n]);
        }

    }
}

double calculatePriorProbabiliy(const Eigen::VectorXd & E, const Eigen::MatrixXd &knownProfiles, const Eigen::VectorXd &coverage,
                                std::vector<Eigen::MatrixXd> potentialParents_m,
                                const double &sampleParameter, const Eigen::VectorXd &mixtureParameters, const double &markerParameter,
                                const std::size_t & numberOfContributors, const double & levelsOfStutterRecursion,
                                const bool &suggestionBool)
{
    if (!suggestionBool)
    {
        return 0.0;
    }

    const std::size_t numberOfKnownContributors = knownProfiles.cols();
    const std::size_t N = E.size();
    const std::size_t M = coverage.size();

    Eigen::MatrixXd genotype = Eigen::MatrixXd::Zero(M, numberOfContributors);
    for (std::size_t k = 0; k < numberOfKnownContributors; k++)
    {
        genotype.col(k) = knownProfiles.col(k);
    }

    Eigen::VectorXd adjustedCoverage = coverage;
    if (suggestionBool)
    {
        adjustCoverage(adjustedCoverage, sampleParameter, mixtureParameters, markerParameter, genotype, potentialParents_m, levelsOfStutterRecursion);
    }
    else
    {
        adjustedCoverage = Eigen::VectorXd::Ones(M);
    }

    double priorProbability = 0.0;
    for (std::size_t n = 0; n < N; n++)
    {
        std::size_t u = std::floor(n / 2);
        Eigen::VectorXd proportionalProbability = adjustedCoverage / adjustedCoverage.sum();

        genotype(E[n], numberOfKnownContributors + u) += 1.0;

        if (suggestionBool)
        {
            Eigen::VectorXd singleAllele = Eigen::VectorXd::Zero(M);
            singleAllele[E[n]] = 1.0;

            priorProbability += std::log(proportionalProbability[E[n]]);
            adjustCoverage(adjustedCoverage, sampleParameter, mixtureParameters.row(u), markerParameter, singleAllele, potentialParents_m,
                           levelsOfStutterRecursion);
        }
    }

    return priorProbability;
}

sampledPriorGenotype samplePriorGenotypeMarker(const Eigen::MatrixXd &knownProfiles, const Eigen::VectorXd &coverage,
                                               const std::vector<Eigen::MatrixXd> &potentialParents_m,
                                               const double &sampleParameter, const Eigen::VectorXd &mixtureParameters,
                                               const double &markerParameter, const std::size_t numberOfContributors,
                                               const double &levelsOfStutterRecursion, const bool & suggestionBool,
                                               const std::size_t &seed)
{
    boost::random::mt19937 rng(seed);
    boost::random::uniform_real_distribution<> uniform(0, 1);

    const std::size_t &N = coverage.size();
    const std::size_t &numberOfKnownContributors = knownProfiles.cols();
    const std::size_t &numberOfUnknownContributors = numberOfContributors - numberOfKnownContributors;

    Eigen::MatrixXd sampledGenotype = Eigen::MatrixXd::Zero(N, numberOfContributors);
    for (std::size_t k = 0; k < numberOfKnownContributors; k++)
        sampledGenotype.col(k) = knownProfiles.col(k);

    Eigen::VectorXd adjustedCoverage = Eigen::VectorXd::Ones(N);
    if (suggestionBool)
    {
        adjustedCoverage = coverage;
        adjustCoverage(adjustedCoverage, sampleParameter, mixtureParameters, markerParameter, sampledGenotype, potentialParents_m,
                       levelsOfStutterRecursion);
    }

    Eigen::VectorXd sampledGenotypeEncoded = Eigen::VectorXd::Zero(2 * numberOfUnknownContributors);
    double sampledPriorProbability = 0.0;
    for (std::size_t u = 0; u < numberOfUnknownContributors; u++)
    {
        for (std::size_t i = 0; i < 2; i++)
        {
            Eigen::VectorXd proportionalProbability = adjustedCoverage / adjustedCoverage.sum();
            Eigen::VectorXd partialSumProportionalProbability = partialSumEigen(proportionalProbability);
            double randomVariate = uniform(rng);
            std::size_t j = 0.0;
            while (randomVariate > partialSumProportionalProbability[j + 1])
                j++;

            sampledGenotype(j, numberOfKnownContributors + u) += 1.0;

            if (suggestionBool)
            {
                Eigen::VectorXd singleAllele = Eigen::VectorXd::Zero(N);
                singleAllele[j] = 1.0;

                sampledPriorProbability += std::log(proportionalProbability[j]);
                adjustCoverage(adjustedCoverage, sampleParameter, mixtureParameters.row(u), markerParameter, singleAllele, potentialParents_m,
                               levelsOfStutterRecursion);
            }

            if (i == 0)
            {
                sampledGenotypeEncoded[2 * u] = j;
            }
            else
            {
                if (j < sampledGenotypeEncoded[2 * u])
                {
                    sampledGenotypeEncoded[2 * u + i] = sampledGenotypeEncoded[2 * u];
                    sampledGenotypeEncoded[2 * u] = j;
                }
                else
                {
                    sampledGenotypeEncoded[2 * u + i] = j;
                }
            }
        }
    }

    sampledPriorGenotype spg(sampledGenotypeEncoded, sampledPriorProbability);
    return spg;
}

//[[Rcpp::export(.samplePosteriorGenotypesGuidedCpp)]]
Rcpp::List samplePosteriorGenotypesGuidedCpp(const Eigen::VectorXd & encodedProfiles, const Eigen::VectorXd & sampleParameters,
                                             const Eigen::VectorXd & noiseParameters, const Eigen::VectorXd & mixtureParameters,
                                             const Eigen::VectorXd markerParameters, const Eigen::VectorXd & coverage,
                                             const std::vector< std::vector< Eigen::MatrixXd > > potentialParents,
                                             const Eigen::MatrixXd & knownProfiles, const Eigen::MatrixXd & allKnownProfiles,
                                             const Eigen::VectorXd & alleleFrequencies, const double & theta,
                                             const std::size_t & numberOfContributors,  const Eigen::VectorXd & numberOfAlleles,
                                             const std::size_t & levelsOfStutterRecursion,
                                             const std::size_t &numberOfSimulations, const bool & suggestionBool,
                                             const std::size_t &seed, const bool & dualEstimation)
{
    boost::random::mt19937 rng(seed);
    boost::random::uniform_int_distribution<> seedShift(0, seed);
    boost::random::uniform_real_distribution<> uniform(0, 1);

    const std::size_t &numberOfKnownContributors = knownProfiles.cols();
    const std::size_t &numberOfMarkers = numberOfAlleles.size();

    const Eigen::VectorXd tolerance = Eigen::VectorXd::Zero(4);
    const ExperimentalSetup ES(1, numberOfAlleles, numberOfContributors, numberOfKnownContributors,
                               knownProfiles, allKnownProfiles, coverage, potentialParents, markerParameters, 0.0,
                               noiseParameters, tolerance, theta, alleleFrequencies, levelsOfStutterRecursion, dualEstimation);

    Eigen::VectorXd currentEncodedGenotype = encodedProfiles;
    double currentPriorProbability = calculatePriorProbabiliy(currentEncodedGenotype, knownProfiles, coverage,
                                                              potentialParents[0], sampleParameters[0], mixtureParameters,
                                                              markerParameters[0], numberOfContributors, levelsOfStutterRecursion,
                                                              suggestionBool);

    Individual currentI(currentEncodedGenotype, sampleParameters, noiseParameters, mixtureParameters, markerParameters, ES);
    double currentLogLikelihood = currentI.Fitness;

    std::vector<Eigen::MatrixXd> sampledGenotypeList(numberOfSimulations);
    Eigen::VectorXd unnormalisedLogLikelihood = Eigen::VectorXd::Zero(numberOfSimulations);
    double acceptedProposals = 0.0;
    for (std::size_t n = 0; n < numberOfSimulations; n++)
    {
        int seedShift_n = seedShift(rng);
        const sampledPriorGenotype & newGenotype = samplePriorGenotypeMarker(knownProfiles, coverage, potentialParents[0],
                                                                             sampleParameters[0], mixtureParameters,
                                                                             markerParameters[0], numberOfContributors, levelsOfStutterRecursion,
                                                                             suggestionBool, seed + n + seedShift_n);

        const Eigen::VectorXd & newEncodedGenotype = newGenotype.encodedGenotype;
        const double & newPriorProbability = newGenotype.priorProbability;

        const Individual newI(newEncodedGenotype, sampleParameters, noiseParameters, mixtureParameters, markerParameters, ES);
        const double & newLogLikelihood = newI.Fitness;

        const double & logHastingsRatio = newLogLikelihood - currentLogLikelihood + currentPriorProbability - newPriorProbability;
        double acceptProbability = 1.0;
        if (logHastingsRatio < 0)
        {
            acceptProbability = std::exp(logHastingsRatio);
        }

        double randomVariate = uniform(rng);
        if (randomVariate < acceptProbability)
        {
            currentEncodedGenotype = newEncodedGenotype;
            currentPriorProbability = newPriorProbability;
            currentLogLikelihood = newLogLikelihood;
            acceptedProposals += 1;
        }

        Eigen::MatrixXd currentDecodedGenotype = decoding(currentEncodedGenotype, numberOfAlleles, 1,
                                                          numberOfContributors - numberOfKnownContributors);
        sampledGenotypeList[n] = currentDecodedGenotype;
        unnormalisedLogLikelihood[n] = currentLogLikelihood;
    }

    return Rcpp::List::create(Rcpp::Named("SampledGenotypes") = sampledGenotypeList,
                              Rcpp::Named("UnnormalisedLogLikelihood") = unnormalisedLogLikelihood,
                              Rcpp::Named("AcceptedProposals") = acceptedProposals);
}


// Copyright 2019 Yan Yan
//
// 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 <tensorview/tensorview.h>
#include <boost/geometry.hpp>
#include <torch/script.h>
#include <vector>

namespace spconv {
namespace functor{
template <typename Device, typename T, typename Index>
struct NonMaxSupressionFunctor {
  Index operator()(const Device &d, tv::TensorView<Index> keep,
                   tv::TensorView<const T> boxes, T threshold, T eps);
};

template <typename Device, typename T, typename Index>
struct rotateNonMaxSupressionFunctor {
  Index operator()(const Device &d, tv::TensorView<Index> keep,
                   tv::TensorView<const T> boxCorners,
                   tv::TensorView<const T> standupIoU, T threshold);
};

}
}

namespace spconv {

namespace functor {
template <typename T, typename Index>
struct NonMaxSupressionFunctor<tv::CPU, T, Index> {
  Index operator()(const tv::CPU &d, tv::TensorView<Index> keep,
                   tv::TensorView<const T> boxes, T threshold, T eps) {
    auto ndets = boxes.dim(0);
    auto suppressed = std::vector<Index>(ndets);
    auto area = std::vector<T>(ndets);
    for (int i = 0; i < ndets; ++i) {
      area[i] =
          (boxes(i, 2) - boxes(i, 0) + eps) * (boxes(i, 3) - boxes(i, 1) + eps);
    }
    int i, j;
    T xx1, xx2, w, h, inter, ovr;
    int keepNum = 0;
    for (int _i = 0; _i < ndets; ++_i) {
      i = _i;
      if (suppressed[i] == 1)
        continue;
      keep[keepNum] = i;
      keepNum += 1;
      for (int _j = _i + 1; _j < ndets; ++_j) {
        j = _j;
        if (suppressed[j] == 1)
          continue;
        xx2 = std::min(boxes(i, 2), boxes(j, 2));
        xx1 = std::max(boxes(i, 0), boxes(j, 0));
        w = xx2 - xx1 + eps;
        if (w > 0) {
          xx2 = std::min(boxes(i, 3), boxes(j, 3));
          xx1 = std::max(boxes(i, 1), boxes(j, 1));
          h = xx2 - xx1 + eps;
          if (h > 0) {
            inter = w * h;
            ovr = inter / (area[i] + area[j] - inter);
            if (ovr >= threshold)
              suppressed[j] = 1;
          }
        }
      }
    }
    return keepNum;
  }
};

template <typename T, typename Index>
struct rotateNonMaxSupressionFunctor<tv::CPU, T, Index> {
  Index operator()(const tv::CPU &d, tv::TensorView<Index> keep,
                   tv::TensorView<const T> boxCorners,
                   tv::TensorView<const T> standupIoU, T threshold) {
    auto ndets = boxCorners.dim(0);
    auto suppressed = std::vector<Index>(ndets);
    int i, j;
    namespace bg = boost::geometry;
    typedef bg::model::point<T, 2, bg::cs::cartesian> point_t;
    typedef bg::model::polygon<point_t> polygon_t;
    polygon_t poly, qpoly;
    std::vector<polygon_t> poly_inter, poly_union;
    T inter_area, union_area, overlap;
    int keepNum = 0;
    for (int _i = 0; _i < ndets; ++_i) {
      i = _i;
      if (suppressed[i] == 1)
        continue;
      keep[keepNum] = i;
      keepNum += 1;
      for (int _j = _i + 1; _j < ndets; ++_j) {
        j = _j;
        if (suppressed[j] == 1)
          continue;
        if (standupIoU(i, j) <= 0.0)
          continue;
        bg::append(poly, point_t(boxCorners(i, 0, 0), boxCorners(i, 0, 1)));
        bg::append(poly, point_t(boxCorners(i, 1, 0), boxCorners(i, 1, 1)));
        bg::append(poly, point_t(boxCorners(i, 2, 0), boxCorners(i, 2, 1)));
        bg::append(poly, point_t(boxCorners(i, 3, 0), boxCorners(i, 3, 1)));
        bg::append(poly, point_t(boxCorners(i, 0, 0), boxCorners(i, 0, 1)));
        bg::append(qpoly, point_t(boxCorners(j, 0, 0), boxCorners(j, 0, 1)));
        bg::append(qpoly, point_t(boxCorners(j, 1, 0), boxCorners(j, 1, 1)));
        bg::append(qpoly, point_t(boxCorners(j, 2, 0), boxCorners(j, 2, 1)));
        bg::append(qpoly, point_t(boxCorners(j, 3, 0), boxCorners(j, 3, 1)));
        bg::append(qpoly, point_t(boxCorners(j, 0, 0), boxCorners(j, 0, 1)));
        bg::intersection(poly, qpoly, poly_inter);

        if (!poly_inter.empty()) {
          inter_area = bg::area(poly_inter.front());
          bg::union_(poly, qpoly, poly_union);
          if (!poly_union.empty()) { // ignore invalid box
            union_area = bg::area(poly_union.front());
            overlap = inter_area / union_area;
            if (overlap >= threshold)
              suppressed[j] = 1;
            poly_union.clear();
          }
        }
        poly.clear();
        qpoly.clear();
        poly_inter.clear();
      }
    }
    return keepNum;
  }
};

} // namespace functor

#define DECLARE_CPU_T_INDEX(T, Index)                                          \
  template struct functor::NonMaxSupressionFunctor<tv::CPU, T, Index>;         \
  template struct functor::rotateNonMaxSupressionFunctor<tv::CPU, T, Index>;

#define DECLARE_CPU_INDEX(Index)                                               \
  DECLARE_CPU_T_INDEX(float, Index);                                           \
  DECLARE_CPU_T_INDEX(double, Index);

DECLARE_CPU_INDEX(int);
DECLARE_CPU_INDEX(long);

#undef DECLARE_CPU_INDEX
#undef DECLARE_CPU_T_INDEX

} // namespace spconv


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_HYPERGEOMETRIC_PFQ_HPP
#define BOOST_MATH_HYPERGEOMETRIC_PFQ_HPP

#include <boost/math/special_functions/detail/hypergeometric_pFq_checked_series.hpp>
#include <chrono>
#include <initializer_list>

namespace boost {
   namespace math {

      namespace detail {

         struct pFq_termination_exception : public std::runtime_error
         {
            pFq_termination_exception(const char* p) : std::runtime_error(p) {}
         };

         struct timed_iteration_terminator
         {
            timed_iteration_terminator(std::uintmax_t i, double t) : max_iter(i), max_time(t), start_time(std::chrono::system_clock::now()) {}

            bool operator()(std::uintmax_t iter)const
            {
               if (iter > max_iter)
                  boost::throw_exception(boost::math::detail::pFq_termination_exception("pFq exceeded maximum permitted iterations."));
               if (std::chrono::duration<double>(std::chrono::system_clock::now() - start_time).count() > max_time)
                  boost::throw_exception(boost::math::detail::pFq_termination_exception("pFq exceeded maximum permitted evaluation time."));
               return false;
            }

            std::uintmax_t max_iter;
            double max_time;
            std::chrono::system_clock::time_point start_time;
         };

      }

      template <class Seq, class Real, class Policy>
      inline typename tools::promote_args<Real, typename Seq::value_type>::type hypergeometric_pFq(const Seq& aj, const Seq& bj, const Real& z, Real* p_abs_error, const Policy& pol)
      {
         typedef typename tools::promote_args<Real, typename Seq::value_type>::type result_type;
         typedef typename policies::evaluation<result_type, Policy>::type value_type;
         typedef typename policies::normalise<
            Policy,
            policies::promote_float<false>,
            policies::promote_double<false>,
            policies::discrete_quantile<>,
            policies::assert_undefined<> >::type forwarding_policy;

         BOOST_MATH_STD_USING

         long long scale = 0;
         std::pair<value_type, value_type> r = boost::math::detail::hypergeometric_pFq_checked_series_impl(aj, bj, value_type(z), pol, boost::math::detail::iteration_terminator(boost::math::policies::get_max_series_iterations<forwarding_policy>()), scale);
         r.first *= exp(Real(scale));
         r.second *= exp(Real(scale));
         if (p_abs_error)
            *p_abs_error = static_cast<Real>(r.second) * boost::math::tools::epsilon<Real>();
         return policies::checked_narrowing_cast<result_type, Policy>(r.first, "boost::math::hypergeometric_pFq<%1%>(%1%,%1%,%1%)");
      }

      template <class Seq, class Real>
      inline typename tools::promote_args<Real, typename Seq::value_type>::type hypergeometric_pFq(const Seq& aj, const Seq& bj, const Real& z, Real* p_abs_error = 0)
      {
         return hypergeometric_pFq(aj, bj, z, p_abs_error, boost::math::policies::policy<>());
      }

      template <class R, class Real, class Policy>
      inline typename tools::promote_args<Real, R>::type hypergeometric_pFq(const std::initializer_list<R>& aj, const std::initializer_list<R>& bj, const Real& z, Real* p_abs_error, const Policy& pol)
      {
         return hypergeometric_pFq<std::initializer_list<R>, Real, Policy>(aj, bj, z, p_abs_error, pol);
      }

      template <class R, class Real>
      inline typename tools::promote_args<Real, R>::type  hypergeometric_pFq(const std::initializer_list<R>& aj, const std::initializer_list<R>& bj, const Real& z, Real* p_abs_error = 0)
      {
         return hypergeometric_pFq<std::initializer_list<R>, Real>(aj, bj, z, p_abs_error);
      }

      template <class T>
      struct scoped_precision
      {
         scoped_precision(unsigned p)
         {
            old_p = T::default_precision();
            T::default_precision(p);
         }
         ~scoped_precision()
         {
            T::default_precision(old_p);
         }
         unsigned old_p;
      };

      template <class Seq, class Real, class Policy>
      Real hypergeometric_pFq_precision(const Seq& aj, const Seq& bj, Real z, unsigned digits10, double timeout, const Policy& pol)
      {
         unsigned current_precision = digits10 + 5;

         for (auto ai = aj.begin(); ai != aj.end(); ++ai)
         {
            current_precision = (std::max)(current_precision, ai->precision());
         }
         for (auto bi = bj.begin(); bi != bj.end(); ++bi)
         {
            current_precision = (std::max)(current_precision, bi->precision());
         }
         current_precision = (std::max)(current_precision, z.precision());

         Real r, norm;
         std::vector<Real> aa(aj), bb(bj);
         do
         {
            scoped_precision<Real> p(current_precision);
            for (auto ai = aa.begin(); ai != aa.end(); ++ai)
               ai->precision(current_precision);
            for (auto bi = bb.begin(); bi != bb.end(); ++bi)
               bi->precision(current_precision);
            z.precision(current_precision);
            try
            {
               long long scale = 0;
               std::pair<Real, Real> rp = boost::math::detail::hypergeometric_pFq_checked_series_impl(aa, bb, z, pol, boost::math::detail::timed_iteration_terminator(boost::math::policies::get_max_series_iterations<Policy>(), timeout), scale);
               rp.first *= exp(Real(scale));
               rp.second *= exp(Real(scale));

               r = rp.first;
               norm = rp.second;

               unsigned cancellation;
               try {
                  cancellation = itrunc(log10(abs(norm / r)));
               }
               catch (const boost::math::rounding_error&)
               {
                  // Happens when r is near enough zero:
                  cancellation = UINT_MAX;
               }
               if (cancellation >= current_precision - 1)
               {
                  current_precision *= 2;
                  continue;
               }
               unsigned precision_obtained = current_precision - 1 - cancellation;
               if (precision_obtained < digits10)
               {
                  current_precision += digits10 - precision_obtained + 5;
               }
               else
                  break;
            }
            catch (const boost::math::evaluation_error&)
            {
               current_precision *= 2;
            }
            catch (const detail::pFq_termination_exception& e)
            {
               //
               // Either we have exhausted the number of series iterations, or the timeout.
               // Either way we quit now.
               throw boost::math::evaluation_error(e.what());
            }
         } while (true);

         return r;
      }
      template <class Seq, class Real>
      Real hypergeometric_pFq_precision(const Seq& aj, const Seq& bj, const Real& z, unsigned digits10, double timeout = 0.5)
      {
         return hypergeometric_pFq_precision(aj, bj, z, digits10, timeout, boost::math::policies::policy<>());
      }

      template <class Real, class Policy>
      Real hypergeometric_pFq_precision(const std::initializer_list<Real>& aj, const std::initializer_list<Real>& bj, const Real& z, unsigned digits10, double timeout, const Policy& pol)
      {
         return hypergeometric_pFq_precision< std::initializer_list<Real>, Real>(aj, bj, z, digits10, timeout, pol);
      }
      template <class Real>
      Real hypergeometric_pFq_precision(const std::initializer_list<Real>& aj, const std::initializer_list<Real>& bj, const Real& z, unsigned digits10, double timeout = 0.5)
      {
         return hypergeometric_pFq_precision< std::initializer_list<Real>, Real>(aj, bj, z, digits10, timeout, boost::math::policies::policy<>());
      }

   }
} // namespaces

#endif // BOOST_MATH_BESSEL_ITERATORS_HPP


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file multi_sensor_sigma_point_update_policy.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/types.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/descriptor.hpp>
#include <fl/model/sensor/joint_sensor_iid.hpp>
#include <fl/filter/gaussian/transform/point_set.hpp>
#include <fl/filter/gaussian/quadrature/sigma_point_quadrature.hpp>

namespace fl
{

// Forward declarations
template <typename...> class MultiSensorSigmaPointUpdatePolicy;

/**
 * \internal
 */
template <
    typename SigmaPointQuadrature,
    typename NonJoinSensor
>
class MultiSensorSigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          NonJoinSensor>
{
    static_assert(
        std::is_base_of<
            internal::JointSensorIidType, NonJoinSensor
        >::value,
        "\n\n\n"
        "====================================================================\n"
        "= Static Assert: You are using the wrong observation model type    =\n"
        "====================================================================\n"
        "  Observation model type must be a JointSensor<...>.      \n"
        "  For single observation model, use the regular Gaussian filter     \n"
        "  or the regular SigmaPointUpdatePolicy if you are specifying       \n"
        "  the update policy explicitly fo the GaussianFilter.               \n"
        "====================================================================\n"
    );
};

/**
 * \brief Represents an update policy update for multiple sensors. This instance
 *        expects a \a JointSensor<>. The model is forwarded  as
 *        NonAdditive<JointSensor> to the actual
 *        implementation. In case you want to use the model as Additive, you may
 *        specify this explicitly using Additive<JointSensor> or
 *        UseAsAdditive<JointSensor>::Type
 */
template <
    typename SigmaPointQuadrature,
    typename MultipleOfLocalSensor
>
class MultiSensorSigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          JointSensor<MultipleOfLocalSensor>>
    : public MultiSensorSigmaPointUpdatePolicy<
                SigmaPointQuadrature,
                NonAdditive<JointSensor<MultipleOfLocalSensor>>>
{ };

/**
 * \brief Represents an update policy update functor for multiple sensors.
 *        This instance expects a \a NonAdditive<JointSensor<>>.
 *        The implementation exploits factorization in the joint observation.
 *        The update is performed for each sensor separately.
 */
template <
    typename SigmaPointQuadrature,
    typename MultipleOfLocalSensor
>
class MultiSensorSigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          NonAdditive<JointSensor<MultipleOfLocalSensor>>>
    : public Descriptor
{
public:
    typedef JointSensor<MultipleOfLocalSensor> JointModel;

    typedef typename JointModel::State State;
    typedef typename JointModel::Obsrv Obsrv;
    typedef typename JointModel::LocalObsrv LocalObsrv;
    typedef typename JointModel::LocalNoise LocalObsrvNoise;

    template <typename Belief>
    void operator()(JointModel& obsrv_function,
                    const SigmaPointQuadrature& quadrature,
                    const Belief& prior_belief,
                    const Obsrv& y,
                    Belief& posterior_belief)
    {
        auto& sensor_model = obsrv_function.local_sensor();

        /* ------------------------------------------ */
        /* - Determine the number of quadrature     - */
        /* - points needed for the given quadrature - */
        /* - in conjunction with the joint Gaussian - */
        /* - p(State, LocalObsrvNoise)              - */
        /* ------------------------------------------ */
        enum : signed int
        {
            NumberOfPoints = SigmaPointQuadrature::number_of_points(
                                 JoinSizes<
                                     SizeOf<State>::Value,
                                     SizeOf<LocalObsrvNoise>::Value
                                 >::Size)
        };

        /* ------------------------------------------ */
        /* - PointSets                              - */
        /* - [p_X, p_Q] ~ p(State, LocalObsrvNoise) - */
        /* ------------------------------------------ */
        PointSet<State, NumberOfPoints> p_X;
        PointSet<LocalObsrvNoise, NumberOfPoints> p_Q;

        /* ------------------------------------------ */
        /* - PointSet [p_Y] = h(p_X, p_Q)           - */
        /* ------------------------------------------ */
        PointSet<LocalObsrv, NumberOfPoints> p_Y;

        /* ------------------------------------------ */
        /* - Transform p(State, LocalObsrvNoise) to - */
        /* - point sets [p_X, p_Q]                  - */
        /* ------------------------------------------ */
        quadrature.transform_to_points(
            prior_belief,
            Gaussian<LocalObsrvNoise>(sensor_model.noise_dimension()),
            p_X,
            p_Q);

        /* ------------------------------------------ */
        /* - Compute expected moments of the state  - */
        /* - E[X], Cov(X, X)                        - */
        /* ------------------------------------------ */
        auto W = p_X.covariance_weights_vector().asDiagonal();
        auto mu_x = p_X.mean();
        auto X = p_X.centered_points();
        auto c_xx_inv = (X * W * X.transpose()).inverse().eval();

        /* ------------------------------------------ */
        /* - Temporary accumulators which will be   - */
        /* - used to updated the belief             - */
        /* ------------------------------------------ */
        auto C = c_xx_inv;
        auto D = State();
        D.setZero(mu_x.size());

        const int sensor_count = obsrv_function.count_local_models();
        const int dim_y = y.size() / sensor_count;

        /* ------------------------------------------ */
        /* - lambda of the sensor observation       - */
        /* - function                               - */
        /* ------------------------------------------ */
        auto&& h = [&](const State& x, const LocalObsrvNoise& w)
        {
            return sensor_model.observation(x, w);
        };

        for (int i = 0; i < sensor_count; ++i)
        {
            // validate sensor value, i.e. make sure it is finite
            if (!is_valid(y, i * dim_y, i * dim_y + dim_y)) continue;

            // select current sensor and propagate the points through h(x, w)
            sensor_model.id(i);
            quadrature.propagate_points(h, p_X, p_Q, p_Y);

            // comute expected moments of the observation and validate
            auto mu_y = p_Y.mean();
            if (!is_valid(mu_y, 0, dim_y)) continue;

            // update accumulatorsa according to the equations in PAPER REF
            auto Y = p_Y.centered_points();
            auto c_xy = (X * W * Y.transpose()).eval();
            auto c_yx = c_xy.transpose().eval();
            auto A_i = (c_yx * c_xx_inv).eval();
            auto c_yy_given_x = (
                     (Y * W * Y.transpose()) - c_yx * c_xx_inv * c_xy
                 ).eval();

            auto innovation = (y.middleRows(i * dim_y, dim_y) - mu_y).eval();
            C += A_i.transpose() * solve(c_yy_given_x, A_i);
            D += A_i.transpose() * solve(c_yy_given_x, innovation);
        }

        /* ------------------------------------------ */
        /* - Update belief according to PAPER REF   - */
        /* ------------------------------------------ */
        // make sure the posterior has the correct dimension
        posterior_belief.dimension(prior_belief.dimension());
        posterior_belief.covariance(C.inverse());
        posterior_belief.mean(mu_x + posterior_belief.covariance() * D);
    }

    virtual std::string name() const
    {
        return "MultiSensorSigmaPointUpdatePolicy<"
                + this->list_arguments(
                       "SigmaPointQuadrature",
                       "NonAdditive<SensorFunction>")
                + ">";
    }

    virtual std::string description() const
    {
        return "Multi-Sensor Sigma Point based filter update policy "
               "for joint observation model of multiple local observation "
               "models with non-additive noise.";
    }

private:
    /**
     * \brief Checks whether all vector components within the range (start, end)
     *        are finiate, i.e. not NAN nor Inf.
     */
    template <typename Vector>
    bool is_valid(Vector&& vector, int start, int end) const
    {
        for (int k = start; k < end; ++k)
        {
            if (!std::isfinite(vector(k))) return false;
        }

        return true;
    }
};

}