/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "spectral_canonization.hpp"

#include <iostream>
#include <numeric>

#include <boost/format.hpp>

#include <core/utils/range_utils.hpp>
#include <core/utils/timer.hpp>

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

tt make_sum( const boost::dynamic_bitset<>& mask )
{
  boost::dynamic_bitset<> sum( std::max<unsigned>( 1 << mask.size(), 64u ) );
  foreach_bit( mask, [&sum]( unsigned pos ) {
      sum ^= tt_nth_var( pos );
    } );
  if ( mask.size() < 6u )
  {
    tt_shrink( sum, mask.size() );
  }

  return sum;
}

std::vector<int> rademacher_walsh_spectrum( const tt& func )
{
  const auto n = tt_num_vars( func );
  std::vector<int> spectrum;

  foreach_bitset( n, [n, &func, &spectrum]( const boost::dynamic_bitset<>& bs ) {
      const auto row = make_sum( bs );
      spectrum.push_back( ( 1 << n ) - 2 * ( row ^ func ).count() );
    } );

  return spectrum;
}

std::vector<int> autocorrelation_spectrum( const tt& func )
{
  const auto n = tt_num_vars( func );
  std::vector<int> spectrum;

  foreach_bitset( n, [n, &func, &spectrum]( const boost::dynamic_bitset<>& bs ) {
      if ( bs.none() )
      {
        spectrum.push_back( 1 << n );
      }
      else
      {
        boost::dynamic_bitset<> fs( func.size() );
        foreach_bitset( n, [&bs, &fs, &func]( const boost::dynamic_bitset<>& bs2 ) {
            fs.set( bs2.to_ulong(), func.test( ( bs2 ^ bs ).to_ulong() ) );
          } );
        spectrum.push_back( ( 1 << n ) - 2 * ( fs ^ func ).count() );
      }
    } );
  return spectrum;
}

void print_spectrum( const std::vector<int>& spectrum, unsigned nvars )
{
  for ( auto i = 0u; i < spectrum.size(); ++i )
  {
    std::cout << boost::dynamic_bitset<>( nvars, i ) << boost::format( " %5d" ) % spectrum[i] << std::endl;
  }
}

void print_spectrum_ordered( const std::vector<int>& spectrum, unsigned nvars )
{
  if ( nvars == 4u )
  {
    for ( auto i : {0u,16u,1u,2u,4u,8u,16u,3u,5u,9u,6u,10u,12u,16u,7u,11u,13u,14u,16u,15u} )
    {
      if ( i == 16u )
      {
        std::cout << " |";
      }
      else
      {
        std::cout << " " << spectrum[i];
      }
    }
    std::cout << std::endl;
  }
  else
  {
    std::cout << any_join( spectrum, " " ) << std::endl;
  }
}

int compare_abs( int a, int b )
{
  if ( abs( a ) < abs( b ) )
  {
    return -1;
  }
  else if ( abs( a ) > abs( b ) )
  {
    return 1;
  }
  else if ( a < b )
  {
    return -1;
  }
  else if ( a > b )
  {
    return 1;
  }
  else
  {
    return 0;
  }
}

/******************************************************************************
 * Spectral operations                                                        *
 ******************************************************************************/

void operation1( std::vector<int>& spectrum, tt& func, const std::vector<unsigned>& perm )
{
  std::vector<int> spectrum_p( spectrum.size() );
  tt func_p = func;

  for ( auto row = 0u; row < spectrum.size(); ++row )
  {
    boost::dynamic_bitset<> idx( perm.size(), row ), idx_p( perm.size() );
    for ( auto i = 0u; i < perm.size(); ++i )
    {
      idx_p[i] = idx[perm[i]];
    }

    spectrum_p[idx_p.to_ulong()] = spectrum[row];
    func_p.set( idx_p.to_ulong(), func.test( row ) );
  }

  spectrum = spectrum_p;
  func = func_p;
}

void operation2( std::vector<int>& spectrum, tt& func, unsigned var )
{
  func = tt_flip( func, var );

  for ( auto row = 1u; row < spectrum.size(); ++row )
  {
    if ( ( row >> var ) & 1 )
    {
      spectrum[row] = -spectrum[row];
    }
  }
}

void operation3( std::vector<int>& spectrum, tt& func )
{
  func.flip();

  std::transform( spectrum.begin(), spectrum.end(), spectrum.begin(), std::negate<int>() );
}

void operation4( std::vector<int>& spectrum, tt& func, unsigned var, unsigned diff )
{
  for ( auto row = 0u; row < spectrum.size(); ++row )
  {
    if ( ( row >> var ) & 1 )
    {
      auto row2 = row ^ diff;
      if ( row < row2 )
      {
        std::swap( spectrum[row], spectrum[row2] );
      }
    }
  }

  const auto nvars = tt_num_vars( func );
  boost::dynamic_bitset<> mask( nvars, diff );
  const auto varbs = onehot_bitset( nvars, var );
  const auto inc_mask = ~( varbs | mask );

  //std::cout << "var = " << var << ", mask = " << mask << ", diff = " << diff << std::endl;
  //std::cout << "func b " << func << " " << func[15] << " " << func[14] << std::endl;

  auto start = varbs;
  do {
    //std::cout << start << std::endl;

    auto pattern = start;
    do {
      //std::cout << " " << pattern;
      if ( ( pattern & mask ).count() % 2 )
      {
        const auto r1 = pattern.to_ulong();
        const auto r2 = ( pattern ^ varbs ).to_ulong();

        const auto tmp = func.test( r1 );
        func.set( r1, func.test( r2 ) );
        func.set( r2, tmp );
        //std::cout << " *, swap with " << ( pattern ^ varbs ) << " (" << r1 << " with " << r2 << ")";
      }
      //std::cout << std::endl;
      inc_pos( pattern, mask );
    } while ( ( pattern & mask ).any() );

    inc_pos( start, inc_mask );
  } while ( ( start & inc_mask ).any() );

  //std::cout << "func a " << func << std::endl;
}

void operation5( std::vector<int>& spectrum, tt& func, unsigned row )
{
  const auto nvars = tt_num_vars( func );
  boost::dynamic_bitset<> mask( nvars, row );

  func ^= make_sum( mask );

  for ( auto i = 0u; i < spectrum.size(); ++i )
  {
    const auto j = i ^ row;
    if ( i < j )
    {
      std::swap( spectrum[i], spectrum[j] );
    }
  }
}

/******************************************************************************
 * Canonization operations                                                    *
 ******************************************************************************/

void maximize_zero_coefficient( std::vector<int>& spectrum, tt& func )
{
  const auto max_coeff = std::max_element( spectrum.begin(), spectrum.end(), []( int a, int b ) { return compare_abs( a, b ) == -1; } );

  if ( *max_coeff < 0 )
  {
    operation3( spectrum, func );
  }

  const auto pos = std::distance( spectrum.begin(), max_coeff );

  if ( pos != 0 )
  {
    operation5( spectrum, func, pos );
  }
}

void minimize_order( std::vector<int>& spectrum, tt& func )
{
  const auto n = tt_num_vars( func );

  for ( auto var = 0u; var < n; ++var )
  {
    const auto row = 1u << var;
    /* sweep through the other ones */
    auto best_row = row;
    auto best_val = spectrum[row];
    for ( auto row2 = row + 1u; row2 < spectrum.size(); ++row2 )
    {
      if ( ( row & row2 ) != row ) continue;

      if ( compare_abs( spectrum[row2], best_val ) == 1 )
      {
        best_val = spectrum[row2];
        best_row = row2;
      }
    }

    /* update values */
    const auto diff = row ^ best_row;
    if ( diff )
    {
      operation4( spectrum, func, var, diff );
    }
  }
}

void sort_inputs( std::vector<int>& spectrum, tt& func )
{
  auto cmp = [&spectrum]( unsigned i1, unsigned i2 ) {
    switch ( compare_abs( spectrum[1 << i1], spectrum[1 << i2] ) )
    {
    case -1: return false;
    case 1: return true;
    default:
    return false;
    }
  };

  auto cmp4 = [&spectrum]( unsigned i1, unsigned i2 ) {
    switch ( compare_abs( spectrum[1 << i1], spectrum[1 << i2] ) )
    {
    case -1: return false;
    case 1: return true;
    default:
    {
      for ( auto row : {3, 5, 9, 6, 10, 12} )
      {
        if ( ( ( row >> i1 ) & 1 ) && !( ( row >> i2 ) & 1 ) )
        {
          auto row2 = ( row ^ ( 1 << i1 ) ) | ( 1 << i2 );
          switch ( compare_abs( spectrum[row], spectrum[row2] ) )
          {
          case -1: return true;
          case 1: return false;
          }
        }
        else if ( !( ( row >> i1 ) & 1 ) && ( ( row >> i2 ) & 1 ) )
        {
          auto row2 = ( row ^ ( 1 << i2 ) ) | ( 1 << i1 );
          switch ( compare_abs( spectrum[row], spectrum[row2] ) )
          {
          case -1: return true;
          case 1: return false;
          }
        }
      }
      return false;
    }
    }
  };

  const auto nvars = tt_num_vars( func );
  std::vector<unsigned> proj( nvars );
  std::iota( proj.begin(), proj.end(), 0u );

  if ( nvars <= 3 )
  {
    std::sort( proj.begin(), proj.end(), cmp );
  }
  else
  {
    std::sort( proj.begin(), proj.end(), cmp4 );
  }

  operation1( spectrum, func, proj );
}

void invert_inputs( std::vector<int>& spectrum, tt& func )
{
  const auto nvars = tt_num_vars( func );

  for ( auto i = 0u; i < nvars; ++i )
  {
    if ( spectrum[1 << i] < 0 )
    {
      operation2( spectrum, func, i );
    }
  }
}

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

tt spectral_canonization( const tt& func, const properties::ptr& settings, const properties::ptr& statistics )
{
  const auto verbose = get( settings, "verbose", false );
  const auto very_verbose = get( settings, "very_verbose", false );

  properties_timer t( statistics );

  const auto nvars = tt_num_vars( func );
  auto spectrum = rademacher_walsh_spectrum( func );
  set( statistics, "spectrum_init", spectrum );
  auto cfunc = func;

  if ( verbose )
  {
    std::cout << "AC " << any_join( autocorrelation_spectrum( func ), " " ) << std::endl;
    std::cout << "before" << std::endl;
    if ( very_verbose ) print_spectrum( spectrum, nvars );
    print_spectrum_ordered( spectrum, nvars );
  }

  maximize_zero_coefficient( spectrum, cfunc );

  if ( verbose )
  {
    std::cout << "after step 1" << std::endl;
    if ( very_verbose ) print_spectrum( spectrum, nvars );
    print_spectrum_ordered( spectrum, nvars );
    std::cout << cfunc << std::endl;
  }

  minimize_order( spectrum, cfunc );

  if ( verbose )
  {
    std::cout << "after step 2" << std::endl;
    if ( very_verbose ) print_spectrum( spectrum, nvars );
    print_spectrum_ordered( spectrum, nvars );
    std::cout << cfunc << std::endl;
  }

  invert_inputs( spectrum, cfunc );

  if ( verbose )
  {
    std::cout << "after step 3" << std::endl;
    if ( very_verbose ) print_spectrum( spectrum, nvars );
    print_spectrum_ordered( spectrum, nvars );
    std::cout << cfunc << std::endl;
  }

  sort_inputs( spectrum, cfunc );

  if ( verbose )
  {
    std::cout << "after step 4" << std::endl;
    if ( very_verbose ) print_spectrum( spectrum, nvars );
    print_spectrum_ordered( spectrum, nvars );
    std::cout << cfunc << std::endl;
  }

  set( statistics, "spectrum_final", spectrum );
  set( statistics, "class", get_spectral_class( func ) );

  // if ( !( ( spectrum == std::vector<int>( {16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} ) ) ||
  //         ( spectrum == std::vector<int>( {14, 2, 2, -2, 2, -2, -2, 2, 2, -2, -2, 2, -2, 2, 2, -2} ) ) ||
  //         ( spectrum == std::vector<int>( {12, 4, 4, -4, 4, -4, -4, 4, 0, 0, 0, 0, 0, 0, 0, 0} ) ) ||
  //         ( spectrum == std::vector<int>( {10, 6, 6, -6, 2, -2, -2, 2, 2, -2, -2, 2, 2, -2, -2, 2} ) ) ||
  //         ( spectrum == std::vector<int>( {8, 8, 8, -8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} ) ) ||
  //         ( spectrum == std::vector<int>( {8, 8, 4, -4, 4, -4, 0, 0, 4, -4, 0, 0, 0, 0, -4, 4} ) ) ||
  //         ( spectrum == std::vector<int>( {6, 6, 6, -2, 6, -2, -2, -2, 6, -2, -2, -2, -2, -2, -2, 6} ) ) ||
  //         ( spectrum == std::vector<int>( {4, 4, 4, 4, 4, 4, -4, -4, 4, -4, 4, -4, -4, 4, 4, -4} ) ) ) )
  // {
  //   std::cout << "[w] wrongly classified " << func << std::endl;
  //   print_spectrum_ordered( spectrum );
  //   print_spectrum( spectrum, nvars );
  // }

  return cfunc;
}

unsigned get_spectral_class( const tt& func )
{
  const auto nvars = tt_num_vars( func );

  assert( nvars >= 2u && nvars <= 5u );

  auto spectrum = rademacher_walsh_spectrum( func );
  std::transform( spectrum.begin(), spectrum.end(), spectrum.begin(), []( int i ) { return abs( i ); } );
  std::stable_sort( spectrum.begin(), spectrum.end(), std::not2( std::less<int>() ) );

  switch ( nvars )
  {
  case 2u:
    return spectrum.front() == 4 ? 0u : 1u;
  case 3u:
    switch ( spectrum.front() )
    {
    case 8: return 0u;
    case 6: return 1u;
    case 4: return 2u;
    } break;
  case 4u:
    switch ( spectrum.front() )
    {
    case 16: return 0u;
    case 14: return 1u;
    case 12: return 2u;
    case 10: return 3u;
    case 8: return spectrum[2u] == 8 ? 4u : 5u;
    case 6: return 6u;
    case 4: return 7u;
    } break;
  case 5u:
    switch ( spectrum.front() )
    {
    case 32: return 0u;
    case 30: return 1u;
    case 28: return 2u;
    case 26: return 3u;
    case 24: return spectrum[4u] == 8 ? 4u : 5u;
    case 22: return spectrum[2u] == 6 ? 6u : 7u;
    case 20:
      if ( spectrum[1u] == 12 && spectrum[2u] == 4 ) return 8u;
      else if ( spectrum[1u] == 8 ) return 9u;
      else if ( spectrum[1u] == 12 && spectrum[2u] == 8 ) return 10u;
      else if ( spectrum[1u] == 12 && spectrum[2u] == 12 ) return 11u;
      else assert( false ); break;
    case 18:
      if ( spectrum[1u] == 10 && spectrum[2u] == 6 ) return 12u;
      else if ( spectrum[1u] == 14 && spectrum[2u] == 6 ) return 13u;
      else if ( spectrum[1u] == 10 && spectrum[2u] == 10 ) return 14u;
      else if ( spectrum[1u] == 14 && spectrum[2u] == 10 ) return 15u;
      else if ( spectrum[1u] == 14 && spectrum[2u] == 14 ) return 16u;
      else assert( false ); break;
    case 16:
      if ( spectrum[1u] == 16 && spectrum[2u] == 16 ) return 17u;
      else if ( spectrum[1u] == 8 && spectrum[9u] == 8 ) return 18u;
      else if ( spectrum[1u] == 16 && spectrum[2u] == 8 && spectrum[6] == 8) return 19u;
      else if ( spectrum[1u] == 8 && spectrum[9u] == 4 ) return 20u;
      else if ( spectrum[1u] == 12 && spectrum[2u] == 8 ) return 21u;
      else if ( spectrum[1u] == 16 && spectrum[2u] == 8 && spectrum[6] == 4 ) return 22u;
      else if ( spectrum[1u] == 12 && spectrum[2u] == 12 ) return 23u;
      else if ( spectrum[1u] == 16 && spectrum[2u] == 12 ) return 24u;
      else assert( false ); break;
    case 14:
      if ( spectrum[1u] == 10 && spectrum[2u] == 10 && spectrum[5u] == 6 )
      {
        auto ac = autocorrelation_spectrum( func );
        std::transform( ac.begin(), ac.end(), ac.begin(), []( int i ) { return abs( i ); } );
        std::stable_sort( ac.begin(), ac.end(), std::not2( std::less<int>() ) );
        return ac[1u] == 12 ? 25u : 26u;
      }
      else if ( spectrum[1u] == 14 && spectrum[2u] == 10 && spectrum[4u] == 6 ) return 27u;
      else if ( spectrum[1u] == 10 && spectrum[2u] == 10 && spectrum[5u] == 10 ) return 28u;
      else if ( spectrum[1u] == 14 && spectrum[2u] == 14 ) return 29u;
      else if ( spectrum[1u] == 14 && spectrum[2u] == 10 && spectrum[4u] == 10 ) return 30;
      else assert( false ); break;
    case 12:
      if ( spectrum[1u] == 8 ) return 33u;
      else if ( spectrum[1u] == 12 && spectrum[2u] == 8 )
      {
        auto ac = autocorrelation_spectrum( func );
        std::transform( ac.begin(), ac.end(), ac.begin(), []( int i ) { return abs( i ); } );
        std::stable_sort( ac.begin(), ac.end(), std::not2( std::less<int>() ) );
        return ac[1u] == 16 ? 34u : 35u;
      }
      else if ( spectrum[1u] == 12 && spectrum[2u] == 12 && spectrum[3u] == 8 ) return 36;
      else if ( spectrum[4u] == 4 )
      {
        auto ac = autocorrelation_spectrum( func );
        std::transform( ac.begin(), ac.end(), ac.begin(), []( int i ) { return abs( i ); } );
        std::stable_sort( ac.begin(), ac.end(), std::not2( std::less<int>() ) );
        return ac[4u] == 8 ? 31u : 32u;
      }
      else if ( spectrum[4u] == 8 )
      {
        auto ac = autocorrelation_spectrum( func );
        std::transform( ac.begin(), ac.end(), ac.begin(), []( int i ) { return abs( i ); } );
        std::stable_sort( ac.begin(), ac.end(), std::not2( std::less<int>() ) );
        return ac[1u] == 16 ? 37u : 38u;
      }
      else if ( spectrum[4u] == 12 ) return 39u;
      else assert( false ); break;
    case 10:
      if ( spectrum[4u] == 6 ) return 40u;
      else if ( spectrum[4u] == 10 )
      {
        auto ac = autocorrelation_spectrum( func );
        std::transform( ac.begin(), ac.end(), ac.begin(), []( int i ) { return abs( i ); } );
        std::stable_sort( ac.begin(), ac.end(), std::not2( std::less<int>() ) );
        return ac[1u] == 12 ? 41u : ( ac[1u] == 20 ? 42u : 43u );
      }
      else assert( false ); break;
    case 8:
      if ( spectrum[12u] == 8 )
      {
        auto ac = autocorrelation_spectrum( func );
        std::transform( ac.begin(), ac.end(), ac.begin(), []( int i ) { return abs( i ); } );
        std::stable_sort( ac.begin(), ac.end(), std::not2( std::less<int>() ) );
        return ac[1u] == 32 ? 44u : ( ac[1u] == 8 ? 45u : 46u );
      }
      else if ( spectrum[12u] == 4 ) return 47u;
      else assert( false ); break;
    } break;
  }

  return 0u;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


#include "qtsoftmatter.h"
#include "ui_qtsoftmatter.h"
#include <boost/format.hpp>

QtSoftMatter::QtSoftMatter(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::QtSoftMatter)
{
    simLock = false;
    ui->setupUi(this);
    initModel();
    paintTimer.setInterval(50);
    simTimer.setInterval(50);
    connect(&paintTimer, SIGNAL(timeout()), this, SLOT(updatePaintWidgets()));
    connect(&simTimer, SIGNAL(timeout()), this, SLOT(computeRK4()));
    paintTimer.start();
    simTimer.start();

    connect(ui->nuDoubleSpinBox, SIGNAL(valueChanged(double)), this, SLOT(initModel()));
    connect(ui->nhSpinBox, SIGNAL(valueChanged(int)), this, SLOT(initModel()));
    connect(ui->dtDoubleSpinBox, SIGNAL(valueChanged(double)), this, SLOT(updateModel()));
    connect(ui->mSpinBox, SIGNAL(valueChanged(int)), this, SLOT(updateModel()));
    connect(ui->nSpinBox, SIGNAL(valueChanged(int)), this, SLOT(updateModel()));
}

QtSoftMatter::~QtSoftMatter()
{
    delete ui;
}

void QtSoftMatter::updatePaintWidgets()
{
    ui->paintWidget->setModel(model);
    ui->paintWidget->update();
}

void QtSoftMatter::initModel()
{
    model.init(ui->dtDoubleSpinBox->value(), ui->mSpinBox->value(), ui->nSpinBox->value(),
               ui->nhSpinBox->value(), 1/ui->nuDoubleSpinBox->value(),
               ui->sv0DoubleSpinBox->value());
    ui->paintWidget->setModel(model);
    ui->packingFractionLabel->setText(QString("%1 %").arg(100*model.packingFractionHCP));
    ui->cutoffLabel->setText(QString("%1 σ").arg(model.cutoff));
}

void QtSoftMatter::computeRK4()
{
    if (simLock)
        return;

    simLock = true;

    int steps = ui->spfSpinBox->value();

    for (int i=0; i<steps; ++i)
        model.rk4step();

    updatePaintWidgets();

    simLock = false;
}

void QtSoftMatter::updateModel()
{
    model.dt = pow(10, ui->dtDoubleSpinBox->value());
    model.m = ui->mSpinBox->value();
    model.n = ui->nSpinBox->value();
    model.update();
    ui->cutoffLabel->setText(QString("%1 σ").arg(model.cutoff));
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/none.hpp>
#include <boost/optional.hpp>
#include <vector>

#include "DataStructures/DataVector.hpp"
#include "ErrorHandling/Error.hpp"
#include "Utilities/Gsl.hpp"

namespace RootFinder {
namespace bracketing_detail {
// Brackets a root, given a functor f(x) that returns a
// boost::optional<double> and given two arrays x and y (with y=f(x))
// containing points that have already been tried for bracketing.
//
// Returns a std::array<double,4> containing {{x1,x2,y1,y2}} where
// x1 and x2 bracket the root, and y1=f(x1) and y2=f(x2).
//
// Note that y might be undefined (i.e. an invalid boost::optional)
// for values of x at or near the endpoints of the interval.  We
// assume that if f(x1) and f(x2) are valid for some x1 and x2, then
// f(x) is valid for all x between x1 and x2.
//
// Assumes that there is a root between the first and last points of
// the array.
// We also assume that there is only one root.
//
// So this means we have only 2 possibilities for the validity of the
// points in the input x,y arrays:
// 1) All points are invalid, e.g. "X X X X X X X X".
//    (here X represents an invalid point)
// 2) All valid points are adjacent, with the same sign, e.g. "X X o o X X"
//    or "o o X X X" or "X X X o o".
//    (here o represents a valid point)
// Note that we assume that all valid points have the same sign; otherwise
// the caller would have known that the root was bracketed and the caller would
// not have called bracket_by_contracting.
//
// Also note that we exclude the case "o o o o o" (no roots), since the
// caller would have known that too.  If such a case is found, an error is
// thrown.  An error is also thrown if the size of the region where the
// sign changes is so small that the number of iterations is exceeded.
//
// For case 1) above, we bisect each pair of points, and call
// bracket_by_contracting recursively until we find a valid point.
// For case 2) above, it is sufficent to check for a bracket only
// between valid and invalid points.  That is, for "X X + + X X" we
// check only between points 1 and 2 and between points 3 and 4 (where
// points are numbered starting from zero).  For "+ + X X X" we check
// only between points 1 and 2.
template <typename Functor>
std::array<double, 4> bracket_by_contracting(
    const std::vector<double>& x, const std::vector<boost::optional<double>>& y,
    const Functor& f, const size_t level = 0) noexcept {
  constexpr size_t max_level = 6;
  if (level > max_level) {
    ERROR("Too many iterations in bracket_by_contracting. Either refine the "
          "initial range/guess or increase max_level.");
  }

  // First check if we have any valid points.
  size_t last_valid_index = y.size();
  for (size_t i = y.size(); i >= 1; --i) {
    if (y[i - 1]) {
      last_valid_index = i - 1;
      break;
    }
  }

  if (last_valid_index == y.size()) {
    // No valid points!

    // Create larger arrays with one point between each of the already
    // computed points.
    std::vector<double> bisected_x(x.size() * 2 - 1);
    std::vector<boost::optional<double>> bisected_y(y.size() * 2 - 1);

    // Copy all even-numbered points in the range.
    for (size_t i = 0; i < x.size(); ++i) {
      bisected_x[2 * i] = x[i];
      bisected_y[2 * i] = y[i];
    }

    // Fill midpoints and check for bracket on each one.
    for (size_t i = 0; i < x.size() - 1; ++i) {
      bisected_x[2 * i + 1] = x[i] + 0.5 * (x[i + 1] - x[i]);
      bisected_y[2 * i + 1] = f(bisected_x[2 * i + 1]);
      if (bisected_y[2 * i + 1]) {
        // Valid point! We know that all the other points are
        // invalid, so we need to check only 3 points in the next
        // iteration: the new valid point and its neighbors.
        return bracket_by_contracting({{x[i], bisected_x[2 * i + 1], x[i + 1]}},
                                      {{y[i], bisected_y[2 * i + 1], y[i + 1]}},
                                      f, level + 1);
      }
    }
    // We still have no valid points. So recurse, using all points.
    // The next iteration will bisect all the points.
    return bracket_by_contracting(bisected_x, bisected_y, f, level + 1);
  }

  // If we get here, we have found a valid point; in particular we have
  // found the last valid point in the array.

  // Find the first valid point in the array.
  size_t first_valid_index = 0;
  for (size_t i = 0; i < y.size(); ++i) {
    if (y[i]) {
      first_valid_index = i;
      break;
    }
  }

  // Make a new set of points that includes only the points that
  // neighbor the boundary between valid and invalid points.
  std::vector<double> x_near_valid_point;
  std::vector<boost::optional<double>> y_near_valid_point;

  if (first_valid_index == 0 and last_valid_index == y.size() - 1) {
    ERROR(
        "bracket_while_contracting: found a case where all points are valid,"
        "which should not happen under our assumptions.");
  }

  if (first_valid_index > 0) {
    // Check for a root between first_valid_index-1 and first_valid_index.
    const double x_test =
        x[first_valid_index - 1] +
        0.5 * (x[first_valid_index] - x[first_valid_index - 1]);
    const auto y_test = f(x_test);
    if (y_test and y[first_valid_index].get() * y_test.get() <= 0.0) {
      // Bracketed!
      return std::array<double, 4>{{x_test, x[first_valid_index], y_test.get(),
                                    y[first_valid_index].get()}};
    } else {
      x_near_valid_point.push_back(x[first_valid_index - 1]);
      y_near_valid_point.push_back(y[first_valid_index - 1]);
      x_near_valid_point.push_back(x_test);
      y_near_valid_point.push_back(y_test);
      x_near_valid_point.push_back(x[first_valid_index]);
      y_near_valid_point.push_back(y[first_valid_index]);
    }
  }
  if (last_valid_index < y.size() - 1) {
    // Check for a root between last_valid_index and last_valid_index+1.
    const double x_test = x[last_valid_index] +
                          0.5 * (x[last_valid_index + 1] - x[last_valid_index]);
    const auto y_test = f(x_test);
    if (y_test and y[last_valid_index].get() * y_test.get() <= 0.0) {
      // Bracketed!
      return std::array<double, 4>{{x[last_valid_index], x_test,
                                    y[last_valid_index].get(), y_test.get()}};
    } else {
      if (first_valid_index != last_valid_index or first_valid_index == 0) {
        x_near_valid_point.push_back(x[last_valid_index]);
        y_near_valid_point.push_back(y[last_valid_index]);
      }  // else we already pushed back last_valid_index (==first_valid_index).
      x_near_valid_point.push_back(x_test);
      y_near_valid_point.push_back(y_test);
      x_near_valid_point.push_back(x[last_valid_index + 1]);
      y_near_valid_point.push_back(y[last_valid_index + 1]);
    }
  }

  // We have one or more valid points but we didn't find a bracket.
  // That is, we have something like "X X o o X X" or "X X o o" or "o o X X".
  // So recurse, zooming in to the boundary (either one boundary or two
  // boundaries) between valid and invalid points.
  // Note that "o o o o" is prohibited by our assumptions, and checked for
  // above just in case it occurs by mistake.
  return bracket_by_contracting(x_near_valid_point, y_near_valid_point, f,
                                level + 1);
}
}  // namespace bracketing_detail

/*!
 * \ingroup NumericalAlgorithmsGroup
 * \brief Brackets the root of the function `f`, assuming a single
 * root in a given interval \f$f[x_\mathrm{lo},x_\mathrm{up}]\f$
 * and assuming that `f` is defined only in an unknown smaller
 * interval \f$f[x_a,x_b]\f$ where
 * \f$x_\mathrm{lo} \leq x_a \leq x_b \leq x_\mathrm{hi}\f$.
 *
 * `f` is a unary invokable that takes a `double` which is the current value at
 * which to evaluate `f`.  `f` returns a `boost::optional<double>` which
 * evaluates to false if the function is undefined at the supplied point.
 *
 * Assumes that there is only one root in the interval.
 *
 * Assumes that if \f$f(x_1)\f$ and \f$f(x_2)\f$ are both defined for
 * some \f$(x_1,x_2)\f$, then \f$f(x)\f$ is defined for all \f$x\f$
 * between \f$x_1\f$ and \f$x_2\f$.
 *
 * On input, assumes that the root lies in the interval
 * [`lower_bound`,`upper_bound`].  Optionally takes a `guess` for the
 * location of the root.  If `guess` is supplied, then evaluates the
 * function first at `guess` and `upper_bound` before trying
 * `lower_bound`: this means that it would be optimal if `guess`
 * underestimates the actual root and if `upper_bound` was less likely
 * to be undefined than `lower_bound`.
 *
 * On return, `lower_bound` and `upper_bound` are replaced with values that
 * bracket the root and for which the function is defined, and
 * `f_at_lower_bound` and `f_at_upper_bound` are replaced with
 * `f` evaluated at those bracketing points.
 *
 * `bracket_possibly_undefined_function_in_interval` throws an error if
 *  all points are valid but of the same sign (because that would indicate
 *  multiple roots but we assume only one root), if no root exists, or
 *  if the range of a sign change is sufficently small relative to the
 *  given interval that the number of iterations to find the root is exceeded.
 *
 */
template <typename Functor>
void bracket_possibly_undefined_function_in_interval(
    const gsl::not_null<double*> lower_bound,
    const gsl::not_null<double*> upper_bound,
    const gsl::not_null<double*> f_at_lower_bound,
    const gsl::not_null<double*> f_at_upper_bound, const Functor& f,
    const double guess) noexcept {
  // Initial values of x1,x2,y1,y2.  Use `guess` and `upper_bound`,
  // because in typical usage `guess` underestimates the actual
  // root, and `lower_bound` is more likely than `upper_bound` to be
  // invalid.
  double x1 = guess;
  double x2 = *upper_bound;
  auto y1 = f(x1);
  auto y2 = f(x2);
  const bool y1_defined = static_cast<bool>(y1);
  const bool y2_defined = static_cast<bool>(y2);
  if (not(y1_defined and y2_defined and y1.get() * y2.get() <= 0.0)) {
    // Root is not bracketed.
    // Before moving to the general algorithm, try the remaining
    // input point that was supplied.
    const double x3 = *lower_bound;
    const auto y3 = f(x3);
    const bool y3_defined = static_cast<bool>(y3);
    if (y1_defined and y3_defined and y1.get() * y3.get() <= 0.0) {
      // Bracketed! Throw out x2,y2.  Rename variables to keep x1 < x2.
      x2 = x1;
      y2 = y1;
      x1 = x3;
      y1 = y3;
    } else {
      // Our simple checks didn't work, so call the more general method.
      // There are 8 cases:
      //
      // y3 y1 y2
      // --------
      // X  X  X
      // o  X  X
      // X  o  X
      // o  o  X
      // X  o  o
      // o  o  o
      // X  X  o
      // o  X  o
      //
      // where X means an invalid point, o means a valid point.
      // All valid points have the same sign, or we would have found a
      // bracket already.
      //
      // Before calling the general case, error on "o o o" and "o X o".
      // Both of these are prohibited by our assumptions (we
      // assume the root is in the interval so no "o o o", and we
      // assume that all invalid points are at the end of interval, so no
      // "o X o").
      if (y2_defined and y3_defined) {
        ERROR(
            "bracket_possibly_undefined_function_in_interval: found "
            "case that should not happen under our assumptions.");
      }
      std::array<double, 4> tmp = bracketing_detail::bracket_by_contracting(
          {{x3, x1, x2}}, {{y3, y1, y2}}, f);
      x1 = tmp[0];
      x2 = tmp[1];
      y1 = tmp[2];
      y2 = tmp[3];
    }
  }
  *f_at_lower_bound = y1.get();
  *f_at_upper_bound = y2.get();
  *lower_bound = x1;
  *upper_bound = x2;
}

/*!
 * \ingroup NumericalAlgorithmsGroup
 * \brief Brackets the single root of the
 * function `f` for each element in a `DataVector`, assuming the root
 * lies in the given interval and that `f` may be undefined at some
 * points in the interval.
 *
 * `f` is a binary invokable that takes a `double` and a `size_t` as
 * arguments.  The `double` is the current value at which to evaluate
 * `f`, and the `size_t` is the index into the `DataVector`s.  `f`
 * returns a `boost::optional<double>` which evaluates to false if the
 * function is undefined at the supplied point.
 *
 * Assumes that there is only one root in the interval.
 *
 * Assumes that if \f$f(x_1)\f$ and \f$f(x_2)\f$ are both defined for
 * some \f$(x_1,x_2)\f$, then \f$f(x)\f$ is defined for all \f$x\f$
 * between \f$x_1\f$ and \f$x_2\f$.
 *
 * On input, assumes that the root lies in the interval
 * [`lower_bound`,`upper_bound`].  Optionally takes a `guess` for the
 * location of the root.
 *
 * On return, `lower_bound` and `upper_bound` are replaced with values that
 * bracket the root and for which the function is defined, and
 * `f_at_lower_bound` and `f_at_upper_bound` are replaced with
 * `f` evaluated at those bracketing points.
 *
 */
template <typename Functor>
void bracket_possibly_undefined_function_in_interval(
    const gsl::not_null<DataVector*> lower_bound,
    const gsl::not_null<DataVector*> upper_bound,
    const gsl::not_null<DataVector*> f_at_lower_bound,
    const gsl::not_null<DataVector*> f_at_upper_bound, const Functor& f,
    const DataVector& guess) noexcept {
  for (size_t s = 0; s < lower_bound->size(); ++s) {
    bracket_possibly_undefined_function_in_interval(
        &((*lower_bound)[s]), &((*upper_bound)[s]), &((*f_at_lower_bound)[s]),
        &((*f_at_upper_bound)[s]),
        [&f, &s ](const double x) noexcept { return f(x, s); }, guess[s]);
  }
}

/*
 * Version of `bracket_possibly_undefined_function_in_interval`
 * without a supplied initial guess; uses the mean of `lower_bound` and
 * `upper_bound` as the guess.
 */
template <typename Functor>
void bracket_possibly_undefined_function_in_interval(
    const gsl::not_null<double*> lower_bound,
    const gsl::not_null<double*> upper_bound,
    const gsl::not_null<double*> f_at_lower_bound,
    const gsl::not_null<double*> f_at_upper_bound, const Functor& f) noexcept {
  bracket_possibly_undefined_function_in_interval(
      lower_bound, upper_bound, f_at_lower_bound, f_at_upper_bound, f,
      *lower_bound + 0.5 * (*upper_bound - *lower_bound));
}

/*
 * Version of `bracket_possibly_undefined_function_in_interval`
 * without a supplied initial guess; uses the mean of `lower_bound` and
 * `upper_bound` as the guess.
 */
template <typename Functor>
void bracket_possibly_undefined_function_in_interval(
    const gsl::not_null<DataVector*> lower_bound,
    const gsl::not_null<DataVector*> upper_bound,
    const gsl::not_null<DataVector*> f_at_lower_bound,
    const gsl::not_null<DataVector*> f_at_upper_bound,
    const Functor& f) noexcept {
  bracket_possibly_undefined_function_in_interval(
      lower_bound, upper_bound, f_at_lower_bound, f_at_upper_bound, f,
      *lower_bound + 0.5 * (*upper_bound - *lower_bound));
}
}  // namespace RootFinder


#include <boost/random/uniform_real.hpp>


/**
 * sudoku_ga.cpp
 *
 * By Sebastian Raaphorst, 2018.
 */

#include <iostream>
#include <memory>
#include <optional>
#include <stdexcept>
#include <string>
#include <vector>

#include <boost/format.hpp>
#include <boost/program_options.hpp>

#include <GenSudokuBoard.h>
#include <GenSudokuBoardGAPopulator.h>
#include <GeneticAlgorithm.h>
#include <PredefinedBoards.h>

#include "timer.h"

namespace po = boost::program_options;
using namespace vorpal::gensudoku;
using namespace vorpal::stochastic;

void conflicting_options(const po::variables_map &vm, const char * const opt1, const char * const opt2) {
    if (vm.count(opt1) && !vm[opt1].defaulted() && vm.count(opt2) && !vm[opt2].defaulted())
        throw std::logic_error((boost::format("Incompatible options: %1% amd %2%") % opt1 % opt2).str());
}

double must_be_01(const po::variables_map &vm, const char * const opt) {
    double p = vm[opt].as<double>();
    if (p < 0 || p > 1)
        throw std::invalid_argument((boost::format("Invalid value for %1%: %2%") % opt % p).str());
    return p;
}

int main(int argc, const char *argv[]) {
    using solver = GeneticAlgorithm<SudokuBoard, size_t>;
    solver::options_type options;

    try {
        po::options_description desc{"options_type"};
        desc.add_options()
                ("help,h", "Display help information")
                ("population-size,P", po::value<size_t>()->default_value(options.population_size), "Size of population")
                ("max-generations,M", po::value<uint64_t>()->default_value(options.max_generations), "Maximum number of generations")
                ("ktournament,k", po::value<size_t>()->default_value(3), "Use a k-tournament selector (default) with k participants")
                ("random,r", "Use a random selector")
                ("roulette,R", "Use a roulette selector")
                ("crossover-probability,c", po::value<double>()->default_value(options.crossover_probability),
                        "Probability of a crossover, i.e. that breeding will occur to replace an older population member")
                ("mutation-probability,m", po::value<double>()->default_value(options.mutation_probability),
                        "Probability of mutation in a population member")
                ("fitness-death-factor,f", po::value<double>()->default_value(options.fitness_death_factor),
                        "Population members that are not within this amount of the best member will be killed")
                ("permissible-dead-rounds,p", po::value<uint64_t>()->default_value(options.permissible_dead_rounds),
                        "Number of rounds permitted to see no improvement before the whole population is killed")
                ("suppress_logging,s", "Suppress logging")
                ("output-rounds,o", po::value<uint64_t>()->default_value(options.output_rounds),
                        "Log output after this many rounds");

        po::options_description hidden{"Hidden options"};
        hidden.add_options()
                ("board", po::value<std::vector<std::string>>(), "board");
        po::positional_options_description p;
        p.add("board", 1);

        po::options_description pall;
        pall.add(desc).add(hidden);

        po::variables_map vm;
        store(po::command_line_parser(argc, argv).
                options(pall).positional(p).run(), vm);
        po::notify(vm);

        if (vm.count("help")) {
            std::cout << desc << '\n';
            std::cout << "Note that board may also be one of: very_easy_board, easy_board, medium_board, very_hard_board, "
                         "impossible_board, or benchmark_board, with default benchmark_board\n";
            return 0;
        }

        // Try to get the board.
        std::vector<std::string> board_vec = vm.count("board") ? vm["board"].as<std::vector<std::string>>() :
                std::vector<std::string>{};
        std::string board_str = board_vec.empty() ? "benchmark_board" : board_vec[0];

        std::optional<SudokuBoard> board = PredefinedBoards::lookupBoard(board_str);
        options.populator = std::make_unique<SudokuBoardGAPopulator>(board ?
                *board : SudokuBoard(board_str));

        conflicting_options(vm, "ktournament", "random");
        conflicting_options(vm, "ktournament", "roulette");
        conflicting_options(vm, "random", "roulette");

        options.population_size = vm["population-size"].as<size_t>();
        options.max_generations = vm["max-generations"].as<uint64_t>();
        options.permissible_dead_rounds = vm["permissible-dead-rounds"].as<uint64_t>();

        if (vm.count("random"))
            options.selector = std::make_unique<RandomSelector<SudokuBoard>>();
        else if (vm.count("roulette"))
            options.selector = std::make_unique<RouletteSelector<SudokuBoard>>();
        else
            options.selector = std::make_unique<KTournamentSelector<SudokuBoard>>(vm["ktournament"].as<size_t>());

        options.crossover_probability = must_be_01(vm, "crossover-probability");
        options.mutation_probability  = must_be_01(vm, "mutation-probability");
        options.fitness_death_factor  = must_be_01(vm, "fitness-death-factor");

        // We can only have output-rounds if logging is on.
        options.output_rounds = vm.count("suppress-logging") ? UINT64_MAX : vm["output-rounds"].as<uint64_t>();

        options.fitness_success_threshold = 243;
    } catch (const std::exception &e) {
        std::cerr << e.what() << '\n';
        return 1;
    }
    run_timed("sudoku", [&options]() {
        const auto &sol = solver::run(options);
        std::flush(std::clog);
        std::cout << "Best solution found has fitness " << sol->fitness() << ":\n";
        for (size_t row = 0; row < 9; ++row) {
            for (size_t col = 0; col < 9; ++col)
                std::cout << (*sol)[row * 9 + col];
            std::cout << '\n';
        }
        std::flush(std::cout);
    });
}


/*
 * Copyright (c) 2021 The Foundation for Research on Information Technologies in Society (IT'IS).
 *
 * This file is part of iSEG
 * (see https://github.com/ITISFoundation/osparc-iseg).
 *
 * This software is released under the MIT License.
 *  https://opensource.org/licenses/MIT
 */
#include "AutoTubeWidget.h"

#include "Data/ItkUtils.h"
#include "Data/Logger.h"
#include "Data/SlicesHandlerITKInterface.h"

#include "Thirdparty/IJ/BinaryThinningImageFilter3D/itkBinaryThinningImageFilter3D.h"
#include "Thirdparty/IJ/NonMaxSuppression/itkNonMaxSuppressionImageFilter.h"

#include <itkBinaryThinningImageFilter.h>
#include <itkBinaryThresholdImageFilter.h>
#include <itkConnectedComponentImageFilter.h>
#include <itkCurvesLevelSetImageFilter.h>
#include <itkFastMarchingImageFilter.h>
#include <itkGradientMagnitudeRecursiveGaussianImageFilter.h>
#include <itkHessianToObjectnessMeasureImageFilter.h>
#include <itkImage.h>
#include <itkMinimumMaximumImageCalculator.h>
#include <itkMultiScaleHessianBasedMeasureImageFilter.h>
#include <itkRelabelComponentImageFilter.h>
#include <itkRescaleIntensityImageFilter.h>
#include <itkSigmoidImageFilter.h>
#include <itkSliceBySliceImageFilter.h>
#include <itkThresholdImageFilter.h>

#include <accumulators/percentile.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/join.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/range/adaptor/transformed.hpp>

#include <QFormLayout>
#include <QMessageBox>
#include <QScrollArea>

#include <algorithm>
#include <sstream>

namespace acc = boost::accumulators;
using boost::adaptors::transformed;
using boost::algorithm::join;

template<typename TInputImage, typename TOutputImage, unsigned int Dimension>
class BinaryThinningImageFilter : public itk::BinaryThinningImageFilter<TInputImage, TOutputImage>
{
};

template<typename TInputImage, typename TOutputImage>
class BinaryThinningImageFilter<TInputImage, TOutputImage, 3> : public itk::BinaryThinningImageFilter3D<TInputImage, TOutputImage>
{
};

AutoTubeWidget::AutoTubeWidget(iseg::SlicesHandlerInterface* hand3D)
		: m_Handler3D(hand3D)
{
	setToolTip(Format("Sandbox Tool"));

	m_Metric2d = new QCheckBox;
	m_Metric2d->setChecked(false);
	m_Metric2d->setToolTip(Format("Use 2D pseudo vesselness (blob feature per slice), or use full 3D vesselness."));

	m_NonMaxSuppression = new QCheckBox;
	m_NonMaxSuppression->setChecked(true);
	m_NonMaxSuppression->setToolTip(Format("Extract approx. one pixel wide paths based on non-maximum suppression."));

	m_Skeletonize = new QCheckBox;
	m_Skeletonize->setChecked(true);
	m_Skeletonize->setToolTip(Format("Compute 1-pixel wide centerlines (skeleton)."));

	m_SigmaLow = new QLineEdit(QString::number(0.3));
	m_SigmaLow->setValidator(new QDoubleValidator);

	m_SigmaHi = new QLineEdit(QString::number(0.6));
	m_SigmaHi->setValidator(new QDoubleValidator);

	m_NumberSigmaLevels = new QLineEdit(QString::number(2));
	m_NumberSigmaLevels->setValidator(new QIntValidator);

	m_Threshold = new QLineEdit;
	m_Threshold->setValidator(new QDoubleValidator);

	m_MaxRadius = new QLineEdit(QString::number(1));
	m_MaxRadius->setValidator(new QDoubleValidator);

	m_MinObjectSize = new QLineEdit(QString::number(10));
	m_MinObjectSize->setValidator(new QIntValidator);

	m_SelectObjectsButton = new QPushButton("Select Mask");
	m_SelectedObjects = new QLineEdit;
	m_SelectedObjects->setReadOnly(true);

	m_ExecuteButton = new QPushButton("Execute");

	auto layout = new QFormLayout;
	layout->addRow("Sigma Min", m_SigmaLow);
	layout->addRow("Sigma Max", m_SigmaHi);
	layout->addRow("Number of Sigmas", m_NumberSigmaLevels);
	layout->addRow("2D Vesselness", m_Metric2d);
	layout->addRow("Feature Threshold", m_Threshold);
	layout->addRow("Non-maximum Suppression", m_NonMaxSuppression);
	layout->addRow("Centerlines", m_Skeletonize);
	//layout->addRow("Maximum radius", _max_radius);
	layout->addRow("Minimum object size", m_MinObjectSize);
	layout->addRow(m_SelectObjectsButton, m_SelectedObjects);
	layout->addRow(m_ExecuteButton);

	auto big_view = new QWidget;
	big_view->setLayout(layout);

	auto scroll_area = new QScrollArea(this);
	scroll_area->setWidget(big_view);

	auto top_layout = new QGridLayout(1, 1);
	top_layout->addWidget(scroll_area, 0, 0);
	setLayout(top_layout);

	QObject_connect(m_SelectObjectsButton, SIGNAL(clicked()), this, SLOT(SelectObjects()));
	QObject_connect(m_ExecuteButton, SIGNAL(clicked()), this, SLOT(DoWork()));
}

void AutoTubeWidget::Init()
{
	OnSlicenrChanged();
	HideParamsChanged();
}

void AutoTubeWidget::NewLoaded()
{
	OnSlicenrChanged();
}

void AutoTubeWidget::OnSlicenrChanged()
{
}

void AutoTubeWidget::Cleanup()
{
	m_SelectedObjects->setText("");
	m_CachedFeatureImage.img = nullptr;
}

void AutoTubeWidget::OnMouseClicked(iseg::Point p)
{
}

void AutoTubeWidget::SelectObjects()
{
	auto sel = m_Handler3D->TissueSelection();
	std::cout << "sel " << sel[0] << std::endl;
	std::string text = join(sel | transformed([](int d) { return std::to_string(d); }), ", ");
	m_SelectedObjects->setText(QString::fromStdString(text));
}

void AutoTubeWidget::DoWork()
{
	iseg::SlicesHandlerITKInterface itk_handler(m_Handler3D);
	try
	{
		if ((true)) //(all_slices->isChecked())
		{
			using input_type = itk::SliceContiguousImage<float>;
			using tissues_type = itk::SliceContiguousImage<iseg::tissues_size_t>;
			auto source = itk_handler.GetSource(true); // active_slices -> correct seed z-position
			auto target = itk_handler.GetTarget(true);
			auto tissues = itk_handler.GetTissues(true);
			DoWorkNd<input_type, tissues_type, input_type>(source, tissues, target);
		}
		else
		{
			using input_type = itk::Image<float, 2>;
			using tissues_type = itk::Image<iseg::tissues_size_t, 2>;
			auto source = itk_handler.GetSourceSlice();
			auto target = itk_handler.GetTargetSlice();
			auto tissues = itk_handler.GetTissuesSlice();
			DoWorkNd<input_type, tissues_type, input_type>(source, tissues, target);
		}
	}
	catch (const std::exception& e)
	{
		QMessageBox::warning(this, "iSeg", QString("Error: ") + e.what(), QMessageBox::Ok | QMessageBox::Default);
	}
}

template<class TInput, class TImage>
typename TImage::Pointer AutoTubeWidget::ComputeFeatureImage(TInput* source) const
{
	itkStaticConstMacro(ImageDimension, size_t, TInput::ImageDimension);
	using hessian_pixel_type = itk::SymmetricSecondRankTensor<float, ImageDimension>;
	using hessian_image_type = itk::Image<hessian_pixel_type, ImageDimension>;
	using feature_filter_type = itk::HessianToObjectnessMeasureImageFilter<hessian_image_type, TImage>;
	using multiscale_hessian_filter_type = itk::MultiScaleHessianBasedMeasureImageFilter<TInput, hessian_image_type, TImage>;

	double sigm_min = m_SigmaLow->text().toDouble();
	double sigm_max = m_SigmaHi->text().toDouble();
	int num_levels = m_NumberSigmaLevels->text().toInt();

	auto objectness_filter = feature_filter_type::New();
	objectness_filter->SetBrightObject(false);
	objectness_filter->SetObjectDimension(1);
	objectness_filter->SetScaleObjectnessMeasure(true);
	objectness_filter->SetAlpha(0.5);
	objectness_filter->SetBeta(0.5);
	objectness_filter->SetGamma(5.0);

	auto multi_scale_enhancement_filter = multiscale_hessian_filter_type::New();
	multi_scale_enhancement_filter->SetInput(source);
	multi_scale_enhancement_filter->SetHessianToMeasureFilter(objectness_filter);
	multi_scale_enhancement_filter->SetSigmaStepMethodToEquispaced();
	multi_scale_enhancement_filter->SetSigmaMinimum(std::min(sigm_min, sigm_max));
	multi_scale_enhancement_filter->SetSigmaMaximum(std::max(sigm_min, sigm_max));
	multi_scale_enhancement_filter->SetNumberOfSigmaSteps(std::min(1, num_levels));
	multi_scale_enhancement_filter->Update();
	return multi_scale_enhancement_filter->GetOutput();
}

template<class TInput, class TImage>
typename TImage::Pointer AutoTubeWidget::ComputeFeatureImage2d(TInput* source) const
{
	itkStaticConstMacro(ImageDimension, size_t, TInput::ImageDimension);
	using input_type = TInput;
	using output_type = TImage;
	using image_type = itk::Image<float, 2>;

	using hessian_filter_type = itk::HessianRecursiveGaussianImageFilter<image_type>;
	using hessian_pixel_type = itk::SymmetricSecondRankTensor<double, 2>;
	using hessian_image_type = itk::Image<hessian_pixel_type, 2>;

	using feature_filter_type = itk::HessianToObjectnessMeasureImageFilter<hessian_image_type, image_type>;
	using multiscale_hessian_filter_type = itk::MultiScaleHessianBasedMeasureImageFilter<image_type, hessian_image_type, image_type>;
	using slice_by_slice_filter_type = itk::SliceBySliceImageFilter<input_type, output_type, multiscale_hessian_filter_type>;

	double sigm_min = m_SigmaLow->text().toDouble();
	double sigm_max = m_SigmaHi->text().toDouble();
	int num_levels = m_NumberSigmaLevels->text().toInt();

	auto objectness_filter = feature_filter_type::New();
	objectness_filter->SetBrightObject(false);
	objectness_filter->SetObjectDimension(ImageDimension == 2 ? 1 : 0); // for 2D analysis we are looking for lines in a single slice
	objectness_filter->SetScaleObjectnessMeasure(true);
	objectness_filter->SetAlpha(0.5);
	objectness_filter->SetBeta(0.5);
	objectness_filter->SetGamma(5.0);

	auto multi_scale_enhancement_filter = multiscale_hessian_filter_type::New();
	multi_scale_enhancement_filter->SetHessianToMeasureFilter(objectness_filter);
	multi_scale_enhancement_filter->SetSigmaStepMethodToEquispaced();
	multi_scale_enhancement_filter->SetSigmaMinimum(std::min(sigm_min, sigm_max));
	multi_scale_enhancement_filter->SetSigmaMaximum(std::max(sigm_min, sigm_max));
	multi_scale_enhancement_filter->SetNumberOfSigmaSteps(std::min(1, num_levels));

	auto slice_filter = slice_by_slice_filter_type::New();
	slice_filter->SetInput(source);
	slice_filter->SetFilter(multi_scale_enhancement_filter);
	slice_filter->Update();

	return slice_filter->GetOutput();
}

template<class TInput, class TTissue, class TTarget>
void AutoTubeWidget::DoWorkNd(TInput* source, TTissue* tissues, TTarget* target)
{
	itkStaticConstMacro(ImageDimension, size_t, TInput::ImageDimension);
	using input_type = TInput;
	using real_type = itk::Image<float, ImageDimension>;
	using mask_type = itk::Image<unsigned char, ImageDimension>;
	using labelfield_type = itk::Image<unsigned short, ImageDimension>;

	using threshold_filter_type = itk::BinaryThresholdImageFilter<real_type, mask_type>;
	using nonmax_filter_type = itk::NonMaxSuppressionImageFilter<real_type>;
	using thinnning_filter_type = BinaryThinningImageFilter<mask_type, mask_type, ImageDimension>;

	typename real_type::Pointer feature_image;
	std::vector<double> feature_params;
	feature_params.push_back(m_SigmaLow->text().toDouble());
	feature_params.push_back(m_SigmaHi->text().toDouble());
	feature_params.push_back(m_NumberSigmaLevels->text().toInt());
	feature_params.push_back(m_Metric2d->isChecked());
	if (!m_CachedFeatureImage.Get(feature_image, feature_params))
	{
		feature_image = m_Metric2d->isChecked()
												? ComputeFeatureImage2d<input_type, real_type>(source)
												: ComputeFeatureImage<input_type, real_type>(source);
		m_CachedFeatureImage.Store(feature_image, feature_params);
		m_CachedSkeleton.img = nullptr;
	}

	// initialize threshold to reasonable value
	bool ok = false;
	float lower = m_Threshold->text().toFloat(&ok);
	if (!ok)
	{
		auto calculator = itk::MinimumMaximumImageCalculator<real_type>::New();
		calculator->SetImage(feature_image);
		calculator->Compute();

		auto min_gm = calculator->GetMinimum();
		auto max_gm = calculator->GetMaximum();

		lower = min_gm + 0.8 * (max_gm - min_gm); // stupid way to guess threshold
		m_Threshold->setText(QString::number(lower));
	}

	// extract IDs if any were set
	std::vector<int> object_ids;
	if (!m_SelectedObjects->text().isEmpty())
	{
		std::vector<std::string> tokens;
		std::string selected_objects_text = m_SelectedObjects->text().toStdString();
		boost::algorithm::split(tokens, selected_objects_text, boost::algorithm::is_any_of(","));
		std::transform(tokens.begin(), tokens.end(), std::back_inserter(object_ids), [](std::string s) {
			boost::algorithm::trim(s);
			return stoi(s);
		});
	}

	// mask feature image before skeletonization
	if (!object_ids.empty())
	{
		using map_functor_type = iseg::Functor::MapLabels<unsigned short, unsigned char>;
		map_functor_type map;
		map.m_Map.assign(m_Handler3D->TissueNames().size() + 1, 0);
		for (size_t i = 0; i < object_ids.size(); i++)
		{
			map.m_Map.at(object_ids[i]) = 1;
		}

		auto map_filter = itk::UnaryFunctorImageFilter<TTissue, mask_type, map_functor_type>::New();
		map_filter->SetFunctor(map);
		map_filter->SetInput(tissues);

		auto masker = itk::MaskImageFilter<real_type, mask_type>::New();
		masker->SetInput(feature_image);
		masker->SetMaskImage(map_filter->GetOutput());
		SAFE_UPDATE(masker, return );
		feature_image = masker->GetOutput();
	}

	typename mask_type::Pointer skeleton;
	std::vector<double> skeleton_params(object_ids.begin(), object_ids.end());
	skeleton_params.push_back(lower);
	skeleton_params.push_back(m_NonMaxSuppression->isChecked());
	skeleton_params.push_back(m_Skeletonize->isChecked());
	skeleton_params.push_back(m_MinObjectSize->text().toInt());
	if (!m_CachedSkeleton.Get(skeleton, skeleton_params))
	{
		// disconnect bright tubes via non-maxi suppression
		if (m_NonMaxSuppression->isChecked())
		{
			auto masking = itk::ThresholdImageFilter<real_type>::New();
			masking->SetInput(feature_image);
			masking->ThresholdBelow(lower);
			masking->SetOutsideValue(std::min(lower, 0.f));

			// do thinning to get brightest pixels ("one" pixel wide)
			auto nonmax_filter = nonmax_filter_type::New();
			nonmax_filter->SetInput(masking->GetOutput());

			auto threshold = threshold_filter_type::New();
			threshold->SetInput(nonmax_filter->GetOutput());
			threshold->SetLowerThreshold(std::nextafter(std::min(lower, 0.f), 1.f));
			SAFE_UPDATE(threshold, return );
			skeleton = threshold->GetOutput();
		}
		else
		{
			auto threshold = threshold_filter_type::New();
			threshold->SetInput(feature_image);
			threshold->SetLowerThreshold(lower);
			SAFE_UPDATE(threshold, return );
			skeleton = threshold->GetOutput();
		}

		if (m_Skeletonize->isChecked())
		{
			// get centerline: either thresholding or non-max suppression must be done before this
			auto thinning = thinnning_filter_type::New();
			thinning->SetInput(skeleton);

			auto rescale = itk::RescaleIntensityImageFilter<mask_type>::New();
			rescale->SetInput(thinning->GetOutput());
			rescale->SetOutputMinimum(0);
			rescale->SetOutputMaximum(255);
			rescale->InPlaceOn();

			SAFE_UPDATE(rescale, return );
			skeleton = rescale->GetOutput();
		}
		m_CachedSkeleton.Store(skeleton, skeleton_params);
	}

	typename mask_type::Pointer output;
	if (skeleton && m_MinObjectSize->text().toInt() > 1)
	{
		auto connectivity = itk::ConnectedComponentImageFilter<mask_type, labelfield_type>::New();
		connectivity->SetInput(skeleton);
		connectivity->FullyConnectedOn();

		auto relabel = itk::RelabelComponentImageFilter<labelfield_type, labelfield_type>::New();
		relabel->SetInput(connectivity->GetOutput());
		relabel->SetMinimumObjectSize(m_MinObjectSize->text().toInt());

		auto threshold = itk::BinaryThresholdImageFilter<labelfield_type, mask_type>::New();
		threshold->SetInput(relabel->GetOutput());
		threshold->SetLowerThreshold(1);
		SAFE_UPDATE(threshold, return );
		output = threshold->GetOutput();
	}

	iseg::DataSelection data_selection;
	data_selection.allSlices = true; // all_slices->isChecked();
	data_selection.sliceNr = m_Handler3D->ActiveSlice();
	data_selection.work = true;
	emit BeginDatachange(data_selection, this);

	if (output && iseg::Paste<mask_type, input_type>(output, target))
	{
		// good, else maybe output is not defined
	}
	else if (!iseg::Paste<mask_type, input_type>(skeleton, target))
	{
		std::cerr << "Error: could not set output because image regions don't match.\n";
	}

	emit EndDatachange(this);
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_INDEG_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_INDEG_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object converts radian to degree.

    @par Header <boost/simd/function/indeg.hpp>

    @par Example:

      @snippet indeg.cpp indeg

    @par Possible output:

      @snippet indeg.txt indeg

  **/
  IEEEValue indeg(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/indeg.hpp>
#include <boost/simd/function/simd/indeg.hpp>

#endif


/*
 * protonReconstruct.cpp
 *
 *  Created on: Dec 22, 2011
 *      Author: u051747
 */
#include <iostream>
#include "parameterparser.h"
#include "cuNDArray.h"
#include "hoCuNDArray.h"

#include "hoCuOperatorPathBackprojection.h"
#include "hoNDArray_fileio.h"
#include "check_CUDA.h"

#include "hoCuGPBBSolver.h"
#include "hoCuPartialDerivativeOperator.h"
#include "hdf5_utils.h"

#include "encodingOperatorContainer.h"
#include "hoCuOperator.h"

#include "identityOperator.h"
#include <boost/program_options.hpp>
#include "vector_td_io.h"

#include "hoCuPartialDerivativeOperator.h"
#include "projectionSpaceOperator.h"
#include "weightingOperator.h"
#include "sbcSolver.h"
#include "cgSolver.h"

using namespace std;
using namespace Gadgetron;
typedef float _real;

namespace po = boost::program_options;
int main( int argc, char** argv)
{

  //
  // Parse command line
  //

  _real background =  0.00106;
  std::string projectionsName;
	std::string splinesName;
	std::string outputFile;
	vector_td<int,3> dimensions;
	vector_td<float,3> physical_dims;
	vector_td<float,3> origin;
	int iterations;
	int outer_iterations;
	int device;
	po::options_description desc("Allowed options");
	desc.add_options()
			("help", "produce help message")
			("projections,p", po::value<std::string>(&projectionsName)->default_value("projections.real"), "File containing the projection data")
			("splines,s", po::value<std::string>(&splinesName)->default_value("splines.real"), "File containing the spline trajectories")
			("dimensions,d", po::value<vector_td<int,3> >(&dimensions)->default_value(vector_td<int,3>(512,512,1)), "Pixel dimensions of the image")
			("size,S", po::value<vector_td<float,3> >(&physical_dims)->default_value(vector_td<float,3>(20,20,5)), "Dimensions of the image")
			("center,c", po::value<vector_td<float,3> >(&origin)->default_value(vector_td<float,3>(0,0,0)), "Center of the reconstruction")
			("iterations,i", po::value<int>(&iterations)->default_value(10), "Inner iteration")
			("outer-iterations,O", po::value<int>(&outer_iterations)->default_value(10), "Outer iteration")
			("output,f", po::value<std::string>(&outputFile)->default_value("image.hdf5"), "Output filename")
			("prior,P", po::value<std::string>(),"Prior image filename")
			("prior-weight,k",po::value<float>(),"Weight of the prior image")
			("wprior-weight,w",po::value<float>(),"Weight of the weighted prior image")
			("PICS-weight,C",po::value<float>(),"TV Weight of the prior image (Prior image compressed sensing)")
			("TV,T",po::value<float>(),"TV Weight ")
			("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")

	;

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

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		std::cout << it->first << ": ";
		if (a.type() == typeid(std::string)) std::cout << it->second.as<std::string>();
		else if (a.type() == typeid(int)) std::cout << it->second.as<int>();
		else if (a.type() == typeid(float)) std::cout << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) std::cout << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) std::cout << it->second.as<vector_td<int,3> >();
		else std::cout << "Unknown type" << std::endl;
		std::cout << std::endl;
	}
	cudaSetDevice(device);
	cudaDeviceReset();

  boost::shared_ptr<hoCuNDArray<vector_td<_real,3> > > splines = boost::static_pointer_cast<hoCuNDArray<vector_td<_real,3> > >(read_nd_array< vector_td<_real,3> >(splinesName.c_str()));
  cout << "Number of spline elements: " << splines->get_number_of_elements() << endl;

  boost::shared_ptr< hoCuNDArray<_real> > projections = boost::static_pointer_cast<hoCuNDArray<_real> > (read_nd_array<_real >(projectionsName.c_str()));

  std::cout << "Number of elements " << projections->get_number_of_elements() << std::endl;

  if (projections->get_number_of_elements() != splines->get_number_of_elements()/4){
	  cout << "Critical error: Splines and projections do not match dimensions" << endl;
	  return 0;
  }

  sbcSolver<hoCuNDArray<_real>,hoCuNDArray<_real>,cgSolver<hoCuNDArray<_real> > > solver;

  solver.set_max_inner_iterations(1);
  solver.set_max_outer_iterations(outer_iterations);

  //solver.set_tc_tolerance( (_real) parms.get_parameter('e')->get_float_value());
  //solver.set_alpha(1e-7);
  solver.get_inner_solver()->set_max_iterations(iterations);
  solver.get_inner_solver()->set_output_mode(cgSolver<hoCuNDArray<_real> >::OUTPUT_VERBOSE);
  solver.get_inner_solver()->set_tc_tolerance(1e-5);

  solver.set_output_mode( sbcSolver<hoCuNDArray<_real>,hoCuNDArray<_real>,cgSolver<hoCuNDArray<_real> > >::OUTPUT_VERBOSE );
  solver.set_non_negativity_filter(true);
  boost::shared_ptr< hoCuOperatorPathBackprojection<_real> > E (new hoCuOperatorPathBackprojection<_real> );
  E->setup(splines,physical_dims,projections,origin,background);

  std::vector<size_t> rhs_dims(&dimensions[0],&dimensions[3]); //Quick and dirty vector_td to vector
  E->set_domain_dimensions(&rhs_dims);
  E->set_codomain_dimensions(projections->get_dimensions().get());

  boost::shared_ptr<hoCuNDArray<_real > > prior;
  if (vm.count("prior")){
 	  std::cout << "Prior image used as initial guess" << std::endl;
		prior = boost::static_pointer_cast<hoCuNDArray<_real > >(read_nd_array<_real >(vm["prior"].as<std::string>().c_str()));

		prior->reshape(&rhs_dims);
		_real offset = _real(0.01);
		//cuNDA_add(offset,prior.get());

		if (vm.count("prior-weight")){
			std::cout << "Prior image difference regularization in use" << std::endl;
			boost::shared_ptr<identityOperator<hoCuNDArray<_real> > > Itmp ( new identityOperator<hoCuNDArray<_real> >);

			boost::shared_ptr<projectionSpaceOperator<hoCuNDArray<_real> > > I (new projectionSpaceOperator<hoCuNDArray<_real> >(Itmp));

			I->set_weight(vm["prior-weight"].as<float>());

			I->set_codomain_dimensions(&rhs_dims);
			I->set_domain_dimensions(&rhs_dims);
			I->set_projections(prior);
			//hoCuNDArray<_real> tmp = *prior;

			//I->mult_M(prior.get(),&tmp);
			solver.add_regularization_operator(I);
		}

		if (vm.count("wprior-weight")){
			std::cout << "Prior image difference regularization in use" << std::endl;

			boost::shared_ptr<weightingOperator<hoCuNDArray<_real> > > Wtmp ( new weightingOperator<hoCuNDArray<_real> >());

			boost::shared_ptr<hoCuNDArray<_real> > weights(new hoCuNDArray<_real>(*prior));

			abs_inplace(weights.get());
			*weights += _real(0.01);

			reciprocal_inplace(weights.get());

			Wtmp->set_weights(weights);

			boost::shared_ptr<projectionSpaceOperator<hoCuNDArray<_real> > > W (new projectionSpaceOperator<hoCuNDArray<_real> >(Wtmp));
			W->set_weight(vm["wprior-weight"].as<float>());

			W->set_codomain_dimensions(&rhs_dims);
			W->set_domain_dimensions(&rhs_dims);

			boost::shared_ptr<hoCuNDArray<_real> > tmp_array(new hoCuNDArray<_real>(*prior));
			Wtmp->mult_M(prior.get(),tmp_array.get());

			W->set_projections(tmp_array);
			//hoCuNDArray<_real> tmp = *prior;

			//I->mult_M(prior.get(),&tmp);
			solver.add_regularization_operator(W);
		}

		if (vm.count("PICS-weight")){
			std::cout << "PICS in used" << std::endl;
			boost::shared_ptr<hoCuPartialDerivativeOperator<_real,3> > Rx(new hoCuPartialDerivativeOperator<_real,3>(0));
			boost::shared_ptr<hoCuPartialDerivativeOperator<_real,3> > Ry(new hoCuPartialDerivativeOperator<_real,3>(1));

			Rx->set_weight(vm["PICS-weight"].as<float>());
			Ry->set_weight(vm["PICS-weight"].as<float>());
			Rx->set_codomain_dimensions(&rhs_dims);
			Ry->set_codomain_dimensions(&rhs_dims);
			Rx->set_domain_dimensions(&rhs_dims);
			Ry->set_domain_dimensions(&rhs_dims);

			solver.add_regularization_group_operator(Rx);
			solver.add_regularization_group_operator(Ry);
			solver.add_group(prior,1);

		}

		//solver.set_x0(prior);
  }

  if (vm.count("TV")){
	  std::cout << "Total variation regularization in use" << std::endl;
		boost::shared_ptr<hoCuPartialDerivativeOperator<_real,3> > Rx(new hoCuPartialDerivativeOperator<_real,3>(0));
		boost::shared_ptr<hoCuPartialDerivativeOperator<_real,3> > Ry(new hoCuPartialDerivativeOperator<_real,3>(1));

		Rx->set_weight(vm["TV"].as<float>());
		Ry->set_weight(vm["TV"].as<float>());
		Rx->set_codomain_dimensions(&rhs_dims);
		Ry->set_codomain_dimensions(&rhs_dims);
		Rx->set_domain_dimensions(&rhs_dims);
		Ry->set_domain_dimensions(&rhs_dims);
		solver.add_regularization_group_operator(Rx);
		solver.add_regularization_group_operator(Ry);
		solver.add_group(1);

  }

  solver.set_encoding_operator(E);

	//hoCuNDA_clear(projections.get());
	//CHECK_FOR_CUDA_ERROR();

	//float res = dot(projections.get(),projections.get());

	boost::shared_ptr< hoCuNDArray<_real> > result = solver.solve(projections.get());

	//write_nd_array<_real>(result.get(), (char*)parms.get_parameter('f')->get_string_value());
	std::stringstream ss;
	for (int i = 0; i < argc; i++){
		ss << argv[i] << " ";
	}
	saveNDArray2HDF5<3>(result.get(),outputFile,physical_dims,origin,ss.str(), solver.get_max_outer_iterations());
}


#include <Eigen/Core>
#include <Eigen/Dense>
#include <ceres/ceres.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

#include <ceres-error-terms/block-pose-prior-error-term.h>
#include <ceres-error-terms/common.h>
#include <ceres-error-terms/loop-closure-block-pose-error-term.h>
#include <ceres-error-terms/parameterization/pose-param-jpl.h>
#include <ceres-error-terms/switch-prior-error-term.h>
#include <ceres-error-terms/test_posegraph/loop_closure_constraint.h>
#include <ceres-error-terms/test_posegraph/posegraph.h>
#include <ceres-error-terms/test_posegraph/posegraph_constraint.h>

using namespace ceres_error_terms;  // NOLINT

void fillPosegraphWithLoopClosure(BlockPosegraph& posegraph) {  // NOLINT
  const int num_poses = 4;
  const int num_loop_closure_constraints = 1;
  const int num_baseframes = 2;
  posegraph.loop_closure_constraints.resize(num_loop_closure_constraints);
  posegraph.vertex_poses = Eigen::MatrixXd::Zero(7, num_poses);

  posegraph.baseframe_poses = Eigen::MatrixXd::Zero(7, num_baseframes);
  pose::Transformation T_G_M1;
  T_G_M1.setRandom(1e4);
  posegraph.baseframe_poses.block<4, 1>(0, 0)
      << T_G_M1.getRotation().toImplementation().inverse().coeffs();
  posegraph.baseframe_poses.block<3, 1>(4, 0) << T_G_M1.getPosition();

  pose::Transformation T_G_M2;
  T_G_M2.setRandom(1e4);
  posegraph.baseframe_poses.block<4, 1>(0, 1)
      << T_G_M2.getRotation().toImplementation().inverse().coeffs();
  posegraph.baseframe_poses.block<3, 1>(4, 1) << T_G_M2.getPosition();

  const pose::Transformation T_G_V0(
      pose::Quaternion(Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)),
      pose::Position3D(0, 0, 0));
  const pose::Transformation T_G_V1(
      pose::Quaternion(
          Eigen::Quaterniond(
              0.9965159041385382, 0.06892005802323266, 0.020435594117957374,
              0.042290245850220926)),
      pose::Position3D(1.1, 0, 0));
  const pose::Transformation T_G_V2(
      pose::Quaternion(
          Eigen::Quaterniond(
              0.9990182922767228, -0.007804703449117966, -0.026464453222036003,
              0.03465791419330294)),
      pose::Position3D(1, 2.5, 0));
  const pose::Transformation T_G_V3(
      pose::Quaternion(
          Eigen::Quaterniond(
              0.9988646699884802, -0.00795566765132752, -0.043763237371076937,
              0.01705454355380988)),
      pose::Position3D(0, 2, 0));
  const pose::Transformation T_M1_V0 = T_G_M1.inverse() * T_G_V0;
  const pose::Transformation T_M1_V1 = T_G_M1.inverse() * T_G_V1;
  const pose::Transformation T_M2_V2 = T_G_M2.inverse() * T_G_V2;
  const pose::Transformation T_M2_V3 = T_G_M2.inverse() * T_G_V3;

  const pose::Transformation T_G_V0_true(
      pose::Quaternion(Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)),
      pose::Position3D(0, 0, 0));
  const pose::Transformation T_G_V1_true(
      pose::Quaternion(Eigen::Quaterniond(sqrt(2) / 2, 0, 0, sqrt(2) / 2)),
      pose::Position3D(1, 0, 0));
  const pose::Transformation T_G_V2_true(
      pose::Quaternion(Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0)),
      pose::Position3D(1, 1, 0));
  const pose::Transformation T_G_V3_true(
      pose::Quaternion(Eigen::Quaterniond(sqrt(2) / 2, 0, 0, -sqrt(2) / 2)),
      pose::Position3D(0, 1, 0));
  const pose::Transformation T_M1_V0_true = T_G_M1.inverse() * T_G_V0_true;
  const pose::Transformation T_M1_V1_true = T_G_M1.inverse() * T_G_V1_true;
  const pose::Transformation T_M2_V2_true = T_G_M2.inverse() * T_G_V2_true;
  const pose::Transformation T_M2_V3_true = T_G_M2.inverse() * T_G_V3_true;

  posegraph.vertex_pose_idx_to_baseframe_idx.emplace(0, 0);
  posegraph.vertex_pose_idx_to_baseframe_idx.emplace(1, 0);
  posegraph.vertex_pose_idx_to_baseframe_idx.emplace(2, 1);
  posegraph.vertex_pose_idx_to_baseframe_idx.emplace(3, 1);

  posegraph.vertex_poses.block<4, 1>(0, 0) =
      T_M1_V0.getRotation().toImplementation().coeffs();
  posegraph.vertex_poses.block<3, 1>(4, 0) = T_M1_V0.getPosition();
  posegraph.vertex_poses.block<4, 1>(0, 1) =
      T_M1_V1.getRotation().toImplementation().coeffs();
  posegraph.vertex_poses.block<3, 1>(4, 1) = T_M1_V1.getPosition();
  posegraph.vertex_poses.block<4, 1>(0, 2) =
      T_M2_V2.getRotation().toImplementation().coeffs();
  posegraph.vertex_poses.block<3, 1>(4, 2) = T_M2_V2.getPosition();
  posegraph.vertex_poses.block<4, 1>(0, 3) =
      T_M2_V3.getRotation().toImplementation().coeffs();
  posegraph.vertex_poses.block<3, 1>(4, 3) = T_M2_V3.getPosition();

  // Add odometry constraints.
  Eigen::Matrix<double, 6, 6> dummy_covariance;
  dummy_covariance.setIdentity();
  posegraph.pose_prior_constriants.emplace_back(
      0, T_M1_V0_true, dummy_covariance);
  posegraph.pose_prior_constriants.emplace_back(
      1, T_M1_V1_true, dummy_covariance);
  posegraph.pose_prior_constriants.emplace_back(
      2, T_M2_V2_true, dummy_covariance);
  posegraph.pose_prior_constriants.emplace_back(
      3, T_M2_V3_true, dummy_covariance);

  // Add loop closure constraints.
  const double initial_switch_variable = 1.0;
  const double switch_variable_covariance = 1.0;
  const pose::Transformation T_V0_V2_lc(
      pose::Position3D(1, 1, 0), pose::Quaternion(0, 0, 0, 1));
  posegraph.loop_closure_constraints[0] = LoopClosureConstraint(
      0, 2, T_V0_V2_lc, dummy_covariance, switch_variable_covariance,
      initial_switch_variable);
}

void fillPosegraphWithIncorrectLoopClosure(
    BlockPosegraph& posegraph) {  // NOLINT
  const int num_poses = 4;
  const int num_loop_closure_constraints = 1;
  const int num_baseframes = 2;
  posegraph.loop_closure_constraints.resize(num_loop_closure_constraints);
  posegraph.vertex_poses = Eigen::MatrixXd::Zero(7, num_poses);

  posegraph.baseframe_poses = Eigen::MatrixXd::Zero(7, num_baseframes);
  pose::Transformation T_G_M1;
  T_G_M1.setRandom(1e4);
  posegraph.baseframe_poses.block<4, 1>(0, 0)
      << T_G_M1.getRotation().toImplementation().inverse().coeffs();
  posegraph.baseframe_poses.block<3, 1>(4, 0) << T_G_M1.getPosition();

  pose::Transformation T_G_M2;
  T_G_M2.setRandom(1e4);
  posegraph.baseframe_poses.block<4, 1>(0, 1)
      << T_G_M2.getRotation().toImplementation().inverse().coeffs();
  posegraph.baseframe_poses.block<3, 1>(4, 1) << T_G_M2.getPosition();

  const pose::Transformation T_G_V0(
      pose::Quaternion(Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)),
      pose::Position3D(0, 0, 0));
  const pose::Transformation T_G_V1(
      pose::Quaternion(
          Eigen::Quaterniond(
              0.9965159041385382, 0.06892005802323266, 0.020435594117957374,
              0.042290245850220926)),
      pose::Position3D(1.1, 0, 0));
  const pose::Transformation T_G_V2(
      pose::Quaternion(
          Eigen::Quaterniond(
              0.9990182922767228, -0.007804703449117966, -0.026464453222036003,
              0.03465791419330294)),
      pose::Position3D(1, 2.5, 0));
  const pose::Transformation T_G_V3(
      pose::Quaternion(
          Eigen::Quaterniond(
              0.9988646699884802, -0.00795566765132752, -0.043763237371076937,
              0.01705454355380988)),
      pose::Position3D(0, 2, 0));
  const pose::Transformation T_M1_V0 = T_G_M1.inverse() * T_G_V0;
  const pose::Transformation T_M1_V1 = T_G_M1.inverse() * T_G_V1;
  const pose::Transformation T_M2_V2 = T_G_M2.inverse() * T_G_V2;
  const pose::Transformation T_M2_V3 = T_G_M2.inverse() * T_G_V3;

  const pose::Transformation T_G_V0_true(
      pose::Quaternion(Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)),
      pose::Position3D(0, 0, 0));
  const pose::Transformation T_G_V1_true(
      pose::Quaternion(Eigen::Quaterniond(sqrt(2) / 2, 0, 0, sqrt(2) / 2)),
      pose::Position3D(1, 0, 0));
  const pose::Transformation T_G_V2_true(
      pose::Quaternion(Eigen::Quaterniond(0.0, 0.0, 0.0, 1.0)),
      pose::Position3D(1, 1, 0));
  const pose::Transformation T_G_V3_true(
      pose::Quaternion(Eigen::Quaterniond(sqrt(2) / 2, 0, 0, -sqrt(2) / 2)),
      pose::Position3D(0, 1, 0));
  const pose::Transformation T_M1_V0_true = T_G_M1.inverse() * T_G_V0_true;
  const pose::Transformation T_M1_V1_true = T_G_M1.inverse() * T_G_V1_true;
  const pose::Transformation T_M2_V2_true = T_G_M2.inverse() * T_G_V2_true;
  const pose::Transformation T_M2_V3_true = T_G_M2.inverse() * T_G_V3_true;

  posegraph.vertex_pose_idx_to_baseframe_idx.emplace(0, 0);
  posegraph.vertex_pose_idx_to_baseframe_idx.emplace(1, 0);
  posegraph.vertex_pose_idx_to_baseframe_idx.emplace(2, 1);
  posegraph.vertex_pose_idx_to_baseframe_idx.emplace(3, 1);

  posegraph.vertex_poses.block<4, 1>(0, 0) =
      T_M1_V0.getRotation().toImplementation().coeffs();
  posegraph.vertex_poses.block<3, 1>(4, 0) = T_M1_V0.getPosition();
  posegraph.vertex_poses.block<4, 1>(0, 1) =
      T_M1_V1.getRotation().toImplementation().coeffs();
  posegraph.vertex_poses.block<3, 1>(4, 1) = T_M1_V1.getPosition();
  posegraph.vertex_poses.block<4, 1>(0, 2) =
      T_M2_V2.getRotation().toImplementation().coeffs();
  posegraph.vertex_poses.block<3, 1>(4, 2) = T_M2_V2.getPosition();
  posegraph.vertex_poses.block<4, 1>(0, 3) =
      T_M2_V3.getRotation().toImplementation().coeffs();
  posegraph.vertex_poses.block<3, 1>(4, 3) = T_M2_V3.getPosition();

  // Add odometry constraints.
  Eigen::Matrix<double, 6, 6> dummy_covariance;
  dummy_covariance.setIdentity();
  posegraph.pose_prior_constriants.emplace_back(
      0, T_M1_V0_true, dummy_covariance);
  posegraph.pose_prior_constriants.emplace_back(
      1, T_M1_V1_true, dummy_covariance);
  posegraph.pose_prior_constriants.emplace_back(
      2, T_M2_V2_true, dummy_covariance);
  posegraph.pose_prior_constriants.emplace_back(
      3, T_M2_V3_true, dummy_covariance);

  // Add loop closure constraints.
  const double initial_switch_variable = 1.0;
  const double switch_variable_covariance = 100;
  pose::Quaternion loop_quaternion(0.6916, 0.6350, 0.0321, 0.3426);
  loop_quaternion.normalize();
  posegraph.loop_closure_constraints[0] = LoopClosureConstraint(
      0, 2, pose::Transformation(pose::Position3D(-2, 0, 0.2), loop_quaternion),
      0.1 * dummy_covariance, switch_variable_covariance,
      initial_switch_variable);
}

void solveWithSwitchableConstraints(BlockPosegraph& posegraph) {  // NOLINT
  ceres::Problem problem;
  ceres::LocalParameterization* pose_parametrization(
      new JplPoseParameterization);

  for (const PosePriorConstraint& ppc : posegraph.pose_prior_constriants) {
    ceres::CostFunction* pose_prior_cost(
        new ceres_error_terms::BlockPosePriorErrorTerm(
            ppc.T_G_I_.getRotation().toImplementation().coeffs(),
            ppc.T_G_I_.getPosition(), ppc.T_G_I_covariance_));

    problem.AddResidualBlock(
        pose_prior_cost, nullptr, posegraph.vertex_poses.col(ppc.node_).data());
  }

  pose::Quaternion G_q_M;
  G_q_M.setIdentity();
  pose::Position3D G_p_M;
  G_p_M.setZero();

  CHECK_EQ(posegraph.baseframe_poses.cols(), 2);

  const double switch_prior = 1.0;
  for (LoopClosureConstraint& lcc : posegraph.loop_closure_constraints) {
    ceres::CostFunction* loop_closure_cost = new ceres::AutoDiffCostFunction<
        LoopClosureBlockPoseErrorTerm,
        LoopClosureBlockPoseErrorTerm::residualBlockSize, poseblocks::kPoseSize,
        poseblocks::kPoseSize, poseblocks::kPoseSize, poseblocks::kPoseSize,
        LoopClosureBlockPoseErrorTerm::switchVariableBlockSize>(
        new LoopClosureBlockPoseErrorTerm(lcc.T_A_B_, lcc.T_A_B_covariance_));
    ceres::CostFunction* switch_variable_cost = new ceres::AutoDiffCostFunction<
        SwitchPriorErrorTermLegacy,
        SwitchPriorErrorTermLegacy::residualBlockSize,
        SwitchPriorErrorTermLegacy::switchVariableBlockSize>(
        new SwitchPriorErrorTermLegacy(
            switch_prior, lcc.switch_variable_covariance_));

    std::unordered_map<int, int>::iterator baseframe_from_iterator =
        posegraph.vertex_pose_idx_to_baseframe_idx.find(lcc.from_node_);
    CHECK(
        baseframe_from_iterator !=
        posegraph.vertex_pose_idx_to_baseframe_idx.end());
    const int baseframe_from_idx = baseframe_from_iterator->second;
    CHECK_GE(baseframe_from_idx, 0);
    CHECK_LT(baseframe_from_idx, posegraph.baseframe_poses.cols());

    std::unordered_map<int, int>::iterator baseframe_to_iterator =
        posegraph.vertex_pose_idx_to_baseframe_idx.find(lcc.to_node_);
    CHECK(
        baseframe_to_iterator !=
        posegraph.vertex_pose_idx_to_baseframe_idx.end());
    const int baseframe_to_idx = baseframe_to_iterator->second;
    CHECK_GE(baseframe_to_idx, 0);
    CHECK_LT(baseframe_to_idx, posegraph.baseframe_poses.cols());

    problem.AddResidualBlock(
        loop_closure_cost, NULL,
        posegraph.baseframe_poses.col(baseframe_from_idx).data(),
        posegraph.vertex_poses.col(lcc.from_node_).data(),
        posegraph.baseframe_poses.col(baseframe_to_idx).data(),
        posegraph.vertex_poses.col(lcc.to_node_).data(), &lcc.switch_variable_);

    problem.AddResidualBlock(switch_variable_cost, NULL, &lcc.switch_variable_);

    problem.SetParameterization(
        posegraph.baseframe_poses.col(baseframe_from_idx).data(),
        pose_parametrization);
    problem.SetParameterization(
        posegraph.baseframe_poses.col(baseframe_to_idx).data(),
        pose_parametrization);
    problem.SetParameterBlockConstant(
        posegraph.baseframe_poses.col(baseframe_from_idx).data());
    problem.SetParameterBlockConstant(
        posegraph.baseframe_poses.col(baseframe_to_idx).data());
  }

  // Add parameterization for quaternion of each graph node.
  for (int col_idx = 0; col_idx < posegraph.vertex_poses.cols(); ++col_idx) {
    problem.SetParameterization(
        posegraph.vertex_poses.col(col_idx).data(), pose_parametrization);
  }

  problem.SetParameterBlockConstant(posegraph.vertex_poses.col(0).data());

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.max_num_iterations = 100;
  options.gradient_tolerance = 1e-16;
  options.function_tolerance = 1e-16;
  options.parameter_tolerance = 1e-16;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  LOG(INFO) << summary.BriefReport();
}

// The test verifies if a posegraph with:
// * noisy initial guesses
// * perfect odometry constraints
// * perfect loop closure constraint
// will end up as perfect square. The switch variable should be equal to 1
// (i.e. the loop closure should be treated as valid).
TEST(PosegraphErrorTerms, SwitchableConstraints_SimpleLoopClosure) {
  BlockPosegraph posegraph;
  fillPosegraphWithLoopClosure(posegraph);
  solveWithSwitchableConstraints(posegraph);

  CHECK_EQ(posegraph.baseframe_poses.cols(), 2);

  const Eigen::Quaterniond q_G_M1_JPL(
      posegraph.baseframe_poses.col(0).head(4).data());
  const pose::Transformation T_G_M1(
      q_G_M1_JPL.inverse(), posegraph.baseframe_poses.col(0).tail(3));

  const Eigen::Quaterniond q_G_M2_JPL(
      posegraph.baseframe_poses.col(1).head(4).data());
  const pose::Transformation T_G_M2(
      q_G_M2_JPL.inverse(), posegraph.baseframe_poses.col(1).tail(3));

  pose::Quaternion q_M1_V0;
  q_M1_V0.toImplementation().coeffs() << posegraph.vertex_poses.col(0).head(4);
  const pose::Transformation T_M1_V0(
      q_M1_V0, posegraph.vertex_poses.col(0).tail(3));
  const pose::Transformation T_G_V0 = T_G_M1 * T_M1_V0;

  pose::Quaternion q_M1_V1;
  q_M1_V1.toImplementation().coeffs() << posegraph.vertex_poses.col(1).head(4);
  const pose::Transformation T_M1_V1(
      q_M1_V1, posegraph.vertex_poses.col(1).tail(3));
  const pose::Transformation T_G_V1 = T_G_M1 * T_M1_V1;

  pose::Quaternion q_M2_V2;
  q_M2_V2.toImplementation().coeffs() << posegraph.vertex_poses.col(2).head(4);
  const pose::Transformation T_M2_V2(
      q_M2_V2, posegraph.vertex_poses.col(2).tail(3));
  const pose::Transformation T_G_V2 = T_G_M2 * T_M2_V2;

  pose::Quaternion q_M2_V3;
  q_M2_V3.toImplementation().coeffs() << posegraph.vertex_poses.col(3).head(4);
  const pose::Transformation T_M2_V3(
      q_M2_V3, posegraph.vertex_poses.col(3).tail(3));
  const pose::Transformation T_G_V3 = T_G_M2 * T_M2_V3;

  const pose::Transformation T_V0_V2 = T_G_V0.inverse() * T_G_V2;

  EXPECT_ZERO_EIGEN(T_G_V0.getPosition(), 1e-8);
  EXPECT_NEAR_EIGEN(T_G_V1.getPosition(), pose::Position3D(1, 0, 0), 1e-8);
  EXPECT_NEAR_EIGEN(T_G_V2.getPosition(), pose::Position3D(1, 1, 0), 1e-8);
  EXPECT_NEAR_EIGEN(T_G_V3.getPosition(), pose::Position3D(0, 1, 0), 1e-8);

  EXPECT_NEAR_EIGEN(
      T_G_V0.getRotation().toImplementation().coeffs(),
      Eigen::Quaterniond(1, 0, 0, 0).coeffs(), 1e-6);

  EXPECT_NEAR_EIGEN(
      T_G_V1.getRotation().toImplementation().coeffs(),
      Eigen::Quaterniond(sqrt(2) / 2, 0, 0, sqrt(2) / 2).coeffs(), 1e-6);
  EXPECT_NEAR_KINDR_QUATERNION(
      T_G_V2.getRotation(),
      kindr::minimal::RotationQuaternion(Eigen::Quaterniond(0, 0, 0, 1)), 1e-6);
  EXPECT_NEAR_KINDR_QUATERNION(
      T_G_V3.getRotation(),
      kindr::minimal::RotationQuaternion(
          Eigen::Quaterniond(sqrt(2) / 2, 0, 0, -sqrt(2) / 2)),
      1e-6);

  EXPECT_NEAR(posegraph.loop_closure_constraints[0].switch_variable_, 1, 1e-5);
}

// The test verifies if a posegraph with:
// * noisy initial guesses
// * perfect odometry constraints
// * wrong loop closure with large variance
// will end up as perfect square. The switch variable should be equal to 0
// (i.e. the loop closure should be completely ignored).
TEST(PosegraphErrorTerms, SwitchableConstraints_IncorrectLoopClosure) {
  BlockPosegraph posegraph;
  fillPosegraphWithIncorrectLoopClosure(posegraph);
  solveWithSwitchableConstraints(posegraph);

  CHECK_EQ(posegraph.baseframe_poses.cols(), 2);

  const Eigen::Quaterniond q_G_M1_JPL(
      posegraph.baseframe_poses.col(0).head(4).data());
  const pose::Transformation T_G_M1(
      q_G_M1_JPL.inverse(), posegraph.baseframe_poses.col(0).tail(3));

  const Eigen::Quaterniond q_G_M2_JPL(
      posegraph.baseframe_poses.col(1).head(4).data());
  const pose::Transformation T_G_M2(
      q_G_M2_JPL.inverse(), posegraph.baseframe_poses.col(1).tail(3));

  pose::Quaternion q_M1_V0;
  q_M1_V0.toImplementation().coeffs() << posegraph.vertex_poses.col(0).head(4);
  const pose::Transformation T_M1_V0(
      q_M1_V0, posegraph.vertex_poses.col(0).tail(3));
  const pose::Transformation T_G_V0 = T_G_M1 * T_M1_V0;

  pose::Quaternion q_M1_V1;
  q_M1_V1.toImplementation().coeffs() << posegraph.vertex_poses.col(1).head(4);
  const pose::Transformation T_M1_V1(
      q_M1_V1, posegraph.vertex_poses.col(1).tail(3));
  const pose::Transformation T_G_V1 = T_G_M1 * T_M1_V1;

  pose::Quaternion q_M2_V2;
  q_M2_V2.toImplementation().coeffs() << posegraph.vertex_poses.col(2).head(4);
  const pose::Transformation T_M2_V2(
      q_M2_V2, posegraph.vertex_poses.col(2).tail(3));
  const pose::Transformation T_G_V2 = T_G_M2 * T_M2_V2;

  pose::Quaternion q_M2_V3;
  q_M2_V3.toImplementation().coeffs() << posegraph.vertex_poses.col(3).head(4);
  const pose::Transformation T_M2_V3(
      q_M2_V3, posegraph.vertex_poses.col(3).tail(3));
  const pose::Transformation T_G_V3 = T_G_M2 * T_M2_V3;

  const pose::Transformation T_V0_V2 = T_G_V0.inverse() * T_G_V2;

  EXPECT_ZERO_EIGEN(T_G_V0.getPosition(), 1e-5);
  EXPECT_NEAR_EIGEN(T_G_V1.getPosition(), pose::Position3D(1, 0, 0), 1e-5);
  EXPECT_NEAR_EIGEN(T_G_V2.getPosition(), pose::Position3D(1, 1, 0), 1e-5);
  EXPECT_NEAR_EIGEN(T_G_V3.getPosition(), pose::Position3D(0, 1, 0), 1e-5);

  EXPECT_NEAR_EIGEN(
      T_G_V0.getRotation().toImplementation().coeffs(),
      Eigen::Quaterniond(1, 0, 0, 0).coeffs(), 1e-6);

  EXPECT_NEAR_EIGEN(
      T_G_V1.getRotation().toImplementation().coeffs(),
      Eigen::Quaterniond(sqrt(2) / 2, 0, 0, sqrt(2) / 2).coeffs(), 1e-6);
  EXPECT_NEAR_KINDR_QUATERNION(
      T_G_V2.getRotation(),
      kindr::minimal::RotationQuaternion(Eigen::Quaterniond(0, 0, 0, 1)), 1e-6);
  EXPECT_NEAR_KINDR_QUATERNION(
      T_G_V3.getRotation(),
      kindr::minimal::RotationQuaternion(
          Eigen::Quaterniond(sqrt(2) / 2, 0, 0, -sqrt(2) / 2)),
      1e-6);

  EXPECT_NEAR(posegraph.loop_closure_constraints[0].switch_variable_, 0, 1e-4);
}

MAPLAB_UNITTEST_ENTRYPOINT


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2012 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef ILUPRECOND_HH
#define ILUPRECOND_HH

#include "globheads.h"
#include "protospp.h"
#include "ios.h"
extern "C" void set_arms_pars(io_t* io,  int Dscale, int *ipar, double *tolcoef, int *lfil);

#include <boost/timer/timer.hpp>
#include "dune/istl/preconditioners.hh"
#include "linalg/triplet.hh"

//---------------------------------------------------------------------
//---------------------------------------------------------------------

namespace Kaskade
{
/**
 * \ingroup linalg
 *
 * PrecondType::ILUT Preconditioner from Saads ITSOL library
 */
template <class Op>
class ILUTPreconditioner: public Dune::Preconditioner<typename Op::Range, typename Op::Range>
{
  typedef typename Op::Range Range;
  typedef Range               Domain;
  typedef typename Op::field_type field_type;

public:
  static int const category = Dune::SolverCategory::sequential;

  ILUTPreconditioner(Op& op, int lfil=240, double tol=1.0e-2, int verbosity=2) {
    boost::timer::cpu_timer iluTimer;
    std::cout << "ilut constructor: ";
    MatrixAsTriplet<field_type> A = op.template get<MatrixAsTriplet<field_type> >();
    int n = A.nrows(), nnz = A.nnz(), ierr;
    FILE *flog = stdout;

    csmat = (csptr)Malloc( sizeof(SparMat), "main" );
    ierr = COOcs(n, nnz,  &A.data[0], &A.cidx[0],  &A.ridx[0], csmat);
    if( ierr != 0 )
      {
        printf(" *** ILU error - COOcs code %d csmat=%p\n", ierr, csmat);
        throw ierr;
      }

    lu = (iluptr)Malloc(sizeof(ILUSpar), "main");
    ierr = ilut(csmat, lu, lfil, tol, flog);
    if( ierr != 0 )
      {
        printf(" *** PrecondType::ILUT error - ilut code %d lu=%p\n", ierr, lu);
        throw ierr;
      }

    xx   = (double *)Malloc(n*sizeof(double), "main");
    yy   = (double *)Malloc(n*sizeof(double), "main");

    if ( verbosity>=2 )
    {
	std::cout << "PrecondType::ILUT: n=" << n << ", nnz=" << nnz << " dropTol=" << tol << ", fillfac="
	          << 1.0*nnz_ilu(lu)/(nnz+1.0) << ", lfil=" << lfil << ", time=" << (double)(iluTimer.elapsed().user)/1e9 << "s\n";
	}
  }
  ~ILUTPreconditioner()
    {
      cleanILU(lu);
      cleanCS(csmat);
      free(xx);
      free(yy);
    }

  virtual void pre (Domain&, Range&) {}
  virtual void post (Domain&) {}

  virtual void apply (Domain& x, Range const& y) {
    y.write(yy);
    lusolC(yy, xx, lu);
    x.read(xx);
  }

private:
  csptr csmat;
  iluptr lu;
  double *xx, *yy;
};

//---------------------------------------------------------------------

/**
 * \ingroup linalg
 *
 * PrecondType::ILUT Preconditioner from Saads ITSOL library
 */
template <class Op>
class ILUKPreconditioner: public Dune::Preconditioner<typename Op::Range, typename Op::Range>
{
  typedef typename Op::Range Range;
  typedef Range               Domain;
  typedef typename Op::field_type field_type;

public:
  static int const category = Dune::SolverCategory::sequential;

  ILUKPreconditioner(Op& op, int fill_lev=3, int verbosity=2) {
    boost::timer::cpu_timer iluTimer;
    MatrixAsTriplet<field_type> A = op.template get<MatrixAsTriplet<field_type> >();
    int n = A.nrows(), nnz = A.nnz(), ierr;
    FILE *flog = stdout;

    csmat = (csptr)Malloc( sizeof(SparMat), "main" );
    ierr = COOcs(n, nnz,  &A.data[0], &A.cidx[0],  &A.ridx[0], csmat);
    if( ierr != 0 )
      {
        printf(" *** ILU error - COOcs code %d csmat=%p\n", ierr, csmat);
        throw ierr;
      }

    lu = (iluptr)Malloc(sizeof(ILUSpar), "main");
    ierr = ilukC(fill_lev, csmat, lu, flog);
    if( ierr != 0 )
      {
        printf(" *** PrecondType::ILUK error - ilut code %d lu=%p\n", ierr, lu);
        throw ierr;
      }

    xx   = (double *)Malloc(n*sizeof(double), "main");
    yy   = (double *)Malloc(n*sizeof(double), "main");

    if ( verbosity>=2 )
    {
	std::cout << "PrecondType::ILUK: n=" << n << ", nnz=" << nnz << ", fillfac="
	          << 1.0*nnz_ilu(lu)/(nnz+1.0) << ", fill_lev="
	          << fill_lev << ", time=" << (double)(iluTimer.elapsed().user)/1e9 << "s\n";
    }
  }
  ~ILUKPreconditioner()
    {
      cleanILU(lu);
      cleanCS(csmat);
      free(xx);
      free(yy);
    }

  virtual void pre (Domain&, Range&) {}
  virtual void post (Domain&) {}

  virtual void apply (Domain& x, Range const& y) {
    y.write(yy);
    lusolC(yy, xx, lu);
    x.read(xx);
  }

private:
  csptr csmat;
  iluptr lu;
  double *xx, *yy;
};
//---------------------------------------------------------------------

/**
 * \ingroup linalg
 *
 * PrecondType::ILUT Preconidoner from Saads ITSOL library
 */
template <class Op>
class ARMSPreconditioner: public Dune::Preconditioner<typename Op::Range, typename Op::Range>
{
  typedef typename Op::Range Range;
  typedef Range               Domain;
  typedef typename Op::field_type field_type;

public:
  static int const category = Dune::SolverCategory::sequential;

  ARMSPreconditioner(Op& op, int lfil=4, double tol=1.0e-2, int lev_reord=1, double tolind = 0.2, int verbosity=2) {
    boost::timer::cpu_timer iluTimer;
    MatrixAsTriplet<field_type> A = op.template get<MatrixAsTriplet<field_type> >();
    int n = A.nrows(), nnz = A.nnz(), ierr, diagscal = 1;

    int lfil_arr[7];
    double droptol[7], dropcoef[7];
    int ipar[18];
    io_t io;
    FILE *flog = stdout;

    csmat = (csptr)Malloc( sizeof(SparMat), "main" );
    ierr = COOcs(n, nnz,  &A.data[0], &A.cidx[0],  &A.ridx[0], csmat);
    if( ierr != 0 )
      {
        printf(" *** PrecondType::ARMS error - COOcs code %d csmat=%p\n", ierr, csmat);
        throw ierr;
      }

    memset(&io, 0, sizeof(io) );
    io.perm_type = 0;
    io.Bsize = 400;
    set_arms_pars(&io, diagscal, ipar, dropcoef, lfil_arr);
    for (int j=0; j<7; j++)
      {
         lfil_arr[j] = lfil*((int) nnz/n);
         droptol[j] =  tol*dropcoef[j];
      }
    ipar[1] = lev_reord;
    ArmsSt = (arms) Malloc(sizeof(armsMat),"main:ArmsSt");
    setup_arms(ArmsSt);

    ierr = arms2(csmat, ipar, droptol, lfil_arr, tolind, ArmsSt, flog);
    if( ierr != 0 )
      {
        printf(" *** PrecondType::ARMS error - arms2 code %d ArmsSt=%p\n", ierr, ArmsSt);
        throw ierr;
      }

    yy   = (double *)Malloc(n*sizeof(double), "main");

    if ( verbosity>=2 )
    {
	std::cout << "PrecondType::ARMS: n=" << n << ", nnz=" << nnz << ", dropTol=" << tol
	          << ", lev_reord=" << lev_reord
	          << ", lfil=" << lfil << ", time=" << (double)(iluTimer.elapsed().user)/1e9 << "s\n";
    }
  }
  ~ARMSPreconditioner()
    {
      cleanARMS(ArmsSt);
      cleanCS(csmat);
      free(yy);
    }

  virtual void pre (Domain&, Range&) {}
  virtual void post (Domain&) {}

  virtual void apply (Domain& x, Range const& y) {
    y.write(yy);
    armsol2(yy, ArmsSt);
    x.read(yy);
  }

private:
  csptr csmat;
  arms ArmsSt;
  double *yy;
};

}  // namespace Kaskade
#endif