/* 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/level2.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/std/vector.hpp>

// Note Franck : sounds useless. It seems it's defined in bindings
// (to be checked, especially on windows)

// #define BIND_FORTRAN_LOWERCASE_UNDERSCORE
namespace siconosBindings = boost::numeric::bindings;

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

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

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

using namespace Siconos;

// ========== Products matrix - vector //

// Computation of y = A*x
//
// Two specific functions are used to handle all the cases where x or y are blocks.
// All of their blocks can also be blocks. Then we use:
// - private_prod to "slice" A when y is block, ie according to its rows.
// - private_addprod to "slice" A when x is block, and to sum over the columns of blocks to compute y = sum subA x[i].

// The following function is private and used inside prod(...) public functions.
// It is required to deal with block vectors of blocks ( of blocks ...).
// It computes res = subA*x +res, subA being a submatrix of A (rows from startRow to startRow+sizeY and columns between startCol and startCol+sizeX).
// If x is a block vector, it call the present function for all blocks.
const SiconosVector prod(const SiconosMatrix& A, const SiconosVector& x)
{
  // To compute y = A * x
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  if (A.size(1) != x.size())
    SiconosMatrixException::selfThrow("prod(matrix,vector) error: inconsistent sizes.");

  unsigned int numA = A.num();
  unsigned int numX = x.num();

  if (numA == 0) // if A is block ...
    SiconosMatrixException::selfThrow("prod(matrix,vector) error: not yet implemented for block matrix.");

  if (numA == 6) // A = 0
    return (DenseVect)(ublas::zero_vector<double>(x.size()));

  else if (numA == 7) // A = Identity
    return x;

  else
  {
    if (numX == 1)
    {
      if (numA == 1)
        return (DenseVect)(prod(*A.dense(), *x.dense()));
      else if (numA == 2)
        return (DenseVect)(prod(*A.triang(), *x.dense()));
      else if (numA == 3)
        return (DenseVect)(prod(*A.sym(), *x.dense()));
      else if (numA == 4)
        return (DenseVect)(prod(*A.sparse(), *x.dense()));
      else // if(numA==5)
        return (DenseVect)(prod(*A.banded(), *x.dense()));
    }
    else //if(numX == 4)
    {
      if (numA == 1)
        return (DenseVect)(prod(*A.dense(), *x.sparse()));
      else if (numA == 2)
        return (DenseVect)(prod(*A.triang(), *x.sparse()));
      else if (numA == 3)
        return (DenseVect)(prod(*A.sym(), *x.sparse()));
      else if (numA == 4)
        return (DenseVect)(prod(*A.sparse(), *x.sparse()));
      else // if(numA==5)
        return (DenseVect)(prod(*A.banded(), *x.sparse()));
    }
  }
}

void prod(double a, const SiconosMatrix& A, const SiconosVector& x, SiconosVector& y, bool init)
{
  // To compute y = a*A * x in an "optimized" way (in comparison with y = prod(A,x) )
  // or y += a*A*x if init = false.
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  if (A.size(1) != x.size())
    SiconosMatrixException::selfThrow("prod(A,x,y) error: inconsistent sizes between A and x.");

  if (A.size(0) != y.size())
    SiconosMatrixException::selfThrow("prod(A,x,y) error: inconsistent sizes between A and y.");

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

  if (numA == 0) // If A is Block
    SiconosMatrixException::selfThrow("prod(A,x,y) error: not yet implemented for block matrices.");

  if (numA == 6) // A = 0
  {
    if (init)
      y.zero();
    //else nothing
  }

  else if (numA == 7) // A = identity
  {
    scal(a, x, y, init);
  }

  else // A is not 0 or identity
  {

    // === First case: y is not a block vector ===
    {
      {
        if (init)
        {
          if (&x != &y) // if no common memory between x and y.
          {
            if (numX == 1)
            {
              if (numY != 1)
                SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");

              if (numA == 1)
                noalias(*y.dense()) = a * ublas::prod(*A.dense(), *x.dense());
              else if (numA == 2)
                noalias(*y.dense()) = a * ublas::prod(*A.triang(), *x.dense());
              else if (numA == 3)
                noalias(*y.dense()) = a * ublas::prod(*A.sym(), *x.dense());
              else if (numA == 4)
                noalias(*y.dense()) = a * ublas::prod(*A.sparse(), *x.dense());
              else //if(numA==5)
                noalias(*y.dense()) = a * ublas::prod(*A.banded(), *x.dense());
            }
            else //if(numX == 4)
            {
              if (numY != 1 && numA != 4)
                SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");

              if (numA == 1)
                noalias(*y.dense()) = a * ublas::prod(*A.dense(), *x.sparse());
              else if (numA == 2)
                noalias(*y.dense()) = a * ublas::prod(*A.triang(), *x.sparse());
              else if (numA == 3)
                noalias(*y.dense()) = a * ublas::prod(*A.sym(), *x.sparse());
              else if (numA == 4)
              {
                if (numY == 1)
                  noalias(*y.dense()) = a * ublas::prod(*A.sparse(), *x.sparse());
                else
                  noalias(*y.sparse()) = a * ublas::prod(*A.sparse(), *x.sparse());
              }
              else //if(numA==5)
                noalias(*y.dense()) = a * ublas::prod(*A.banded(), *x.sparse());
            }
          }
          else // if x and y are the same object => alias
          {
            if (numX == 1)
            {
              if (numA == 1)
                *y.dense() = a * ublas::prod(*A.dense(), *x.dense());
              else if (numA == 2)
                *y.dense() = a * ublas::prod(*A.triang(), *x.dense());
              else if (numA == 3)
                *y.dense() = a * ublas::prod(*A.sym(), *x.dense());
              else if (numA == 4)
                *y.dense() = a * ublas::prod(*A.sparse(), *x.dense());
              else //if(numA==5)
                *y.dense() = a * ublas::prod(*A.banded(), *x.dense());
            }
            else //if(numX == 4)
            {
              if (numA == 1)
                *y.sparse() = a * ublas::prod(*A.dense(), *x.sparse());
              else if (numA == 2)
                *y.sparse() = a * ublas::prod(*A.triang(), *x.sparse());
              else if (numA == 3)
                *y.sparse() = a * ublas::prod(*A.sym(), *x.sparse());
              else if (numA == 4)
                *y.sparse() = a * ublas::prod(*A.sparse(), *x.sparse());
              else //if(numA==5)
                *y.sparse() = a * ublas::prod(*A.banded(), *x.sparse());
            }
          }
        }
        else // += case
        {
          if (&x != &y) // if no common memory between x and y.
          {
            if (numX == 1)
            {
              if (numY != 1)
                SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");

              if (numA == 1)
                noalias(*y.dense()) += a * ublas::prod(*A.dense(), *x.dense());
              else if (numA == 2)
                noalias(*y.dense()) += a * ublas::prod(*A.triang(), *x.dense());
              else if (numA == 3)
                noalias(*y.dense()) += a * ublas::prod(*A.sym(), *x.dense());
              else if (numA == 4)
                noalias(*y.dense()) += a * ublas::prod(*A.sparse(), *x.dense());
              else //if(numA==5)
                noalias(*y.dense()) += a * ublas::prod(*A.banded(), *x.dense());
            }
            else //if(numX == 4)
            {
              if (numY != 1 && numA != 4)
                SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");

              if (numA == 1)
                noalias(*y.dense()) += a * ublas::prod(*A.dense(), *x.sparse());
              else if (numA == 2)
                noalias(*y.dense()) += a * ublas::prod(*A.triang(), *x.sparse());
              else if (numA == 3)
                noalias(*y.dense()) += a * ublas::prod(*A.sym(), *x.sparse());
              else if (numA == 4)
              {
                if (numY == 1)
                  noalias(*y.dense()) += a * ublas::prod(*A.sparse(), *x.sparse());
                else
                  noalias(*y.sparse()) += a * ublas::prod(*A.sparse(), *x.sparse());
              }
              else //if(numA==5)
                noalias(*y.dense()) += a * ublas::prod(*A.banded(), *x.sparse());
            }
          }
          else // if x and y are the same object => alias
          {
            if (numX == 1)
            {
              if (numA == 1)
                *y.dense() += a * ublas::prod(*A.dense(), *x.dense());
              else if (numA == 2)
                *y.dense() += a * ublas::prod(*A.triang(), *x.dense());
              else if (numA == 3)
                *y.dense() += a * ublas::prod(*A.sym(), *x.dense());
              else if (numA == 4)
                *y.dense() += a * ublas::prod(*A.sparse(), *x.dense());
              else //if(numA==5)
                *y.dense() += a * ublas::prod(*A.banded(), *x.dense());
            }
            else //if(numX == 4)
            {
              if (numA == 1)
                *y.sparse() += a * ublas::prod(*A.dense(), *x.sparse());
              else if (numA == 2)
                *y.sparse() += a * ublas::prod(*A.triang(), *x.sparse());
              else if (numA == 3)
                *y.sparse() += a * ublas::prod(*A.sym(), *x.sparse());
              else if (numA == 4)
                *y.sparse() += a * ublas::prod(*A.sparse(), *x.sparse());
              else //if(numA==5)
                *y.sparse() += a * ublas::prod(*A.banded(), *x.sparse());
            }
          }
        }
      }
    }
  }
}

void prod(const SiconosVector& x, const SiconosMatrix& A, SiconosVector& y, bool init)
{
  // To compute y = trans(A) * x in an "optimized" way, if init = true
  // (or y = trans(A) * x + y if init = false
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  if (A.size(0) != x.size())
    SiconosMatrixException::selfThrow("prod(x,A,y) error: inconsistent sizes between A and x.");

  if (A.size(1) != y.size())
    SiconosMatrixException::selfThrow("prod(x,A,y) error: inconsistent sizes between A and y.");

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

  if (numA == 0) // If A is Block
    SiconosMatrixException::selfThrow("prod(x,A,y) error: not yet implemented for block matrices.");

  if (numA == 6) // A = 0
  {
    if (init)
      y.zero();
    // else nothing
  }

  else if (numA == 7) // A = identity
  {
    if (!init)
      y += x;
    else
    {
      if (&x != &y) y = x ; // if x and y do not share memory (ie are different objects)
      // else nothing
    }
  }

  else // A is not 0 or identity
  {
    {
      if (init)
      {

        if (&x != &y) // if no common memory between x and y.
        {
          if (numX == 1)
          {
            if (numY != 1)
              SiconosMatrixException::selfThrow("prod(x,A,y) error: y (output) must be a dense vector.");

            if (numA == 1)
              noalias(*y.dense()) = ublas::prod(trans(*A.dense()), *x.dense());
            else if (numA == 2)
              noalias(*y.dense()) = ublas::prod(trans(*A.triang()), *x.dense());
            else if (numA == 3)
              noalias(*y.dense()) = ublas::prod(trans(*A.sym()), *x.dense());
            else if (numA == 4)
              noalias(*y.dense()) = ublas::prod(trans(*A.sparse()), *x.dense());
            else //if(numA==5)
              noalias(*y.dense()) = ublas::prod(trans(*A.banded()), *x.dense());
          }
          else //if(numX == 4)
          {
            if (numY != 1 && numA != 4)
              SiconosMatrixException::selfThrow("prod(x,A,y) error: y (output) must be a dense vector.");

            if (numA == 1)
              noalias(*y.dense()) = ublas::prod(trans(*A.dense()), *x.sparse());
            else if (numA == 2)
              noalias(*y.dense()) = ublas::prod(trans(*A.triang()), *x.sparse());
            else if (numA == 3)
              noalias(*y.dense()) = ublas::prod(trans(*A.sym()), *x.sparse());
            else if (numA == 4)
            {
              if (numY == 1)
                noalias(*y.dense()) = ublas::prod(trans(*A.sparse()), *x.sparse());
              else
                noalias(*y.sparse()) = ublas::prod(trans(*A.sparse()), *x.sparse());
            }
            else //if(numA==5)
              noalias(*y.dense()) = ublas::prod(trans(*A.banded()), *x.sparse());
          }
        }
        else // if x and y are the same object => alias
        {
          if (numX == 1)
          {
            if (numA == 1)
              *y.dense() = ublas::prod(trans(*A.dense()), *x.dense());
            else if (numA == 2)
              *y.dense() = ublas::prod(trans(*A.triang()), *x.dense());
            else if (numA == 3)
              *y.dense() = ublas::prod(trans(*A.sym()), *x.dense());
            else if (numA == 4)
              *y.dense() = ublas::prod(trans(*A.sparse()), *x.dense());
            else //if(numA==5)
              *y.dense() = ublas::prod(trans(*A.banded()), *x.dense());
          }
          else //if(numX == 4)
          {
            if (numA == 1)
              *y.sparse() = ublas::prod(trans(*A.dense()), *x.sparse());
            else if (numA == 2)
              *y.sparse() = ublas::prod(trans(*A.triang()), *x.sparse());
            else if (numA == 3)
              *y.sparse() = ublas::prod(trans(*A.sym()), *x.sparse());
            else if (numA == 4)
              *y.sparse() = ublas::prod(trans(*A.sparse()), *x.sparse());
            else //if(numA==5)
              *y.sparse() = ublas::prod(trans(*A.banded()), *x.sparse());
          }
        }
      }
      else // += case
      {

        if (&x != &y) // if no common memory between x and y.
        {
          if (numX == 1)
          {
            if (numY != 1)
              SiconosMatrixException::selfThrow("prod(x,A,y) error: y (output) must be a dense vector.");

            if (numA == 1)
              noalias(*y.dense()) += ublas::prod(trans(*A.dense()), *x.dense());
            else if (numA == 2)
              noalias(*y.dense()) += ublas::prod(trans(*A.triang()), *x.dense());
            else if (numA == 3)
              noalias(*y.dense()) += ublas::prod(trans(*A.sym()), *x.dense());
            else if (numA == 4)
              noalias(*y.dense()) += ublas::prod(trans(*A.sparse()), *x.dense());
            else //if(numA==5)
              noalias(*y.dense()) += ublas::prod(trans(*A.banded()), *x.dense());
          }
          else //if(numX == 4)
          {
            if (numY != 1 && numA != 4)
              SiconosMatrixException::selfThrow("prod(x,A,y) error: y (output) must be a dense vector.");

            if (numA == 1)
              noalias(*y.dense()) += ublas::prod(trans(*A.dense()), *x.sparse());
            else if (numA == 2)
              noalias(*y.dense()) += ublas::prod(trans(*A.triang()), *x.sparse());
            else if (numA == 3)
              noalias(*y.dense()) += ublas::prod(trans(*A.sym()), *x.sparse());
            else if (numA == 4)
            {
              if (numY == 1)
                noalias(*y.dense()) += ublas::prod(trans(*A.sparse()), *x.sparse());
              else
                noalias(*y.sparse()) += ublas::prod(trans(*A.sparse()), *x.sparse());
            }
            else //if(numA==5)
              noalias(*y.dense()) += ublas::prod(trans(*A.banded()), *x.sparse());
          }
        }
        else // if x and y are the same object => alias
        {
          if (numX == 1)
          {
            if (numA == 1)
              *y.dense() += ublas::prod(trans(*A.dense()), *x.dense());
            else if (numA == 2)
              *y.dense() += ublas::prod(trans(*A.triang()), *x.dense());
            else if (numA == 3)
              *y.dense() += ublas::prod(trans(*A.sym()), *x.dense());
            else if (numA == 4)
              *y.dense() += ublas::prod(trans(*A.sparse()), *x.dense());
            else //if(numA==5)
              *y.dense() += ublas::prod(trans(*A.banded()), *x.dense());
          }
          else //if(numX == 4)
          {
            if (numA == 1)
              *y.sparse() += ublas::prod(trans(*A.dense()), *x.sparse());
            else if (numA == 2)
              *y.sparse() += ublas::prod(trans(*A.triang()), *x.sparse());
            else if (numA == 3)
              *y.sparse() += ublas::prod(trans(*A.sym()), *x.sparse());
            else if (numA == 4)
              *y.sparse() += ublas::prod(trans(*A.sparse()), *x.sparse());
            else //if(numA==5)
              *y.sparse() += ublas::prod(trans(*A.banded()), *x.sparse());
          }
        }
      }
    }
  }
}

void prod(const SiconosMatrix& A, const SiconosVector& x, SiconosVector& y, bool init)
{
  // To compute y = A * x in an "optimized" way (in comparison with y = prod(A,x) )
  // or y += A*x if init = false.
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  if (A.size(1) != x.size())
    SiconosMatrixException::selfThrow("prod(A,x,y) error: inconsistent sizes between A and x.");

  if (A.size(0) != y.size())
    SiconosMatrixException::selfThrow("prod(A,x,y) error: inconsistent sizes between A and y.");

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

  if (numA == 0) // If A is Block
    SiconosMatrixException::selfThrow("prod(A,x,y) error: not yet implemented for block matrices.");

  if (numA == 6) // A = 0
  {
    if (init)
      y.zero();
    //else nothing
  }

  else if (numA == 7) // A = identity
  {
    if (!init)
      y += x;
    else
    {
      if (&x != &y) y = x ; // if x and y do not share memory (ie are different objects)
      // else nothing
    }
  }

  else // A is not 0 or identity
  {

    // === First case: y is not a block vector ===
    if (init)
    {
      if (&x != &y) // if no common memory between x and y.
      {
        if (numX == 1)
        {
          if (numY != 1)
            SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");

          assert(y.dense() != x.dense());

          if (numA == 1)
            noalias(*y.dense()) = ublas::prod(*A.dense(), *x.dense());
          else if (numA == 2)
            noalias(*y.dense()) = ublas::prod(*A.triang(), *x.dense());
          else if (numA == 3)
            noalias(*y.dense()) = ublas::prod(*A.sym(), *x.dense());
          else if (numA == 4)
            noalias(*y.dense()) = ublas::prod(*A.sparse(), *x.dense());
          else //if(numA==5)
            noalias(*y.dense()) = ublas::prod(*A.banded(), *x.dense());
        }
        else //if(numX == 4)
        {
          if (numY != 1 && numA != 4)
            SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");

          if (numA == 1)
            noalias(*y.dense()) = ublas::prod(*A.dense(), *x.sparse());
          else if (numA == 2)
            noalias(*y.dense()) = ublas::prod(*A.triang(), *x.sparse());
          else if (numA == 3)
            noalias(*y.dense()) = ublas::prod(*A.sym(), *x.sparse());
          else if (numA == 4)
          {
            if (numY == 1)
              noalias(*y.dense()) = ublas::prod(*A.sparse(), *x.sparse());
            else
              noalias(*y.sparse()) = ublas::prod(*A.sparse(), *x.sparse());
          }
          else //if(numA==5)
            noalias(*y.dense()) = ublas::prod(*A.banded(), *x.sparse());
        }
      }
      else // if x and y are the same object => alias
      {
        if (numX == 1)
        {
          if (numA == 1)
            *y.dense() = ublas::prod(*A.dense(), *x.dense());
          else if (numA == 2)
            *y.dense() = ublas::prod(*A.triang(), *x.dense());
          else if (numA == 3)
            *y.dense() = ublas::prod(*A.sym(), *x.dense());
          else if (numA == 4)
            *y.dense() = ublas::prod(*A.sparse(), *x.dense());
          else //if(numA==5)
            *y.dense() = ublas::prod(*A.banded(), *x.dense());
        }
        else //if(numX == 4)
        {
          if (numA == 1)
            *y.sparse() = ublas::prod(*A.dense(), *x.sparse());
          else if (numA == 2)
            *y.sparse() = ublas::prod(*A.triang(), *x.sparse());
          else if (numA == 3)
            *y.sparse() = ublas::prod(*A.sym(), *x.sparse());
          else if (numA == 4)
            *y.sparse() = ublas::prod(*A.sparse(), *x.sparse());
          else //if(numA==5)
            *y.sparse() = ublas::prod(*A.banded(), *x.sparse());
        }
      }
    }
    else // += case
    {
      if (&x != &y) // if no common memory between x and y.
      {
        if (numX == 1)
        {
          if (numY != 1)
            SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");

          if (numA == 1)
            noalias(*y.dense()) += ublas::prod(*A.dense(), *x.dense());
          else if (numA == 2)
            noalias(*y.dense()) += ublas::prod(*A.triang(), *x.dense());
          else if (numA == 3)
            noalias(*y.dense()) += ublas::prod(*A.sym(), *x.dense());
          else if (numA == 4)
            noalias(*y.dense()) += ublas::prod(*A.sparse(), *x.dense());
          else //if(numA==5)
            noalias(*y.dense()) += ublas::prod(*A.banded(), *x.dense());
        }
        else //if(numX == 4)
        {
          if (numY != 1 && numA != 4)
            SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");

          if (numA == 1)
            noalias(*y.dense()) += ublas::prod(*A.dense(), *x.sparse());
          else if (numA == 2)
            noalias(*y.dense()) += ublas::prod(*A.triang(), *x.sparse());
          else if (numA == 3)
            noalias(*y.dense()) += ublas::prod(*A.sym(), *x.sparse());
          else if (numA == 4)
          {
            if (numY == 1)
              noalias(*y.dense()) += ublas::prod(*A.sparse(), *x.sparse());
            else
              noalias(*y.sparse()) += ublas::prod(*A.sparse(), *x.sparse());
          }
          else //if(numA==5)
            noalias(*y.dense()) += ublas::prod(*A.banded(), *x.sparse());
        }
      }
      else // if x and y are the same object => alias
      {
        if (numX == 1)
        {
          if (numA == 1)
            *y.dense() += ublas::prod(*A.dense(), *x.dense());
          else if (numA == 2)
            *y.dense() += ublas::prod(*A.triang(), *x.dense());
          else if (numA == 3)
            *y.dense() += ublas::prod(*A.sym(), *x.dense());
          else if (numA == 4)
            *y.dense() += ublas::prod(*A.sparse(), *x.dense());
          else //if(numA==5)
            *y.dense() += ublas::prod(*A.banded(), *x.dense());
        }
        else //if(numX == 4)
        {
          if (numA == 1)
            *y.sparse() += ublas::prod(*A.dense(), *x.sparse());
          else if (numA == 2)
            *y.sparse() += ublas::prod(*A.triang(), *x.sparse());
          else if (numA == 3)
            *y.sparse() += ublas::prod(*A.sym(), *x.sparse());
          else if (numA == 4)
            *y.sparse() += ublas::prod(*A.sparse(), *x.sparse());
          else //if(numA==5)
            *y.sparse() += ublas::prod(*A.banded(), *x.sparse());
        }
      }
    }
  }
}

void axpy_prod(const SiconosMatrix& A, const SiconosVector& x, SiconosVector& y, bool init)
{
  // To compute y = A * x ( init = true) or y += A * x (init = false) using ublas::axpy_prod
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  if (A.size(1) != x.size())
    SiconosMatrixException::selfThrow("prod(A,x,y) error: inconsistent sizes between A and x.");

  if (A.size(0) != y.size())
    SiconosMatrixException::selfThrow("prod(A,x,y) error: inconsistent sizes between A and y.");

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

  if (numA == 0) // If A is Block
    SiconosMatrixException::selfThrow("axpy_prod(A,x,y) error: not yet implemented for block matrices.");

  if (numA == 6) // A = 0
  {
    if (init) y.zero(); // else nothing ...
  }

  else if (numA == 7) // A = identity
  {
    if (!init) y += x;
    else
    {
      if (&x != &y)
        y = x ; // if x and y do not share memory (ie are different objects)
    }
    // else nothing
  }

  else // A is not 0 or identity
  {
    {
      {
        if (&x != &y) // if no common memory between x and y.
        {
          if (numX == 1)
          {
            if (numY != 1)
              SiconosMatrixException::selfThrow("prod(A,x,y) error: y (output) must be a dense vector.");

            if (numA == 1)
              ublas::axpy_prod(*A.dense(), *x.dense(), *y.dense(), init);
            else if (numA == 2)
              ublas::axpy_prod(*A.triang(), *x.dense(), *y.dense(), init);
            else if (numA == 3)
              ublas::axpy_prod(*A.sym(), *x.dense(), *y.dense(), init);
            else if (numA == 4)
              ublas::axpy_prod(*A.sparse(), *x.dense(), *y.dense(), init);
            else //if(numA==5)
              ublas::axpy_prod(*A.banded(), *x.dense(), *y.dense(), init);
          }
          else //if(numX == 4)
          {
            if (numY != 1 && numA != 4)
              SiconosMatrixException::selfThrow("axpy_prod(A,x,y) error: y (output) must be a dense vector.");

            if (numA == 1)
              ublas::axpy_prod(*A.dense(), *x.sparse(), *y.dense(), init);
            else if (numA == 2)
              ublas::axpy_prod(*A.triang(), *x.sparse(), *y.dense(), init);
            else if (numA == 3)
              ublas::axpy_prod(*A.sym(), *x.sparse(), *y.dense(), init);
            else if (numA == 4)
            {
              if (numY == 1)
                ublas::axpy_prod(*A.sparse(), *x.sparse(), *y.dense(), init);
              else
                ublas::axpy_prod(*A.sparse(), *x.sparse(), *y.sparse(), init);
            }
            else //if(numA==5)
              ublas::axpy_prod(*A.banded(), *x.sparse(), *y.dense(), init);
          }
        }
        else // if x and y are the same object => alias
        {
          if (numX == 1)
          {
            if (numA == 1)
              ublas::axpy_prod(*A.dense(), *x.dense(), *x.dense(), init);
            else if (numA == 2)
              ublas::axpy_prod(*A.triang(), *x.dense(), *x.dense(), init);
            else if (numA == 3)
              ublas::axpy_prod(*A.sym(), *x.dense(), *x.dense(), init);
            else if (numA == 4)
              ublas::axpy_prod(*A.sparse(), *x.dense(), *x.dense(), init);
            else //if(numA==5)
              ublas::axpy_prod(*A.banded(), *x.dense(), *x.dense(), init);
          }
          else //if(numX == 4)
          {
            if (numA == 1)
              ublas::axpy_prod(*A.dense(), *x.sparse(), *x.sparse(), init);
            else if (numA == 2)
              ublas::axpy_prod(*A.triang(), *x.sparse(), *x.sparse(), init);
            else if (numA == 3)
              ublas::axpy_prod(*A.sym(), *x.sparse(), *x.sparse(), init);
            else if (numA == 4)
              ublas::axpy_prod(*A.sparse(), *x.sparse(), *x.sparse(), init);
            else //if(numA==5)
              ublas::axpy_prod(*A.banded(), *x.sparse(), *x.sparse(), init);
          }
        }
      }
    }
  }
}

void gemvtranspose(double a, const SiconosMatrix& A, const SiconosVector& x, double b, SiconosVector& y)
{
  if (A.isBlock())
    SiconosMatrixException::selfThrow("gemv(...) not yet implemented for block vectors or matrices.");
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  unsigned int numA = A.num();
  unsigned int numX = x.num();
  unsigned int numY = y.num();
  if (numA != 1 || numX != 1 || numY != 1)
    SiconosMatrixException::selfThrow("gemv(...) failed: reserved to dense matrices or vectors.");

  siconosBindings::blas::gemv(a, siconosBindings::trans(*A.dense()), *x.dense(), b, *y.dense());
}

void gemv(double a, const SiconosMatrix& A, const SiconosVector& x, double b, SiconosVector& y)
{
  if (A.isBlock())
    SiconosMatrixException::selfThrow("gemv(...) not yet implemented for block vectors or matrices.");
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  unsigned int numA = A.num();
  unsigned int numX = x.num();
  unsigned int numY = y.num();
  if (numA != 1 || numX != 1 || numY != 1)
    SiconosMatrixException::selfThrow("gemv(...) failed: reserved to dense matrices or vectors.");

  siconosBindings::blas::gemv(a, *A.dense(), *x.dense(), b, *y.dense());
}


/**
 * @file   VolumeBrickOctree.cpp
 * @author Sebastian Maisch <sebastian.maisch@uni-ulm.de>
 * @date   2015.08.17
 *
 * @brief  Implementation of the layered octree.
 */

#include "VolumeBrickOctree.h"
#include "gfx/glrenderer/GLTexture3D.h"
#include "gfx/glrenderer/GLTexture.h"
#include "app/ApplicationBase.h"
#include <boost/assign.hpp>
#include <glm/gtc/matrix_transform.hpp>

namespace cgu {

    namespace cguOctreeMath {
        inline unsigned int calculateOverlapPixels(unsigned int val) {
            return (val / VolumeBrickOctree::MAX_SIZE) << 1;
        }
    }

    VolumeBrickOctree::VolumeBrickOctree(const glm::uvec3& pos, const glm::uvec3& size, const glm::vec3& scale,
        unsigned int lvl, GPUProgram* minMaxProg, const std::vector<BindingLocation> uniformNames) :
        posOffset(pos),
        origSize(size),
        voxelScale(scale),
        level(lvl),
        hasAnyData(true),
        minTexValue(0.0f),
        maxTexValue(1.0f),
        maxLevel(0),
        streamFile(nullptr),
        dataSize(0),
        brickTextureDesc(4, GL_RGBA8, GL_RGBA, GL_UNSIGNED_BYTE),
        minMaxMapProgram(minMaxProg),
        minMaxMapUniformNames(uniformNames)
    {
    }

    /**
     *  Constructor.
     *  @param texData the 3D texture resource to create the tree from.
     *  @param pos the position in the texture.
     *  @param size the size of this part of the tree (in voxels).
     *  @param scale the scale of a voxel in this tree.
     *  @param minMaxProg the program used to create Min-/Max-Maps.
     *  @param uniformNames the uniform names of the minMaxProg.
     *  @param app the application object.
     */
    VolumeBrickOctree::VolumeBrickOctree(const GLTexture3D* texData, const glm::uvec3& pos, const glm::uvec3& size,
        const glm::vec3& scale, GPUProgram* minMaxProg, const std::vector<BindingLocation> uniformNames,
        ApplicationBase* app) :
        VolumeBrickOctree(pos, size, scale, 0, minMaxProg, uniformNames)
    {
        if (origSize.x > MAX_SIZE || origSize.y > MAX_SIZE || origSize.z > MAX_SIZE) {
            auto ovlp = cguOctreeMath::calculateOverlapPixels(glm::max(origSize.x, glm::max(origSize.y, origSize.z)));
            glm::uvec3 sizeOverlap{ ovlp };
            auto sizeWithOverlap = origSize + sizeOverlap;
            glm::uvec3 sizePowerOfTwo{ cguMath::roundupPow2(origSize.x), cguMath::roundupPow2(origSize.y),
                cguMath::roundupPow2(origSize.z) };
            if (sizeWithOverlap.x > sizePowerOfTwo.x) {
                sizeWithOverlap.x = cguMath::roundupPow2(sizeWithOverlap.x);
                sizeOverlap <<= 1;
            } else sizeWithOverlap.x = sizePowerOfTwo.x;
            if (sizeWithOverlap.y > sizePowerOfTwo.y) {
                sizeWithOverlap.y = cguMath::roundupPow2(sizeWithOverlap.y);
                sizeOverlap <<= 1;
            } else sizeWithOverlap.y = sizePowerOfTwo.y;
            if (sizeWithOverlap.z > sizePowerOfTwo.z) {
                sizeWithOverlap.z = cguMath::roundupPow2(sizeWithOverlap.z);
                sizeOverlap <<= 1;
            } else sizeWithOverlap.z = sizePowerOfTwo.z;
            glm::uvec3 childSizeBase{ sizeWithOverlap.x >> 1, sizeWithOverlap.y >> 1, sizeWithOverlap.z >> 1 };

            CreateNode(childSizeBase, texData, app);
        } else {
            CreateLeafTexture(texData);
        }
    }

    VolumeBrickOctree::VolumeBrickOctree(const GLTexture3D* texData, const glm::uvec3& pos,
        const glm::uvec3& size, unsigned int lvl, const glm::vec3& scale, GPUProgram* minMaxProg,
        const std::vector<BindingLocation> uniformNames, ApplicationBase* app) :
        VolumeBrickOctree(pos, size, scale, lvl, minMaxProg, uniformNames)
    {
        if (origSize.x > MAX_SIZE || origSize.y > MAX_SIZE || origSize.z > MAX_SIZE) {
            glm::uvec3 sizePowerOfTwo{ cguMath::roundupPow2(origSize.x), cguMath::roundupPow2(origSize.y),
                cguMath::roundupPow2(origSize.z) };
            glm::uvec3 childSizeBase{ sizePowerOfTwo.x >> 1, sizePowerOfTwo.y >> 1, sizePowerOfTwo.z >> 1 };

            CreateNode(childSizeBase, texData, app);

        } else {
            CreateLeafTexture(texData);
        }
    }

    void VolumeBrickOctree::CreateNode(const glm::uvec3& childSizeBase, const GLTexture3D* texData, ApplicationBase* app)
    {
        glm::uvec3 posOffsets[8];
        posOffsets[0] = glm::uvec3(0, 0, 0);
        posOffsets[1] = glm::uvec3(0, 0, 1);
        posOffsets[2] = glm::uvec3(0, 1, 0);
        posOffsets[3] = glm::uvec3(0, 1, 1);
        posOffsets[4] = glm::uvec3(1, 0, 0);
        posOffsets[5] = glm::uvec3(1, 0, 1);
        posOffsets[6] = glm::uvec3(1, 1, 0);
        posOffsets[7] = glm::uvec3(1, 1, 1);

        auto ovlp = cguOctreeMath::calculateOverlapPixels(glm::max(childSizeBase.x, glm::max(childSizeBase.y, childSizeBase.z)));

        for (unsigned int i = 0; i < 8; ++i) {
            auto childPosOffset = posOffsets[i] * (childSizeBase - glm::uvec3(ovlp));
            auto childPos = posOffset + childPosOffset;
            auto childSize = glm::uvec3(glm::max(glm::ivec3(0),
                glm::ivec3(glm::min(origSize, childPosOffset + childSizeBase)) - glm::ivec3(childPosOffset)));

            if (childSizeBase.x == ovlp && posOffsets[i].x == 1) childSize.x = 0;
            if (childSizeBase.y == ovlp && posOffsets[i].y == 1) childSize.y = 0;
            if (childSizeBase.z == ovlp && posOffsets[i].z == 1) childSize.z = 0;

            children[i].reset(new VolumeBrickOctree(texData, childPos, childSize, level + 1, voxelScale,
                minMaxMapProgram, minMaxMapUniformNames, app));
        }
        texSize.x = (children[0]->texSize.x + children[4]->texSize.x) >> 1;
        texSize.y = (children[0]->texSize.y + children[2]->texSize.y) >> 1;
        texSize.z = (children[0]->texSize.z + children[1]->texSize.z) >> 1;

        for (auto& child : children) {
            maxLevel = glm::max(maxLevel, child->maxLevel);
        }

        CalculateTexBorders(texData);

        CombineChildTextures(app);
        WriteBrickTextureToTempFile();

        for (auto& child : children) child->ResetAllData();
    }

    void VolumeBrickOctree::CreateLeafTexture(const GLTexture3D* texData)
    {
        texSize = origSize;
        maxLevel = level;

        if (texSize.x * texSize.y * texSize.z == 0) {
            dataSize = 0;
            hasAnyData = false;
            return;
        }

        CalculateTexBorders(texData);

        brickTexture = texData->CreateMinMaxTexture(posOffset, origSize, brickTextureDesc);
        WriteBrickTextureToTempFile();
    }

    void VolumeBrickOctree::CalculateTexBorders(const GLTexture3D* texData)
    {
        if (posOffset.x != 0) minTexValue.x = 1.0f / static_cast<float>(texSize.x);
        if (posOffset.y != 0) minTexValue.y = 1.0f / static_cast<float>(texSize.y);
        if (posOffset.z != 0) minTexValue.z = 1.0f / static_cast<float>(texSize.z);

        if (posOffset.x + origSize.x < texData->GetSize().x) maxTexValue.x = (static_cast<float>(texSize.x) - 1.0f) / static_cast<float>(texSize.x);
        if (posOffset.y + origSize.y < texData->GetSize().y) maxTexValue.y = (static_cast<float>(texSize.y) - 1.0f) / static_cast<float>(texSize.y);
        if (posOffset.z + origSize.z < texData->GetSize().z) maxTexValue.z = (static_cast<float>(texSize.z) - 1.0f) / static_cast<float>(texSize.z);
    }

    /**
     *  Destructor.
     */
    VolumeBrickOctree::~VolumeBrickOctree()
    {
        if (streamFile) std::fclose(streamFile);
        streamFile = nullptr;
    }

    /**
     *  Reloads all data from this tree node.
     */
    void VolumeBrickOctree::ReloadData()
    {
        if (dataSize == 0) return;
        std::fseek(streamFile, 0, SEEK_SET);
        std::vector<uint8_t> data;
        data.resize(dataSize);
        std::fread(data.data(), sizeof(uint8_t), dataSize, streamFile);

        brickTexture.reset(new GLTexture(texSize.x, texSize.y, texSize.z, brickTextureDesc, nullptr));
        brickTexture->UploadData(data);
        brickTexture->GenerateMinMaxMaps(minMaxMapProgram, minMaxMapUniformNames);
        brickTexture->SampleLinear();
        brickTexture->SampleWrapClamp();
    }

    /**
     *  Releases all the data from this tree node.
     */
    void VolumeBrickOctree::ResetData()
    {
        brickTexture.reset(nullptr);
    }

    /**
     *  Releases all the data from this tree node and its children.
     */
    void VolumeBrickOctree::ResetAllData()
    {
        if (hasAnyData) {
            brickTexture.reset(nullptr);
            if (children[0]) for (auto& child : children) child->ResetAllData();
            hasAnyData = false;
        }
    }

    /**
     *  Writes the brick texture to a temporary file.
     */
    void VolumeBrickOctree::WriteBrickTextureToTempFile()
    {
        if (texSize.x * texSize.y * texSize.z == 0) {
            dataSize = 0;
            hasAnyData = false;
            return;
        }
        std::vector<uint8_t> data;
        brickTexture->DownloadData(data);

        dataSize = static_cast<unsigned int>(data.size());
        streamFile = std::tmpfile();
        std::fwrite(data.data(), sizeof(uint8_t), dataSize, streamFile);
        ReloadData();
    }

    /**
     *  Combines the 8 textures of this nodes children to a new texture.
     *  @param app the application object.
     */
    void VolumeBrickOctree::CombineChildTextures(ApplicationBase* app)
    {
        if (texSize.x * texSize.y * texSize.z == 0) return;
        GPUProgram* combineProg = nullptr;
        brickTextureDesc.bytesPP = children[0]->brickTextureDesc.bytesPP;
        brickTextureDesc.format = children[0]->brickTextureDesc.format;
        brickTextureDesc.type = children[0]->brickTextureDesc.type;
        brickTextureDesc.internalFormat = children[0]->brickTextureDesc.internalFormat;

        if (brickTextureDesc.type == GL_UNSIGNED_BYTE) combineProg = app->GetGPUProgramManager()->GetResource("combineChildTextures8.cp");
        else if (brickTextureDesc.type == GL_UNSIGNED_SHORT) combineProg = app->GetGPUProgramManager()->GetResource("combineChildTextures16.cp");
        else if (brickTextureDesc.type == GL_UNSIGNED_INT) combineProg = app->GetGPUProgramManager()->GetResource("combineChildTextures32.cp");
        else throw std::runtime_error("Pixel-type not supported.");

        auto uniformNames = combineProg->GetUniformLocations({ "childTex", "combineTex", "childShift", "maxChunkSize" });
        brickTexture.reset(new GLTexture(texSize.x, texSize.y, texSize.z, brickTextureDesc, nullptr));

        //glm::uvec3 chunkSize = glm::uvec3(glm::ceil(glm::vec3(texSize) / 2.0f));
        combineProg->UseProgram();
        combineProg->SetUniform(uniformNames[0], 0);
        combineProg->SetUniform(uniformNames[1], 1);

        glm::uvec3 childShifts[8];
        childShifts[0] = glm::uvec3(0, 0, 0);
        childShifts[1] = glm::uvec3(0, 0, 1);
        childShifts[2] = glm::uvec3(0, 1, 0);
        childShifts[3] = glm::uvec3(0, 1, 1);
        childShifts[4] = glm::uvec3(1, 0, 0);
        childShifts[5] = glm::uvec3(1, 0, 1);
        childShifts[6] = glm::uvec3(1, 1, 0);
        childShifts[7] = glm::uvec3(1, 1, 1);

        brickTexture->ActivateImage(1, 0, GL_WRITE_ONLY);
        for (unsigned int i = 0; i < 8; ++i) {
            if (children[i]->dataSize != 0) {
                auto numGroups = glm::ivec3(glm::ceil(glm::vec3(children[i]->texSize) / 8.0f));

                combineProg->SetUniform(uniformNames[3], children[i]->texSize);
                combineProg->SetUniform(uniformNames[2], childShifts[i]);
                children[i]->brickTexture->ActivateImage(0, 0, GL_READ_ONLY);
                OGL_CALL(glDispatchCompute, numGroups.x, numGroups.y, numGroups.z);
            }
        }

        OGL_CALL(glMemoryBarrier, GL_ALL_BARRIER_BITS);
        OGL_SCALL(glFinish);
    }

    /**
     *  Updates the loading state of the tree depending on the view frustum (in object space).
     *  @param camera the current camera to calculate the frustum for.
     *  @param world the world matrix of the parent node.
     *  @return whether there are any children with loaded data in the sub-tree.
     */
    bool VolumeBrickOctree::UpdateFrustum(const cgu::CameraView& camera, const glm::mat4& world)
    {
        if (dataSize == 0) return false;
        auto isCorrectLod = false; // TODO: add LOD mechanism here. [8/26/2015 Sebastian Maisch]
        // if (level == maxLevel - 1) isCorrectLod = true;

        auto localWorld = GetLocalWorld(world);

        cguMath::AABB3<float> box{ { { glm::vec3(0.0f), glm::vec3(1.0f) } } };

        if (!cguMath::AABBInFrustumTest(camera.GetViewFrustum(localWorld), box)) ResetAllData();
        else if (!children[0]) {
            if (!IsLoaded()) { ReloadData(); hasAnyData = true; }
        } else {
            if (isCorrectLod) {
                if (IsLoaded()) for (auto& child : children) child->ResetAllData();
                else { ReloadData(); hasAnyData = true; }
            } else {
                if (IsLoaded()) {
                    ResetData();
                }
                hasAnyData = false;
                for (auto& child : children) hasAnyData = child->UpdateFrustum(camera, world) || hasAnyData;
            }
        }
        return hasAnyData;
    }

    void VolumeBrickOctree::GetRenderedBricksList(const cgu::CameraView& camera, const glm::mat4& world,
        std::vector<std::pair<const VolumeBrickOctree*, float>>& result) const
    {
        if (!hasAnyData || dataSize == 0) return;
        else if (IsLoaded()) {
            auto childWorld = GetLocalWorld(world);
            result.push_back(std::make_pair(this, camera.GetSignedDistanceToUnitAABB2(childWorld)));
        }
        else {
            for (auto& child : children) child->GetRenderedBricksList(camera, world, result);
        }
    }

    glm::mat4 VolumeBrickOctree::GetLocalWorld(const glm::mat4& world) const
    {
        // glm::mat4 scaleVoxelMat = glm::scale(glm::mat4(), glm::vec3(origSize));
        auto scaleVoxelMat = glm::scale(glm::mat4(), glm::vec3(origSize) * (maxTexValue - minTexValue));
        auto scaleToWorldMat = glm::scale(glm::mat4(), voxelScale);
        // glm::mat4 translateOffsetMat = glm::translate(glm::mat4(), glm::vec3(posOffset));
        auto translateOffsetMat = glm::translate(glm::mat4(), glm::vec3(posOffset) + (minTexValue * glm::vec3(texSize)));
        return world * scaleToWorldMat * translateOffsetMat * scaleVoxelMat;
    }
}


//          Copyright Gavin Band 2008 - 2012.
// 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 METRO_LIKELIHOOD_SUM_HPP
#define METRO_LIKELIHOOD_SUM_HPP

#include <memory>
#include <Eigen/Core>
#include <boost/ptr_container/ptr_vector.hpp>

namespace metro {
	namespace likelihood {
		template< typename Scalar, typename Vector, typename Matrix >
		struct Sum: public metro::LogLikelihood< Scalar, Vector, Matrix >
		{
			typedef metro::LogLikelihood< Scalar, Vector, Matrix > LogLikelihood ;
			void add_term( LogLikelihood::UniquePtr term ) ;

			Scalar get_value_of_function() const {
				Scalar result = 0.0 ;
				for( std::size_t i = 0; i < m_terms.size(); ++i ) {
					result += m_terms[i].get_value_of_function() ;
				}
				return result ;
			}

			Vector get_value_of_first_derivative() const {
				Vector result ;
				for( std::size_t i = 0; i < m_terms.size(); ++i ) {
					if( i == 0 ) {
						result = m_terms[i].get_value_of_first_derivative() ;
					} else {
						result += m_terms[i].get_value_of_first_derivative() ;
					}
				}
				return result ;
			}

			Matrix get_value_of_second_derivative() const {
				Matrix result ;
				for( std::size_t i = 0; i < m_terms.size(); ++i ) {
					if( i == 0 ) {
						result = m_terms[i].get_value_of_second_derivative() ;
					} else {
						result += m_terms[i].get_value_of_second_derivative() ;
					}
				}
				return result ;
			}

			std::string get_spec() const {
				std::string result =  "Sum( " ;
				for( std::size_t i = 0; i < m_terms.size(); ++i ) {
					if( i > 0 ) {
						result += ", " ;
					}
					result += m_terms[i].get_spec() ;
				}
				return result + ")" ;
			}

		private:
			boost::ptr_vector< LogLikelihood > m_terms ;
		} ;
	}
}

#endif