//  Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_ENABLE_ASSERT_HANDLER
#define BOOST_MATH_MAX_SERIES_ITERATION_POLICY INT_MAX
// for consistent behaviour across compilers/platforms:
#define BOOST_MATH_PROMOTE_DOUBLE_POLICY false
// overflow to infinity is OK, we treat these as zero error as long as the sign is correct!
#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

#include <iostream>
#include <ctime>
#include <boost/multiprecision/mpfr.hpp>
#include <boost/math/special_functions/hypergeometric_1F1.hpp>
#include <boost/math/special_functions/hypergeometric_pFq.hpp>
#include <boost/math/special_functions/relative_difference.hpp>

#include <boost/random.hpp>
#include <set>
#include <fstream>
#include <boost/iostreams/tee.hpp>
#include <boost/iostreams/stream.hpp>

typedef double test_type;
using boost::multiprecision::mpfr_float;

namespace boost {
   //
   // We convert assertions into exceptions, so we can log them and continue:
   //
   void assertion_failed(char const * expr, char const *, char const * file, long line)
   {
      std::ostringstream oss;
      oss << file << ":" << line << " Assertion failed: " << expr;
      throw std::runtime_error(oss.str());
   }

}

void print_value(double x, std::ostream& os = std::cout)
{
   int e;
   double m = std::frexp(x, &e);
   m = std::ldexp(m, 54);
   e -= 54;
   boost::int64_t val = (boost::int64_t)m;
   BOOST_ASSERT(std::ldexp((double)val, e) == x);
   os << "std::ldexp((double)" << val << ", " << e << ")";
}

void print_row(double a, double b, double z, mpfr_float result, std::ostream& os = std::cout)
{
   os << "  {{ ";
   print_value(a, os);
   os << ", ";
   print_value(b, os);
   os << ", ";
   print_value(z, os);
   os << ", SC_(" << std::setprecision(45) << result << ") }}" << std::endl;
}

struct error_data
{
   error_data(double a, double b, double z, boost::intmax_t e)
      : a(a), b(b), z(z), error(e) {}
   double a, b, z;
   boost::intmax_t error;
   bool operator<(const error_data& other)const
   {
      return error < other.error;
   }
};

int main()
{
   try {
      test_type max_a, max_b, max_z, min_a, min_b, min_z;

      unsigned number_of_samples;

      std::ofstream log_stream, incalculable_stream, unevaluated_stream, bins_stream;
      std::string basename;

      std::cout << "Enter range for a: ";
      std::cin >> min_a >> max_a;
      std::cout << "Enter range for b: ";
      std::cin >> min_b >> max_b;
      std::cout << "Enter range for z: ";
      std::cin >> min_z >> max_z;
      std::cout << "Enter number of samples: ";
      std::cin >> number_of_samples;
      std::cout << "Enter basename for log files: ";
      std::cin >> basename;

      typedef boost::iostreams::tee_device<std::ostream, std::ostream> tee_sink;
      typedef boost::iostreams::stream<tee_sink> tee_stream;

      log_stream.open((basename + ".log").c_str());
      tee_stream tee_log(tee_sink(std::cout, log_stream));
      incalculable_stream.open((basename + "_incalculable.log").c_str());
      unevaluated_stream.open((basename + "_unevaluated.log").c_str());
      bins_stream.open((basename + "_bins.csv").c_str());
      tee_stream tee_bins(tee_sink(std::cout, bins_stream));

      boost::random::mt19937 gen(std::time(0));
      boost::random::uniform_real_distribution<test_type> a_dist(min_a, max_a);
      boost::random::uniform_real_distribution<test_type> b_dist(min_b, max_b);
      boost::random::uniform_real_distribution<test_type> z_dist(min_z, max_z);

      std::multiset<error_data> errors;
      std::map<std::pair<int, int>, int> bins;

      unsigned incalculable = 0;
      unsigned evaluation_errors = 0;
      test_type max_error = 0;

      do
      {
         test_type a = a_dist(gen);
         test_type b = b_dist(gen);
         test_type z = z_dist(gen);
         test_type found, expected;
         mpfr_float mp_expected;

         try {
            mp_expected = boost::math::hypergeometric_pFq_precision({ mpfr_float(a) }, { mpfr_float(b) }, mpfr_float(z), 25, 200.0);
            expected = (test_type)mp_expected;
         }
         catch (const std::exception&)
         {
            // Unable to compute reference value:
            ++incalculable;
            tee_log << "Unable to compute reference value in reasonable time: " << std::endl;
            print_row(a, b, z, mpfr_float(0), tee_log);
            incalculable_stream << std::setprecision(6) << std::scientific << a << "," << b << "," << z << "\n";
            continue;
         }
         try
         {
            found = boost::math::hypergeometric_1F1(a, b, z);
         }
         catch (const std::exception&)
         {
            ++evaluation_errors;
            --number_of_samples;
            log_stream << "Unexpected exception calculating value: " << std::endl;
            print_row(a, b, z, mp_expected, log_stream);
            unevaluated_stream << std::setprecision(6) << std::scientific << a << "," << b << "," << z << "\n";
            continue;
         }
         test_type err = boost::math::epsilon_difference(found, expected);
         if (err > max_error)
         {
            tee_log << "New maximum error is: " << err << std::endl;
            print_row(a, b, z, mp_expected, tee_log);
            max_error = err;
         }
         try {
            errors.insert(error_data(a, b, z, boost::math::lltrunc(err)));
         }
         catch (...)
         {
            errors.insert(error_data(a, b, z, INT_MAX));
         }
         --number_of_samples;
         if (number_of_samples % 500 == 0)
            std::cout << number_of_samples << " samples to go" << std::endl;
      } while (number_of_samples);

      tee_log << "Max error found was: " << max_error << std::endl;

      unsigned current_bin = 0;
      unsigned lim = 1;
      unsigned old_lim = 0;

      while (errors.size())
      {
         old_lim = lim;
         lim *= 2;
         //std::cout << "Enter upper limit for bin " << current_bin << ": ";
         //std::cin >> lim;
         auto p = errors.upper_bound(error_data(0, 0, 0, lim));
         int bin_count = std::distance(errors.begin(), p);
         if (bin_count)
         {
            std::ofstream os((basename + "_errors_" + std::to_string(current_bin + 1) + ".csv").c_str());
            os << "a,b,z,error\n";
            bins[std::make_pair(old_lim, lim)] = bin_count;
            for (auto pos = errors.begin(); pos != p; ++pos)
            {
               os << pos->a << "," << pos->b << "," << pos->z << "," << pos->error << "\n";
            }
            errors.erase(errors.begin(), p);
         }
         ++current_bin;
      }

      tee_bins << "Results:\n\n";
      tee_bins << "#bin,Range,2^N,Count\n";
      int hash = 0;
      for (auto p = bins.begin(); p != bins.end(); ++p, ++hash)
      {
         tee_bins << hash << "," << p->first.first << "-" << p->first.second << "," << hash+1 << "," << p->second << std::endl;
      }
      if (evaluation_errors)
      {
         tee_bins << ",Failed,," << evaluation_errors << std::endl;
      }
      if (incalculable)
      {
         tee_bins << ",Incalculable,," << incalculable << std::endl;
      }
   }
   catch (const std::exception& e)
   {
      std::cout << "Terminating with unhandled exception: " << e.what() << std::endl;
   }

   return 0;
}


// Copyright (c) 2012  Tel-Aviv University (Israel).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
// Author(s)     : Alex Tsui <alextsui05@gmail.com>

#include "ArrangementDemoWindow.h"
#include "NewTabDialog.h"
#include "OverlayDialog.h"
#include "ArrangementDemoPropertiesDialog.h"
#include "ArrangementDemoTab.h"
#include "Conic_reader.h"

#include "DeleteCurveMode.h"
#include "ArrangementGraphicsItem.h"

#include <boost/math/special_functions/fpclassify.hpp>

#include <QActionGroup>
#include <QFileDialog>
#include <QMessageBox>
#include <QColorDialog>

#include <CGAL/IO/Arr_with_history_iostream.h>
#include <CGAL/IO/Arr_text_formatter.h>
#include <CGAL/IO/Arr_with_history_text_formatter.h>

ArrangementDemoWindow::ArrangementDemoWindow(QWidget* parent) :
  CGAL::Qt::DemosMainWindow( parent ),
  lastTabIndex(static_cast<unsigned int>(-1)),
  ui( new Ui::ArrangementDemoWindow )
{
  this->setupUi( );

  // set up the demo window
  // ArrangementDemoTabBase* demoTab =
  this->makeTab( SEGMENT_TRAITS );
  this->setupStatusBar( );
  this->setupOptionsMenu( );
  this->addAboutDemo( ":/help/about.html" );
  this->addAboutCGAL( );

  // set up callbacks
  QObject::connect( this->modeGroup, SIGNAL( triggered( QAction* ) ),
                    this, SLOT( updateMode( QAction* ) ) );
  QObject::connect( this->envelopeGroup, SIGNAL( triggered( QAction* ) ),
                    this, SLOT( updateEnvelope( QAction* ) ) );
  QObject::connect( this->snapGroup, SIGNAL( triggered( QAction* ) ),
                    this, SLOT( updateSnapping( QAction* ) ) );

  QObject::connect( this->conicTypeGroup, SIGNAL( triggered( QAction* ) ),
                    this, SLOT( updateConicType( QAction* ) ) );
}

ArrangementDemoWindow::~ArrangementDemoWindow() {}

ArrangementDemoTabBase* ArrangementDemoWindow::makeTab( TraitsType tt )
{
  static int tabLabelCounter = 1;
  QString tabLabel;

  ArrangementDemoTabBase* demoTab;
  Seg_arr* seg_arr;
  Pol_arr* pol_arr;

#ifdef CGAL_USE_CORE
  Conic_arr* conic_arr;
#endif

  Lin_arr* lin_arr;
  Arc_arr* arc_arr;
  // Alg_seg_arr* alg_seg_arr;
  CGAL::Object arr;

  switch ( tt )
  {
   default:
   case SEGMENT_TRAITS:
    seg_arr = new Seg_arr;
    demoTab = new ArrangementDemoTab< Seg_arr >( seg_arr, 0 );
    arr = CGAL::make_object( seg_arr );
    tabLabel = QString( "%1 - Segment" ).arg( tabLabelCounter++ );
    break;
   case POLYLINE_TRAITS:
    pol_arr = new Pol_arr;
    demoTab = new ArrangementDemoTab< Pol_arr >( pol_arr, 0 );
    arr = CGAL::make_object( pol_arr );
    tabLabel = QString( "%1 - Polyline" ).arg( tabLabelCounter++ );
    break;

#ifdef CGAL_USE_CORE
   case CONIC_TRAITS:
    conic_arr = new Conic_arr;
    demoTab = new ArrangementDemoTab< Conic_arr >( conic_arr, 0 );
    arr = CGAL::make_object( conic_arr );
    tabLabel = QString( "%1 - Conic" ).arg( tabLabelCounter++ );
    break;
#endif

   case LINEAR_TRAITS:
    lin_arr = new Lin_arr;
    demoTab = new ArrangementDemoTab< Lin_arr >( lin_arr, 0 );
    arr = CGAL::make_object( lin_arr );
    tabLabel = QString( "%1 - Linear" ).arg( tabLabelCounter++ );
    break;
   case CIRCULAR_ARC_TRAITS:
    arc_arr = new Arc_arr;
    demoTab = new ArrangementDemoTab< Arc_arr >( arc_arr, 0 );
    arr = CGAL::make_object( arc_arr );
    tabLabel = QString( "%1 - Circular Arc" ).arg( tabLabelCounter++ );
    break;
   // case ALGEBRAIC_TRAITS:
   //  alg_seg_arr = new Alg_seg_arr;
   //  demoTab = new ArrangementDemoTab< Alg_seg_arr >( alg_seg_arr, 0 );
   //  arr = CGAL::make_object( alg_seg_arr );
   //  tabLabel = QString( "%1 - Algebraic" ).arg( tabLabelCounter++ );
   //  break;
  }

  this->arrangements.push_back( arr );
  this->tabs.push_back( demoTab );

  QGraphicsView* view = demoTab->getView( );
  this->addNavigation( view );
  this->ui->tabWidget->addTab( demoTab, tabLabel );
  this->lastTabIndex = this->ui->tabWidget->currentIndex( );
  this->ui->tabWidget->setCurrentWidget( demoTab );

  this->resetCallbackState( this->ui->tabWidget->currentIndex( ) );
  this->removeCallback( this->ui->tabWidget->currentIndex( ) );
  this->updateMode( this->modeGroup->checkedAction( ) );
  this->updateFillColorSwatch( );

  return demoTab;
}

ArrangementDemoTabBase* ArrangementDemoWindow::getTab( unsigned int tabIndex )
  const
{
  if (tabIndex > this->tabs.size()) return NULL;
  return this->tabs[tabIndex];
}

ArrangementDemoTabBase* ArrangementDemoWindow::getCurrentTab( ) const
{
  int currentIndex = this->ui->tabWidget->currentIndex( );
  if ( currentIndex == -1 )
    return NULL;

  ArrangementDemoTabBase* res = this->tabs[ currentIndex ];
  return res;
}

std::vector< QString > ArrangementDemoWindow::getTabLabels( ) const
{
  std::vector< QString > res;
  for ( int i = 0; i < this->ui->tabWidget->count( ); ++i )
  {
    res.push_back( this->ui->tabWidget->tabText( i ) );
  }
  return res;
}

std::vector< CGAL::Object > ArrangementDemoWindow::getArrangements( ) const
{
  std::vector< CGAL::Object > res;
  for ( unsigned int i = 0; i < this->arrangements.size( ); ++i )
  {
    res.push_back( this->arrangements[ i ] );
  }
  return res;
}

void ArrangementDemoWindow::setupUi( )
{
  this->ui->setupUi( this );

  this->modeGroup = new QActionGroup( this );
  this->modeGroup->addAction( this->ui->actionDrag );
  this->modeGroup->addAction( this->ui->actionInsert );
  this->modeGroup->addAction( this->ui->actionDelete );
  this->modeGroup->addAction( this->ui->actionPointLocation );
  this->modeGroup->addAction( this->ui->actionRayShootingUp );
  this->modeGroup->addAction( this->ui->actionRayShootingDown );
  this->modeGroup->addAction( this->ui->actionMerge );
  this->modeGroup->addAction( this->ui->actionSplit );
  this->modeGroup->addAction( this->ui->actionFill );
  this->activeModes.push_back( this->ui->actionInsert );

  this->envelopeGroup = new QActionGroup( this );
  this->envelopeGroup->addAction( this->ui->actionLowerEnvelope );
  this->envelopeGroup->addAction( this->ui->actionUpperEnvelope );
  this->envelopeGroup->setExclusive( false );

  this->snapGroup = new QActionGroup( this );
  this->snapGroup->addAction( this->ui->actionSnapMode );
  this->snapGroup->addAction( this->ui->actionGridSnapMode );
  this->snapGroup->setExclusive( false );
  this->ui->actionGridSnapMode->setEnabled( false );

  this->conicTypeGroup = new QActionGroup( this );
  this->conicTypeGroup->addAction( this->ui->actionConicSegment );
  this->conicTypeGroup->addAction( this->ui->actionConicCircle );
  this->conicTypeGroup->addAction( this->ui->actionConicEllipse );
  this->conicTypeGroup->addAction( this->ui->actionConicThreePoint );
  this->conicTypeGroup->addAction( this->ui->actionConicFivePoint );
  this->conicTypeGroup->addAction( this->ui->actionCurveRay );
  this->conicTypeGroup->addAction( this->ui->actionCurveLine );

  this->updateFillColorSwatch( );
}

void ArrangementDemoWindow::updateMode( QAction* newMode )
{
  // QWidget* widget = this->ui->tabWidget->currentWidget( );
  // ArrangementDemoTabBase* demoTab =
  //   static_cast< ArrangementDemoTabBase* >( widget );
  const unsigned int TabIndex = this->ui->tabWidget->currentIndex( );
  if (TabIndex == static_cast<unsigned int>(-1)) return;
  ArrangementDemoTabBase* activeTab = this->tabs[ TabIndex ];
  QGraphicsScene* activeScene = activeTab->getScene( );
  QGraphicsView* activeView = activeTab->getView( );

  this->resetCallbackState( TabIndex );
  this->removeCallback( TabIndex );

  // update the active mode
  this->activeModes.at( 0 ) = newMode;

  // hook up the new active mode
  if ( newMode == this->ui->actionInsert )
  {
    activeScene->installEventFilter( activeTab->getCurveInputCallback( ) );
  }
  else if ( newMode == this->ui->actionDrag )
  {
    activeView->setDragMode( QGraphicsView::ScrollHandDrag );
  }
  else if ( newMode == this->ui->actionDelete )
  {
    activeScene->installEventFilter( activeTab->getDeleteCurveCallback( ) );
  }
  else if ( newMode == this->ui->actionPointLocation )
  {
    activeScene->installEventFilter( activeTab->getPointLocationCallback( ) );
  }
  else if ( newMode == this->ui->actionRayShootingUp )
  {
    // -y is up for Qt, so we shoot down
    activeTab->getVerticalRayShootCallback( )->setShootingUp( true );
    activeScene->installEventFilter( activeTab->getVerticalRayShootCallback());
  }
  else if ( newMode == this->ui->actionRayShootingDown )
  {
    // the bottom of the viewport for Qt is +y, so we shoot up
    activeTab->getVerticalRayShootCallback( )->setShootingUp( false );
    activeScene->installEventFilter( activeTab->getVerticalRayShootCallback());
  }
  else if ( newMode == this->ui->actionMerge )
  {
    activeScene->installEventFilter( activeTab->getMergeEdgeCallback( ) );
  }
  else if ( newMode == this->ui->actionSplit )
  {
    activeScene->installEventFilter( activeTab->getSplitEdgeCallback( ) );
  }
  else if ( newMode == this->ui->actionFill )
  {
    activeScene->installEventFilter( activeTab->getFillFaceCallback( ) );
  }
  this->updateFillColorSwatch( );
}

void ArrangementDemoWindow::resetCallbackState( unsigned int tabIndex )
{
  if (tabIndex == static_cast<unsigned int>(-1)
      || tabIndex >= this->tabs.size( )) return;

  ArrangementDemoTabBase* activeTab = this->tabs[ tabIndex ];

  QAction* activeMode = this->activeModes.at( 0 );

  // unhook the old active mode
  if ( activeMode == this->ui->actionInsert )
  {  }
  else if ( activeMode == this->ui->actionDrag )
  {  }
  else if ( activeMode == this->ui->actionDelete )
  {
    activeTab->getDeleteCurveCallback( )->reset( );
  }
  else if ( activeMode == this->ui->actionPointLocation )
  {
    activeTab->getPointLocationCallback( )->reset( );
  }
  else if ( activeMode == this->ui->actionRayShootingUp )
  {
    activeTab->getVerticalRayShootCallback( )->reset( );
  }
  else if ( activeMode == this->ui->actionRayShootingDown )
  {
    activeTab->getVerticalRayShootCallback( )->reset( );
  }
  else if ( activeMode == this->ui->actionMerge )
  {
    activeTab->getMergeEdgeCallback( )->reset( );
  }
  else if ( activeMode == this->ui->actionSplit )
  {
    activeTab->getSplitEdgeCallback( )->reset( );
  }
  else if ( activeMode == this->ui->actionFill )
  {
    activeTab->getFillFaceCallback( )->reset( );
  }
}

void ArrangementDemoWindow::removeCallback( unsigned int tabIndex )
{
  if (tabIndex == static_cast<unsigned int>(-1)) return;

  ArrangementDemoTabBase* activeTab = this->tabs[ tabIndex ];
  QGraphicsScene* activeScene = activeTab->getScene( );
  QGraphicsView* activeView = activeTab->getView( );
#if 0
  QAction* activeMode = this->activeModes[ tabIndex ];
#endif

  activeScene->removeEventFilter( activeTab->getCurveInputCallback( ) );
  activeView->setDragMode( QGraphicsView::NoDrag );
  activeScene->removeEventFilter( activeTab->getDeleteCurveCallback( ) );
  activeScene->removeEventFilter( activeTab->getPointLocationCallback( ) );
  activeScene->removeEventFilter( activeTab->getVerticalRayShootCallback( ) );
  activeScene->removeEventFilter( activeTab->getMergeEdgeCallback( ) );
  activeScene->removeEventFilter( activeTab->getSplitEdgeCallback( ) );
  activeScene->removeEventFilter( activeTab->getVerticalRayShootCallback( ) );
  activeScene->removeEventFilter( activeTab->getFillFaceCallback( ) );

  // unhook the old active mode
#if 0
  if ( activeMode == this->ui->actionInsert )
  {
    activeScene->removeEventFilter( activeTab->getCurveInputCallback( ) );
  }
  else if ( activeMode == this->ui->actionDrag )
  {
    activeView->setDragMode( QGraphicsView::NoDrag );
  }
  else if ( activeMode == this->ui->actionDelete )
  {
    activeScene->removeEventFilter( activeTab->getDeleteCurveCallback( ) );
  }
  else if ( activeMode == this->ui->actionPointLocation )
  {
    activeScene->removeEventFilter( activeTab->getPointLocationCallback( ) );
  }
  else if ( activeMode == this->ui->actionRayShootingUp )
  {
    activeScene->removeEventFilter( activeTab->getVerticalRayShootCallback( ) );
  }
  else if ( activeMode == this->ui->actionRayShootingDown )
  {
    activeScene->removeEventFilter( activeTab->getVerticalRayShootCallback( ) );
  }
  else if ( activeMode == this->ui->actionMerge )
  {
    activeScene->removeEventFilter( activeTab->getMergeEdgeCallback( ) );
  }
  else if ( activeMode == this->ui->actionSplit )
  {
    activeScene->removeEventFilter( activeTab->getSplitEdgeCallback( ) );
  }
#endif
}

void ArrangementDemoWindow::updateFillColorSwatch( )
{
  unsigned int currentTabIndex = this->ui->tabWidget->currentIndex( );
  if (currentTabIndex == static_cast<unsigned int>(-1)) return;
  ArrangementDemoTabBase* currentTab = this->tabs[ currentTabIndex ];
  FillFaceCallbackBase* fillFaceCallback = currentTab->getFillFaceCallback( );
  QColor fillColor = fillFaceCallback->getColor( );
  if ( !fillColor.isValid( ) )
  {
    fillColor = ::Qt::black;
  }

  QPixmap fillColorPixmap( 16, 16 );
  fillColorPixmap.fill( fillColor );
  QIcon fillColorIcon( fillColorPixmap );
  this->ui->actionFillColor->setIcon( fillColorIcon );
}

void ArrangementDemoWindow::openArrFile( QString filename )
{
  int index = this->ui->tabWidget->currentIndex( );
  if ( index == -1 )
  {
    QMessageBox::information( this, "Oops", "Create a new tab first" );
    return;
  }
  if ( filename.isNull( ) )
  {
    return;
  }

  std::ifstream ifs( filename.toStdString( ).c_str( ) );
  CGAL::Object arr = this->arrangements[ index ];
  Seg_arr* seg;
  Pol_arr* pol;

#ifdef CGAL_USE_CORE
  Conic_arr* conic;
#endif

  // Alg_seg_arr* alg;
  if ( CGAL::assign( seg, arr ) )
  {
    typedef CGAL::Arr_text_formatter<Seg_arr>           Seg_text_formatter;
    typedef CGAL::Arr_with_history_text_formatter<Seg_text_formatter>
      ArrFormatter;
    typedef ArrangementDemoTab<Seg_arr>                 TabType;

    ArrFormatter arrFormatter;
    CGAL::read( *seg, ifs, arrFormatter );
    this->arrangements[ index ] = CGAL::make_object( seg );
    TabType* tab = static_cast< TabType* >( this->tabs[ index ] );
    tab->setArrangement( seg );
  }
  else if ( CGAL::assign( pol, arr ) )
  {
    typedef CGAL::Arr_text_formatter< Pol_arr >         Pol_text_formatter;
    typedef CGAL::Arr_with_history_text_formatter<Pol_text_formatter>
      ArrFormatter;
    typedef ArrangementDemoTab< Pol_arr >               TabType;

    ArrFormatter arrFormatter;
    CGAL::read( *pol, ifs, arrFormatter );
    this->arrangements[ index ] = CGAL::make_object( pol );
    TabType* tab = static_cast< TabType* >( this->tabs[ index ] );
    tab->setArrangement( pol );
  }

#ifdef CGAL_USE_CORE
  else if (CGAL::assign(conic, arr)) {
#if 0
    typedef CGAL::Arr_text_formatter<Conic_arr>         Conic_text_formatter;
    typedef CGAL::Arr_with_history_text_formatter<Conic_text_formatter>
      ArrFormatter;
    ArrFormatter                                        arrFormatter;
    CGAL::read( *conic, ifs, arrFormatter );
    this->arrangements[ index ] = CGAL::make_object( conic );
    tab->setArrangement( conic );
#endif
    typedef ArrangementDemoTab< Conic_arr > TabType;
    Conic_reader< Conic_arr::Geometry_traits_2 > conicReader;
    std::vector< Conic_arr::Curve_2 > curve_list;
    CGAL::Bbox_2 bbox;
    conicReader.read_data( filename.toStdString( ).c_str( ),
                           std::back_inserter( curve_list ), bbox );
    this->arrangements[ index ] = CGAL::make_object( conic );
    TabType* tab = static_cast< TabType* >( this->tabs[ index ] );

    CGAL::insert( *conic, curve_list.begin(), curve_list.end() );
    tab->setArrangement( conic );
    //QMessageBox::information( this, "Oops",
    //  "Reading conic arrangement not supported" );
  }
#endif

  // else if ( CGAL::assign( alg, arr ) )
  // {
  //   typedef CGAL::Arr_text_formatter< Alg_seg_arr >     Seg_text_formatter;
  //   typedef CGAL::Arr_with_history_text_formatter<Seg_text_formatter>
  //     ArrFormatter;
  //   typedef ArrangementDemoTab< Alg_seg_arr >           TabType;

  //   ArrFormatter arrFormatter;
  //   CGAL::read( *alg, ifs, arrFormatter );
  //   this->arrangements[ index ] = CGAL::make_object( alg );
  //   TabType* tab = static_cast< TabType* >( this->tabs[ index ] );
  //   tab->setArrangement( alg );
  // }
  ifs.close( );
}

void ArrangementDemoWindow::openDatFile( QString filename )
{
  int index = this->ui->tabWidget->currentIndex( );
  if ( index == -1 )
  {
    QMessageBox::information( this, "Oops", "Create a new tab first" );
    return;
  }
  if ( filename.isNull( ) )
  {
    return;
  }

  std::ifstream inputFile( filename.toStdString( ).c_str( ) );
  CGAL::Object arr = this->arrangements[ index ];
  Seg_arr* seg;
  Pol_arr* pol;

#ifdef CGAL_USE_CORE
  Conic_arr* conic;
#endif

  // Alg_seg_arr* alg;

  // Creates an ofstream object named inputFile
  if (! inputFile.is_open() ) // Always test file open
  {
    std::cerr << "Error opening input file" << std::endl;
    return;
  }

  Pol_traits traits;
  Pol_traits::Construct_curve_2 poly_const =
    traits.construct_curve_2_object();

  if ( CGAL::assign( pol, arr ) )
  {
    pol->clear( );

    std::vector<Arr_pol_point_2> points;

    unsigned int num_polylines;
    inputFile >> num_polylines;
    std::list<Arr_pol_2> pol_list;

    unsigned int i;
    for (i = 0; i < num_polylines; i++)
    {
      unsigned int num_segments;
      inputFile >> num_segments;
      points.clear();
      unsigned int j;
      for (j = 0; j < num_segments; j++)
      {
        int ix, iy;
        inputFile >> ix >> iy;
        points.push_back (Arr_pol_point_2(NT(ix),NT(iy)));
      }

      Arr_pol_2 curve = poly_const(points.begin(), points.end());
      pol_list.push_back(curve);
    }
    CGAL::insert(*pol, pol_list.begin(), pol_list.end());

    typedef ArrangementDemoTab< Pol_arr > TabType;
    TabType* tab = static_cast< TabType* >( this->tabs[ index ] );
    tab->setArrangement( pol );
  }
  else if ( CGAL::assign( seg, arr ) )
  {
    seg->clear( );

    int count;
    inputFile >> count;
    int i;
    std::list<Arr_seg_2> seg_list;
    for (i = 0; i < count; i++)
    {
      NT x0, y0, x1, y1;
      inputFile >> x0 >> y0 >> x1 >> y1;

      Arr_seg_point_2 p1(x0, y0);
      Arr_seg_point_2 p2(x1, y1);

      Arr_seg_2 curve(p1, p2);

      seg_list.push_back(curve);
    }

    CGAL::insert(*(seg), seg_list.begin(), seg_list.end());

    typedef ArrangementDemoTab< Seg_arr > TabType;
    TabType* tab = static_cast< TabType* >( this->tabs[ index ] );
    tab->setArrangement( seg );
  }

#ifdef CGAL_USE_CORE
  else if (CGAL::assign( conic, arr)) {
    conic->clear( );
    Conic_reader< Conic_arr::Geometry_traits_2 > reader;
    std::list<Arr_conic_2> curve_list;
    CGAL::Bbox_2 bbox;
    reader.read_data( filename.toStdString().c_str(),
                      std::back_inserter(curve_list), bbox );
    CGAL::insert (*conic, curve_list.begin(), curve_list.end());

    typedef ArrangementDemoTab< Conic_arr > TabType;
    TabType* tab = static_cast< TabType* >( this->tabs[ index ] );
    tab->setArrangement( conic );
  }
#endif

  inputFile.close();
}

void ArrangementDemoWindow::updateEnvelope( QAction* newMode )
{
  if ( this->ui->tabWidget->currentIndex( ) == -1 ) return;
  ArrangementDemoTabBase* activeTab =
    this->tabs[ this->ui->tabWidget->currentIndex( ) ];
  // QGraphicsScene* activeScene = activeTab->getScene( );
  // QGraphicsView* activeView = activeTab->getView( );

  bool show = newMode->isChecked( );
  if ( newMode == this->ui->actionLowerEnvelope )
  {
    activeTab->getEnvelopeCallback( )->showLowerEnvelope( show );
  }
  else if ( newMode == this->ui->actionUpperEnvelope )
  {
    activeTab->getEnvelopeCallback( )->showUpperEnvelope( show );
  }
}

void ArrangementDemoWindow::updateSnapping( QAction* newMode )
{
  ArrangementDemoTabBase* activeTab =
    this->tabs[ this->ui->tabWidget->currentIndex( ) ];
  QGraphicsScene* activeScene = activeTab->getScene( );
  ArrangementDemoGraphicsView* activeView = activeTab->getView( );

  bool enabled = newMode->isChecked( );
  if ( newMode == this->ui->actionSnapMode )
  {
    activeTab->getCurveInputCallback( )->setSnappingEnabled( enabled );
    activeTab->getSplitEdgeCallback( )->setSnappingEnabled( enabled );
    if ( ! enabled )
    {
      this->ui->actionGridSnapMode->setChecked( false );
      this->ui->actionGridSnapMode->setEnabled( false );
      activeTab->getCurveInputCallback( )->setSnapToGridEnabled( false );
      activeTab->getSplitEdgeCallback( )->setSnapToGridEnabled( false );
    }
    else
    {
      this->ui->actionGridSnapMode->setEnabled( true );
    }
  }
  else if ( newMode == this->ui->actionGridSnapMode )
  {
    activeTab->getCurveInputCallback( )->setSnapToGridEnabled( enabled );
    activeTab->getSplitEdgeCallback( )->setSnapToGridEnabled( enabled );
    activeView->setShowGrid( enabled );
  }
  activeScene->update( );
}

void ArrangementDemoWindow::updateConicType( QAction* newType )
{
  ArrangementDemoTabBase* activeTab =
    this->tabs[ this->ui->tabWidget->currentIndex( ) ];
  // QGraphicsScene* activeScene = activeTab->getScene( );
  // ArrangementDemoGraphicsView* activeView = activeTab->getView( );

#ifdef CGAL_USE_CORE
  Conic_arr* conic_arr;
  bool isConicArr =
    CGAL::assign( conic_arr,
                  this->arrangements[ this->ui->tabWidget->currentIndex( ) ] );
#endif

  Lin_arr* lin_arr;
  bool isLinearArr =
    CGAL::assign( lin_arr,
                  this->arrangements[ this->ui->tabWidget->currentIndex( ) ] );

  if ( isLinearArr )
  {
    typedef Lin_arr::Geometry_traits_2         Line_geom_traits;
    typedef CGAL::Qt::GraphicsViewCurveInput<Line_geom_traits>
      LinearCurveInputCallback;
    LinearCurveInputCallback* curveInputCallback =
      ( LinearCurveInputCallback* ) activeTab->getCurveInputCallback( );
    if ( newType == this->ui->actionConicSegment )
    {
      curveInputCallback->setCurveType( LinearCurveInputCallback::SEGMENT );
    }
    else if ( newType == this->ui->actionCurveRay )
  {
      curveInputCallback->setCurveType( LinearCurveInputCallback::RAY );
    }
    else if ( newType == this->ui->actionCurveLine )
    {
      curveInputCallback->setCurveType( LinearCurveInputCallback::LINE );
    }
    //std::cout << "do nothing" << std::endl;
  }

#ifdef CGAL_USE_CORE
  else if (isConicArr) {
    // std::cout << "do something conic arr related" << std::endl;
    typedef Conic_arr::Geometry_traits_2       Conic_geom_traits;
    typedef CGAL::Qt::GraphicsViewCurveInput<Conic_geom_traits>
      ConicCurveInputCallback;
    ConicCurveInputCallback* curveInputCallback =
      ( ConicCurveInputCallback* ) activeTab->getCurveInputCallback( );
    if ( newType == this->ui->actionConicSegment )
    {
      curveInputCallback->setConicType(ConicCurveInputCallback::CONIC_SEGMENT);
    }
    else if ( newType == this->ui->actionConicCircle )
    {
      curveInputCallback->setConicType(ConicCurveInputCallback::CONIC_CIRCLE);
    }
    else if ( newType == this->ui->actionConicEllipse )
    {
      curveInputCallback->setConicType(ConicCurveInputCallback::CONIC_ELLIPSE);
    }
    else if ( newType == this->ui->actionConicThreePoint )
    {
      curveInputCallback->setConicType( ConicCurveInputCallback::
                                        CONIC_THREE_POINT );
    }
    else if ( newType == this->ui->actionConicFivePoint )
    {
      curveInputCallback->setConicType( ConicCurveInputCallback::
                                        CONIC_FIVE_POINT );
    }
  }
#endif
}

void ArrangementDemoWindow::on_actionSaveAs_triggered( )
{
  int index = this->ui->tabWidget->currentIndex( );
  if ( index == -1 )
    return;
  QString filename =
    QFileDialog::getSaveFileName( this, tr( "Save file" ),
                                  "", "Arrangement (*.arr)" );
  if ( filename.isNull( ) )
    return;

  std::ofstream ofs( filename.toStdString( ).c_str( ) );
  CGAL::Object arr = this->arrangements[ index ];
  Seg_arr* seg;
  Pol_arr* pol;

#ifdef CGAL_USE_CORE
  Conic_arr* conic;
#endif

  if ( CGAL::assign( seg, arr ) )
  {
    typedef CGAL::Arr_text_formatter<Seg_arr>           Seg_text_formatter;
    typedef CGAL::Arr_with_history_text_formatter<Seg_text_formatter>
      ArrFormatter;
    ArrFormatter                                        arrFormatter;
    CGAL::write( *seg, ofs, arrFormatter );
  }
  else if ( CGAL::assign( pol, arr ) )
  {
    typedef CGAL::Arr_text_formatter<Pol_arr>           Pol_text_formatter;
    typedef CGAL::Arr_with_history_text_formatter<Pol_text_formatter>
      ArrFormatter;
    ArrFormatter                                        arrFormatter;
    CGAL::write( *pol, ofs, arrFormatter );
  }

#ifdef CGAL_USE_CORE
  else if (CGAL::assign(conic, arr)) {
#if 0
    typedef CGAL::Arr_text_formatter<Conic_arr>         Conic_text_formatter;
    typedef CGAL::Arr_with_history_text_formatter<Conic_text_formatter>
      ArrFormatter;
    ArrFormatter                                        arrFormatter;
    CGAL::write( *conic, ofs, arrFormatter );
#endif
    ofs << conic->number_of_curves( ) << std::endl;
    for ( Conic_arr::Curve_iterator it = conic->curves_begin( );
          it != conic->curves_end( ); ++it )
    {
      if ( it->is_full_conic( ) )
      {
        ofs << "F ";
        ofs << it->r( ) << " ";
        ofs << it->s( ) << " ";
        ofs << it->t( ) << " ";
        ofs << it->u( ) << " ";
        ofs << it->v( ) << " ";
        ofs << it->w( ) << " ";
        ofs << std::endl;
      }
      else if ( it->orientation( ) == CGAL::COLLINEAR )
      {
        ofs << "S ";
        ofs << it->source( ) << " ";
        ofs << it->target( ) << " ";
        ofs << std::endl;
      }
      else
      {
        ofs << "A ";
        ofs << it->r( ) << " ";
        ofs << it->s( ) << " ";
        ofs << it->t( ) << " ";
        ofs << it->u( ) << " ";
        ofs << it->v( ) << " ";
        ofs << it->w( ) << " ";
        if ( it->orientation( ) == CGAL::COUNTERCLOCKWISE )
          ofs << "1 ";
        else if ( it->orientation( ) == CGAL::CLOCKWISE )
          ofs << "-1 ";
        else
          ofs << "0 ";
        ofs << it->source( ) << " ";
        ofs << it->target( ) << " ";
        ofs << std::endl;
      }
    }
  }
#endif

  ofs.close( );
}

void ArrangementDemoWindow::on_actionOpen_triggered( )
{
  int index = this->ui->tabWidget->currentIndex( );
  if ( index == -1 )
  {
    QMessageBox::information( this, "Oops", "Create a new tab first" );
    return;
  }
  QString filename =
    QFileDialog::getOpenFileName( this, tr( "Open file" ),
                                  "", "Arrangement files (*.arr *.dat);;All files (*.*)" );
  if ( filename.isNull( ) )
    return;

  if ( filename.endsWith( ".arr" ) )
  {
    this->openArrFile( filename );
  }
  else
  {
    this->openDatFile( filename );
  }

  ArrangementDemoTabBase* currentTab = this->tabs[ index ];
  CGAL::Qt::ArrangementGraphicsItemBase* agi =
    currentTab->getArrangementGraphicsItem( );
  QRectF bb = agi->boundingRect( );
  QGraphicsView* view = currentTab->getView( );
  // std::cout << bb.left( ) << " " << bb.bottom( ) << ", " << bb.right( )
  //           << " " << bb.top( ) << std::endl;
  if ( boost::math::isinf(bb.left( )) ||
       boost::math::isinf(bb.right( )) ||
       boost::math::isinf(bb.top( )) ||
       boost::math::isinf(bb.bottom( )) )
  {
    // std::cout << "unbounded; using default bb" << std::endl;
    bb = QRectF( -100, -100, 200, 200 );
    view->setSceneRect( bb );
  }
  else
  {
    view->fitInView( bb, ::Qt::KeepAspectRatio );
    view->setSceneRect( bb );
  }
#if 0
  view->centerOn( bb.center( ) );
#endif
}

void ArrangementDemoWindow::on_actionQuit_triggered( )
{
  qApp->exit( );
}

void ArrangementDemoWindow::on_actionNewTab_triggered( )
{
  NewTabDialog* newTabDialog = new NewTabDialog;
  if ( newTabDialog->exec( ) == QDialog::Accepted )
  {
    int id = newTabDialog->checkedId( );
    if ( id == SEGMENT_TRAITS )
    {
      this->makeTab( SEGMENT_TRAITS );
    }
    else if ( id == POLYLINE_TRAITS )
    {
      this->makeTab( POLYLINE_TRAITS );
    }
    else if ( id == CONIC_TRAITS )
    {
      this->makeTab( CONIC_TRAITS );
    }
    else if ( id == LINEAR_TRAITS )
    {
      this->makeTab( LINEAR_TRAITS );
    }
    else if ( id == CIRCULAR_ARC_TRAITS )
    {
      this->makeTab( CIRCULAR_ARC_TRAITS );
    }
    // else if ( id == ALGEBRAIC_TRAITS )
    // {
    //   this->makeTab( ALGEBRAIC_TRAITS );
    // }
    else
    {
      std::cout << "Sorry, this trait is not yet supported" << std::endl;
    }
  }
  delete newTabDialog;
}

void ArrangementDemoWindow::on_tabWidget_currentChanged( )
{
  // std::cout << "Tab changed" << std::endl;
  // disable the callback for the previously active tab
  this->resetCallbackState( this->lastTabIndex );
  this->removeCallback( this->lastTabIndex );
  this->lastTabIndex = this->ui->tabWidget->currentIndex( );

  this->updateMode( this->modeGroup->checkedAction( ) );

  CGAL::Object arr;
  if ( this->ui->tabWidget->currentIndex( ) != -1 )
    arr = this->arrangements[ this->ui->tabWidget->currentIndex( ) ];

  // Seg_arr* seg;
  // Pol_arr* pol;
  Lin_arr* lin;

#ifdef CGAL_USE_CORE
  Conic_arr* conic;
#endif

  if ( CGAL::assign( lin, arr ) )
  {
    this->ui->actionConicSegment->setChecked( true );

    this->ui->actionCurveRay->setVisible( true );
    this->ui->actionCurveLine->setVisible( true );

    this->ui->actionConicCircle->setVisible( false );
    this->ui->actionConicEllipse->setVisible( false );
    this->ui->actionConicThreePoint->setVisible( false );
    this->ui->actionConicFivePoint->setVisible( false );

    this->conicTypeGroup->setEnabled( true );
  }

#ifdef CGAL_USE_CORE
  else if (CGAL::assign( conic, arr)) {
    this->ui->actionConicSegment->setChecked( true );

    this->ui->actionCurveRay->setVisible( false );
    this->ui->actionCurveLine->setVisible( false );

    this->ui->actionConicCircle->setVisible( true );
    this->ui->actionConicEllipse->setVisible( true );
    this->ui->actionConicThreePoint->setVisible( true );
    this->ui->actionConicFivePoint->setVisible( true );

    this->conicTypeGroup->setEnabled( true );
  }
#endif

  else { // segment or polyline
    this->ui->actionConicSegment->setChecked( true );

    this->ui->actionCurveRay->setVisible( false );
    this->ui->actionCurveLine->setVisible( false );

    this->ui->actionConicCircle->setVisible( false );
    this->ui->actionConicEllipse->setVisible( false );
    this->ui->actionConicThreePoint->setVisible( false );
    this->ui->actionConicFivePoint->setVisible( false );

    this->conicTypeGroup->setEnabled( true );
  }
}

void ArrangementDemoWindow::on_actionOverlay_triggered( )
{
  OverlayDialog* overlayDialog = new OverlayDialog( this );
  if ( overlayDialog->exec( ) == QDialog::Accepted )
  {
    std::vector< CGAL::Object > arrs = overlayDialog->selectedArrangements( );
    if ( arrs.size( ) == 2 )
    {
      Seg_arr* seg_arr;
      Seg_arr* seg_arr2;
      Pol_arr* pol_arr;
      Pol_arr* pol_arr2;

#ifdef CGAL_USE_CORE
      Conic_arr* conic_arr;
      Conic_arr* conic_arr2;
#endif

      Lin_arr* lin_arr;
      Lin_arr* lin_arr2;
      Arc_arr* arc_arr;
      Arc_arr* arc_arr2;
      // Alg_seg_arr* alg_arr;
      // Alg_seg_arr* alg_arr2;
      if ( CGAL::assign( seg_arr, arrs[ 0 ] ) &&
           CGAL::assign( seg_arr2, arrs[ 1 ] ) )
      {
        this->makeOverlayTab( seg_arr, seg_arr2 );
      }
      if ( CGAL::assign( pol_arr, arrs[ 0 ] ) &&
           CGAL::assign( pol_arr2, arrs[ 1 ] ) )
      {
        this->makeOverlayTab( pol_arr, pol_arr2 );
      }

#ifdef CGAL_USE_CORE
      if (CGAL::assign(conic_arr, arrs[0]) && CGAL::assign(conic_arr2, arrs[1]))
      {
        this->makeOverlayTab( conic_arr, conic_arr2 );
      }
#endif

      if ( CGAL::assign( lin_arr, arrs[ 0 ] ) &&
           CGAL::assign( lin_arr2, arrs[ 1 ] ) )
      {
        this->makeOverlayTab( lin_arr, lin_arr2 );
      }
      if ( CGAL::assign( arc_arr, arrs[ 0 ] ) &&
           CGAL::assign( arc_arr2, arrs[ 1 ] ) )
      {
        this->makeOverlayTab( arc_arr, arc_arr2 );
      }
      // if ( CGAL::assign( alg_arr, arrs[ 0 ] ) &&
      //      CGAL::assign( alg_arr2, arrs[ 1 ] ) )
      // {
      //   this->makeOverlayTab( alg_arr, alg_arr2 );
      // }

#if 0
      if ( CGAL::assign( conic_arr, arrs[ 0 ] ) ||
           CGAL::assign( conic_arr, arrs[ 1 ] ) )
      {
        this->makeOverlayTab< Conic_arr >( arrs[ 0 ], arrs[ 1 ] );
      }
      else if ( CGAL::assign( pol_arr, arrs[ 0 ] ) ||
                CGAL::assign( pol_arr, arrs[ 1 ] ) )
      {
        this->makeOverlayTab< Pol_arr >( arrs[ 0 ], arrs[ 1 ] );
      }
      else
      {
        this->makeOverlayTab< Seg_arr >( arrs[ 0 ], arrs[ 1 ] );
      }
#endif
    }
  }
  delete overlayDialog;
}

void ArrangementDemoWindow::on_actionCloseTab_triggered( )
{
  unsigned int currentTabIndex = this->ui->tabWidget->currentIndex( );
  if (! this->ui->tabWidget->count() ||
      (currentTabIndex == static_cast<unsigned int>(-1)))
    return;

  // delete the tab
  this->ui->tabWidget->removeTab( currentTabIndex );
  this->tabs.erase( this->tabs.begin( ) + currentTabIndex );

  // delete the arrangement
  this->arrangements.erase( this->arrangements.begin( ) + currentTabIndex );
}

void ArrangementDemoWindow::on_actionPrintConicCurves_triggered( )
{
  // int currentTabIndex = this->ui->tabWidget->currentIndex( );
  // Conic_arr* arr;
  // if (currentTabIndex == static_cast<unsigned int>(-1)) return;
  // CGAL::Object o = this->arrangements[ currentTabIndex ];
  // if ( ! CGAL::assign( arr, o ) )
  //     return;
  // typedef typename Conic_arr::Curve_iterator Curve_iterator;
  // // std::cout << arr->number_of_curves( ) << std::endl;
  // Curve_iterator it;
  // for (it = arr->curves_begin(); it != arr->curves_end(); ++it) {
  //     std::cout << *it << std::endl;
  //     if ( it->is_full_conic( ) )
  //     {
  //         std::cout << "F ";
  //         std::cout << it->r( ) << " ";
  //         std::cout << it->s( ) << " ";
  //         std::cout << it->t( ) << " ";
  //         std::cout << it->u( ) << " ";
  //         std::cout << it->v( ) << " ";
  //         std::cout << it->w( ) << " ";
  //         std::cout << std::endl;
  //     }
  //     else if ( it->orientation( ) == CGAL::COLLINEAR )
  //     {
  //         std::cout << "S ";
  //         std::cout << it->source( ) << " ";
  //         std::cout << it->target( ) << " ";
  //         std::cout << std::endl;
  //     }
  //     else
  //     {
  //         std::cout << "A ";
  //         std::cout << it->r( ) << " ";
  //         std::cout << it->s( ) << " ";
  //         std::cout << it->t( ) << " ";
  //         std::cout << it->u( ) << " ";
  //         std::cout << it->v( ) << " ";
  //         std::cout << it->w( ) << " ";
  //         if ( it->orientation( ) == CGAL::COUNTERCLOCKWISE )
  //             std::cout << "1 ";
  //         else if ( it->orientation( ) == CGAL::CLOCKWISE )
  //             std::cout << "-1 ";
  //         else
  //             std::cout << "0 ";
  //         std::cout << it->source( ) << " ";
  //         std::cout << it->target( ) << " ";
  //         std::cout << std::endl;
  //     }
  // }
}

void ArrangementDemoWindow::on_actionZoomIn_triggered( )
{
  unsigned int currentTabIndex = this->ui->tabWidget->currentIndex( );
  if (currentTabIndex == static_cast<unsigned int>(-1)) return;
  ArrangementDemoTabBase* currentTab = this->tabs[ currentTabIndex ];
  QGraphicsView* view = currentTab->getView( );
  view->scale( 2.0, 2.0 );
}

void ArrangementDemoWindow::on_actionZoomOut_triggered( )
{
  unsigned int currentTabIndex = this->ui->tabWidget->currentIndex( );
  if (currentTabIndex == static_cast<unsigned int>(-1)) return;
  ArrangementDemoTabBase* currentTab = this->tabs[ currentTabIndex ];
  QGraphicsView* view = currentTab->getView( );
  view->scale( 0.5, 0.5 );
}

void ArrangementDemoWindow::on_actionPreferences_triggered( )
{
  unsigned int currentTabIndex = this->ui->tabWidget->currentIndex( );
  if (currentTabIndex == static_cast<unsigned int>(-1)) return;
  ArrangementDemoTabBase* currentTab = this->tabs[ currentTabIndex ];
  CGAL::Qt::ArrangementGraphicsItemBase* agi =
    currentTab->getArrangementGraphicsItem( );
  ArrangementDemoGraphicsView* view = currentTab->getView( );
  EnvelopeCallbackBase* envelopeCallback = currentTab->getEnvelopeCallback( );
  VerticalRayShootCallbackBase* verticalRayShootCallback =
    currentTab->getVerticalRayShootCallback( );
  SplitEdgeCallbackBase* splitEdgeCallback = currentTab->getSplitEdgeCallback( );

#if 0
  QPen vertexPen = agi->getVerticesPen( );
  QPen edgePen = agi->getEdgesPen( );
  QBrush vertexPenBrush = vertexPen.brush( );
  QColor vertexColor = vertexPenBrush.color( );
  QBrush edgePenBrush = edgePen.brush( );
  QColor edgeColor = edgePenBrush.color( );
#endif

  ArrangementDemoPropertiesDialog* dialog =
    new ArrangementDemoPropertiesDialog( this );
  if ( dialog->exec( ) == QDialog::Accepted )
  {
    typedef ArrangementDemoPropertiesDialog Dialog;

    QColor edgeColor =  dialog->property(Dialog::EDGE_COLOR_KEY).value<QColor>();

    unsigned int edgeWidth = dialog->property(Dialog::EDGE_WIDTH_KEY).value<unsigned int>();

    QColor vertexColor = dialog->property(Dialog::VERTEX_COLOR_KEY).value<QColor>();

    unsigned int vertexRadius = dialog->property(Dialog::VERTEX_RADIUS_KEY).value<unsigned int>();

    QColor envelopeEdgeColor = dialog->property(Dialog::ENVELOPE_EDGE_COLOR_KEY).value<QColor>();

    unsigned int envelopeEdgeWidth = dialog->property(Dialog::ENVELOPE_EDGE_WIDTH_KEY).value<unsigned int>();

    QColor envelopeVertexColor = dialog->property(Dialog::ENVELOPE_VERTEX_COLOR_KEY).value<QColor>();

    unsigned int envelopeVertexRadius = dialog->property(Dialog::ENVELOPE_VERTEX_RADIUS_KEY).value<unsigned int>();

    QColor verticalRayEdgeColor = dialog->property(Dialog::VERTICAL_RAY_EDGE_COLOR_KEY).value<QColor>();

    unsigned int verticalRayEdgeWidth = dialog->property(Dialog::VERTICAL_RAY_EDGE_WIDTH_KEY).value<unsigned int>();

    DeleteCurveMode mode = dialog->property(Dialog::DELETE_CURVE_MODE_KEY).value<DeleteCurveMode>();

    unsigned int gridSize = dialog->property(Dialog::GRID_SIZE_KEY).value<unsigned int>();

    QColor gridColor = dialog->property(Dialog::GRID_COLOR_KEY).value<QColor>();
    //end new for Qt5 version !

    QPen edgesPen(QBrush(edgeColor), edgeWidth);
    QPen verticesPen(QBrush(vertexColor), vertexRadius);
    agi->setEdgesPen( edgesPen );
    agi->setVerticesPen( verticesPen );
    agi->modelChanged( );
    view->setGridSize( gridSize );
    view->setGridColor( gridColor );
    envelopeCallback->setEnvelopeEdgeColor( envelopeEdgeColor );
    envelopeCallback->setEnvelopeEdgeWidth( envelopeEdgeWidth );
    envelopeCallback->setEnvelopeVertexColor( envelopeVertexColor );
    envelopeCallback->setEnvelopeVertexRadius( envelopeVertexRadius );
    verticalRayShootCallback->setEdgeColor( verticalRayEdgeColor );
    verticalRayShootCallback->setEdgeWidth( verticalRayEdgeWidth );
    splitEdgeCallback->setColor( edgeColor );

#if 0
    std::cout << edgeColor.name( ).toStdString( ) << std::endl;
    std::cout << edgeWidth << std::endl;
    std::cout << vertexColor.name( ).toStdString( ) << std::endl;
    std::cout << vertexRadius << std::endl;
    std::cout << DeleteCurveMode::ToString( mode ).toStdString( ) << std::endl;
#endif

  }
}

void ArrangementDemoWindow::on_actionFillColor_triggered( )
{
  unsigned int currentTabIndex = this->ui->tabWidget->currentIndex( );
  if (currentTabIndex == static_cast<unsigned int>(-1)) return;
  ArrangementDemoTabBase* currentTab = this->tabs[ currentTabIndex ];
  FillFaceCallbackBase* fillFaceCallback = currentTab->getFillFaceCallback( );
  QColor fillColor = fillFaceCallback->getColor( );

  QColor selectedColor = QColorDialog::getColor( fillColor );
  if ( selectedColor.isValid( ) )
  {
    fillFaceCallback->setColor( selectedColor );
    this->updateFillColorSwatch( );
  }
}


/* test_beta_distribution.cpp
 *
 * Copyright Steven Watanabe 2014
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id$
 *
 */

#include <boost/random/beta_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::beta_distribution<>
#define BOOST_RANDOM_ARG1 alpha
#define BOOST_RANDOM_ARG2 beta
#define BOOST_RANDOM_ARG1_DEFAULT 1.0
#define BOOST_RANDOM_ARG2_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE 7.5
#define BOOST_RANDOM_ARG2_VALUE 0.25

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX 1
#define BOOST_RANDOM_DIST1_MIN 0
#define BOOST_RANDOM_DIST1_MAX 1
#define BOOST_RANDOM_DIST2_MIN 0
#define BOOST_RANDOM_DIST2_MAX 1

#define BOOST_RANDOM_TEST1_PARAMS
#define BOOST_RANDOM_TEST1_MIN 0.0
#define BOOST_RANDOM_TEST1_MAX 1.0

#define BOOST_RANDOM_TEST2_PARAMS (1.0, 1000000.0)
#define BOOST_RANDOM_TEST2_MIN 0.0
#define BOOST_RANDOM_TEST2_MAX 1.0

#include "test_distribution.ipp"


#include <boost/test/unit_test.hpp>
#include "functions/addition.hh"
#include "functions/polynomial.hh"
#include "functions/std_functions.hh"
#include "functions/factor.hh"
#include <boost/mpl/size.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/for_each.hpp>
#include "functions/streaming.hh"
#include "functions/operators.hh"

BOOST_AUTO_TEST_CASE(factoring_test) {
  using namespace manifolds;
  static_assert(HighestVariable<Variable<3> >::value == 3, "");
  static_assert(
      HighestVariable<std::tuple<Variable<0>, Variable<2> > >::value == 2, "");
  static_assert(
      HighestVariable<Addition<Variable<0>, Sin, Variable<1> > >::value == 1,
      "");
  static_assert(
      HighestVariable<Addition<Variable<1>,
                               Addition<Variable<2>, UnaryMinus<Variable<5> > >,
                               Addition<Sin, Variable<3> > > >::value == 5,
      "");
  // Check that bug in Boost's m_item class
  using namespace boost::mpl;
  static_assert(size<insert<map<>, pair<int, int> >::type::type>::value == 1,
                "");

  static_assert(equal<Factors<Sin>::type,
                      map<pair<Sin, manifolds::int_<1> > > >::type::value,
                "");

  static_assert(
      size<Factors<decltype(sin_(x) + cos_(x))>::type>::type::value == 3, "");

  static_assert(size<Factors<decltype(
                    tan_(x) + GetPolynomial(1.0)(x))>::type>::type::value == 1,
                "");

  typedef decltype(x * sin_(x) + x * cos_(x) * sin_(x) + tan_(x)) Add;
  std::cout << Add{} << '\n';
  for_each<Factors<Add>::type>([](auto x) {
    std::cout << "(" << typename decltype(x)::first{} << ", "
              << decltype(x)::second::value << "), ";
  });
  std::cout << '\n';
  for_each<Filter1OffFactors<Factors<Add>::type>::type>([](auto x) {
    std::cout << "(" << x << "), ";
  });
  std::cout << '\n';
  static_assert(is_reorderable<Addition<Sin, Cos> >::value, "");

  for_each<AssignVariables<Add>::type>([](auto x) {
    typedef decltype(x) type;
    std::cout << '(' << typename type::first{} << ' ' << typename type::second{}
              << ')';
  });
}


#include <stdio.h>

#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/eigen_helpers.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <arc_utilities/simple_robot_model_interface.hpp>
#include <map>
#include <random>
#include <vector>

#ifndef SIMPLE_SE3_ROBOT_MODEL_HPP
#define SIMPLE_SE3_ROBOT_MODEL_HPP

namespace simple_se3_robot_model {
using SimpleSE3Configuration = Eigen::Isometry3d;
using SimpleSE3ConfigAlloc = Eigen::aligned_allocator<Eigen::Isometry3d>;

class SimpleSE3ConfigSerializer {
 public:
  static inline std::string TypeName() { return std::string("EigenIsometry3dSerializer"); }

  static inline uint64_t Serialize(const SimpleSE3Configuration& value, std::vector<uint8_t>& buffer) {
    return EigenHelpers::Serialize(value, buffer);
  }

  static inline std::pair<SimpleSE3Configuration, uint64_t> Deserialize(const std::vector<uint8_t>& buffer,
                                                                        const uint64_t current) {
    return EigenHelpers::Deserialize<Eigen::Isometry3d>(buffer, current);
  }
};

class SimpleSE3Robot
    : public simple_robot_model_interface::SimpleRobotModelInterface<SimpleSE3Configuration, SimpleSE3ConfigAlloc> {
 protected:
  SimpleSE3Configuration config_;
  double position_distance_weight_;
  double rotation_distance_weight_;
  std::string link_name_;

  inline void SetConfig(const Eigen::Isometry3d& new_config) { config_ = new_config; }

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  SimpleSE3Robot(const SimpleSE3Configuration& initial_position, const double position_distance_weight,
                 const double rotation_distance_weight, const std::string& link_name)
      : simple_robot_model_interface::SimpleRobotModelInterface<SimpleSE3Configuration, SimpleSE3ConfigAlloc>(),
        position_distance_weight_(std::abs(position_distance_weight)),
        rotation_distance_weight_(std::abs(rotation_distance_weight)),
        link_name_(link_name) {
    SetPosition(initial_position);
  }

  virtual simple_robot_model_interface::SimpleRobotModelInterface<SimpleSE3Configuration, SimpleSE3ConfigAlloc>* Clone()
      const {
    return new SimpleSE3Robot(static_cast<const SimpleSE3Robot&>(*this));
  }

  inline const SimpleSE3Configuration& GetPosition() const { return config_; }

  inline const SimpleSE3Configuration& SetPosition(const SimpleSE3Configuration& position) {
    SetConfig(position);
    return GetPosition();
  }

  virtual std::vector<std::string> GetLinkNames() const { return std::vector<std::string>{link_name_}; }

  inline Eigen::Isometry3d GetLinkTransform(const std::string& link_name) const {
    if (link_name == link_name_) {
      return GetPosition();
    } else {
      throw std::invalid_argument("Invalid link_name");
    }
  }

  virtual EigenHelpers::VectorIsometry3d GetLinkTransforms() const {
    return EigenHelpers::VectorIsometry3d(1, config_);
  }

  virtual EigenHelpers::MapStringIsometry3d GetLinkTransformsMap() const {
    EigenHelpers::MapStringIsometry3d link_transforms_map;
    link_transforms_map[link_name_] = config_;
    return link_transforms_map;
  }

  virtual Eigen::VectorXd ComputePerDimensionConfigurationSignedDistance(const SimpleSE3Configuration& config1,
                                                                         const SimpleSE3Configuration& config2) const {
    // This should change to use a 6dof twist instead
    const Eigen::Vector3d tc1 = config1.translation();
    const Eigen::Quaterniond qc1(config1.rotation());
    const Eigen::Vector3d tc2 = config2.translation();
    const Eigen::Quaterniond qc2(config2.rotation());
    Eigen::VectorXd dim_distances(4);
    dim_distances(0) = tc2.x() - tc1.x();
    dim_distances(1) = tc2.y() - tc1.y();
    dim_distances(2) = tc2.z() - tc1.z();
    dim_distances(3) = EigenHelpers::Distance(qc1, qc2);
    return dim_distances;
  }

  virtual double ComputeConfigurationDistance(const SimpleSE3Configuration& config1,
                                              const SimpleSE3Configuration& config2) const {
    const Eigen::Vector3d v1 = config1.translation();
    const Eigen::Quaterniond q1(config1.rotation());
    const Eigen::Vector3d v2 = config2.translation();
    const Eigen::Quaterniond q2(config2.rotation());
    const double vdist = EigenHelpers::Distance(v1, v2);
    const double qdist = EigenHelpers::Distance(q1, q2);
    return (vdist * position_distance_weight_) + (qdist * rotation_distance_weight_);
  }

  virtual Eigen::Isometry3d InterpolateBetweenConfigurations(const SimpleSE3Configuration& start,
                                                             const SimpleSE3Configuration& end,
                                                             const double ratio) const {
    return EigenHelpers::Interpolate(start, end, ratio);
  }

  virtual Eigen::Isometry3d AverageConfigurations(
      const std::vector<SimpleSE3Configuration, SimpleSE3ConfigAlloc>& configurations) const {
    if (configurations.size() > 0) {
      return EigenHelpers::AverageEigenIsometry3d(configurations);
    } else {
      return Eigen::Isometry3d::Identity();
    }
  }

  virtual Eigen::Matrix<double, 3, Eigen::Dynamic> ComputeLinkPointTranslationJacobian(
      const std::string& link_name, const Eigen::Vector4d& link_relative_point) const {
    if (link_name == link_name_) {
      const Eigen::Matrix3d rot_matrix = GetPosition().rotation();
      const Eigen::Matrix3d hatted_link_relative_point = EigenHelpers::Skew(link_relative_point.block<3, 1>(0, 0));
      Eigen::Matrix<double, 3, 6> body_velocity_jacobian = Eigen::Matrix<double, 3, 6>::Zero();
      body_velocity_jacobian.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
      body_velocity_jacobian.block<3, 3>(0, 3) = -hatted_link_relative_point;
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
      const Eigen::Matrix<double, 3, 6> jacobian = rot_matrix * body_velocity_jacobian;
#pragma GCC diagnostic pop
      return jacobian;
    } else {
      throw std::invalid_argument("Invalid link_name");
    }
  }
};

}  // namespace simple_se3_robot_model

#endif  // SIMPLE_SE3_ROBOT_MODEL_HPP


// Copyright (c) 2017, University of Minnesota
//
// ADMM-Elastic Uses the BSD 2-Clause License (http://www.opensource.org/licenses/BSD-2-Clause)
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other materials
//    provided with the distribution.
// THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR  A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF MINNESOTA, DULUTH OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
// IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

//
// TODO: An implementation of fast svd
//

#ifndef ADMM_FASTSVD_H
#define ADMM_FASTSVD_H 1

#include <Eigen/Dense>

// Relevent papers:
// Computing the Singular Value Decomposition of 3x3 matrices with minimal branching and elementary floating point operations, McAdams et al.
// Energetically Consistent Invertible Elasticity, Stomakhin et al.
// Invertible Finite Elements For Robust Simulation of Large Deformation, Irving et al.

namespace admm {
	namespace fsvd {
		template <typename T> using Vec3 = Eigen::Matrix<T,3,1>;
		template <typename T> using Mat3 = Eigen::Matrix<T,3,3>;
	}

	// Projection, Singular Values, SVD's U, SVD's V
	template <typename T>
	static inline void signed_svd( const fsvd::Mat3<T> &F, fsvd::Vec3<T> &S, fsvd::Mat3<T> &U, fsvd::Mat3<T> &V ){
		using namespace Eigen;

		JacobiSVD< fsvd::Mat3<T> > svd( F, ComputeFullU | ComputeFullV );
		S = svd.singularValues();
		U = svd.matrixU();
		V = svd.matrixV();
		fsvd::Mat3<T> J = Matrix3d::Identity();
		J(2,2) = -1.0;

		// Check for inversion: U
		if( U.determinant() < 0.0 ){
			U = U * J;
			S[2] = -S[2];
		}

		// Check for inversion: V
		if( V.determinant() < 0.0 ){
			fsvd::Mat3<T> Vt = V.transpose();
			Vt = J * Vt;
			V = Vt.transpose();
			S[2] = -S[2];
		}

	} // end signed svd

} // endns admm

#endif


#include "optim.h"
#include <limits>
// #include <cstddef>
#include <boost/range/combine.hpp>
#include <boost/range/adaptor/reversed.hpp>

//////////////////////////////////////////////////////////////////
//////////////////////// COO Matrix Struct ///////////////////////
//////////////////////////////////////////////////////////////////

matCOO::matCOO(Eigen::SparseMatrix<double> const & m){
    nr = m.rows();
    nc = m.cols();
    for (int k=0; k < m.outerSize(); ++k){
        for (Eigen::SparseMatrix<double>::InnerIterator it(m,k); it; ++it)
        {
            row.push_back(it.row());
            col.push_back(it.col());
            val.push_back(it.value());
        }
    }
};

Eigen::SparseMatrix<double> matCOO::toSparseEigen(){
  std::vector<T> tripletList;
  tripletList.reserve(row.size());
  int i,j;
  double v;
  for(auto tup : boost::combine(row,col,val)){
    boost::tie(i,j,v) = tup;
    tripletList.push_back(T(i,j,v));
  }
  Eigen::SparseMatrix<double> mat(nr,nc);
  mat.setFromTriplets(tripletList.begin(), tripletList.end());
  return mat;
};

Eigen::SparseMatrix<double> matCOO::Extract(std::vector<bool> Mask ,std::vector<int> const & NewIndice, int size){
    std::vector<T> tripletList;
    tripletList.reserve(row.size());
    int i,j;
    double v;
    for(auto tup : boost::combine(row,col,val)){
        boost::tie(i,j,v) = tup;
        if (Mask[i] && Mask[j]){
            tripletList.push_back(T(NewIndice[i],NewIndice[j],v));
        }

    }
    Eigen::SparseMatrix<double> mat(size,size);
    mat.setFromTriplets(tripletList.begin(), tripletList.end());
    return mat;
};

//////////////////////////////////////////////////////////////////
/////////////////////////// BFGS CLASS ///////////////////////////
//////////////////////////////////////////////////////////////////

BFGS::BFGS() : memorySize(15), problemSize(1000){
}
BFGS::BFGS(uint memSize,uint pbSize) : memorySize(memSize), problemSize(pbSize){
    this->memoryX    = std::list<Eigen::VectorXd>();
    this->memoryGrad = std::list<Eigen::VectorXd>();
}
void BFGS::addDirection(double step, Eigen::VectorXd direction, Eigen::VectorXd gradNew, Eigen::VectorXd gradOld){
    if(this->memoryX.size()==memorySize)    this->memoryX.pop_back();
    if(this->memoryGrad.size()==memorySize) this->memoryGrad.pop_back();
    this->memoryX.push_front(step*direction);
    this->memoryGrad.push_front(gradOld - gradNew);
}

Eigen::VectorXd BFGS::findDirection(Eigen::VectorXd gradf){
    if(this->memoryX.size() == 0){
        return gradf;
    }
    else{
        Eigen::VectorXd r = -1.*gradf;
        std::vector<double> rhoVect = std::vector<double>(this->memoryX.size());
        Eigen::VectorXd x,grad;
        int compt = 0;
        double dotValue;
        for(auto tup : boost::combine(this->memoryX,this->memoryGrad)){
            boost::tie(x, grad) = tup;
            dotValue = grad.dot(x);
            rhoVect[compt] = std::abs(dotValue) < 1e-42 ? 0 : 1./dotValue;
            compt++;
        }
        double gamma = this->memoryX.front().dot(this->memoryGrad.front())/(this->memoryGrad.front().dot(this->memoryGrad.front()));
        std::vector<double> alpha = std::vector<double>(this->memoryGrad.size());
        compt = 0;
        for(auto tup : boost::combine(this->memoryX,this->memoryGrad)){
            boost::tie(x, grad) = tup;
            alpha[compt] = rhoVect[compt]*x.dot(r);
            r -= alpha[compt]*grad;
            compt++;
        }
        r *= gamma;
        double beta;
        compt = this->memoryX.size()-1;
        for(auto tup : boost::adaptors::reverse(boost::combine(this->memoryX,this->memoryGrad))){
            boost::tie(x, grad) = tup;
            beta = rhoVect[compt]*grad.dot(r);
            r += (alpha[compt] - beta)*x;
            compt--;
        }
        return -r;

    }
}

//////////////////////////////////////////////////////////////////
////////////////// gradientDescent CLASS /////////////////////////
//////////////////////////////////////////////////////////////////
gradientDescent::gradientDescent() : stepSize(1e-2), gradNormThres(1e-5), iterMax(2000) {
}
gradientDescent::gradientDescent(double step, double gradN, int nbIter) : stepSize(step),
                                gradNormThres(gradN), iterMax((uint) nbIter){
}

//////////////////////////////////////////////////////////////////
///////////////// gradientStrongWolf CLASS ///////////////////////
//////////////////////////////////////////////////////////////////
gradientStrongWolf::gradientStrongWolf() : stepSize(1e-2), gradNormThres(1e-5),
                                           alpha1(0.001), alpha2(0.9), maxStepReduction(12), iterMax(2000) {
}
gradientStrongWolf::gradientStrongWolf(double step, double gradN, double wOne, double wTwo,int innerLoop, int nbIter) :
                                stepSize(step), gradNormThres(gradN), alpha1(wOne), alpha2(wTwo),
                                maxStepReduction((uint) innerLoop), iterMax((uint) nbIter){
}

//////////////////////////////////////////////////////////////////
////////////////// barzilaiBorwein CLASS /////////////////////////
//////////////////////////////////////////////////////////////////
barzilaiBorwein::barzilaiBorwein() : gradNormThres(1e-5), iterMax(2000){
}
barzilaiBorwein::barzilaiBorwein(double gradN,int nbIter) : gradNormThres(gradN), iterMax((uint) nbIter) {
}

//////////////////////////////////////////////////////////////////
/////////////////// barzilaiBorweinLS CLASS //////////////////////
//////////////////////////////////////////////////////////////////
barzilaiBorweinLS::barzilaiBorweinLS() : gradNormThres(1e-5), lineSearchFactor(.70),gammaWolfe(1e-4), iterMax(2000),memorySize(10){
}
barzilaiBorweinLS::barzilaiBorweinLS(double gradN,double lineSearchFactor,double gammaW,int nbIter,int memSize) :
                 gradNormThres(gradN), lineSearchFactor(.70),gammaWolfe(gammaW),iterMax((uint) nbIter),memorySize((uint) memSize) {
}
void barzilaiBorweinLS::addCost(double cost){
    if(costFunctionList.size() > memorySize){
        costFunctionList.pop_back();
    }
    costFunctionList.push_front(cost);
}
double barzilaiBorweinLS::minCost(){
    if(costFunctionList.size() < 2){
        return std::numeric_limits<double>::min();
    }
    else{
        double minValue = -2.0*costFunctionList.front()-1.;
        for( const auto & elm : costFunctionList){
            if( elm < minValue ){
                minValue = elm;
            }
        }
        return minValue;
    }
}

//////////////////////////////////////////////////////////////////
/////////////////////// heavyBall CLASS //////////////////////////
//////////////////////////////////////////////////////////////////
heavyBall::heavyBall() : Lips(1000.), gradNormThres(1e-5), iterMax(2000){
}
heavyBall::heavyBall(double LispchitzConstant,double gradN,int nbIter) : Lips(LispchitzConstant),gradNormThres(gradN), iterMax((uint) nbIter) {
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_ULP_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_ULP_HPP_INCLUDED
#include <boost/simd/ieee/functions/ulp.hpp>

#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/ieee/functions/simd/common/details/ulp.hpp>
#include <boost/dispatch/attributes.hpp>

#if defined(BOOST_SIMD_NO_DENORMALS)
#include <boost/simd/include/functions/simd/ldexp.hpp>
#include <boost/simd/include/functions/simd/is_eqz.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>
#include <boost/simd/include/constants/mindenormal.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/include/constants/smallestposval.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( ulp_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<arithmetic_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0&)const
    {
      return One<A0>();
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( ulp_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      const A0 x = boost::simd::abs(a0);

      #if !defined(BOOST_SIMD_NO_DENORMALS)

      return details::ulp(x);

      #else

      typedef typename dispatch::meta::as_integer<A0>::type in_t;
      const in_t n = simd::Nbmantissabits<A0>();

      return if_else( is_eqz(x)
                    , Mindenormal<A0>()
                    , if_else ( x < simd::Smallestposval<A0>()
                              , simd::ldexp ( details::ulp(simd::ldexp(x,n))
                                            , -n
                                            )
                              , details::ulp(x)
                              )
                    );
      #endif
    }
  };

} } }

#endif


/*
 * ScalarCurveConfig.hpp
 *
 *  Created on: Mar 5, 2015
 *      Author: Paul Furgale, Renaud Dube, Péter Fankhauser
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

#include "curves/SE3Config.hpp"
#include "curves/Curve.hpp"
#include <Eigen/Core>

namespace curves {

// Curves over SE3 inherit the interface from Curve and CurveBase and define specific
// methods to support physical interpretations of temporal derivatives.
//
// For the purposes of these uses, the curve is defined between Frame a and Frame b
// such that evaluate() returns \f$ \mathbf{T}_{a,b} \f$, the transformation that takes
// points from Frame b to Frame a.
//
class SE3Curve : public Curve<SE3Config> {
 public:
  SE3Curve();
  virtual ~SE3Curve();

  typedef Curve<SE3Config> Parent;
  typedef Parent::ValueType ValueType;
  typedef Parent::DerivativeType DerivativeType;

  /// \brief Evaluate the angular velocity of Frame b as seen from Frame a, expressed in Frame a.
  virtual Eigen::Vector3d evaluateAngularVelocityA(Time time) = 0;

  /// \brief Evaluate the angular velocity of Frame a as seen from Frame b, expressed in Frame b.
  virtual Eigen::Vector3d evaluateAngularVelocityB(Time time) = 0;

  /// \brief Evaluate the velocity of Frame b as seen from Frame a, expressed in Frame a.
  virtual Eigen::Vector3d evaluateLinearVelocityA(Time time) = 0;

  /// \brief Evaluate the velocity of Frame a as seen from Frame b, expressed in Frame b.
  virtual Eigen::Vector3d evaluateLinearVelocityB(Time time) = 0;

  /// \brief evaluate the velocity/angular velocity of Frame b as seen from Frame a,
  ///        expressed in Frame a. The return value has the linear velocity (0,1,2),
  ///        and the angular velocity (3,4,5).
  virtual Vector6d evaluateTwistA(Time time) = 0;

  /// \brief evaluate the velocity/angular velocity of Frame a as seen from Frame b,
  ///        expressed in Frame b. The return value has the linear velocity (0,1,2),
  ///        and the angular velocity (3,4,5).
  virtual Vector6d evaluateTwistB(Time time) = 0;

  /// \brief Evaluate the angular derivative of Frame b as seen from Frame a, expressed in Frame a.
  virtual Eigen::Vector3d evaluateAngularDerivativeA(unsigned derivativeOrder, Time time) = 0;

  /// \brief Evaluate the angular derivative of Frame a as seen from Frame b, expressed in Frame b.
  virtual Eigen::Vector3d evaluateAngularDerivativeB(unsigned derivativeOrder, Time time) = 0;

  /// \brief Evaluate the derivative of Frame b as seen from Frame a, expressed in Frame a.
  virtual Eigen::Vector3d evaluateLinearDerivativeA(unsigned derivativeOrder, Time time) = 0;

  /// \brief Evaluate the derivative of Frame a as seen from Frame b, expressed in Frame b.
  virtual Eigen::Vector3d evaluateLinearDerivativeB(unsigned derivativeOrder, Time time) = 0;

  /// \brief evaluate the velocity/angular derivative of Frame b as seen from Frame a,
  ///        expressed in Frame a. The return value has the linear velocity (0,1,2),
  ///        and the angular velocity (3,4,5).
  virtual Vector6d evaluateDerivativeA(unsigned derivativeOrder, Time time) = 0;

  /// \brief evaluate the velocity/angular velocity of Frame a as seen from Frame b,
  ///        expressed in Frame b. The return value has the linear velocity (0,1,2),
  ///        and the angular velocity (3,4,5).
  virtual Vector6d evaluateDerivativeB(unsigned derivativeOrder, Time time) = 0;

  // Following functions added from SlerpSE3 curve .. todo clean

  /// \brief set the minimum sampling period
  virtual void setMinSamplingPeriod(Time time) = 0;

  /// \brief Set the sampling ratio.
  ///   eg. 4 will add a coefficient every 4 extend
  virtual void setSamplingRatio(const int ratio) = 0;

  virtual void clear() = 0;

  /// \brief Perform a rigid transformation on the left side of the curve
  virtual void transformCurve(const ValueType T) = 0;

  virtual void saveCurveTimesAndValues(const std::string& filename) const = 0;

  virtual void saveCurveAtTimes(const std::string& filename, std::vector<Time> times) const = 0;

  virtual void saveCorrectionCurveAtTimes(const std::string& filename, std::vector<Time> times) const = 0;

  virtual void getCurveTimes(std::vector<Time>* outTimes) const = 0;

  // Fake functions to comply with the current interfaces of trajectories_optimizer
  // todo : tidy up

  /// \brief Returns the number of coefficients in the correction curve
  virtual int correctionSize() const = 0;

  /// \brief Fold in the correction curve into the base curve and reinitialize
  ///        correction curve coefficients to identity transformations.
  virtual void foldInCorrections() = 0;

  /// \brief Add coefficients to the correction curve at given times.
  virtual void setCorrectionTimes(const std::vector<Time>& times) = 0;

  /// \brief Remove a correction coefficient at the specified time.
  virtual void removeCorrectionCoefficientAtTime(Time time) = 0;

  /// \brief Set the correction coefficient value at the specified time.
  virtual void setCorrectionCoefficientAtTime(Time time, ValueType value) = 0;

  /// \brief Reset the correction curve to identity values with knots at desired times
  virtual void resetCorrectionCurve(const std::vector<Time>& times) = 0;

  /// \brief Set the base curve to given values with knots at desired times
  /// Resets the curve beforehand.
  virtual void setBaseCurve(const std::vector<Time>& times, const std::vector<ValueType>& values) = 0;

  /// \brief Add / replace the given coefficients without resetting the curve.
  virtual void setBaseCurvePart(const std::vector<Time>& times, const std::vector<ValueType>& values) = 0;

  /// \brief Modifies values of the base coefficient in batch, starting at times[0] and assuming that
  /// a coefficient exists at all the specified times.
  virtual void modifyBaseCoefficientsValuesInBatch(const std::vector<Time>& times, const std::vector<ValueType>& values) = 0;

  virtual void getBaseCurveTimes(std::vector<Time>* outTimes) const = 0;

  virtual void getBaseCurveTimesInWindow(std::vector<Time>* outTimes, Time begTime, Time endTime) const = 0;

  virtual bool isEmpty() const = 0;

  virtual int size() const = 0;

  virtual int baseSize() const = 0;

  virtual void saveCorrectionCurveTimesAndValues(const std::string& filename) const = 0;

  /// \brief Get the dimension of this curve
  //virtual size_t dim() const;
  ///@}
 private:

};

} // namespace


/* Copyright (c) 2016, the Cap authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#include <cap/post_processor.templates.h>
#include <boost/numeric/odeint.hpp>

namespace cap
{

template class PostprocessorParameters<2>;
template class Postprocessor<2>;
template class PostprocessorParameters<3>;
template class Postprocessor<3>;

template class SuperCapacitorPostprocessorParameters<2>;
template class SuperCapacitorPostprocessor<2>;
template class SuperCapacitorPostprocessorParameters<3>;
template class SuperCapacitorPostprocessor<3>;

namespace internal
{
class energy_odeint
{
public:
  energy_odeint(std::vector<double> const &time,
                std::vector<double> const &power);

  void operator()(std::vector<double> const &, std::vector<double> &dxdt,
                  double const current_time);

private:
  std::vector<double> const time;
  std::vector<double> const power;
};

energy_odeint::energy_odeint(std::vector<double> const &time,
                             std::vector<double> const &power)
    : time(time), power(power)
{
}

void energy_odeint::operator()(std::vector<double> const &,
                               std::vector<double> &dxdt,
                               double const current_time)
{
  unsigned int pos = 0;
  for (unsigned int i = 0; i < time.size(); ++i, ++pos)
  {
    if (time[i] >= current_time)
      break;
  }
  if (pos == 0)
    dxdt[0] = power[0];
  else
    dxdt[0] = power[pos - 1] +
              (current_time - time[pos - 1]) / (time[pos] - time[pos - 1]) *
                  (power[pos] - power[pos - 1]);
}

void evaluate_energy(std::vector<double> const &computed_time,
                     std::vector<double> const &computed_energy,
                     std::vector<double> const &time,
                     std::vector<double> &energy)
{
  energy.clear();

  for (auto current_time : time)
  {
    unsigned int pos = 0;
    for (unsigned int i = 0; i < computed_time.size(); ++i, ++pos)
    {
      if (computed_time[i] >= current_time)
        break;
    }
    if (pos == 0)
      energy.push_back(computed_energy[0]);
    else
      energy.push_back(computed_energy[pos - 1] +
                       (current_time - computed_time[pos - 1]) /
                           (computed_time[pos] - computed_time[pos - 1]) *
                           (computed_energy[pos] - computed_energy[pos - 1]));
  }
}
}

void compute_energy(std::vector<double> const &time,
                    std::vector<double> const &power,
                    std::vector<double> &energy)

{
  bool const valid_input =
      (time.size() == power.size()) && (time.size() == energy.size());
  if (!valid_input)
    throw std::runtime_error("invalid input");
  internal::energy_odeint ode(time, power);
  std::vector<double> computed_energy;
  std::vector<double> computed_time;
  std::vector<double> x0(1);
  x0[0] = 0.;
  // Find the smallest time step
  double dt = time[1] - time[0];
  if (time.size() > 2)
    for (unsigned int i = 2; i < time.size(); ++i)
      if (dt > time[i] - time[i - 1])
        dt = time[i] - time[i - 1];

  double const abs_tol = 1e-9;
  double const rel_tol = 1e-9;
  boost::numeric::odeint::integrate_const(
      boost::numeric::odeint::make_dense_output(
          abs_tol, rel_tol,
          boost::numeric::odeint::runge_kutta_dopri5<std::vector<double>>()),
      ode, x0, time[0], time.back(), dt / 2.,
      [&](std::vector<double> const &x, double const &t)
      {
        computed_energy.push_back(x[0]);
        computed_time.push_back(t);
      });

  internal::evaluate_energy(computed_time, computed_energy, time, energy);
}

} // end namespace cap


//STL
#include <iostream>
#include <string>

#include <cstdlib>

//MRPT
#include <mrpt/poses/CPose3D.h>
#include <mrpt/utils/utils_defs.h>
#include <mrpt/utils/CConfigFile.h>

//Eigen
#include <Eigen/Dense>

//Boost
#include <boost/filesystem.hpp>

#include "Calibration.hpp"
#include "RawlogHelper.hpp"
#include "ObservationPair.hpp"
#include "Parameters.hpp"
#include "Segmentation.hpp"

inline Eigen::Matrix4f fromCPose3D(const mrpt::poses::CPose3D &p)
{
    mrpt::math::CMatrixDouble44 m;
    p.getHomogeneousMatrix(m);

    Eigen::Matrix4f t(m.cast<float>());
    return t;
}

int main (int argc, char ** argv)
{
    /// Input config

    if (argc != 2)
    {
        std::cout << "Usage: " << argv[0] << " <config file>" << std::endl;
        return -1;
    }

    //Load config paramters
    std::string config_file(argv[1]);
    if (!boost::filesystem::is_regular_file(config_file))
    {
        std::cerr << "Invalid config file: " << config_file << std::endl;
        return -1;
    }

    mrpt::utils::CConfigFile cfg(config_file);

    ParametersPtr params(new Parameters);
    params->rawlog_file = cfg.read_string("Global", "rawlog_file", "", true);
    params->output_dir = cfg.read_string("Global", "output_dir", "", true);
    params->max_time_diff = cfg.read_float("Global", "max_dt", 0.015f, false);
    params->verbose = cfg.read_bool("Global", "verbose", true, false);

    params->visualize = cfg.read_bool("Segmentation", "visualize", true, false);
    params->sigma_s = cfg.read_float("Segmentation", "sigma_s", 5.0f, false);
    params->sigma_r = cfg.read_float("Segmentation", "sigma_r", 0.005f, false);
    params->max_change_factor = cfg.read_float("Segmentation", "max_change_factor", 0.02f, false);
    params->smoothing_size = cfg.read_float("Segmentation", "smoothing_size", 10.0f, false);
    params->inliers_ratio = cfg.read_float("Segmentation", "inliers_ratio", 0.2f, false);
    params->angular_th = cfg.read_float("Segmentation", "angular_th", 0.10f, false);
    params->distance_th = cfg.read_float("Segmentation", "distance_th", 0.03f, false);

    params->max_angle = cfg.read_float("Calibration", "max_angle", 7.f, false);
    params->max_distance = cfg.read_float("Calibration", "max_distance", 0.03f, false);

    params->approx_rotation = cfg.read_float("Calibration", "approx_rotation", 10.f, false);
    params->approx_translation = cfg.read_float("calibration", "approx_translation", 0.05f, false);

    //int decimation = cfg.read_int("Global", "decimation", 1, false);
    //bool display = cfg.read_bool("Global", "display", true, false);

    std::vector< std::string > all_labels;
    cfg.getAllSections(all_labels);

    for (std::vector<std::string>::iterator it = all_labels.begin(); it != all_labels.end(); ++it)
    {
        if (cfg.read_bool(*it, std::string("is_sensor"), false))
        {
            mrpt::poses::CPose3D pose = mrpt::poses::CPose3D(
                        cfg.read_float(*it, "pose_x", 0, true),
                        cfg.read_float(*it, "pose_y", 0, true),
                        cfg.read_float(*it, "pose_z", 0, true),
                        mrpt::utils::DEG2RAD(cfg.read_float(*it, "pose_yaw", 0, true)),
                        mrpt::utils::DEG2RAD(cfg.read_float(*it, "pose_pitch", 0, true)),
                        mrpt::utils::DEG2RAD(cfg.read_float(*it, "pose_roll", 0, true))
                        );

            params->sensor_labels.push_back(*it);
            params->init_pose.push_back(fromCPose3D(pose));
        }
    }

    if (params->sensor_labels.size() <= 1)
    {
        std::cerr << "Config file must contain more than one sensor specification" << std::endl;
        return -1;
    }

    //TODO: remove 2 sensors only constraint
    if (params->sensor_labels.size() != 2)
    {
        std::cerr << "Allowing 2 sensors only..." << std::endl;
        return -1;
    }

    if (!boost::filesystem::exists(params->rawlog_file))
    {
        std::cerr << "Invalid rawlog file: " << params->rawlog_file << std::endl;
        return -1;
    }

    if (!boost::filesystem::is_directory(params->output_dir))
    {
        std::cerr << "Invalid ouput_dir in config file: " << params->output_dir << std::endl;
        return -1;
    }

    if (params->verbose)
    {
        std::cout << "Calibration config: " << std::endl;
        std::cout << "rawlog_file: " << params->rawlog_file << std::endl;
        std::cout << "output_dir: " << params->output_dir << std::endl;

        std::cout << "max_time_diff: " << params->max_time_diff << std::endl;

        std::cout << "visualize: " << params->visualize << std::endl;
        std::cout << "sigma_s: " << params->sigma_s << std::endl;
        std::cout << "sigma_r: " << params->sigma_r << std::endl;
        std::cout << "max_change_factor: " << params->max_change_factor << std::endl;
        std::cout << "smoothing_size: " << params->smoothing_size << std::endl;
        std::cout << "inliers_ratio: " << params->inliers_ratio << std::endl;
        std::cout << "angular_th: " << params->angular_th << std::endl;
        std::cout << "distance_th: " << params->distance_th << std::endl;

        std::cout << "max_angle: " << params->max_angle << std::endl;
        std::cout << "max_distance: " << params->max_distance << std::endl;
        std::cout << "approx_rotation: " << params->approx_rotation << std::endl;
        std::cout << "approx_translation: " << params->approx_translation << std::endl;

//        std::cout << "decimation: " << decimation << std::endl;
//        std::cout << "display: " << display << std::endl;

        std::cout << "Number of sensors: " << params->sensor_labels.size() << std::endl;

        std::cout << std::endl;
        std::cout << "Loading rawlog file..." << std::endl;
    }

    /// Plane Segmentation & Matching

    RawlogHelper rawlog(params);

    if (params->verbose)
        std::cout << " done." << std::endl;

    //output_path = boost::filesystem::path(output_dir);

    Segmentation seg(params);
    Calibration calib(params);

    int cntr = 0;

    ObservationPair obs2;
    while (rawlog.getNextPair(obs2))
    {
        cntr++;

        PlanesList p1, p2;
        seg.segmentPlanes(obs2.first, p1);
        seg.segmentPlanes(obs2.second, p2);

        if (params->verbose)
        {
            std::cout << "Number of planes " << obs2.first->sensorLabel << ": " << p1.size() << std::endl;
            std::cout << "Number of planes " << obs2.second->sensorLabel << ": " << p2.size() << std::endl;
        }

        Correspondences matches = calib.findCorrespondences(obs2, p1, p2, false);

        if (params->verbose)
            std::cout << "Found correspondences: " << matches.size() << std::endl;
    }

    if (params->verbose)
    {
        std::cout << "Observations: " << cntr << "/" << rawlog.m << std::endl;
        std::cout << "Matches: " << calib.all_matches[0].size() << "/" << cntr << std::endl;
        std::cout << "Outliers: " << calib.outliers << "/" << calib.all_matches[0].size() << std::endl;
    }

    /// Calibration

    Eigen::Matrix4f T_cf = calib.runCalibration();

    std::cout << "CF:" << std::endl;
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            std::cout << T_cf(i, j) << ", ";
        }
        std::cout << std::endl;
    }

    calib.Tini = Eigen::Matrix4d::Identity();
    Eigen::Matrix4f T_lm = calib.runCalibrationLM(0, 1, false);

    std::cout << "LM:" << std::endl;
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            std::cout << T_lm(i, j) << ", ";
        }
        std::cout << std::endl;
    }

    Eigen::Matrix4f T_lmr = calib.runCalibrationLM(0, 1, true);

    std::cout << "LMR:" << std::endl;
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            std::cout << T_lmr(i, j) << ", ";
        }
        std::cout << std::endl;
    }

    return 0;
}


/*!
@file
Forward declares `boost::hana::IntegralDomain`.

@copyright Louis Dionne 2015
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_HANA_FWD_INTEGRAL_DOMAIN_HPP
#define BOOST_HANA_FWD_INTEGRAL_DOMAIN_HPP

#include <boost/hana/detail/std/forward.hpp>
#include <boost/hana/fwd/core/datatype.hpp>
#include <boost/hana/fwd/core/operators.hpp>

namespace boost { namespace hana {
    //! @ingroup group-concepts
    //! The `IntegralDomain` concept represents a commutative `Ring` whose
    //! multiplication has some "nice" properties.
    //!
    //! In a Ring `(R, +, *)`, it follows quite easily from the axioms that
    //! `x * 0 == 0` for any ring element `x`. However, there is nothing that
    //! mandates `0` to be the only ring element sending other elements to `0`.
    //! Hence, in some Rings, it is possible to have elements `x` and `y` such
    //! that `x * y == 0` while not having `x == 0` nor `y == 0`. We call these
    //! elements divisors of zero, or zero divisors. For example, this situation
    //! arises in the Ring of integers modulo 4 (the set `{0, 1, 2, 3}`)
    //! with addition and multiplication `mod 4` as binary operations.
    //! In this case, we have that
    //! @code
    //!     2 * 2 == 4
    //!           == 0 (mod 4)
    //! @endcode
    //! even though `2 != 0 (mod 4)`.
    //!
    //! Following this line of thought, an [IntegralDomain][1] is simply a
    //! `Ring` whose only zero divisor is zero itself. In other words, an
    //! `IntegralDomain` is a `Ring` whose multiplication won't send
    //! non-zero elements to zero. Also note that since multiplication in a
    //! `Ring` is not necessarily commutative, it is not always the case that
    //! @code
    //!     x * y == 0  implies  y * x == 0
    //! @endcode
    //! To be rigorous, we should then distinguish between elements that are
    //! zero divisors when multiplied to the right and to the left.
    //! Fortunately, the concept of an `IntegralDomain` requires the Ring
    //! multiplication to be commutative. Hence,
    //! @code
    //!     x * y == y * x
    //! @endcode
    //! and we do not have to distinguish between left and right zero divisors.
    //!
    //! Typical examples of `IntegralDomain`s include integers, integers
    //! modulo a prime number and polynomials over a field. The method
    //! names used here refer to the integral domain of integers under
    //! the usual addition and multiplication.
    //!
    //!
    //! Superclasses
    //! ------------
    //! `Monoid`, `Group`, `Ring`
    //!
    //!
    //! Laws
    //! ----
    //! To simplify the reading, we will use the `+`, `*`, `/` and `%`
    //! operators with infix notation to denote the application of the
    //! corresponding methods in Monoid, Group, Ring and IntegralDomain.
    //! For all objects `a`, `b` and `k` of an `IntegralDomain` `D`, the
    //! following laws must be satisfied:
    //! @code
    //!     a * b == b * a // commutativity
    //!     (a / b) * b + a % b == a    if b is non-zero
    //!     zero<D>() % b == zero<D>()  if b is non-zero
    //! @endcode
    //!
    //!
    //! Minimal complete definitions
    //! ----------------------------
    //! `quot` and `rem` satisfying the above laws
    //!
    //!
    //! Provided models
    //! ---------------
    //! 1. For non-boolean integral data types\n
    //! A data type `T` is integral if `std::is_integral<T>::%value` is true.
    //! For a non-boolean integral data type `T`, a model of `IntegralDomain`
    //! is automatically defined by using the `Ring` model provided for
    //! arithmetic data types and setting
    //! @code
    //!     quot(x, y) = (x / y)
    //!     rem(x, y)  = (x % y)
    //! @endcode
    //!
    //! @note
    //! The rationale for not providing an IntegralDomain model for `bool` is
    //! the same as for not providing Monoid, Group and Ring models.
    //!
    //!
    //! Operators
    //! ---------
    //! For convenience, the following operators are provided as an
    //! equivalent way of calling the corresponding method:
    //! @code
    //!     /  ->  quot
    //!     %  ->  rem
    //! @endcode
    //!
    //!
    //! @todo
    //! Implement other utility methods like `divides`, `even` and `odd`.
    //! Also ponder adding `div_mod`, which might allow optimizations.
    //!
    //! @todo
    //! IntegralDomain is the wrong name for this concept. In abstract algebra,
    //! an IntegralDomain is not enough to provide `quot` and `rem`, I believe.
    //!
    //! [1]: http://en.wikipedia.org/wiki/Integral_domain
    struct IntegralDomain { };

    //! Generalized integer quotient.
    //! @relates IntegralDomain
    //!
    //!
    //! Cross-type version of the method
    //! --------------------------------
    //! The `quot` method is "overloaded" to handle distinct data types
    //! with certain properties. Specifically, `quot` is defined for
    //! _distinct_ data types `A` and `B` such that
    //! 1. `A` and `B` share a common data type `C`, as determined by the
    //!    `common` metafunction
    //! 2. `A`, `B` and `C` are all `IntegralDomain`s when taken individually
    //! 3. `to<C> : A -> B` and `to<C> : B -> C` are `Ring`-embeddings, as
    //!    determined by the `is_embedding` metafunction.
    //!
    //! In that case, the `quot` method is defined as
    //! @code
    //!     quot(x, y) = quot(to<C>(x), to<C>(y))
    //! @endcode
    //!
    //!
    //! Example
    //! -------
    //! @snippet example/integral_domain.cpp quot
    //!
    //!
    //! @todo
    //! Consider moving this to a `Field` concept?
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto quot = [](auto&& x, auto&& y) -> decltype(auto) {
        return tag-dispatched;
    };
#else
    template <typename T, typename U, typename = void>
    struct quot_impl;

    struct _quot {
        template <typename X, typename Y>
        constexpr decltype(auto) operator()(X&& x, Y&& y) const {
            return quot_impl<
                typename datatype<X>::type, typename datatype<Y>::type
            >::apply(
                detail::std::forward<X>(x),
                detail::std::forward<Y>(y)
            );
        }
    };

    constexpr _quot quot{};
#endif

    //! Generalized integer remainder.
    //! @relates IntegralDomain
    //!
    //! Given two elements of an IntegralDomain `x` and `y`, with `y`
    //! nonzero, `rem` returns the remainder of the division of `x` by `y`.
    //! In other words, `rem` can be seen as an equivalent to `%`.
    //!
    //! Cross-type version of the method
    //! --------------------------------
    //! The `rem` method is "overloaded" to handle distinct data types
    //! with certain properties. Specifically, `rem` is defined for
    //! _distinct_ data types `A` and `B` such that
    //! 1. `A` and `B` share a common data type `C`, as determined by the
    //!    `common` metafunction
    //! 2. `A`, `B` and `C` are all `IntegralDomain`s when taken individually
    //! 3. `to<C> : A -> B` and `to<C> : B -> C` are `Ring`-embeddings, as
    //!    determined by the `is_embedding` metafunction.
    //!
    //! In that case, `rem` is defined as
    //! @code
    //!     rem(x, y) = rem(to<C>(x), to<C>(y))
    //! @endcode
    //!
    //!
    //! Example
    //! -------
    //! @snippet example/integral_domain.cpp rem
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto rem = [](auto&& x, auto&& y) -> decltype(auto) {
        return tag-dispatched;
    };
#else
    template <typename T, typename U, typename = void>
    struct rem_impl;

    struct _rem {
        template <typename X, typename Y>
        constexpr decltype(auto) operator()(X&& x, Y&& y) const {
            return rem_impl<
                typename datatype<X>::type, typename datatype<Y>::type
            >::apply(
                detail::std::forward<X>(x),
                detail::std::forward<Y>(y)
            );
        }
    };

    constexpr _rem rem{};
#endif

    template <>
    struct operators::of<IntegralDomain>
        : decltype(quot), decltype(rem)
    { };
}} // end namespace boost::hana

#endif // !BOOST_HANA_FWD_INTEGRAL_DOMAIN_HPP


/*
MIT License

Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland

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 <gtest/gtest.h>

#include <Eigen/Geometry>

using namespace Eigen;

TEST(TfTest, Rotation) {
  // 90deg yaw.
  Quaterniond q_WB;
  q_WB = AngleAxisd(90 * M_PI / 180.0, Vector3d::UnitZ());
  // 3 m/s**2 acceleration in x direction in body frame.
  Vector3d a_B = Vector3d(3.0, 0.0, 0.0);
  // Expected acceleration in world frame is 3 m/s**2 in y direction.
  Vector3d a_W = Vector3d(0.0, 3.0, 0.0);
  EXPECT_TRUE(a_W.isApprox(q_WB.toRotationMatrix() * a_B, 1.0e-9));
}

int main(int argc, char** argv) {
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}