#include <vector>
#include <armadillo>
#include <cstdio>
#include <cmath>
#include <sys/time.h>
#include <SDL2/SDL.h>
#include <signal.h>
#include <ctime>
#include <iostream>
#include "lidar.h"
#include "gridmap.h"

static int stop_signal;
static FILE *testdata;
static int testcounter;

#define WINDOW_SIZE 500

void stopTest(int signo) {
  stop_signal = 1;
}

void nsleep(double seconds) {
  struct timespec sleeptime;
  sleeptime.tv_sec = (int)floor(seconds);
  sleeptime.tv_nsec = (int)floor((seconds - (double)sleeptime.tv_sec) * 1000000000);
  nanosleep(&sleeptime, NULL);
}

std::vector<polar_t> test_readPoints(void) {
  char *line = NULL;
  size_t n;
  std::vector<polar_t> points;
  polar_t pt;
  ssize_t status;
  if (!testdata) {
    testdata = fopen("lidardata.txt", "r");
    getline(&line, &n , testdata);
  }
  while ((status = getline(&line, &n, testdata)) != -1) {
    if (strstr(line, "segment")) {
      printf("%s", line);
      free(line);
      break;
    }
    sscanf(line, "%f %f\n", &pt.radius, &pt.theta);
    points.push_back(pt);
    free(line);
    line = NULL;
  }
  if (status == -1) {
    if (testdata) {
      fclose(testdata);
      testdata = NULL;
      testcounter = -1;
    }
  }
  return points;
}

int main(int argc, char *argv[]) {

  signal(SIGINT, stopTest);

  if (argc != 4) {
    fprintf(stderr, "usage: %s [folder name] [x] [y]\n", argv[0]);
    return 1;
  }

  // get the lidar device
  Lidar lidar;
  if (!lidar.connected()) {
    fprintf(stderr, "Error occurred in connecting to the lidar\n");
    return 1;
  }

  // create a gridmap
  //printf("Initializing the grid map\n");
  gridmap_t map;
  gridmap_create(&map);
//  map.load(argv[1]);

  // create a window to display the graph
  printf("Initializing the SDL layer\n");
  if (SDL_Init(SDL_INIT_VIDEO) != 0) {
    fprintf(stderr, "Error occurred in initializing the video: %s\n", SDL_GetError());
    return 1;
  }
  SDL_Window *window = SDL_CreateWindow(
      "Minimap",
      SDL_WINDOWPOS_CENTERED,
      SDL_WINDOWPOS_CENTERED,
      WINDOW_SIZE, WINDOW_SIZE, SDL_WINDOW_SHOWN);
  if (!window) {
    fprintf(stderr, "Error occurred in creating the window: %s\n", SDL_GetError());
    SDL_Quit();
    return 1;
  }
  // get the renderer from the GPU
  SDL_Renderer *renderer = SDL_CreateRenderer(window, -1,
      SDL_RENDERER_ACCELERATED | SDL_RENDERER_PRESENTVSYNC);
  if (!renderer) {
    fprintf(stderr, "Error occurred in creating the renderer: %s\n", SDL_GetError());
    SDL_DestroyWindow(window);
    SDL_Quit();
    return 1;
  }
  // create an image for the minimap
  SDL_Surface *screen = SDL_CreateRGBSurface(0, WINDOW_SIZE, WINDOW_SIZE, 32, 0, 0, 0, 0);
  if (!screen) {
    fprintf(stderr, "Error occurred in creating a surface for the minimap: %s\n", SDL_GetError());
    SDL_DestroyRenderer(renderer);
    SDL_DestroyWindow(window);
    SDL_Quit();
    return 1;
  }
  SDL_Texture *texture = SDL_CreateTextureFromSurface(renderer, screen);
  if (!texture) {
    fprintf(stderr, "Error occurred in creating a texture from a surface and renderer: %s\n", SDL_GetError());
    SDL_FreeSurface(screen);
    SDL_DestroyRenderer(renderer);
    SDL_DestroyWindow(window);
    SDL_Quit();
    return 1;
  }

  // take a distribution of the map (noise and everything)
  printf("Displaying sample test\n");
  int x0 = atoi(argv[2]);
  int y0 = atoi(argv[3]);
  bool done = false;
  std::vector<arma::vec> points;
  uint8_t framebuffer[WINDOW_SIZE * WINDOW_SIZE];
  struct timeval lasttime;
  gettimeofday(&lasttime, NULL);
  while (!stop_signal) {
    // detect for exiting the window
    SDL_Event e;
    while (SDL_PollEvent(&e)) {
      if (e.type == SDL_QUIT) {
        done = true;
        break;
      }
    }
    if (done) {
      break;
    }
    // place read points onto grid map
    std::vector<polar_t> points = lidar.readPoints();
    //std::vector<polar_t> points = test_readPoints();
    printf("Got lidar data\n");
    for (polar_t pt : points) {
      float x = pt.radius * cos(pt.theta) + (float)x0;
      float y = pt.radius * sin(pt.theta) + (float)y0;
      gridmap_set(&map, x, y, 255);
    }
    // see if we can update the minimap
    /*struct timeval currtime;
    gettimeofday(&currtime, NULL);
    if ((currtime.tv_usec - lasttime.tv_usec) * 1000000 +
        (currtime.tv_sec - lasttime.tv_sec) < 100000) { // 10 fps
      continue;
    }
    memcpy(&lasttime, &currtime, sizeof(struct timeval));*/
    // grab and display the minimap
    gridmap_query(&map, x0, y0, M_PI, framebuffer, WINDOW_SIZE, 1);
    SDL_FillRect(screen, NULL, SDL_MapRGB(screen->format, 0, 0, 0));
    for (int i = 0; i < WINDOW_SIZE; i++) {
      for (int j = 0; j < WINDOW_SIZE; j++) {
        uint8_t colorvalue = framebuffer[i * WINDOW_SIZE + j];
        uint32_t color = SDL_MapRGB(screen->format, 0, colorvalue, colorvalue);
        ((uint32_t *)screen->pixels)[i * WINDOW_SIZE + j] = color;
      }
    }
    ((uint32_t *)screen->pixels)[WINDOW_SIZE / 2 * (WINDOW_SIZE + 1)] =
      SDL_MapRGB(screen->format, 255, 255, 255);
    SDL_UpdateTexture(texture, NULL, screen->pixels, screen->pitch);
    // clear the renderer in the GPU
    SDL_RenderClear(renderer);
    // draw the texture
    SDL_RenderCopy(renderer, texture, NULL, NULL);
    // update the screen
    SDL_RenderPresent(renderer);
    SDL_Delay(100);
  }

  // store the map
  //printf("Storing the data\n");
  //map.store(argv[1]);

  printf("Clearing everything up\n");
  SDL_DestroyTexture(texture);
  SDL_FreeSurface(screen);
  SDL_DestroyRenderer(renderer);
  SDL_DestroyWindow(window);
  SDL_Quit();
  gridmap_destroy(&map);
  return 0;
}


#pragma once

#include <cassert>
#include <cstdint>
#include <limits>
#include <type_traits>

#include <boost/endian/conversion.hpp>

#include <rocksdb/db.h>

#include "TypeHelpers.hpp"

namespace perq {
template <typename TKey, typename TPrefix>
class PrefixedNumericalKeyConverter {

  static_assert(sizeof(TKey) > sizeof(TPrefix),
                "Key size must be greater than the prefix size");
  static_assert(std::is_unsigned<TKey>() && std::is_unsigned<TPrefix>(),
                "Key and prefix types must be unsigned");

public:
  constexpr PrefixedNumericalKeyConverter(TPrefix prefix)
    : _keyTemplate(static_cast<TKey>(prefix) << (sizeof(TKey) - sizeof(TPrefix)) * 8) {}

  constexpr TKey ToKey(TKey id) const {
    id = _keyTemplate | (GetMaxId() & id);
    return boost::endian::native_to_big(id);
  }

  constexpr TKey ToId(rocksdb::Slice const& slice) const {
    assert(slice.size() == sizeof(TKey));
    return ToId(*reinterpret_cast<TKey const*>(slice.data()));
  }

  constexpr TKey ToId(TKey key) const {
    return boost::endian::big_to_native(key) & GetMaxId();
  }

  static constexpr TKey GetMaxId() {
    return static_cast<TKey>(
      ~(~typename std::conditional<sizeof(TKey) < 4, size_t, TKey>::type(0)
        << (sizeof(TKey) - sizeof(TPrefix)) * 8));
  }

private:
  TKey _keyTemplate;
};

template <typename TKey>
class PrefixedNumericalKeyConverter<TKey, NoPrefix> {

  static_assert(std::is_unsigned<TKey>(), "Key type must be unsigned");

public:
  constexpr PrefixedNumericalKeyConverter(char) {}

  constexpr TKey ToKey(TKey id) const { return boost::endian::native_to_big(id); }

  constexpr TKey ToId(rocksdb::Slice const& slice) const {
    assert(slice.size() == sizeof(TKey));
    return ToId(*reinterpret_cast<TKey const*>(slice.data()));
  }

  constexpr TKey ToId(TKey key) const { return boost::endian::big_to_native(key); }

  static constexpr TKey GetMaxId() {
    return static_cast<TKey>(
      ~typename std::conditional<sizeof(TKey) < 4, size_t, TKey>::type(0));
  }
};
}


#include <boost/test/unit_test.hpp>
#include "functions/full_function_defs.hh"
#include "functions/variables.hh"
#include "functions/std_functions.hh"

struct ReturnTwo : manifolds::Function<manifolds::int_<1000000>, 1, 2> {
  auto operator()() const { return manifolds::make_my_tuple(1, 2); }
};

std::ostream &operator<<(std::ostream &s, ReturnTwo) { return s << "{1,2}"; }

BOOST_AUTO_TEST_CASE(full_function_test) {
  using namespace manifolds;
  auto f = Sin()(x);

  BOOST_CHECK_EQUAL(f(3, 4, 5), std::sin(3));

  // Testing automatic tuple-unpacking
  ReturnTwo rt;
  auto one = x(rt);
  auto two = y(rt);

  BOOST_CHECK_EQUAL(one(), 1);
  BOOST_CHECK_EQUAL(two(), 2);
}


// Copyright Louis Dionne 2015
// Distributed under the Boost Software License, Version 1.0.

#include <boost/fusion/adapted/mpl.hpp>
#include <boost/fusion/include/as_vector.hpp>
#include <boost/fusion/include/at.hpp>

#include <boost/mpl/apply.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/quote.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/sort.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/zip_view.hpp>
namespace fusion = boost::fusion;
using namespace boost::mpl;

// sample(indexed_sort-then-impl)
template <typename Types, typename Predicate = quote2<less>>
struct indexed_sort {
  using indexed_types = typename copy<
    zip_view<vector<Types, range_c<long, 0l, size<Types>::value>>>,
    back_inserter<vector<>>
  >::type;

  using sorted = typename sort<
    indexed_types,
    apply2<typename lambda<Predicate>::type, front<_1>, front<_2>>
  >::type;

  using type = pair<
    typename transform<sorted, front<_1>>::type,
    typename transform<sorted, back<_1>>::type
  >;
};
// end-sample

int main() {
// sample(indexed_sort-then-usage1)
using Types   = vector<int[3], int[2], int[1]>;
using Indexed = indexed_sort<Types, less<sizeof_<_1>,
                                         sizeof_<_2>>>::type;

using Sorted  = Indexed::first;
using Indices = Indexed::second;

static_assert(equal<Sorted, vector<int[1], int[2], int[3]>>::value, "");

static_assert(equal<Indices, vector_c<int, 2, 1, 0>,
  // mpl::equal does a shallow comparison without this
  quote2<equal_to>
>::value, "");
// end-sample

// sample(indexed_sort-then-usage2)
using Sequence = fusion::result_of::as_vector<Sorted>::type;
using IndexMap = indexed_sort<Indices>::type::second;

Sequence s;
int (&a)[3] = fusion::at_c<at_c<IndexMap, 0>::type::value>(s);
int (&b)[2] = fusion::at_c<at_c<IndexMap, 1>::type::value>(s);
int (&c)[1] = fusion::at_c<at_c<IndexMap, 2>::type::value>(s);
// end-sample

(void)a;
(void)b;
(void)c;
}


//Copyright (c) 2014 - 2020, The Trustees of Indiana University.
//
//Licensed under the Apache License, Version 2.0 (the "License");
//you may not use this file except in compliance with the License.
//You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
//Unless required by applicable law or agreed to in writing, software
//distributed under the License is distributed on an "AS IS" BASIS,
//WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//See the License for the specific language governing permissions and
//limitations under the License.

#ifndef TOPPIC_MS_FEATURE_SAVITZKY_GOLAY_HPP_
#define TOPPIC_MS_FEATURE_SAVITZKY_GOLAY_HPP_

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace toppic {

class SavitzkyGolay {
 public:
  SavitzkyGolay(int point_num, int poly_order);

  std::vector<double> smooth(std::vector<double> &values);

 private:
  int point_num_;
  boost::numeric::ublas::matrix<double> coef_mat_;
};

typedef std::shared_ptr<SavitzkyGolay> SavitzkyGolayPtr;

}

#endif


/****************************************************************************
 * Copyright (c) 2021 by the Picasso authors                                *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the Picasso library. Picasso is distributed under a *
 * BSD 3-clause license. For the licensing terms see the LICENSE file in    *
 * the top-level directory.                                                 *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#ifndef PICASSO_LEVELSET_HPP
#define PICASSO_LEVELSET_HPP

#include <Picasso_FieldManager.hpp>
#include <Picasso_LevelSetRedistance.hpp>
#include <Picasso_Types.hpp>

#include <Cajita.hpp>

#include <boost/property_tree/ptree.hpp>

#include <cfloat>
#include <cmath>

//---------------------------------------------------------------------------//
namespace Picasso
{
//---------------------------------------------------------------------------//
// Level set. Composes a signed distance function.
template <class MeshType, class SignedDistanceLocation>
class LevelSet
{
  public:
    using mesh_type = MeshType;
    using memory_space = typename mesh_type::memory_space;
    using location_type = SignedDistanceLocation;
    using entity_type = typename location_type::entity_type;
    using array_type = Cajita::Array<double, entity_type,
                                     Cajita::UniformMesh<double>, memory_space>;
    using halo_type = Cajita::Halo<memory_space>;

    /*!
      \brief Construct the level set over the given mesh.
      \param ptree Level set settings.
      \param mesh The mesh over which to build the signed distance function.
      \param The particle color over which to build the level set. Use -1 if
      the level set is to be built over all particles.
    */
    LevelSet( const boost::property_tree::ptree& ptree,
              const std::shared_ptr<MeshType>& mesh )
        : _mesh( mesh )
    {
        // Create array data.
        _distance_estimate =
            createArray( *_mesh, location_type(), Field::DistanceEstimate() );
        _signed_distance =
            createArray( *_mesh, location_type(), Field::SignedDistance() );
        _halo = Cajita::createHalo<double, memory_space>(
            *( _signed_distance->layout() ), Cajita::FullHaloPattern() );

        // Cell size.
        _dx = _mesh->localGrid()->globalGrid().globalMesh().cellSize( 0 );

        // Extract parameters.
        const auto& params = ptree.get_child( "level_set" );

        // Get the Hopf-Lax redistancing parameters.
        _redistance_secant_tol =
            params.get<double>( "redistance_secant_tol", 0.25 );
        _redistance_max_secant_iter =
            params.get<int>( "redistance_max_secant_iter", 10 );
        _redistance_num_random_guess =
            params.get<int>( "redistance_num_random_guess", 5 );
        _redistance_projection_tol =
            params.get<double>( "redistance_projection_tol", 1.0e-4 );
        _redistance_max_projection_iter =
            params.get<int>( "redistance_max_projection_iter", 200 );
    }

    /*!
      \brief Redistance the signed distance function. The distance estimate on
      the fine grid local entities must have been computed before this
      function is called.
      \param exec_space The execution space to use for parallel kernels.
    */
    template <class ExecutionSpace>
    void redistance( const ExecutionSpace& exec_space )
    {
        // Local mesh.
        auto local_mesh =
            Cajita::createLocalMesh<memory_space>( *( _mesh->localGrid() ) );

        // Views.
        auto estimate_view = _distance_estimate->view();
        auto distance_view = _signed_distance->view();

        // Local-to-global indexer.
        auto l2g = Cajita::IndexConversion::createL2G( *( _mesh->localGrid() ),
                                                       entity_type() );

        // Gather to get updated ghost values.
        _halo->gather( exec_space, *_distance_estimate );

        // Redistance on the coarse grid.
        auto own_entities = _mesh->localGrid()->indexSpace(
            Cajita::Own(), entity_type(), Cajita::Local() );
        Kokkos::parallel_for(
            "Picasso::LevelSet::RedistanceCoarse",
            Cajita::createExecutionPolicy( own_entities, exec_space ),
            KOKKOS_LAMBDA( const int i, const int j, const int k ) {
                // Get the global id of the entity.
                int gi, gj, gk;
                l2g( i, j, k, gi, gj, gk );

                // Only redistance on even-numbered entities. This effectively
                // generates a coarse grid where 2x2x2 blocks of cells on the
                // fine grid are combined.
                if ( !( gi % 2 ) && !( gj % 2 ) && !( gk % 2 ) )
                {
                    int entity_index[3] = { i, j, k };
                    distance_view( i, j, k, 0 ) =
                        LevelSetRedistance::redistanceEntity(
                            entity_type(), estimate_view, local_mesh,
                            entity_index, _redistance_secant_tol,
                            _redistance_max_secant_iter,
                            _redistance_num_random_guess,
                            _redistance_projection_tol,
                            _redistance_max_projection_iter );
                }
            } );

        // Gather the coarse grid estimate to get updated ghost values.
        _halo->gather( exec_space, *_signed_distance );

        // Interpolate from coarse grid to fine grid.
        Kokkos::parallel_for(
            "Picasso::LevelSet::RedistanceInterpolate",
            Cajita::createExecutionPolicy( own_entities, exec_space ),
            KOKKOS_LAMBDA( const int i, const int j, const int k ) {
                // Get the global id of the entity.
                int gi, gj, gk;
                l2g( i, j, k, gi, gj, gk );

                // Interpolate even-numbered entities to the other entities.
                // All odd case
                if ( ( gi % 2 ) && ( gj % 2 ) && ( gk % 2 ) )
                {
                    estimate_view( i, j, k, 0 ) =
                        0.125 * ( distance_view( i - 1, j - 1, k - 1, 0 ) +
                                  distance_view( i - 1, j - 1, k + 1, 0 ) +
                                  distance_view( i - 1, j + 1, k - 1, 0 ) +
                                  distance_view( i - 1, j + 1, k + 1, 0 ) +
                                  distance_view( i + 1, j - 1, k - 1, 0 ) +
                                  distance_view( i + 1, j - 1, k + 1, 0 ) +
                                  distance_view( i + 1, j + 1, k - 1, 0 ) +
                                  distance_view( i + 1, j + 1, k + 1, 0 ) );
                }

                // Even i case
                else if ( !( gi % 2 ) && ( gj % 2 ) && ( gk % 2 ) )
                {
                    estimate_view( i, j, k, 0 ) =
                        0.25 * ( distance_view( i, j - 1, k - 1, 0 ) +
                                 distance_view( i, j - 1, k + 1, 0 ) +
                                 distance_view( i, j + 1, k - 1, 0 ) +
                                 distance_view( i, j + 1, k + 1, 0 ) );
                }

                // Even j case
                else if ( ( gi % 2 ) && !( gj % 2 ) && ( gk % 2 ) )
                {
                    estimate_view( i, j, k, 0 ) =
                        0.25 * ( distance_view( i - 1, j, k - 1, 0 ) +
                                 distance_view( i - 1, j, k + 1, 0 ) +
                                 distance_view( i + 1, j, k - 1, 0 ) +
                                 distance_view( i + 1, j, k + 1, 0 ) );
                }

                // Even k case.
                else if ( ( gi % 2 ) && ( gj % 2 ) && !( gk % 2 ) )
                {
                    estimate_view( i, j, k, 0 ) =
                        0.25 * ( distance_view( i - 1, j - 1, k, 0 ) +
                                 distance_view( i - 1, j + 1, k, 0 ) +
                                 distance_view( i + 1, j - 1, k, 0 ) +
                                 distance_view( i + 1, j + 1, k, 0 ) );
                }

                // Even ij
                else if ( !( gi % 2 ) && !( gj % 2 ) && ( gk % 2 ) )
                {
                    estimate_view( i, j, k, 0 ) =
                        0.5 * ( distance_view( i, j, k - 1, 0 ) +
                                distance_view( i, j, k + 1, 0 ) );
                }

                // Even ik
                else if ( !( gi % 2 ) && ( gj % 2 ) && !( gk % 2 ) )
                {
                    estimate_view( i, j, k, 0 ) =
                        0.5 * ( distance_view( i, j - 1, k, 0 ) +
                                distance_view( i, j + 1, k, 0 ) );
                }

                // Even jk
                else if ( ( gi % 2 ) && !( gj % 2 ) && !( gk % 2 ) )
                {
                    estimate_view( i, j, k, 0 ) =
                        0.5 * ( distance_view( i - 1, j, k, 0 ) +
                                distance_view( i + 1, j, k, 0 ) );
                }

                // Even-numbered entities don't change.
                else
                {
                    estimate_view( i, j, k, 0 ) = distance_view( i, j, k, 0 );
                }
            } );

        // Gather to get updated ghost values.
        _halo->gather( exec_space, *_distance_estimate );

        // Narrow-band redistance on the fine grid. Only redistance within the
        // threshold tolerance. We can't resolve the level set any further
        // than the width of the halo.
        auto threshold = _dx * _mesh->localGrid()->haloCellWidth();
        Kokkos::parallel_for(
            "Picasso::LevelSet::RedistanceFine",
            Cajita::createExecutionPolicy( own_entities, exec_space ),
            KOKKOS_LAMBDA( const int i, const int j, const int k ) {
                // Only redistance on the fine grid if the estimate is less
                // than the threshold distance.
                if ( fabs( estimate_view( i, j, k, 0 ) ) < threshold )
                {
                    int entity_index[3] = { i, j, k };
                    distance_view( i, j, k, 0 ) =
                        LevelSetRedistance::redistanceEntity(
                            entity_type(), estimate_view, local_mesh,
                            entity_index, _redistance_secant_tol,
                            _redistance_max_secant_iter,
                            _redistance_num_random_guess,
                            _redistance_projection_tol,
                            _redistance_max_projection_iter );
                }

                // Otherwise just assign the distance to be our estimate.
                else
                {
                    distance_view( i, j, k, 0 ) = estimate_view( i, j, k, 0 );
                }
            } );
    }

    // Get the signed distance estimate.
    std::shared_ptr<array_type> getDistanceEstimate() const
    {
        return _distance_estimate;
    }

    // Get the redistanced signed distance function.
    std::shared_ptr<array_type> getSignedDistance() const
    {
        return _signed_distance;
    }

    // Get the halo for the signed distance arrays.
    std::shared_ptr<halo_type> getHalo() const { return _halo; }

  private:
    std::shared_ptr<MeshType> _mesh;
    std::shared_ptr<array_type> _distance_estimate;
    std::shared_ptr<array_type> _signed_distance;
    std::shared_ptr<halo_type> _halo;
    double _dx;
    double _redistance_secant_tol;
    int _redistance_max_secant_iter;
    int _redistance_num_random_guess;
    double _redistance_projection_tol;
    int _redistance_max_projection_iter;
};

//---------------------------------------------------------------------------//
/*!
  \brief Create a level set over particles of the given color.
  \param ptree Level set settings.
  \param mesh The mesh over which to build the signed distance function.
*/
template <class SignedDistanceLocation, class MeshType>
std::shared_ptr<LevelSet<MeshType, SignedDistanceLocation>>
createLevelSet( const boost::property_tree::ptree& ptree,
                const std::shared_ptr<MeshType>& mesh )
{
    return std::make_shared<LevelSet<MeshType, SignedDistanceLocation>>( ptree,
                                                                         mesh );
}

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

} // end namespace Picasso

#endif // end PICASSO_LEVELSET_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

#ifndef BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_EQUALS_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_EQUALS_HPP

#include <boost/geometry/algorithms/equals.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace equals
{

template
<
    std::size_t Dimension,
    std::size_t DimensionCount
>
struct nsphere_nsphere
{
    template <typename S1, typename S2>
    static inline bool apply(S1 const& s1, S2 const& s2)
    {
        if ( !geometry::math::equals(get<Dimension>(s1), get<Dimension>(s2)) )
        {
            return false;
        }
        return nsphere_nsphere<Dimension + 1, DimensionCount>::apply(s1, s2);
    }
};

template <std::size_t DimensionCount>
struct nsphere_nsphere<DimensionCount, DimensionCount>
{
    template <typename S1, typename S2>
    static inline bool apply(S1 const& , S2 const& )
    {
        return true;
    }
};

}} // namespace detail::equals
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename NSphere1, typename NSphere2, std::size_t DimensionCount, bool Reverse>
struct equals<NSphere1, NSphere2, nsphere_tag, nsphere_tag, DimensionCount, Reverse>
{
    template <typename S1, typename S2>
    static inline bool apply(S1 const& s1, S2 const& s2)
    {
        return detail::equals::nsphere_nsphere<0, DimensionCount>::apply(s1, s2)
            && geometry::math::equals(get_radius<0>(s1), get_radius<0>(s2));
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_EQUALS_HPP


#include "SupportGraphs.hpp"

#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/property_map/function_property_map.hpp>

#include <xtensor/xvectorize.hpp>
#include <xtensor/xview.hpp>

tsplp::graph::PiSigmaSupportGraph::PiSigmaSupportGraph(const xt::xtensor<Variable, 3>& variables,
    const DependencyGraph& dependencies, const Model& model)
    : m_graph(variables.shape(1)), m_variables(variables), m_dependencies(dependencies), m_model(model)
{
    const auto N = variables.shape(1);
    assert(variables.shape(2) == N);

    for (size_t u = 0; u < N; ++u)
    {
        for (size_t v = 0; v < N; ++v)
        {
            if (u != v)
                add_edge(u, v, m_graph);
        }
    }

    assert(num_vertices(m_graph) == N);
    assert(num_edges(m_graph) == N * N - N);
}

std::pair<double, std::vector<std::pair<tsplp::graph::PiSigmaVertex, tsplp::graph::PiSigmaVertex>>>
tsplp::graph::PiSigmaSupportGraph::FindMinCut(PiSigmaVertex s, PiSigmaVertex t, ConstraintType x)
{
    struct Filter // cannot be a lambda because the filter must be default constructible
    {
        const DependencyGraph* d;
        ConstraintType x;
        PiSigmaVertex s;
        PiSigmaVertex t;

        bool operator()(PiSigmaVertex v) const
        {
            switch (x)
            {
            case ConstraintType::Pi: return !d->HasArc(v, s);
            case ConstraintType::Sigma: return !d->HasArc(t, v);
            case ConstraintType::PiSigma: return !d->HasArc(v, s) && !d->HasArc(t, v);
            }
            return true;
        }
    };

    auto filteredSupportGraph = make_filtered_graph(m_graph, boost::keep_all{}, Filter{ &m_dependencies, x, s, t });

    const auto getCapacity = [&](const PiSigmaEdge& e)
    {
        // const auto vf = xt::vectorize([](Variable v) { return v.GetObjectiveValue(); });
        // const auto values = xt::view(m_variables, xt::all(), source(e, filteredSupportGraph), target(e, filteredSupportGraph));
        // return xt::sum(vf(values))();

        const auto A = m_variables.shape(0);

        // it turns out that this manual summing is much faster than the outcommented "nicer" version above
        double sum = 0.0;
        for (size_t a = 0; a < A; ++a)
            sum += m_variables(a, source(e, filteredSupportGraph), target(e, filteredSupportGraph)).GetObjectiveValue(m_model);
        return sum;
    };

    const auto getReverseEdge = [&](const PiSigmaEdge& e)
    {
        return edge(target(e, filteredSupportGraph), source(e, filteredSupportGraph), filteredSupportGraph).first;
    };

    const auto cutSize = boost::boykov_kolmogorov_max_flow(filteredSupportGraph,
        boost::make_function_property_map<PiSigmaEdge>(getCapacity),
        get(boost::edge_residual_capacity, filteredSupportGraph),
        boost::make_function_property_map<PiSigmaEdge>(getReverseEdge),
        get(boost::vertex_predecessor, filteredSupportGraph),
        get(boost::vertex_color, filteredSupportGraph),
        get(boost::vertex_distance, filteredSupportGraph),
        get(boost::vertex_index, filteredSupportGraph),
        s, t);

    if (cutSize >= 1.0 - 1.e-10)
        return { cutSize, {} };

    const auto colorMap = get(boost::vertex_color, filteredSupportGraph);

    [[maybe_unused]] const auto black = get(colorMap, s);
    assert(black == boost::black_color);
    assert(get(colorMap, t) != black);

    std::vector<std::pair<PiSigmaVertex, PiSigmaVertex>> result;
    result.reserve(num_edges(filteredSupportGraph));
    for (const auto& edge : boost::make_iterator_range(edges(filteredSupportGraph)))
    {
        const auto u = source(edge, filteredSupportGraph);
        const auto v = target(edge, filteredSupportGraph);
        if (get(colorMap, u) == boost::black_color && get(colorMap, v) != boost::black_color)
            result.emplace_back(u, v);
    }

    return { cutSize, result };
}


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020, Clearpath Robotics
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * 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.
 *   * Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "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
 *  COPYRIGHT HOLDER 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.
 */
#include <fuse_constraints/normal_delta_pose_2d.h>
#include <fuse_constraints/normal_delta_pose_2d_cost_functor.h>

#include <benchmark/benchmark.h>

#include <ceres/autodiff_cost_function.h>
#include <Eigen/Dense>

#include <vector>

class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture
{
public:
  NormalDeltaPose2DBenchmarkFixture()
    : jacobians(num_parameter_blocks)
    , J(num_parameter_blocks)
  {
    for (size_t i = 0; i < num_parameter_blocks; ++i)
    {
      J[i].resize(num_residuals, block_sizes[i]);
      jacobians[i] = J[i].data();
    }
  }

  // Delta and sqrt information matrix
  static const fuse_core::Vector3d delta;
  static const fuse_core::Matrix3d sqrt_information;

  // Parameters
  static const double* parameters[];

  // Residuals
  fuse_core::Vector3d residuals;

  static const std::vector<int32_t>& block_sizes;
  static const size_t num_parameter_blocks;

  static const size_t num_residuals;

  // Jacobians
  std::vector<double*> jacobians;

private:
  // Cost function covariance
  static const double covariance_diagonal[];

  static const fuse_core::Matrix3d covariance;

  // Parameter blocks
  static const double position1[];
  static const double orientation1[];
  static const double position2[];
  static const double orientation2[];

  // Jacobian matrices
  std::vector<fuse_core::MatrixXd> J;
};

// Cost function covariance
const double NormalDeltaPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 };

const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::covariance =
    fuse_core::Vector3d(covariance_diagonal).asDiagonal();

// Parameter blocks
const double NormalDeltaPose2DBenchmarkFixture::position1[] = { 0.0, 1.0 };
const double NormalDeltaPose2DBenchmarkFixture::orientation1[] = {0.5};
const double NormalDeltaPose2DBenchmarkFixture::position2[] = { 2.0, 3.0 };
const double NormalDeltaPose2DBenchmarkFixture::orientation2[] = {1.5};

// Delta and sqrt information matrix
const fuse_core::Vector3d NormalDeltaPose2DBenchmarkFixture::delta{ 1.0, 2.0, 3.0 };
const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::sqrt_information(covariance.inverse().llt().matrixU());

// Parameters
const double* NormalDeltaPose2DBenchmarkFixture::parameters[] = { position1, orientation1, position2, orientation2 };

const std::vector<int32_t>& NormalDeltaPose2DBenchmarkFixture::block_sizes = { 2, 1, 2, 1 };
const size_t NormalDeltaPose2DBenchmarkFixture::num_parameter_blocks = block_sizes.size();

const size_t NormalDeltaPose2DBenchmarkFixture::num_residuals = 3;

BENCHMARK_DEFINE_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2D)(benchmark::State& state)
{
  // Create analytic cost function
  const fuse_constraints::NormalDeltaPose2D cost_function{ sqrt_information.topRows(state.range(0)), delta };

  for (auto _ : state)
  {
    cost_function.Evaluate(parameters, residuals.data(), jacobians.data());
  }
}

BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2D)->DenseRange(1, 3);

BENCHMARK_DEFINE_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)(benchmark::State& state)
{
  // Create cost function using automatic differentiation on the cost functor
  const auto partial_sqrt_information = sqrt_information.topRows(state.range(0));
  const ceres::AutoDiffCostFunction<fuse_constraints::NormalDeltaPose2DCostFunctor, ceres::DYNAMIC, 2, 1, 2, 1>
      cost_function_autodiff(new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, delta),
                             partial_sqrt_information.rows());

  for (auto _ : state)
  {
    cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians.data());
  }
}

BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)->DenseRange(1, 3);

BENCHMARK_MAIN();


/*
Copyright 2013, 2014 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#ifndef MATH_COST_HPP_INCLUDED
#define MATH_COST_HPP_INCLUDED

#include <algorithm>
#include <limits>
#include <iosfwd>
#include <functional>

#include <boost/utility/enable_if.hpp>
#include <boost/functional/hash_fwd.hpp>

#include "magma.hpp"

#include "detail/is_close.hpp"

namespace math {

/**
Semiring that is helpful to minimise a cost.
It is also known as the "tropical" semiring.

\ref times adds costs; \ref plus and \ref choose pick the lowest-cost argument.

This type supports Boost.Hash, if \c boost/functional/hash.hpp is included.

\tparam Type
    Arithmetic type that represents the cost.
    The type needs to be able to represent infinity, so that the additive
    identity can be implemented.
    <c>std::numeric_limits<Type>::has_infinity</c> must be \c true.
*/
template <class Type> class cost;

template <class Type> struct cost_tag;

template <class Type> struct decayed_magma_tag <cost <Type>>
{ typedef cost_tag <Type> type; };

template <class Type> class cost {
private:
    Type value_;

    static_assert (std::numeric_limits <Type>::has_infinity,
        "The additive identity is the infinity, which type 'Type' must have.");
public:
    /**
    Initialise with infinite cost.
    This results in the additive identity, also known as "zero".
    \post <c>this->value() == std::numeric_limits<Type>::infinity()</c>
    */
    cost()
    : value_(std::numeric_limits <Type>::infinity()) {}

    /**
    Initialise with a cost of \a value.
    */
    explicit cost (Type const & value) : value_ (value) {}

    /**
    Return the underlying value.
    */
    Type const & value() const { return value_; }
};

template <class Type> inline std::size_t hash_value (cost <Type> const & c)
{ return boost::hash <Type>() (c.value()); }

namespace detail {

    template <class Type> struct is_cost_tag : boost::mpl::false_ {};
    template <class Type> struct is_cost_tag <cost_tag <Type>>
    : boost::mpl::true_ {};

} // namespace detail

MATH_MAGMA_GENERATE_OPERATORS (detail::is_cost_tag)

namespace operation {

    /* Queries. */

    template <class Type>
        struct is_member <cost_tag <Type>, typename boost::enable_if_c <
            std::numeric_limits <Type>::has_quiet_NaN>::type>
    {
        // Detect NaN.
        bool operator() (cost <Type> const & v) const { return v == v; }
    };

    template <class Type> struct equal <cost_tag <Type>> {
        bool operator() (cost <Type> const & left, cost <Type> const & right)
            const
        { return left.value() == right.value(); }
    };

    template <class Type> struct approximately_equal <cost_tag <Type>> {
        bool operator() (cost <Type> const & left, cost <Type> const & right)
            const
        {
            static Type const tolerance = 1e-5;
            return math::detail::is_close (left.value(), right.value(),
                tolerance);
        }
    };

    template <class Type> struct compare <cost_tag <Type>> {
        bool operator() (cost <Type> const & left, cost <Type> const & right)
            const
        { return left.value() < right.value(); }
    };

    // "choose" and "plus" both select the lowest-cost element.
    template <class Type> struct order <cost_tag <Type>, callable::choose>
    : compare <cost_tag <Type>> {};

    template <class Type> struct order <cost_tag <Type>, callable::plus>
    : compare <cost_tag <Type>> {};

    /* Produce. */

    // Return the not-a-number value, not-a-number, if it is available.
    template <class Type> struct non_member <cost_tag <Type>, typename
        boost::enable_if_c <std::numeric_limits <Type>::has_quiet_NaN>::type>
    {
        cost <Type> operator() () const
        { return cost <Type> (std::numeric_limits <Type>::quiet_NaN()); }
    };

    template <class Type> struct identity <cost_tag <Type>, callable::times>
    { cost <Type> operator() () const { return cost <Type> (0); } };

    template <class Type> struct identity <cost_tag <Type>, callable::choose> {
        cost <Type> operator() () const
        { return cost <Type> (std::numeric_limits <Type>::infinity()); }
    };

    template <class Type> struct identity <cost_tag <Type>, callable::plus>
    : identity <cost_tag <Type>, callable::choose> {};

    // Multiplicative annihilator: forward to additive identity.
    template <class Type> struct annihilator <cost_tag <Type>, callable::times>
    : identity <cost_tag <Type>, callable::plus> {};

    /* Operations. */

    // "choose" and "plus" are implemented implicitly, since "order" is
    // implemented for them.

    template <class Type> struct times <cost_tag <Type>>
    : approximate, associative, commutative
    {
        cost <Type> operator() (
            cost <Type> const & left, cost <Type> const & right) const
        { return cost <Type> (left.value() + right.value()); }
    };

    // Semiring in both directions.
    template <class Type> struct is_semiring <
        cost_tag <Type>, either, callable::times, callable::choose>
    : rime::true_type {};

    template <class Type> struct is_semiring <
        cost_tag <Type>, either, callable::times, callable::plus>
    : rime::true_type {};

    template <class Type> struct divide <cost_tag <Type>, either>
    : approximate
    {
        cost <Type> operator() (
            cost <Type> const & left, cost <Type> const & right) const
        { return cost <Type> (left.value() - right.value()); }
    };

    template <class Type>
        struct invert <cost_tag <Type>, either, callable::times>
    {
        cost <Type> operator() (cost <Type> const & c) const
        { return cost <Type> (-c.value()); }
    };

    template <class Type> struct print <cost_tag <Type>> {
        template <class Stream>
            void operator() (Stream & stream, cost <Type> const & c) const
        { stream << c.value(); }
    };

} // namespace operation

} // namespace math

#endif // MATH_COST_HPP_INCLUDED


/**
 * @example ocean/test/reflect_loss_test.cc
 */
#include <boost/test/unit_test.hpp>
#include <usml/ocean/ocean.h>
#include <fstream>

BOOST_AUTO_TEST_SUITE(reflect_loss_test)

using namespace boost::unit_test;
using namespace usml::ocean;

/**
 * @ingroup ocean_test
 * @{
 */

/**
 * Test the basic features of the reflection loss model using
 * the reflect_loss_constant model.
 * Generate errors if values differ by more that 1E-6 percent.
 */
BOOST_AUTO_TEST_CASE( constant_reflect_loss_test ) {
    cout << "=== reflect_loss_test: constant_reflect_loss_test ===" << endl ;

    // simple values for points and distance

    wposition1 points ;

    // compute reflect_loss

    seq_log freq( 10.0, 10.0, 7 ) ;
    cout << "freq:  " << freq << endl ;
    vector<double> amplitude( freq.size() ) ;

    double value = 3.0 ;
    reflect_loss_constant model( value ) ;
    model.reflect_loss( points, freq, 0.1, &amplitude ) ;
    cout << "amplitude: " << amplitude << endl ;

    // check the answer

    for ( size_t f=0 ; f < freq.size() ; ++f ) {
        BOOST_CHECK_CLOSE( amplitude(f), value, 1e-6 ) ;
    }
}

/**
 * Compare values of the Rayleigh model to test case (a)
 * from of the Rayleigh model in Jensen, Kuperman, Porter,
 * Schmidt, "Computational Ocean Acoustics",
 * Table 1.4 and Figure 1.22, Chapter 1.6.1, pp 46-47.
 * Write results to CSV file for processing in Excel or Matlab.
 */
BOOST_AUTO_TEST_CASE( rayleigh_test_a ) {
    cout << "=== reflect_loss_test: rayleigh_test_a ===" << endl ;
    const char* name = USML_TEST_DIR "/ocean/test/rayleigh_test_a.csv" ;
    std::ofstream os(name) ;
    cout << "writing tables to " << name << endl ;

    // simple values for points and distance

    wposition1 points ;
    points.altitude(-1000.0) ;

    seq_log freq( 10.0, 10.0, 7 ) ;
    cout << "freq:  " << freq << endl ;
    vector<double> amplitude( freq.size() ) ;

    // test case (a) - variations with sediment sound speed

    static double speed[] = { 1550.0, 1600.0, 1800 } ;
    os << "angle,cp=1550,cp=1600,cp=1800" << endl ;
    for ( int angle = 0 ; angle <= 90 ; ++angle ) {
        os << angle ;
        for ( int n=0 ; n < 3 ; ++n ) {
            reflect_loss_rayleigh model( 2.0, speed[n]/1500.0, 0.5 ) ;
            model.reflect_loss(
                points, freq, to_radians(angle), &amplitude ) ;
            os << "," << amplitude(0) ;
        }
        os << endl ;
    }
}

/**
 * Compare values of the Rayleigh model to test case (b)
 * from of the Rayleigh model in Jensen, Kuperman, Porter,
 * Schmidt, "Computational Ocean Acoustics",
 * Table 1.4 and Figure 1.22, Chapter 1.6.1, pp 46-47.
 * Write results to CSV file for processing in Excel or Matlab.
 */
BOOST_AUTO_TEST_CASE( rayleigh_test_b ) {
    cout << "=== reflect_loss_test: rayleigh_test_b ===" << endl ;
    const char* name = USML_TEST_DIR "/ocean/test/rayleigh_test_b.csv" ;
    std::ofstream os(name) ;
    cout << "writing tables to " << name << endl ;

    // simple values for points and distance

    wposition1 points ;
    points.altitude(-1000.0) ;

    seq_log freq( 10.0, 10.0, 7 ) ;
    cout << "freq:  " << freq << endl ;
    vector<double> amplitude( freq.size() ) ;

    // test case (a) - variations with sediment attenuation

    static double atten[] = { 1.0, 0.5, 0.0 } ;
    os << "angle,a=1.0,a=0.5,a=0.0" << endl ;
    for ( int angle = 0 ; angle <= 90 ; ++angle ) {
        os << angle ;
        for ( int n=0 ; n < 3 ; ++n ) {
            reflect_loss_rayleigh model( 2.0, 1600.0/1500.0, atten[n] ) ;
            model.reflect_loss(
                points, freq, to_radians(angle), &amplitude ) ;
            os << "," << amplitude(0) ;
        }
        os << endl ;
    }
}

/**
 * Compare values of the Rayleigh model to test case (c)
 * from of the Rayleigh model in Jensen, Kuperman, Porter,
 * Schmidt, "Computational Ocean Acoustics",
 * Table 1.4 and Figure 1.22, Chapter 1.6.1, pp 46-47.
 * Write results to CSV file for processing in Excel or Matlab.
 */
BOOST_AUTO_TEST_CASE( rayleigh_test_c ) {
    cout << "=== reflect_loss_test: rayleigh_test_c ===" << endl ;
    const char* name = USML_TEST_DIR "/ocean/test/rayleigh_test_c.csv" ;
    std::ofstream os(name) ;
    cout << "writing tables to " << name << endl ;

    // simple values for points and distance

    wposition1 points ;
    points.altitude(-1000.0) ;

    seq_log freq( 10.0, 10.0, 7 ) ;
    cout << "freq:  " << freq << endl ;
    vector<double> amplitude( freq.size() ) ;

    // test case (c) - variations with density

    static double density[] = { 1.5, 2.0, 2.5 } ;
    os << "angle,d=1.5,d=2.0,d=2.5" << endl ;
    for ( int angle = 0 ; angle <= 90 ; ++angle ) {
        os << angle ;
        for ( int n=0 ; n < 3 ; ++n ) {
            reflect_loss_rayleigh model( density[n], 1600.0/1500.0, 0.5 ) ;
            model.reflect_loss(
                points, freq, to_radians(angle), &amplitude ) ;
            os << "," << amplitude(0) ;
        }
        os << endl ;
    }
}

/**
 * Compare values of the Rayleigh model to test case (b)
 * from of the Rayleigh model in Jensen, Kuperman, Porter,
 * Schmidt, "Computational Ocean Acoustics",
 * Table 1.4 and Figure 1.22, Chapter 1.6.1, pp 46-47.
 * Write results to CSV file for processing in Excel or Matlab.
 *
 * Although Table 1.4 indicates an attenuation of 0.5,
 * you can tell from the cs=0 case that an attenuation of 0.0
 * was used to make Figure 1.22.
 */
BOOST_AUTO_TEST_CASE( rayleigh_test_d ) {
    cout << "=== reflect_loss_test: rayleigh_test_d ===" << endl ;
    const char* name = USML_TEST_DIR "/ocean/test/rayleigh_test_d.csv" ;
    std::ofstream os(name) ;
    cout << "writing tables to " << name << endl ;

    // simple values for points and distance

    wposition1 points ;
    points.altitude(-1000.0) ;

    seq_log freq( 10.0, 10.0, 7 ) ;
    cout << "freq:  " << freq << endl ;
    vector<double> amplitude( freq.size() ) ;

    // test case (d) - variations with shear speed

    static double shear[] = { 0.0, 200.0, 400.0, 600.0 } ;
    os << "angle,cs=0.0,cs=200,cs=400,cs=600" << endl ;
    for ( int angle = 0 ; angle <= 90 ; ++angle ) {
        os << angle ;
        for ( int n=0 ; n < 4 ; ++n ) {
            reflect_loss_rayleigh model(
                2.0, 1600.0/1500.0, 0.0, shear[n]/1500.0 ) ;
            model.reflect_loss(
                points, freq, to_radians(angle), &amplitude ) ;
            os << "," << amplitude(0) ;
        }
        os << endl ;
    }
}

/**
 * Compute Rayleigh model values for generic sediments.
 * Write results to CSV file for processing in Excel or Matlab.
 */
BOOST_AUTO_TEST_CASE( plot_rayleigh_sediments ) {
    cout << "=== reflect_loss_test: plot_rayleigh_sediments ===" << endl ;
    const char* name = USML_TEST_DIR "/ocean/test/rayleigh_sediments.csv" ;
    std::ofstream os(name) ;
    cout << "writing tables to " << name << endl ;

    // simple values for points and distance

    wposition1 points ;
    points.altitude(-1000.0) ;

    seq_log freq( 10.0, 10.0, 7 ) ;
    cout << "freq:  " << freq << endl ;
    vector<double> amplitude( freq.size() ) ;

    // test case (a) - variations with sediment sound speed

    static reflect_loss_rayleigh::bottom_type_enum sediment[] = {
        reflect_loss_rayleigh::CLAY,
        reflect_loss_rayleigh::SILT,
        reflect_loss_rayleigh::SAND,
        reflect_loss_rayleigh::GRAVEL,
        reflect_loss_rayleigh::MORAINE,
        reflect_loss_rayleigh::CHALK,
        reflect_loss_rayleigh::LIMESTONE,
        reflect_loss_rayleigh::BASALT
    } ;

    os << "angle,clay,silt,sand,gravel,moraine,chalk,limestone,basalt" << endl ;
    for ( int angle = 0 ; angle <= 90 ; ++angle ) {
        os << angle ;
        for ( int n=0 ; n < 8 ; ++n ) {
            reflect_loss_rayleigh model( sediment[n] ) ;
            model.reflect_loss(
                points, freq, to_radians(angle), &amplitude ) ;
            os << "," << amplitude(0) ;
        }
        os << endl ;
    }
}

/**
 * Test the basic features of the reflection loss model using
 * the netCDF bottom type file.
 * Generate errors if values differ by more that 1E-5 percent.
 */
BOOST_AUTO_TEST_CASE( reflect_loss_netcdf_test ) {
	cout << " === reflection_loss_test: reflection_loss_netcdf bottom type file === " << endl;
	reflect_loss_netcdf netcdf( USML_DATA_DIR "/bottom_province/sediment_test.nc" ) ;

    seq_linear frequency(1000.0, 1000.0, 0.01) ;
    double angle = M_PI_2 ;
    vector<double> amplitude( frequency.size() ) ;

    double limestone = 3.672875 ;
    double sand = 10.166660 ;
    double tolerance = 4e-4 ;

	/** bottom type numbers in the center of the data field top left, right, bottom left, right */
	netcdf.reflect_loss(wposition1(29.5, -83.4), frequency, angle, &amplitude) ;
	BOOST_CHECK_CLOSE(amplitude(0), limestone, tolerance) ;
	netcdf.reflect_loss(wposition1(30.5, -83.4), frequency, angle, &amplitude) ;
	BOOST_CHECK_CLOSE(amplitude(0), sand, tolerance) ;
	netcdf.reflect_loss(wposition1(29.5, -84.2), frequency, angle, &amplitude) ;
	BOOST_CHECK_CLOSE(amplitude(0), sand, tolerance) ;
	netcdf.reflect_loss(wposition1(30.5, -84.2), frequency, angle, &amplitude) ;
	BOOST_CHECK_CLOSE(amplitude(0), limestone, tolerance) ;
	/** bottom type numbers at the corners of the data field top left, bottom left, top right, bottom right */
	netcdf.reflect_loss(wposition1(26, -80), frequency, angle, &amplitude) ;
	BOOST_CHECK_CLOSE(amplitude(0), sand, tolerance) ;
	netcdf.reflect_loss(wposition1(26, -89), frequency, angle, &amplitude) ;
	BOOST_CHECK_CLOSE(amplitude(0), limestone, tolerance) ;
	netcdf.reflect_loss(wposition1(35, -80), frequency, angle, &amplitude) ;
	BOOST_CHECK_CLOSE(amplitude(0), sand, tolerance) ;
	netcdf.reflect_loss(wposition1(35, -89), frequency, angle, &amplitude) ;
	BOOST_CHECK_CLOSE(amplitude(0), limestone, tolerance) ;
}

/**
 * Test the accuracy Pierson and Moskowitz model for computing wave height
 * from wind speed. Compare to significant wave height plot from
 * http://www.wikiwaves.org/Ocean-Wave_Spectra.
 */
BOOST_AUTO_TEST_CASE( wave_height_pierson_test ) {
    cout << "=== reflect_loss_test: wave_height_pierson_test ===" << endl;

    // display plotting data

    const char* csv_name = USML_TEST_DIR "/ocean/test/wave_height_pierson_test.csv" ;
    std::ofstream os(csv_name) ;
    cout << "writing tables to " << csv_name << endl ;
	os << "wind,Hsig" << endl ;
    for ( double wind=0.0 ; wind <= 25.0 ; wind += 1.0 ) {
    	os << wind << "," << 4*wave_height_pierson(wind) << endl ;
    }

    // check the answer against key points in plot

    BOOST_CHECK_CLOSE(wave_height_pierson(0.0), 0.0, 1e-6);
    BOOST_CHECK_CLOSE(wave_height_pierson(15.0), 5.0/4.0, 5.0 );
    BOOST_CHECK_CLOSE(wave_height_pierson(25.0), 14.0/4.0, 5.0 );
}

/**
 * Compare the results of the reflect_loss_eckart surface reflection loss model
 * to Figures 2-5 in Jones et. al.  Returns error if the values at 10 degrees
 * are different from those that we hand calculated.
 *

 * Figure	Wind Speed	Frequency	Loss at 10 deg
 * ------	----------	---------	--------------
 * 2		4.6 m/s		3150 Hz		1.15 dB
 * 3		4.6 m/s		5000 Hz		2.90 dB
 * 4		14.4 m/s	1000 Hz		11.15 dB
 * 5		14.4 m/s	5000 Hz		278.76 dB
 *

 *
 * @xref Adrian D. Jones, Janice Sendt, Alec J. Duncan, Paul A. Clarke and
 * Amos Maggi, "Modelling the acoustic reflection loss at the rough
 * ocean surface," Proceedings of ACOUSTICS 2009, Australian Acoustical Society,
 * 23-25 November 2009, Adelaide, Australia.
 */
BOOST_AUTO_TEST_CASE( reflect_loss_eckart_test ) {
    cout << "=== reflect_loss_test: reflect_loss_eckart_test ===" << endl;
    const char* name = USML_TEST_DIR "/ocean/test/reflect_loss_eckart_test.csv" ;
    std::ofstream os(name) ;
    cout << "writing tables to " << name << endl ;

    // simple values for points and distance

    wposition1 points ;
    seq_log freqA( 1000.0, 1.0, 1 ) ;
    seq_log freqB( 3150.0, 1.0, 1 ) ;
    seq_log freqC( 5000.0, 1.0, 1 ) ;
    vector<double> amplitude( freqA.size() ) ;

    // variations with wind speed and frequency

    os << "angle,figure2,figure3,figure4,figure5" << endl ;

    reflect_loss_eckart trackQ( 4.6 ) ;
    reflect_loss_eckart trackO( 14.4 ) ;

    for ( double angle = 0.0 ; angle <= 10.0 ; angle += 0.1 ) {
        os << angle ;

        trackQ.reflect_loss( points, freqB, to_radians(angle), &amplitude ) ;
        os << "," << amplitude(0) ;
        if ( angle >= 10.0 ) BOOST_CHECK_CLOSE(amplitude(0), 1.15, 1.0 );

        trackQ.reflect_loss( points, freqC, to_radians(angle), &amplitude ) ;
        os << "," << amplitude(0) ;
        if ( angle >= 10.0 ) BOOST_CHECK_CLOSE(amplitude(0), 2.90, 1.0 );

        trackO.reflect_loss( points, freqA, to_radians(angle), &amplitude ) ;
        os << "," << amplitude(0) ;
        if ( angle >= 10.0 ) BOOST_CHECK_CLOSE(amplitude(0), 11.15, 1.0 );

        trackO.reflect_loss( points, freqC, to_radians(angle), &amplitude ) ;
        os << "," << amplitude(0) << endl ;
        if ( angle >= 10.0 ) BOOST_CHECK_CLOSE(amplitude(0), 278.76, 1.0 );
    }
}

/**
 * Compare the results of the reflect_loss_beckmann surface reflection loss model
 * to Figures 2-5 in Jones et. al.  Returns error if the values at 10 degrees
 * are different from those that we hand calculated.
 *

 * Figure	Wind Speed	Frequency	Loss at 10 deg
 * ------	----------	---------	--------------
 * 2		4.6 m/s		3150 Hz		1.15 dB
 * 3		4.6 m/s		5000 Hz		2.90 dB
 * 4		14.4 m/s	1000 Hz		11.15 dB
 * 5		14.4 m/s	5000 Hz		278.76 dB
 *

 *
 * @xref Adrian D. Jones, Janice Sendt, Alec J. Duncan, Paul A. Clarke and
 * Amos Maggi, "Modelling the acoustic reflection loss at the rough
 * ocean surface," Proceedings of ACOUSTICS 2009, Australian Acoustical Society,
 * 23-25 November 2009, Adelaide, Australia.
 */
BOOST_AUTO_TEST_CASE( reflect_loss_beckmann_test ) {
    cout << "=== reflect_loss_test: reflect_loss_beckmann_test ===" << endl;
    const char* name = USML_TEST_DIR "/ocean/test/reflect_loss_beckmann_test.csv" ;
    std::ofstream os(name) ;
    cout << "writing tables to " << name << endl ;

    // simple values for points and distance

    wposition1 points ;
    seq_log freqA( 1000.0, 1.0, 1 ) ;
    seq_log freqB( 3150.0, 1.0, 1 ) ;
    seq_log freqC( 5000.0, 1.0, 1 ) ;
    vector<double> amplitude( freqA.size() ) ;

    // variations with wind speed and frequency

    os << "angle,figure2,figure3,figure4,figure5" << endl ;

    reflect_loss_beckmann trackQ( 4.6 ) ;
    reflect_loss_beckmann trackO( 14.4 ) ;

    for ( double angle = 0.0 ; angle <= 90.0 ; angle += 1.0 ) {
        os << angle ;

        trackQ.reflect_loss( points, freqB, to_radians(angle), &amplitude ) ;
        os << "," << amplitude(0) ;
//        if ( angle >= 10.0 ) BOOST_CHECK_CLOSE(amplitude(0), 1.15, 1.0 );

        trackQ.reflect_loss( points, freqC, to_radians(angle), &amplitude ) ;
        os << "," << amplitude(0) ;
//        if ( angle >= 10.0 ) BOOST_CHECK_CLOSE(amplitude(0), 2.90, 1.0 );

        trackO.reflect_loss( points, freqA, to_radians(angle), &amplitude ) ;
        os << "," << amplitude(0) ;
//        if ( angle >= 10.0 ) BOOST_CHECK_CLOSE(amplitude(0), 11.15, 1.0 );

        trackO.reflect_loss( points, freqC, to_radians(angle), &amplitude ) ;
        os << "," << amplitude(0) << endl ;
//        if ( angle >= 10.0 ) BOOST_CHECK_CLOSE(amplitude(0), 278.76, 1.0 );
    }
}

/// @}

BOOST_AUTO_TEST_SUITE_END()


#include <cfloat>
#include <boost/format.hpp>
#include <simba_logging.h>
#include "Utils.h"

bool build_even_motion_relative_time_samples(float                 i_relative_shutter_open,
											 float                 i_relative_shutter_close,
											 Alembic::Abc::uint8_t i_motion_sample_count,
											 FloatContainer&       o_earlier_sampling_time_vector,
											 FloatContainer&       o_later_sampling_time_vector)
{
	if (i_motion_sample_count<2)
		return false;
	float shutter_delta = (i_relative_shutter_close - i_relative_shutter_open)/(i_motion_sample_count-1);
	if (shutter_delta < FLT_EPSILON)
		return false;
	o_earlier_sampling_time_vector.clear();
	o_later_sampling_time_vector.clear();
	for (Alembic::Abc::uint8_t sample_index=0;sample_index<i_motion_sample_count;sample_index++)
	{
		float time_sample  = i_relative_shutter_open + sample_index * shutter_delta;
		// std::cout << boost::format("time_sample = %1%") % time_sample << std::endl;
		if (time_sample>=0.0f)
			o_later_sampling_time_vector.push_back(time_sample);
		else
			o_earlier_sampling_time_vector.push_back(time_sample);
	}
	return true;

}

bool build_single_even_motion_relative_time_samples(float                 i_relative_shutter_open,
													float                 i_relative_shutter_close,
													Alembic::Abc::uint8_t i_motion_sample_count,
													FloatContainer&       o_sampling_time_vector)
{
	if (i_motion_sample_count<2)
		return false;
	float shutter_delta = (i_relative_shutter_close - i_relative_shutter_open)/(i_motion_sample_count-1);
	if (shutter_delta < FLT_EPSILON)
		return false;
	o_sampling_time_vector.clear();
	for (Alembic::Abc::uint8_t sample_index=0;sample_index<i_motion_sample_count;sample_index++)
	{
		float time_sample  = i_relative_shutter_open + sample_index * shutter_delta;
		// std::cout << boost::format("time_sample = %1%") % time_sample << std::endl;
		o_sampling_time_vector.push_back(time_sample);
	}
	return true;

}

void build_interim_points(const Alembic::AbcGeom::IPointsSchema::Sample* i_sample,
						  AlembicPointsDataIndexedMap&                   o_interim_points)
{
	Alembic::AbcGeom::P3fArraySamplePtr positions = i_sample->getPositions();
	Alembic::AbcGeom::V3fArraySamplePtr velocities = i_sample->getVelocities();
	Alembic::AbcGeom::UInt64ArraySamplePtr ids = i_sample->getIds();
	size_t num_positions = positions->size();
	size_t num_velocities = velocities->size();
	size_t num_ids = ids->size();
	assert ( num_positions == num_ids );
	assert ( num_velocities == num_ids );
	std::pair<AlembicPointsDataIndexedMap::iterator,bool> ret;
	for (size_t positions_index = 0;positions_index < num_positions; positions_index++)
	{
		ret = o_interim_points.insert(AlembicPointsDataIndexedMap::value_type(
				ids->get()[positions_index],
				AlembicPointsData(Imath::V3f(positions->get()[positions_index].x,positions->get()[positions_index].y,positions->get()[positions_index].z),
						Imath::V3f(velocities->get()[positions_index].x,velocities->get()[positions_index].y,velocities->get()[positions_index].z))));
		if (!ret.second)
		{
			LOG(ERROR) << "Failed to insert into map" << std::endl;
			return;
		}
	}
	// std::cout << boost::format("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX o_interim_points = %1%") % o_interim_points.size() << std::endl;
}

// == Emacs ================
// -------------------------
// Local variables:
// tab-width: 4
// indent-tabs-mode: t
// c-basic-offset: 4
// end:
//
// == vi ===================
// -------------------------
// Format block
// ex:ts=4:sw=4:expandtab
// -------------------------


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 combinatorial toolbox - is_prime/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of combinatorial components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 07/03/2011
///
#include <nt2/toolbox/combinatorial/include/functions/is_prime.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/saturate.hpp>

#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/toolbox/constant/constant.hpp>
#include <nt2/table.hpp>

NT2_TEST_CASE_TPL ( is_prime_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::is_prime;
  using nt2::tag::is_prime_;

  // specific values tests
  NT2_TEST(!is_prime(nt2::Eight<T>()));
  NT2_TEST(is_prime(nt2::Seven<T>()));
  NT2_TEST(is_prime(nt2::Two<T>()));
  NT2_TEST(!is_prime(nt2::One<T>()));
  NT2_TEST_ASSERT(is_prime(nt2::Mone<T>()));
} // end of test for floating_

NT2_TEST_CASE_TPL ( is_prime_integer__1_0,  NT2_INTEGRAL_TYPES)
{

  using nt2::is_prime;
  using nt2::tag::is_prime_;
  NT2_TEST(!is_prime(nt2::Eight<T>()));
  NT2_TEST(is_prime(nt2::Seven<T>()));
  NT2_TEST(is_prime(nt2::Two<T>()));
  NT2_TEST(!is_prime(nt2::One<T>()));
 } // end of test for integer_

NT2_TEST_CASE_TPL ( is_prime_integer__2_0,  NT2_INTEGRAL_TYPES)
{

  using nt2::is_prime;
  using nt2::tag::is_prime_;
  nt2::table<T> p =  nt2::primes(T(8));
  NT2_TEST(!is_prime(nt2::Eight<T>(), p));
  NT2_TEST(is_prime(nt2::Seven<T>(), p));
  NT2_TEST(is_prime(nt2::Two<T>(), p));
  NT2_TEST(!is_prime(nt2::One<T>(), p));
 } // end of test for integer_

NT2_TEST_CASE_TPL ( is_prime_real__2_0,  NT2_REAL_TYPES)
{

  using nt2::is_prime;
  using nt2::tag::is_prime_;
  nt2::table<uint32_t> p =  nt2::primes(uint32_t(8));
  NT2_TEST(!is_prime(nt2::Eight<T>(), p));
  NT2_TEST(is_prime(nt2::Seven<T>(), p));
  NT2_TEST(is_prime(nt2::Two<T>(), p));
  NT2_TEST(!is_prime(nt2::One<T>(), p));
  NT2_TEST_ASSERT(is_prime(nt2::Mone<T>()));
} // end of test for integer_


/******************************************************************************
 * Copyright (C) 2014 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

/** \file ErrorTermVelocitiesTest.cpp
    \brief This file tests the ErrorTermVelocities class.
  */

#include <cmath>

#include <boost/make_shared.hpp>

#include <gtest/gtest.h>

#include <aslam/backend/test/ErrorTermTestHarness.hpp>
#include <aslam/backend/EuclideanPoint.hpp>
#include <aslam/backend/EuclideanExpression.hpp>

#include <aslam/calibration/statistics/NormalDistribution.h>

#include "aslam/calibration/car/error-terms/ErrorTermVelocities.h"

using namespace aslam::backend;
using namespace aslam::calibration;

TEST(AslamCalibrationTestSuite, testErrorTermVelocities) {

  // estimated velocities
  const ErrorTermVelocities::Input v_e((ErrorTermVelocities::Input() <<
    5, 6, 7).finished());
  const ErrorTermVelocities::Input om_e((ErrorTermVelocities::Input() <<
    M_PI / 2.0, M_PI / 4.0, M_PI / 8.0).finished());

  // covariance matrix
  ErrorTermVelocities::Covariance Q = ErrorTermVelocities::Covariance::Zero();
  Q(0, 0) = 1e-4;
  Q(1, 1) = 1e-4;
  Q(2, 2) = 1e-4;

  // measured velocities
  const ErrorTermVelocities::Input v_m(
    NormalDistribution<3>(v_e, Q).getSample());
  const ErrorTermVelocities::Input om_m(
    NormalDistribution<3>(om_e, Q).getSample());

  // encapsulate the estimated velocities into Euclidean expression
  auto r_v_mr = boost::make_shared<EuclideanPoint>(v_e);
  EuclideanExpression r_v_mr_e(r_v_mr);
  auto r_om_mr = boost::make_shared<EuclideanPoint>(om_e);
  EuclideanExpression r_om_mr_e(r_om_mr);

  // build the error term
  ErrorTermVelocities e1(r_v_mr_e, r_om_mr_e, v_m, om_m, Q, Q);

  // test the error term
  try {
    ErrorTermTestHarness<6> harness(&e1);
    harness.testAll();
  }
  catch (const std::exception& e) {
    FAIL() << e.what();
  }
}


//
// Copyright (c) 2016-2017 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// 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)
//
// Official repository: https://github.com/boostorg/beast
//

#ifndef BOOST_BEAST_DETAIL_INTEGER_SEQUENCE_HPP
#define BOOST_BEAST_DETAIL_INTEGER_SEQUENCE_HPP

#include <boost/config.hpp>
#include <cstddef>
#include <type_traits>
#include <utility>

namespace boost {
namespace beast {
namespace detail {

template<class T, T... Ints>
struct integer_sequence
{
    using value_type = T;
    BOOST_STATIC_ASSERT(std::is_integral<T>::value);

    static std::size_t constexpr static_size = sizeof...(Ints);

    static std::size_t constexpr size()
    {
        return sizeof...(Ints);
    }
};

template<std::size_t... Ints>
using index_sequence = integer_sequence<std::size_t, Ints...>;

// This workaround is needed for broken sizeof...
template<class... Args>
struct sizeof_workaround
{
    static std::size_t constexpr size = sizeof... (Args);
};

#ifdef BOOST_MSVC

// This implementation compiles on real MSVC and clang but not gcc

template<class T, unsigned long long N, class Seq>
struct make_integer_sequence_unchecked;

template<class T, unsigned long long N, unsigned long long ...Indices>
struct make_integer_sequence_unchecked<
    T, N, integer_sequence<T, Indices...>>
{
    using type = typename make_integer_sequence_unchecked<
        T, N-1, integer_sequence<T, N-1, Indices...>>::type;
};

template<class T, unsigned long long ...Indices>
struct make_integer_sequence_unchecked<
    T, 0, integer_sequence<T, Indices...>>
{
    using type = integer_sequence<T, Indices...>;
};

template<class T, T N>
struct make_integer_sequence_checked
{
    BOOST_STATIC_ASSERT(std::is_integral<T>::value);
    BOOST_STATIC_ASSERT(N >= 0);

    using type = typename make_integer_sequence_unchecked<
        T, N, integer_sequence<T>>::type;
};

template<class T, T N>
using make_integer_sequence =
    typename make_integer_sequence_checked<T, N>::type;

template<std::size_t N>
using make_index_sequence = make_integer_sequence<std::size_t, N>;

template<class... Args>
using index_sequence_for =
    make_index_sequence<sizeof_workaround<Args...>::size>;

#else

// This implementation compiles on gcc but not MSVC

template<std::size_t... Ints>
struct index_tuple
{
    using next = index_tuple<Ints..., sizeof... (Ints)>;

};

template<std::size_t N>
struct build_index_tuple
{
    using type = typename build_index_tuple<N-1>::type::next;
};

template<>
struct build_index_tuple<0>
{
    using type = index_tuple<>;
};

template<class T, T N,
    class Seq = typename build_index_tuple<N>::type
>
struct integer_sequence_helper;

template<class T, T N, std::size_t... Ints>
struct integer_sequence_helper<T, N, index_tuple<Ints...>>
{
    BOOST_STATIC_ASSERT(std::is_integral<T>::value);
    BOOST_STATIC_ASSERT(N >= 0);

    using type = integer_sequence<T, static_cast<T> (Ints)...>;
};

template<class T, T N>
using make_integer_sequence =
    typename integer_sequence_helper<T, N>::type;

template<std::size_t N>
using make_index_sequence = make_integer_sequence<std::size_t, N>;

template<class... Args>
using index_sequence_for =
    make_index_sequence<sizeof_workaround<Args...>::size>;

#endif

} // detail
} // beast
} // boost

#endif


#ifndef YASMIC_GENERIC_MATRIX_OPERATIONS
#define YASMIC_GENERIC_MATRIX_OPERATIONS

#include <boost/tuple/tuple.hpp>
#include <yasmic/smatrix_traits.hpp>
#include <utility>

namespace yasmic
{
	/**
	 * Simple nonzero class for most of the work.
	 */

	template <class Index, class Value, class NonzeroIndex>
	struct simple_nonzero
	{
		simple_nonzero()
			: _row(0), _column(0), _val(0), _nzi(0)
		{}

		simple_nonzero(Index row, Index col, Value val, NonzeroIndex nzi)
			: _row(row), _column(col), _val(val), _nzi(nzi)
		{}

		Index _row;
		Index _column;
		Value _val;
		NonzeroIndex _nzi;

		bool operator!= (const simple_nonzero& other)
		{
			return (_row != other._row || _column || other._column && _val || other._val && _nzi || other._nzi);
		}

		bool operator== (const simple_nonzero& other)
		{
			return (_row == other._row && _column == other._column && _val == other._val && _nzi == other._nzi);
		}

		typedef Index index_type;
		typedef Value value_type;
		typedef NonzeroIndex nz_index_type;
	};

	template <class Index, class Value, class NonzeroIndex>
	simple_nonzero<Index, Value, NonzeroIndex>
	make_simple_nonzero(Index r, Index c, Value v, NonzeroIndex nzi)
	{
		return simple_nonzero<Index, Value, NonzeroIndex>(r, c, v, nzi);
	}

    /**
     * Determine the number of nonzeros in a matrix.
     *
     * This function fowards the call to the matrix class itself,
     * implement a partial specialization if you need other behavior.
     *
     * @param m a reference to the matrix.
     *
     * @return the number of nonzeros in a matrix.
     */
	template <class Matrix>
	inline typename smatrix_traits<Matrix>::size_type
    nnz(Matrix& m)
    {
    	return (m.nnz());
    }

    /**
     * Return the dimensions of the matrix (nrows, ncols).
     *
     * This function fowards the call to the matrix class itself,
     * implement a partial specialization if you need other behavior.
     *
     * @param m a reference to the matrix.
     *
     * @return (nrows, ncols).
     */
    template <class Matrix>
    inline std::pair<typename smatrix_traits<Matrix>::size_type,
                      typename smatrix_traits<Matrix>::size_type>
    dimensions(Matrix& m)
    {
    	return (m.dimensions());
    }

    template <class Matrix>
    inline typename smatrix_traits<Matrix>::size_type
    ncols(Matrix& m)
	{
		typename smatrix_traits<Matrix>::size_type nrows,ncols;

		boost::tie(nrows, ncols) = dimensions(m);

		return (ncols);
	}

	template <class Matrix>
    inline typename smatrix_traits<Matrix>::size_type
	nrows(Matrix& m)
	{
		typename smatrix_traits<Matrix>::size_type nrows,ncols;

		boost::tie(nrows, ncols) = dimensions(m);

		return (nrows);
	}

    template <class Matrix>
    inline typename smatrix_traits<Matrix>::index_type
    row(boost::tuples::tuple<
            typename smatrix_traits<Matrix>::index_type,
            typename smatrix_traits<Matrix>::index_type,
            typename smatrix_traits<Matrix>::value_type> n, const Matrix& f)
    {
    	return (boost::tuples::get<0>(n));
    }

	template <class Matrix>
	inline typename smatrix_traits<Matrix>::index_type
	row(simple_nonzero<
			typename smatrix_traits<Matrix>::index_type,
			typename smatrix_traits<Matrix>::value_type,
			typename smatrix_traits<Matrix>::nz_index_type> n, const Matrix& f)
	{
		return n._row;
	}

    template <class Matrix>
    inline typename smatrix_traits<Matrix>::index_type
    column(boost::tuples::tuple<
            typename smatrix_traits<Matrix>::index_type,
            typename smatrix_traits<Matrix>::index_type,
            typename smatrix_traits<Matrix>::value_type> n, const Matrix& f)
    {
    	return (boost::tuples::get<1>(n));
    }

	template <class Matrix>
	inline typename smatrix_traits<Matrix>::index_type
	column(simple_nonzero<
			typename smatrix_traits<Matrix>::index_type,
			typename smatrix_traits<Matrix>::value_type,
			typename smatrix_traits<Matrix>::nz_index_type> n, const Matrix& f)
	{
		return n._column;
	}

	/*template <class Matrix>
    inline typename smatrix_traits<Matrix>::index_type
    column(boost::tuples::tuple<
            typename smatrix_traits<Matrix>::index_type,
            typename smatrix_traits<Matrix>::value_type> n, const Matrix& f)
    {
    	return (boost::tuples::get<0>(n));
    }*/

	template <class Matrix>
    inline typename smatrix_traits<Matrix>::index_type
		column(std::pair<
            typename smatrix_traits<Matrix>::index_type,
            typename smatrix_traits<Matrix>::value_type> n, const Matrix& f)
    {
    	return (n.first);
    }

    template <class Matrix>
    inline typename smatrix_traits<Matrix>::value_type
    value(boost::tuples::tuple<
            typename smatrix_traits<Matrix>::index_type,
            typename smatrix_traits<Matrix>::index_type,
            typename smatrix_traits<Matrix>::value_type> n, const Matrix& f)
    {
    	return (boost::tuples::get<2>(n));
    }

	template <class Matrix>
	inline typename smatrix_traits<Matrix>::value_type
	value(simple_nonzero<
			typename smatrix_traits<Matrix>::index_type,
			typename smatrix_traits<Matrix>::value_type,
			typename smatrix_traits<Matrix>::nz_index_type> n, const Matrix& f)
	{
		return n._val;
	}

	/**
	 * Overloaded function to return the value for a row/column non-zero as well.
	 */
	/*template <class Matrix>
    inline typename smatrix_traits<Matrix>::value_type
    row_value(boost::tuples::tuple<
            typename smatrix_traits<Matrix>::index_type,
            typename smatrix_traits<Matrix>::value_type> n, const Matrix& f)
    {
    	return (boost::tuples::get<1>(n));
    }*/

	/*template <class Matrix>
	inline typename smatrix_traits<Matrix>::value_type
	row_value(boost::tuples::tuple<
            typename smatrix_traits<Matrix>::index_type,
            typename smatrix_traits<Matrix>::index_type,
            typename smatrix_traits<Matrix>::value_type> n, const Matrix& f)
	{
		return (boost::tuples::get<1>(n));
	}*/

	template <class Matrix>
    inline typename smatrix_traits<Matrix>::value_type
	value(std::pair<
			typename smatrix_traits<Matrix>::index_type,
            typename smatrix_traits<Matrix>::value_type> n, const Matrix& f)
    {
    	return (n.second);
    }

    template <class Matrix>
    inline std::pair<typename smatrix_traits<Matrix>::nonzero_iterator,
                      typename smatrix_traits<Matrix>::nonzero_iterator>
    nonzeros(Matrix& m)
    {
		return (std::make_pair(m.begin_nonzeros(), m.end_nonzeros()));
    }

    template <class Matrix>
    inline std::pair<typename smatrix_traits<Matrix>::row_iterator,
                     typename smatrix_traits<Matrix>::row_iterator>
    rows(Matrix& m)
    {
        return (std::make_pair(m.begin_rows(), m.end_rows()));
    }

    template <class Matrix>
    inline std::pair<typename smatrix_traits<Matrix>::row_nonzero_iterator,
                      typename smatrix_traits<Matrix>::row_nonzero_iterator>
    row_nonzeros(typename smatrix_traits<Matrix>::index_type r, const Matrix& m)
    {
		typename smatrix_traits<Matrix>::row_nonzero_iterator br = m.begin_row(r);
		typename smatrix_traits<Matrix>::row_nonzero_iterator be = m.end_row(r);
		return std::make_pair(br, be);
    	//return (make_pair(m.begin_row(r), m.end_row(r)));
    }

    template <class Matrix>
    typename smatrix_traits<Matrix>::value_type value(
        typename smatrix_traits<Matrix>::index_type r,
        typename smatrix_traits<Matrix>::index_type c,
        const Matrix& m)
    {
        using namespace yasmic;

        //BOOST_STATIC_ASSERT(0 == 1);

        typedef typename smatrix_traits<Matrix>::index_type itype;
        typedef typename smatrix_traits<Matrix>::value_type vtype;

        typename smatrix_traits<Matrix>::nonzero_iterator nzi, nziend;
		for (boost::tie(nzi,nziend) = nonzeros(m);
			 nzi != nziend; ++nzi)
		{
			if (row(*nzi,m)==r && column(*nzi,m)==c)
            {
                return (value(*nzi,m));
            }
		}

        return (0);
    }

	template <class Matrix, class Iter1, class Iter2>
	void mult(Matrix& m, Iter1 x, Iter2 y)
	{
		using namespace yasmic;

		typedef typename smatrix_traits<Matrix>::index_type itype;
		typedef typename smatrix_traits<Matrix>::size_type stype;
		typedef typename smatrix_traits<Matrix>::value_type vtype;

		stype nr = nrows(m);

		// first zero the vector
		for (itype r=0; r < nr; ++r)
		{
			y[r] = vtype();
		}

		typename smatrix_traits<Matrix>::nonzero_iterator nzi, nziend;
		for (boost::tie(nzi,nziend) = nonzeros(m);
			 nzi != nziend; ++nzi)
		{
			y[row(*nzi,m)] += value(*nzi,m)*x[column(*nzi,m)];
		}
	}

	template <class Matrix, class Iter1, class Iter2>
	void trans_mult(Matrix& m, Iter1 x, Iter2 y)
	{
		using namespace yasmic;

		typedef typename smatrix_traits<Matrix>::index_type itype;
		typedef typename smatrix_traits<Matrix>::size_type stype;
		typedef typename smatrix_traits<Matrix>::value_type vtype;

		stype nc = ncols(m);

		// first zero the vector
		for (itype c=0; c < nc; ++c)
		{
			y[c] = vtype();
		}

		typename smatrix_traits<Matrix>::nonzero_iterator nzi, nziend;
		for (boost::tie(nzi,nziend) = nonzeros(m);
			 nzi != nziend; ++nzi)
		{
			y[column(*nzi,m)] += value(*nzi,m)*x[row(*nzi,m)];
		}
	}

}

#endif // YASMIC_GENERIC_MATRIX_OPERATIONS


//
//  Copyright (c) 2018-2019, Cem Bassoy, cem.bassoy@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer IOSB, Ettlingen, Germany
//

#ifndef BOOST_UBLAS_TENSOR_EXPRESSIONS_HPP
#define BOOST_UBLAS_TENSOR_EXPRESSIONS_HPP

#include <cstddef>
#include <boost/numeric/ublas/expression_types.hpp>

#include "tags.hpp"

namespace boost::numeric::ublas::detail
{

/** @\brief base class for tensor expressions
 *
 * \note implements crtp - no use of virtual function calls
 *
 * \tparam T type of the tensor
 * \tparam E type of the derived expression (crtp)
 *
 **/
template<class T, class E>
struct tensor_expression
    : public ublas_expression<E>
{
    //	static const unsigned complexity = 0;
    using expression_type = E;
    using type_category = tensor_tag;
    using tensor_type = T;

    inline
    constexpr auto const& operator()() const noexcept { return *static_cast<const expression_type*> (this); }

    ~tensor_expression() = default;
    tensor_expression(const tensor_expression&) = delete;
    tensor_expression(tensor_expression&&) noexcept = delete;
    tensor_expression& operator=(const tensor_expression&) = delete;
    tensor_expression& operator=(tensor_expression&&) noexcept = delete;

protected :
    explicit tensor_expression() = default;
};

template<class T, class EL, class ER, class OP>
struct binary_tensor_expression
    : public tensor_expression <T, binary_tensor_expression<T,EL,ER,OP>>
{
    using self_type = binary_tensor_expression<T,EL,ER,OP>;
    using tensor_type  = T;
    using binary_operation = OP;
    using expression_type_left  = EL;
    using expression_type_right = ER;
    using derived_type =  tensor_expression <tensor_type,self_type>;

    using size_type = typename tensor_type::size_type;

    explicit constexpr binary_tensor_expression(expression_type_left  const& l, expression_type_right const& r, binary_operation o) : el(l) , er(r) , op(std::move(o)) {}
    constexpr binary_tensor_expression(binary_tensor_expression&& l) noexcept = delete;
    constexpr binary_tensor_expression& operator=(binary_tensor_expression&& l) noexcept = delete;
    ~binary_tensor_expression() = default;

    binary_tensor_expression() = delete;
    binary_tensor_expression(const binary_tensor_expression& l) = delete;
    binary_tensor_expression& operator=(binary_tensor_expression const& l) noexcept = delete;

    [[nodiscard]] inline
    constexpr decltype(auto) operator()(size_type i) const { return op(el(i), er(i)); }

    expression_type_left const& el;
    expression_type_right const& er;
    binary_operation op;
};

/// @brief helper function to simply instantiation of lambda proxy class
template<class T1, class T2, class EL, class ER, class OP>
[[nodiscard]] inline
constexpr auto make_binary_tensor_expression( tensor_expression<T1,EL> const& el, tensor_expression<T2,ER> const& er, OP op) noexcept
{
    static_assert( std::is_same_v< typename T1::value_type, typename T2::value_type>,
        "boost::numeric::ublas::make_binary_tensor_expression(T1,T2) : LHS tensor and RHS tensor should have same value type"
    );
    return binary_tensor_expression<T1,EL,ER,OP>( el(), er(), op) ;
}

template<class T, class EL, class ER, class OP>
[[nodiscard]] inline
constexpr auto make_binary_tensor_expression( matrix_expression<EL> const& el, tensor_expression<T,ER> const& er, OP op) noexcept
{
    return binary_tensor_expression<T,EL,ER,OP>( el(), er(), op) ;
}

template<class T, class EL, class ER, class OP>
[[nodiscard]] inline
constexpr auto make_binary_tensor_expression( tensor_expression<T,EL> const& el, matrix_expression<ER> const& er, OP op) noexcept
{
    return binary_tensor_expression<T,EL,ER,OP>( el(), er(), op) ;
}

template<class T, class EL, class ER, class OP>
[[nodiscard]] inline
constexpr auto make_binary_tensor_expression( vector_expression<EL> const& el, tensor_expression<T,ER> const& er, OP op) noexcept
{
    return binary_tensor_expression<T,EL,ER,OP>( el(), er(), op) ;
}

template<class T, class EL, class ER, class OP>
[[nodiscard]] inline
constexpr auto make_binary_tensor_expression( tensor_expression<T,EL> const& el, vector_expression<ER> const& er, OP op) noexcept
{
    return binary_tensor_expression<T,EL,ER,OP>( el(), er(), op) ;
}

template<class T, class E, class OP>
struct unary_tensor_expression
    : public tensor_expression <T, unary_tensor_expression<T,E,OP>>
{

    using self_type = unary_tensor_expression<T,E,OP>;
    using tensor_type  = T;
    using expression_type = E;
    using unary_operation = OP;
    using derived_type = tensor_expression <T, unary_tensor_expression<T,E,OP>>;

    using size_type = typename tensor_type::size_type;

    explicit constexpr unary_tensor_expression(expression_type const& ee, unary_operation o) : e(ee) , op(std::move(o)) {}
    constexpr unary_tensor_expression(unary_tensor_expression&& l) noexcept = delete;
    constexpr unary_tensor_expression& operator=(unary_tensor_expression&& l) noexcept = delete;

    constexpr unary_tensor_expression() = delete;
    unary_tensor_expression(unary_tensor_expression const& l) = delete;
    unary_tensor_expression& operator=(unary_tensor_expression const& l) noexcept = delete;
    ~unary_tensor_expression() = default;

    [[nodiscard]] inline constexpr
      decltype(auto) operator()(size_type i) const { return op(e(i)); }

    E const& e;
    OP op;
};

// \brief helper function to simply instantiation of lambda proxy class
template<class T, class E, class OP>
[[nodiscard]] inline
constexpr auto make_unary_tensor_expression( tensor_expression<T,E> const& e, OP op) noexcept
{
    return unary_tensor_expression<T,E,OP>( e() , op);
}

template<class T, class E, class OP>
[[nodiscard]] inline
constexpr auto make_unary_tensor_expression( matrix_expression<E> const& e, OP op) noexcept
{
    return unary_tensor_expression<T,E,OP>( e() , op);
}

template<class T, class E, class OP>
[[nodiscard]] inline
constexpr auto make_unary_tensor_expression( vector_expression<E> const& e, OP op) noexcept
{
    return unary_tensor_expression<T,E,OP>( e() , op);
}

} // namespace boost::numeric::ublas::detail

#endif // BOOST_UBLAS_TENSOR_EXPRESSIONS_HPP


#define BOOST_TEST_MODULE "test_boundary"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include <periortree/boundary_condition.hpp>
#include <test/point_type.hpp>

BOOST_AUTO_TEST_CASE(test_xyz_unlimited)
{
    {
        const perior::test::xyz p(1., 2., 3.);
        const perior::unlimited_boundary<perior::test::xyz> boundary;

        const perior::test::xyz ap = adjust_position(p, boundary);
        BOOST_CHECK_EQUAL(ap.x, p.x);
        BOOST_CHECK_EQUAL(ap.y, p.y);
        BOOST_CHECK_EQUAL(ap.z, p.z);
        const perior::test::xyz ad = adjust_direction(p, boundary);
        BOOST_CHECK_EQUAL(ad.x, p.x);
        BOOST_CHECK_EQUAL(ad.y, p.y);
        BOOST_CHECK_EQUAL(ad.z, p.z);
    }
}

BOOST_AUTO_TEST_CASE(test_xyz_cubic_periodic)
{
    const perior::test::xyz lw(0, 0, 0);
    const perior::test::xyz up(1, 1, 1);
    const perior::cubic_periodic_boundary<perior::test::xyz> boundary(lw, up);
    {
        const perior::test::xyz p(0.3, 0.3, 0.3);

        const perior::test::xyz ap = adjust_position(p, boundary);
        BOOST_CHECK_EQUAL(ap.x, p.x);
        BOOST_CHECK_EQUAL(ap.y, p.y);
        BOOST_CHECK_EQUAL(ap.z, p.z);
        const perior::test::xyz ad = adjust_direction(p, boundary);
        BOOST_CHECK_EQUAL(ad.x, p.x);
        BOOST_CHECK_EQUAL(ad.y, p.y);
        BOOST_CHECK_EQUAL(ad.z, p.z);
    }

    {
        const perior::test::xyz p(1.3, 0.3, 0.3);

        const perior::test::xyz ap = adjust_position(p, boundary);
        BOOST_CHECK_CLOSE(ap.x, 1.3 - 1.0, 1e-10);
        BOOST_CHECK_EQUAL(ap.y, p.y);
        BOOST_CHECK_EQUAL(ap.z, p.z);
    }
    {
        const perior::test::xyz p(0.6, 0.3, 0.3);
        const perior::test::xyz ad = adjust_direction(p, boundary);
        BOOST_CHECK_CLOSE(ad.x, 0.6 - 1.0, 1e-10);
        BOOST_CHECK_EQUAL(ad.y, p.y);
        BOOST_CHECK_EQUAL(ad.z, p.z);
    }
}