/*=========================================================================

  Program:   Visualization Toolkit
  Module:    $RCSfile: vtkTreeLayoutStrategy.cxx,v $

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*----------------------------------------------------------------------------
 Copyright (c) Sandia Corporation
 See Copyright.txt or http://www.paraview.org/HTML/Copyright.html for details.
----------------------------------------------------------------------------*/

#include "vtkTreeLayoutStrategy.h"

#include "vtkAbstractArray.h"
#ifdef VTK_USE_BOOST
#include "vtkBoostBreadthFirstSearchTree.h"
#endif
#include "vtkDataArray.h"
#include "vtkIdTypeArray.h"
#include "vtkMath.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"
#include "vtkTree.h"
#include "vtkTreeDFSIterator.h"

vtkCxxRevisionMacro(vtkTreeLayoutStrategy, "$Revision: 1.3 $");
vtkStandardNewMacro(vtkTreeLayoutStrategy);

vtkTreeLayoutStrategy::vtkTreeLayoutStrategy()
{
  this->Angle = 90;
  this->Radial = false;
  this->LogSpacingValue = 1.0;
  this->LeafSpacing = 0.9;
  this->DistanceArrayName = NULL;
}

vtkTreeLayoutStrategy::~vtkTreeLayoutStrategy()
{
  this->SetDistanceArrayName(NULL);
}

// Tree layout method
void vtkTreeLayoutStrategy::Layout()
{
  vtkTree* tree = vtkTree::SafeDownCast(this->Graph);
  if (tree == NULL)
    {
#ifdef VTK_USE_BOOST
    // Use the BFS search tree to perform the layout
    vtkBoostBreadthFirstSearchTree* bfs = vtkBoostBreadthFirstSearchTree::New();
    bfs->CreateGraphVertexIdArrayOn();
    bfs->SetInput(this->Graph);
    bfs->Update();
    tree = vtkTree::New();
    tree->ShallowCopy(bfs->GetOutput());
    bfs->Delete();
#else
    vtkErrorMacro("Layout only works on vtkTree unless VTK_USE_BOOST is on.");
#endif
    }

  vtkPoints* newPoints = vtkPoints::New();
  newPoints->SetNumberOfPoints(tree->GetNumberOfVertices());

  // Check if the distance array is defined.
  vtkDataArray* distanceArr = NULL;
  if (this->DistanceArrayName != NULL)
    {
    vtkAbstractArray* aa = tree->GetVertexData()->
      GetAbstractArray(this->DistanceArrayName);
    if (!aa)
      {
      vtkErrorMacro("Distance array not found.");
      return;
      }
    distanceArr = vtkDataArray::SafeDownCast(aa);
    if (!distanceArr)
      {
      vtkErrorMacro("Distance array must be a data array.");
      return;
      }
    }
  double maxDistance = 1.0;
  if (distanceArr)
    {
    maxDistance = distanceArr->GetMaxNorm();
    }

  // Count the number of leaves in the tree
  // and get the maximum depth
  vtkIdType leafCount = 0;
  vtkIdType maxLevel = 0;
  vtkTreeDFSIterator* iter = vtkTreeDFSIterator::New();
  iter->SetTree(tree);
  while (iter->HasNext())
    {
    vtkIdType vertex = iter->Next();
    if (tree->IsLeaf(vertex))
      {
      leafCount++;
      }
    if (tree->GetLevel(vertex) > maxLevel)
      {
      maxLevel = tree->GetLevel(vertex);
      }
    }
  // Don't count the root in the list of internal nodes.
  vtkIdType internalCount = tree->GetNumberOfVertices() - leafCount - 1;
  double leafSpacing = this->LeafSpacing / static_cast<double>(leafCount);
  double internalSpacing = (1.0 - this->LeafSpacing) / static_cast<double>(internalCount);

  double angleRad = this->Angle * vtkMath::Pi() / 180.0;
  double spacing;
  if (this->LogSpacingValue == 1.0)
    {
    if (this->Radial)
      {
      spacing = 1.0 / maxLevel;
      }
    else
      {
      spacing = 0.5 / tan(angleRad / 2);
      }
    }
  else
    {
    spacing = this->LogSpacingValue;
    }

  double curPlace = 0;
  iter->SetMode(vtkTreeDFSIterator::FINISH);
  while (iter->HasNext())
    {
    vtkIdType vertex = iter->Next();

    double height;
    if (distanceArr != NULL)
      {
      height = spacing * distanceArr->GetTuple1(vertex) / maxDistance;
      }
    else
      {
      if (this->LogSpacingValue == 1.0)
        {
        height = spacing * tree->GetLevel(vertex) / static_cast<double>(maxLevel);
        }
      else
        {
        height = (1 - pow(spacing, tree->GetLevel(vertex) + 1.0)) / (1 - spacing) - 1.0;
        }
      }

    double x, y;
    if (this->Radial)
      {
      double ang;
      if (tree->IsLeaf(vertex))
        {
        ang = 2.0 * vtkMath::Pi() * curPlace;
        ang *= this->Angle / 360.0;
        ang -= vtkMath::Pi() / 2.0 + vtkMath::Pi()*this->Angle / 180.0;
        curPlace += leafSpacing;
        }
      else
        {
        curPlace += internalSpacing;
        vtkIdType nchildren;
        const vtkIdType* children;
        tree->GetChildren(vertex, nchildren, children);
        double minAng = 2*vtkMath::Pi();
        double maxAng = 0.0;
        double angSinSum = 0.0;
        double angCosSum = 0.0;
        for (vtkIdType c = 0; c < nchildren; c++)
          {
          double pt[3];
          newPoints->GetPoint(children[c], pt);
          double leafAngle = atan2(pt[1], pt[0]);
          if (leafAngle < 0)
            {
            leafAngle += 2*vtkMath::Pi();
            }
          if (leafAngle < minAng)
            {
            minAng = leafAngle;
            }
          if (leafAngle > maxAng)
            {
            maxAng = leafAngle;
            }
          angSinSum += sin(leafAngle);
          angCosSum += cos(leafAngle);
          }
        // This is how to take the average of the two angles minAng, maxAng
        ang = atan2(sin(minAng) + sin(maxAng), cos(minAng) + cos(maxAng));

        // Make sure the angle is on the same "side" as the average angle.
        // If not, add pi to the angle. This handles some border cases.
        double avgAng = atan2(angSinSum, angCosSum);
        if (sin(ang)*sin(avgAng) + cos(ang)*cos(avgAng) < 0)
          {
          ang += vtkMath::Pi();
          }
        }
      x = height * cos(ang);
      y = height * sin(ang);
      }
    else
      {
      double width = 2.0 * tan(vtkMath::Pi()*this->Angle / 180.0 / 2.0);
      y = -height;
      if (tree->IsLeaf(vertex))
        {
        x = width * curPlace;
        curPlace += leafSpacing;
        }
      else
        {
        curPlace += internalSpacing;
        vtkIdType nchildren;
        const vtkIdType* children;
        tree->GetChildren(vertex, nchildren, children);
        double minX = VTK_DOUBLE_MAX;
        double maxX = VTK_DOUBLE_MIN;
        for (vtkIdType c = 0; c < nchildren; c++)
          {
          double pt[3];
          newPoints->GetPoint(children[c], pt);
          if (pt[0] < minX)
            {
            minX = pt[0];
            }
          if (pt[0] > maxX)
            {
            maxX = pt[0];
            }
          }
        x = (minX + maxX) / 2.0;
        }
      }
    newPoints->SetPoint(vertex, x, y, 0.0);
    }

  // Copy coordinates back into the original graph
  if (vtkTree::SafeDownCast(this->Graph))
    {
    this->Graph->SetPoints(newPoints);
    }
#ifdef VTK_USE_BOOST
  else
    {
    // Reorder the points based on the mapping back to graph vertex ids
    vtkPoints* reordered = vtkPoints::New();
    reordered->SetNumberOfPoints(newPoints->GetNumberOfPoints());
    for (vtkIdType i = 0; i < reordered->GetNumberOfPoints(); i++)
      {
      reordered->SetPoint(i, 0, 0, 0);
      }
    vtkIdTypeArray* graphVertexIdArr = vtkIdTypeArray::SafeDownCast(
      tree->GetVertexData()->GetAbstractArray("GraphVertexId"));
    for (vtkIdType i = 0; i < graphVertexIdArr->GetNumberOfTuples(); i++)
      {
      reordered->SetPoint(graphVertexIdArr->GetValue(i), newPoints->GetPoint(i));
      }
    this->Graph->SetPoints(reordered);
    tree->Delete();
    reordered->Delete();
    }
#endif

  // Clean up.
  iter->Delete();
  newPoints->Delete();
}

void vtkTreeLayoutStrategy::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);
  os << indent << "Angle: " << this->Angle << endl;
  os << indent << "Radial: " << (this->Radial ? "true" : "false") << endl;
  os << indent << "LogSpacingValue: " << this->LogSpacingValue << endl;
  os << indent << "LeafSpacing: " << this->LeafSpacing << endl;
  os << indent << "DistanceArrayName: "
     << (this->DistanceArrayName ? this->DistanceArrayName : "(null)") << endl;
}

#if 0
// Code storage

#include "vtkGraphToBoostAdapter.h"
#include "vtkTreeToBoostAdapter.h"
#include <boost/graph/visitors.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/property_map.hpp>
#include <boost/vector_property_map.hpp>
#include <boost/pending/queue.hpp>

using namespace boost;

// Redefine the bfs visitor, the only visitor we
// are using is the tree_edge visitor.
template <typename PlacementMap>
  class placement_visitor : public default_dfs_visitor
  {
  public:
    placement_visitor() { }
    placement_visitor(PlacementMap dist, typename property_traits<PlacementMap>::value_type ii = 1)
      : d(dist), cur_place(0), internal_inc(ii) { }

    template <typename Vertex, typename Graph>
    void finish_vertex(Vertex v, const Graph& g)
    {
      put(d, v, cur_place);
      if (g->IsLeaf(v))
        {
        cur_place += 1;
        }
      else
        {
        cur_place += internal_inc;
        }
    }

    typename property_traits<PlacementMap>::value_type max_place() { return cur_place; }

  private:
    PlacementMap d;
    typename property_traits<PlacementMap>::value_type cur_place;
    typename property_traits<PlacementMap>::value_type internal_inc;
  };

    // Create a color map (used for marking visited nodes)
    //vector_property_map<default_color_type> color;
    //vtkDoubleArray* placement = vtkDoubleArray::New();
    //depth_first_search(tree,
    //  placement_visitor<vtkDoubleArray*>(placement, 1.0),
    //  color, tree->GetRoot());
#endif


#ifndef NEPOMUK_ALGORITHM_SCC_HPP_
#define NEPOMUK_ALGORITHM_SCC_HPP_

#include <algorithm>
#include <cstddef>
#include <cstdint>
#include <iostream>
#include <limits>
#include <stack>
#include <vector>

#include <boost/assert.hpp>

#include "tool/status/progress.hpp"

namespace nepomuk
{
namespace algorithm
{

// the actual strongly connected components
class StronglyConnectedComponent
{
  public:
    // the size of the container / the number of components compute
    std::size_t size() const;
    // the size of a component (numbered from 0 to size()-1)
    std::size_t size(std::size_t const component_id) const;

    // the component of a node offset
    std::uint64_t component(std::size_t const offset) const;

    using ComponentID = std::uint32_t;

  private:
    std::vector<ComponentID> node_to_component;
    std::vector<std::size_t> component_sizes;

    template <typename graph_type> friend class TarjanSCCAlgorithm;
};

// implementation of tarjans algorithm to compute strongly connected components in a directed
// network. `graph_type` needs to supply `nodes` / `edges` lookup
template <typename graph_type> class TarjanSCCAlgorithm
{
  private:
    StronglyConnectedComponent::ComponentID const static constexpr UNSET_COMPONENT_ID =
        std::numeric_limits<StronglyConnectedComponent::ComponentID>::max();

    struct StackFrame
    {
        explicit StackFrame(std::size_t const node_offset, std::size_t const edge_offset)
            : node_offset(node_offset), edge_offset(edge_offset)
        {
        }
        std::size_t node_offset;
        std::size_t edge_offset;
    };

    struct SCCState
    {
        // the maximum reachable depth / used as invalid value
        auto const static constexpr MAXIMUM_DEPTH = std::numeric_limits<std::uint64_t>::max();

        SCCState() : depth(MAXIMUM_DEPTH), low_link(MAXIMUM_DEPTH), on_stack(false) {}
        SCCState(std::uint64_t depth) : depth(depth), low_link(depth), on_stack(true) {}

        // the depth within the dfs
        std::uint64_t depth;
        // the lowest index reachable by any node
        std::uint64_t low_link;
        // check if node is on the component_stack
        bool on_stack;
    };

  public:
    static StronglyConnectedComponent compute(const graph_type &graph);
};

// location for our static variable
template <typename graph_type>
StronglyConnectedComponent::ComponentID const TarjanSCCAlgorithm<graph_type>::UNSET_COMPONENT_ID;

template <typename graph_type>
std::uint64_t const TarjanSCCAlgorithm<graph_type>::SCCState::MAXIMUM_DEPTH;

template <typename graph_type>
StronglyConnectedComponent TarjanSCCAlgorithm<graph_type>::compute(const graph_type &graph)
{
    tool::status::FunctionTimingGuard scc_timing_guard("SCC computation");
    StronglyConnectedComponent component;
    BOOST_ASSERT(!graph.empty());
    component.node_to_component.resize(graph.size(), UNSET_COMPONENT_ID);

    // The following is a hack to distinguish between stuff that happens
    // before the recursive call and stuff that happens after. The differentiation is required due
    // to the iterative implementation of the dfs, rather than a stack based approach.
    std::stack<StackFrame> recursion_stack;
    std::stack<std::size_t> component_nodes;

    std::vector<SCCState> node_state(graph.size());

    // the id of the next scc
    StronglyConnectedComponent::ComponentID component_id = 0;
    std::uint64_t depth = 0;

    auto const depth_is_set = [](SCCState const &state) {
        return state.depth < SCCState::MAXIMUM_DEPTH;
    };

    auto const update_link = [](SCCState &state, std::uint64_t const low_link) {
        state.low_link = std::min(state.low_link, low_link);
    };

    for (auto const &node : graph.nodes())
    {
        // start a component search at any uncategorised node
        if (!depth_is_set(node_state[graph.offset(node)]))
        {
            auto const nid = graph.offset(node);
            node_state[nid] = SCCState(depth++);
            component_nodes.push(nid);
            recursion_stack.emplace(StackFrame(nid, 0));
        }

        while (!recursion_stack.empty())
        {
            auto current_frame = recursion_stack.top();

            auto const current_node_index = current_frame.node_offset;
            auto const current_edge_index = current_frame.edge_offset;

            auto edge_range = graph.edges(graph.node(current_node_index));
            for (auto itr = edge_range.begin() + current_edge_index; itr != edge_range.end();
                 ++itr, ++recursion_stack.top().edge_offset)
            {
                auto const neighbor = graph.offset(itr->target());
                if (!depth_is_set(node_state[neighbor]))
                {
                    // new node
                    node_state[neighbor] = SCCState(depth++);
                    component_nodes.push(neighbor);
                    recursion_stack.emplace(StackFrame(neighbor, 0));
                    break;
                }
                else if (node_state[neighbor].on_stack)
                {
                    update_link(node_state[current_node_index], node_state[neighbor].low_link);
                }
            }

            // we didn't recurse anymore, so this is where the recursive implementation is
            // backtracking in the dfs
            if (recursion_stack.top().node_offset == current_node_index)
            {
                recursion_stack.pop();

                // a component is strongly conneected if the low_link equals the index of the
                // component root. In the case we find such a situaiton, all nodes on the
                // component_nodes stack form a strongly connected component.
                if (node_state[current_node_index].low_link == node_state[current_node_index].depth)
                {
                    std::size_t node_offset = 0;
                    std::size_t component_size = 0;
                    do
                    {
                        node_offset = component_nodes.top();
                        component_nodes.pop();
                        node_state[node_offset].on_stack = false;
                        component.node_to_component[node_offset] = component_id;
                        ++component_size;
                    } while (current_node_index != node_offset);

                    component.component_sizes.emplace_back(component_size);

                    ++component_id;
                }
            }
        }
    }
    std::cout << "[components] " << component.size() << " components.\n";
    return component;
}

// helper function to allow computation without specifying the graph type
template <typename graph_type> StronglyConnectedComponent computeSCC(graph_type const &graph)
{
    return TarjanSCCAlgorithm<graph_type>::compute(graph);
}

} // namespace algorithm
} // namespace nepomuk

#endif // NEPOMUK_ALGORITHM_SCC_HPP_


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

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

#pragma once

#include <Eigen/Dense>

#include <utility>

#include <fl/util/traits.hpp>
#include <fl/util/meta.hpp>

#include <fl/model/transition/interface/transition_function.hpp>

namespace fl
{

// Forward declarations
template <typename ... Models> class JointTransition;

/**
 * Traits of JointTransition<MultipleOf<Transition, Count>>
 */
template <
    typename Transition,
    int Count
>
struct Traits<JointTransition<MultipleOf<Transition, Count>>>
{
    enum : signed int { ModelCount = Count };

    typedef Transition LocalTransition;
    typedef typename Traits<Transition>::Scalar Scalar;
    typedef typename Traits<Transition>::State LocalState;
    typedef typename Traits<Transition>::Input LocalInput;
    typedef typename Traits<Transition>::Noise LocalNoise;

    enum : signed int
    {
        StateDim = ExpandSizes<LocalState::SizeAtCompileTime, Count>::Size,
        NoiseDim = ExpandSizes<LocalNoise::SizeAtCompileTime, Count>::Size,
        InputDim = ExpandSizes<LocalInput::SizeAtCompileTime, Count>::Size
    };

    typedef Eigen::Matrix<Scalar, StateDim, 1> State;
    typedef Eigen::Matrix<Scalar, NoiseDim, 1> Noise;
    typedef Eigen::Matrix<Scalar, InputDim, 1> Input;

    typedef TransitionInterface<
                State,
                Noise,
                Input
            > TransitionBase;
};

/**
 * \ingroup transitions
 */
template <
    typename LocalTransition,
    int Count
>
class JointTransition<MultipleOf<LocalTransition, Count>>
    : public Traits<
                 JointTransition<MultipleOf<LocalTransition, Count>>
             >::TransitionBase
{
private:
    /** Typdef of \c This for #from_traits(TypeName) helper */
    typedef JointTransition This;

public:
    typedef from_traits(State);
    typedef from_traits(Noise);
    typedef from_traits(Input);

public:
    JointTransition(const LocalTransition& local_transition,
                      int count = ToDimension<Count>())
        : local_transition_(local_transition),
          count_(count)
    {
        assert(count_ > 0);
    }

    JointTransition(const MultipleOf<LocalTransition, Count>& mof)
        : local_transition_(mof.instance),
          count_(mof.count)
    {
        assert(count_ > 0);
    }

    virtual ~JointTransition() noexcept { }

    virtual State predict_state(double delta_time,
                                const State& state,
                                const Noise& noise,
                                const Input& input)
    {
        State x = State::Zero(state_dimension(), 1);

        int state_dim = local_transition_.state_dimension();
        int noise_dim = local_transition_.noise_dimension();
        int input_dim = local_transition_.input_dimension();

        for (int i = 0; i < count_; ++i)
        {
            x.middleRows(i * state_dim, state_dim) =
                local_transition_.predict_state(
                    delta_time,
                    state.middleRows(i * state_dim, state_dim),
                    noise.middleRows(i * noise_dim, noise_dim),
                    input.middleRows(i * input_dim, input_dim));
        }

        return x;
    }

    virtual int state_dimension() const
    {
        return local_transition_.state_dimension() * count_;
    }

    virtual int noise_dimension() const
    {
        return local_transition_.noise_dimension() * count_;
    }

    virtual int input_dimension() const
    {
        return local_transition_.input_dimension() * count_;
    }

    LocalTransition& local_transition()
    {
        return local_transition_;
    }

    const LocalTransition& local_transition() const
    {
        return local_transition_;
    }

protected:
    LocalTransition local_transition_;
    int count_;
};

}


//==============================================================================
//         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
//==============================================================================

#include <nt2/linalg/details/blas/blas1.hpp>
#include <boost/fusion/include/at.hpp>
#include <cmath>
#include <cstdlib>
#include <vector>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/gflops.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/constant.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

template<typename T> struct fortran_axpy;

template<> struct fortran_axpy<float>
{
  static BOOST_FORCEINLINE
  void run( nt2_la_int s, float alpha, float* x, nt2_la_int incx, float* y, nt2_la_int incy )
  {
    NT2_F77NAME(saxpy)( &s, &alpha, x, &incx, y, &incy);
  }
};

template<> struct fortran_axpy<double>
{
  static BOOST_FORCEINLINE
  void run( nt2_la_int s, double alpha, double* x, nt2_la_int incx, double* y, nt2_la_int incy )
  {
    NT2_F77NAME(daxpy)( &s, &alpha, x, &incx, y, &incy);
  }
};

template<typename T> struct axpy_mkl
{
 template<typename Setup>
  axpy_mkl(Setup const& s)
              :  alpha(boost::fusion::at_c<1>(s))
              ,  size_(boost::fusion::at_c<0>(s))
  {
    incx = incy = 1;
    x.resize(size_);
    y.resize(size_);
    for(int i = 0; i<size_; i++) x[i] = y[i] = T(i);
  }

  void operator()()
  {
    fortran_axpy<T>::run(size_, alpha, &x[0], incx, &y[0], incy);
  }

  friend std::ostream& operator<<(std::ostream& os, axpy_mkl<T> const& p)
  {
    return os << "(" << p.size()  << ")";
  }

  void reset()
  {
    x.resize(size_);
    y.resize(size_);
    for(int i = 0; i<size_; i++) x[i] = y[i] = T(i);
  }

  std::size_t size() const { return size_; }
  double flops() const { return 2.*size_; }

private:
  nt2_la_int incx, incy;
  T alpha;
  std::vector<T> x, y;
  nt2_la_int size_;
};

NT2_REGISTER_BENCHMARK_TPL( axpy_mkl, NT2_SIMD_REAL_TYPES )
{
  std::size_t size_min = args("size_min", 16);
  std::size_t size_max = args("size_max", 4096);
  std::size_t size_step = args("size_step", 2);
  T alpha = args("alpha", 1.);

  run_during_with< axpy_mkl<T> > ( 1.
                                  , and_( geometric(size_min,size_max,size_step)
                                        , constant(alpha)
                                  )
                                  , gflops<stats::median_>()
                                  );
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/rol.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/function/bitwise_cast.hpp>

STF_CASE_TPL (" rol integer", STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::rol;
  STF_EXPR_IS( rol(T(), T()), T);

  T w = sizeof(T)*CHAR_BIT;

  for(T i=0;i<w;++i)
  {
    std::cout << int(i) << " -> ";
    STF_EQUAL(rol(T(1),T(i)), T(T(1)<<i) );
  }

/*
  STF_ASSERT(rol(T(1),T(-1)));
  STF_ASSERT(rol(T(1),T(w+1)));
*/
}

STF_CASE_TPL (" rolreal", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::rol;
  using bs::bitwise_cast;
  using iT = bd::as_integer_t<T> ;

  STF_EXPR_IS( rol(T(), iT()), T);

  iT w = sizeof(T)*CHAR_BIT;

  for(iT i=0;i<w;++i)
    STF_EQUAL( rol(bitwise_cast<T>(iT(1)),iT(i))
                  , bitwise_cast<T>(iT(1)<<i)
                  );
/*
  STF_ASSERT(rol(T(1),iT(-1)));
  STF_ASSERT(rol(T(1),iT(w+1)));
*/
}


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "Combinatorics.h"
#include "ContainerIO.h"

#define BOOST_TEST_MODULE CWR test
#include <boost/test/unit_test.hpp>

using namespace cppqedutils;
using namespace std;

BOOST_AUTO_TEST_CASE( CWR )
{
  CWR_Dir<4> dir{5};

  cout<<dir()<<endl;

  const auto c12=dir[12];
  BOOST_CHECK(dir[c12]==12);

  BOOST_CHECK( ( dir[{2,2,0,1}]==43 ) );

  BOOST_CHECK_THROW( ( dir[{1,2,0,1}] ) , CWR_Dir<4>::SubscriptingException );

  BOOST_CHECK_THROW( ( dir[{2,2,1,1}] ) , CWR_Dir<4>::SubscriptingException );

  BOOST_CHECK_THROW( ( dir[{2,3,1}] ) , CWR_Dir<4>::SubscriptingException );

  // const boost::array<size_t,5> idxBoostErr={{2,2,0,1,2}};

}


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

// Copyright (c) 2007-2016 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014-2018.
// Modifications copyright (c) 2014-2018 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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_STRATEGIES_GEOGRAPHIC_DISTANCE_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/radius.hpp>

#include <boost/geometry/formulas/andoyer_inverse.hpp>
#include <boost/geometry/formulas/meridian_inverse.hpp>
#include <boost/geometry/formulas/flattening.hpp>

#include <boost/geometry/srs/spheroid.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/geometries/point_xy.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Distance calculation for geographic coordinates on a spheroid
\ingroup strategies
\tparam FormulaPolicy Formula used to calculate azimuths
\tparam Spheroid The spheroid model
\tparam CalculationType \tparam_calculation

\qbk{
[heading See also]
\* [link geometry.reference.algorithms.distance.distance_3_with_strategy distance (with strategy)]
\* [link geometry.reference.srs.srs_spheroid srs::spheroid]
}
*/
template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef Spheroid model_type;

    inline geographic()
        : m_spheroid()
    {}

    explicit inline geographic(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename CT>
    static inline CT apply(CT lon1, CT lat1, CT lon2, CT lat2,
                           Spheroid const& spheroid)
    {
        typedef typename formula::meridian_inverse
                <
                CT, strategy::default_order<FormulaPolicy>::value
                > meridian_inverse;

        typename meridian_inverse::result res =
                 meridian_inverse::apply(lon1, lat1, lon2, lat2, spheroid);

        if (res.meridian)
        {
            return res.distance;
        }

        return FormulaPolicy::template inverse
                <
                    CT, true, false, false, false, false
                >::apply(lon1, lat1, lon2, lat2, spheroid).distance;
    }

    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& point1, Point2 const& point2) const
    {
        typedef typename calculation_type<Point1, Point2>::type CT;

        CT lon1 = get_as_radian<0>(point1);
        CT lat1 = get_as_radian<1>(point1);
        CT lon2 = get_as_radian<0>(point2);
        CT lat2 = get_as_radian<1>(point2);

        return apply(lon1, lat1, lon2, lat2, m_spheroid);
    }

    inline Spheroid const& model() const
    {
        return m_spheroid;
    }

private :
    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template
<
    typename FormulaPolicy,
    typename Spheroid,
    typename CalculationType
>
struct tag<geographic<FormulaPolicy, Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template
<
    typename FormulaPolicy,
    typename Spheroid,
    typename CalculationType,
    typename P1,
    typename P2
>
struct return_type<geographic<FormulaPolicy, Spheroid, CalculationType>, P1, P2>
    : geographic<FormulaPolicy, Spheroid, CalculationType>::template calculation_type<P1, P2>
{};

template
<
    typename FormulaPolicy,
    typename Spheroid,
    typename CalculationType
>
struct comparable_type<geographic<FormulaPolicy, Spheroid, CalculationType> >
{
    typedef geographic<FormulaPolicy, Spheroid, CalculationType> type;
};

template
<
    typename FormulaPolicy,
    typename Spheroid,
    typename CalculationType
>
struct get_comparable<geographic<FormulaPolicy, Spheroid, CalculationType> >
{
    static inline geographic<FormulaPolicy, Spheroid, CalculationType>
        apply(geographic<FormulaPolicy, Spheroid, CalculationType> const& input)
    {
        return input;
    }
};

template
<
    typename FormulaPolicy,
    typename Spheroid,
    typename CalculationType,
    typename P1,
    typename P2
>
struct result_from_distance<geographic<FormulaPolicy, Spheroid, CalculationType>, P1, P2>
{
    template <typename T>
    static inline typename return_type<geographic<FormulaPolicy, Spheroid, CalculationType>, P1, P2>::type
        apply(geographic<FormulaPolicy, Spheroid, CalculationType> const& , T const& value)
    {
        return value;
    }
};

template <typename Point1, typename Point2>
struct default_strategy<point_tag, point_tag, Point1, Point2, geographic_tag, geographic_tag>
{
    typedef strategy::distance::geographic
                <
                    strategy::andoyer,
                    srs::spheroid
                        <
                            typename select_coordinate_type<Point1, Point2>::type
                        >
                > type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_HPP


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <pybind11/numpy.h>

#include <Eigen/Core>

#include "pyinterp/detail/broadcast.hpp"
#include "pyinterp/detail/geodetic/coordinates.hpp"
#include "pyinterp/detail/thread.hpp"
#include "pyinterp/eigen.hpp"
#include "pyinterp/geodetic/system.hpp"

namespace pyinterp::geodetic {

/// Wrapper
class Coordinates : public detail::geodetic::Coordinates {
 public:
  /// The constructor defaults the ellipsoid parameters to WGS84.
  explicit Coordinates(const std::optional<System>& system)
      : detail::geodetic::Coordinates(system) {}

  /// Gets the WGS used by this instance
  [[nodiscard]] inline auto system() const noexcept -> System {
    return System(detail::geodetic::Coordinates::system());
  }

  /// Converts Cartesian coordinates to Geographic latitude, longitude, and
  /// altitude. Cartesian coordinates should be in meters. The returned latitude
  /// and longitude are in degrees, and the altitude will be in meters.
  template <typename T>
  auto ecef_to_lla(const Eigen::Ref<const Vector<T>>& x,
                   const Eigen::Ref<const Vector<T>>& y,
                   const Eigen::Ref<const Vector<T>>& z,
                   const size_t num_threads) const -> pybind11::tuple {
    detail::check_eigen_shape("x", x, "y", y, "z", z);
    auto size = x.size();
    auto lon = pybind11::array_t<T>(pybind11::array::ShapeContainer{{size}});
    auto lat = pybind11::array_t<T>(pybind11::array::ShapeContainer{{size}});
    auto alt = pybind11::array_t<T>(pybind11::array::ShapeContainer{{size}});
    auto _lon = lon.template mutable_unchecked<1>();
    auto _lat = lat.template mutable_unchecked<1>();
    auto _alt = alt.template mutable_unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      // Captures the detected exceptions in the calculation function
      // (only the last exception captured is kept)
      auto except = std::exception_ptr(nullptr);

      detail::dispatch(
          [&](size_t start, size_t end) {
            try {
              for (size_t ix = start; ix < end; ++ix) {
                auto lla = detail::geodetic::Coordinates::ecef_to_lla(
                    detail::geometry::Point3D<T>{x(ix), y(ix), z(ix)});
                _lon(ix) = boost::geometry::get<0>(lla);
                _lat(ix) = boost::geometry::get<1>(lla);
                _alt(ix) = boost::geometry::get<2>(lla);
              }
            } catch (...) {
              except = std::current_exception();
            }
          },
          size, num_threads);

      if (except != nullptr) {
        std::rethrow_exception(except);
      }
    }
    return pybind11::make_tuple(lon, lat, alt);
  }

  /// Converts Geographic coordinates latitude, longitude, and altitude to
  /// Cartesian coordinates. The latitude and longitude should be in degrees and
  /// the altitude in meters. The returned ECEF coordinates will be in meters.
  template <typename T>
  auto lla_to_ecef(const Eigen::Ref<const Vector<T>>& lon,
                   const Eigen::Ref<const Vector<T>>& lat,
                   const Eigen::Ref<const Vector<T>>& alt,
                   const size_t num_threads) const -> pybind11::tuple {
    detail::check_eigen_shape("lon", lon, "lat", lat, "alt", alt);
    auto size = lon.size();
    auto x = pybind11::array_t<T>(pybind11::array::ShapeContainer{{size}});
    auto y = pybind11::array_t<T>(pybind11::array::ShapeContainer{{size}});
    auto z = pybind11::array_t<T>(pybind11::array::ShapeContainer{{size}});
    auto x_ = x.template mutable_unchecked<1>();
    auto y_ = y.template mutable_unchecked<1>();
    auto z_ = z.template mutable_unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      // Captures the detected exceptions in the calculation function
      // (only the last exception captured is kept)
      auto except = std::exception_ptr(nullptr);

      detail::dispatch(
          [&](size_t start, size_t end) {
            try {
              for (size_t ix = start; ix < end; ++ix) {
                auto ecef = detail::geodetic::Coordinates::lla_to_ecef(
                    detail::geometry::EquatorialPoint3D<T>{lon(ix), lat(ix),
                                                           alt(ix)});
                x_(ix) = boost::geometry::get<0>(ecef);
                y_(ix) = boost::geometry::get<1>(ecef);
                z_(ix) = boost::geometry::get<2>(ecef);
              }
            } catch (...) {
              except = std::current_exception();
            }
          },
          size, num_threads);

      if (except != nullptr) {
        std::rethrow_exception(except);
      }
    }
    return pybind11::make_tuple(x, y, z);
  }

  /// Transform points between two coordinate systems defined by the
  /// Coordinates instances this and target.
  template <typename T>
  auto transform(const Coordinates& target,
                 const Eigen::Ref<const Vector<T>>& lon1,
                 const Eigen::Ref<const Vector<T>>& lat1,
                 const Eigen::Ref<const Vector<T>>& alt1,
                 const size_t num_threads) const -> pybind11::tuple {
    detail::check_eigen_shape("lon1", lon1, "lat1", lat1, "alt1", alt1);
    auto size = lon1.size();
    auto lon2 = pybind11::array_t<T>(pybind11::array::ShapeContainer{{size}});
    auto lat2 = pybind11::array_t<T>(pybind11::array::ShapeContainer{{size}});
    auto alt2 = pybind11::array_t<T>(pybind11::array::ShapeContainer{{size}});
    auto _lon2 = lon2.template mutable_unchecked<1>();
    auto _lat2 = lat2.template mutable_unchecked<1>();
    auto _alt2 = alt2.template mutable_unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      // Captures the detected exceptions in the calculation function
      // (only the last exception captured is kept)
      auto except = std::exception_ptr(nullptr);

      detail::dispatch(
          [&](size_t start, size_t end) {
            try {
              for (size_t ix = start; ix < end; ++ix) {
                auto lla = detail::geodetic::Coordinates::transform(
                    target, detail::geometry::EquatorialPoint3D<T>{
                                lon1(ix), lat1(ix), alt1(ix)});
                _lon2(ix) = boost::geometry::get<0>(lla);
                _lat2(ix) = boost::geometry::get<1>(lla);
                _alt2(ix) = boost::geometry::get<2>(lla);
              }
            } catch (...) {
              except = std::current_exception();
            }
          },
          size, num_threads);

      if (except != nullptr) {
        std::rethrow_exception(except);
      }
    }
    return pybind11::make_tuple(lon2, lat2, alt2);
  }

  /// Get a tuple that fully encodes the state of this instance
  [[nodiscard]] auto getstate() const -> pybind11::tuple {
    return system().getstate();
  }

  /// Create a new instance from a registered state of an instance of this
  /// object.
  static auto setstate(const pybind11::tuple& state) -> Coordinates {
    return Coordinates(System::setstate(state));
  }
};

}  // namespace pyinterp::geodetic


#ifndef SM_TRANSFORMATION_HPP
#define SM_TRANSFORMATION_HPP

#include <sm/kinematics/quaternion_algebra.hpp>
#include <boost/serialization/nvp.hpp>
#include <sm/eigen/serialization.hpp>
#include "HomogeneousPoint.hpp"
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/version.hpp>
#include <sm/kinematics/UncertainVector.hpp>

namespace sm {
  namespace kinematics {

    class UncertainTransformation;
    class UncertainHomogeneousPoint;

      ///
    /// @class Transformation
    /// @brief a class that represents a transformation.
    /// @todo describe how these transformations work
    ///
    class Transformation
    {
    public:
      // See: http://eigen.tuxfamily.org/dox-devel/TopicStructHavingEigenMembers.html
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      ///
      /// Default constructor. The transformation will
      /// be set to identity.
      ///
      Transformation();

      ///
      /// Constructor. The transformation will be set to T_ab
      ///
      /// @param T_ab the initializing transformation.
      ///
      Transformation(const Eigen::Matrix4d & T_a_b);

      ///
      /// Constructor.
      ///
      /// @param q_a_b The quaternion that transforms vectors from b to a
      /// @param t_a_b_a the vector from the origin of frame a, to the origin of frame b, expresessed in frame a.
      ///
      Transformation(const Eigen::Vector4d & q_a_b, const Eigen::Vector3d & t_a_b_a);

      virtual ~Transformation();

      ///
      ///
      /// @return The underlying transformation
      ///
      Eigen::Matrix4d T() const;

      /// @return the rotation matrix
      Eigen::Matrix3d C() const;

      /// @return the translation vector
      const Eigen::Vector3d & t() const;

      const Eigen::Vector4d & q() const;

      Eigen::Matrix<double, 3,4> T3x4() const;

      /**
       * Return a copy of the transformation inverted.
       *
       * @return The inverted transformation
       */
      Transformation inverse() const;

      /// \brief Set this to a random transformation.
      virtual void setRandom();

        virtual void set( const Eigen::Matrix4d & T_a_b );

      /// \brief Set this to a random transformation with bounded rotation and translation.
      virtual void setRandom( double translationMaxMeters, double rotationMaxRadians);
      /// \brief Set this transformation to identity
      void setIdentity();

      Transformation operator*(const Transformation & rhs) const;
      Eigen::Vector3d operator*(const Eigen::Vector3d & rhs) const;
      Eigen::Vector4d operator*(const Eigen::Vector4d & rhs) const;
      HomogeneousPoint operator*(const HomogeneousPoint & rhs) const;

      virtual UncertainTransformation operator*(const UncertainTransformation & rhs) const;
      virtual UncertainHomogeneousPoint operator*(const UncertainHomogeneousPoint & rhs) const;

      void checkTransformationIsValid( void ) const;

        /// \brief rotate a point (do not translate)
        Eigen::Vector3d rotate(const Eigen::Vector3d & p) const;

        /// \brief rotate a point (do not translate)
        Eigen::Vector4d rotate(const Eigen::Vector4d & p) const;

        /// \brief rotate a point (do not translate)
        UncertainVector3 rotate(const UncertainVector3 & p) const;

        double * qptr();
        double * tptr();

        enum {CLASS_SERIALIZATION_VERSION = 0};

        BOOST_SERIALIZATION_SPLIT_MEMBER()
      ///
      /// Serialize the Transformation to a boost::serialization archive.
      ///
      /// @param ar The archive
      /// @param version The archive file version number.
      ///
      template<class Archive>
      void save(Archive & ar, const unsigned int version) const;

      template<class Archive>
      void load(Archive & ar, const unsigned int version);

      bool isBinaryEqual(const Transformation & rhs) const;

      /// \brief The update step for this transformation from a minimal update.
      void oplus(const Eigen::Matrix<double,6,1> & dt);

      /// \brief Return the S matrix that puts the oplus operation in the form
      ///        of a small transformation.
      Eigen::Matrix<double,6,6> S() const;

    protected:

      /// The quaternion that will become a rotation matrix C_a_b that
      /// transforms vectors from b to a.
      Eigen::Vector4d _q_a_b;

      /// The vector from the origin of a to the origin of b, expressed in a
      Eigen::Vector3d _t_a_b_a;

    };

    template<class Archive>
    void Transformation::save(Archive & ar, const unsigned int /* version */) const
    {
      ar & BOOST_SERIALIZATION_NVP(_q_a_b);
      ar & BOOST_SERIALIZATION_NVP(_t_a_b_a);
    }

    template<class Archive>
    void Transformation::load(Archive & ar, const unsigned int version)
    {
        SM_ASSERT_LE(std::runtime_error, version, (unsigned int)CLASS_SERIALIZATION_VERSION, "Unsupported serialization version");
        ar >> BOOST_SERIALIZATION_NVP(_q_a_b);
        ar >> BOOST_SERIALIZATION_NVP(_t_a_b_a);
    }

      // Interpolate the transformation at time si between T0 (at time s0) and T1 (at time s1)
      Transformation interpolateTransformations(const Transformation & T0, double s0,
                                                const Transformation & T1, double s1,
                                                double si);

      /// brief linear interpolate between T0 and T1 as si moves from 0.0 to 1.0
      Transformation slerpTransformations(const Transformation & T0,
                                                const Transformation & T1,
                                                double si);

  } // namespace kinematics

} // namespace sm

BOOST_CLASS_VERSION(sm::kinematics::Transformation, sm::kinematics::Transformation::CLASS_SERIALIZATION_VERSION)

#endif /* SM_TRANSFORMATION_HPP */


#ifndef INCLUDE_MIMKL_MODELS_AVERAGE_HPP_
#define INCLUDE_MIMKL_MODELS_AVERAGE_HPP_

#include <dlib/optimization.h>
#include <mimkl/definitions.hpp>
#include <mimkl/linear_algebra.hpp>
#include <mimkl/models/model.hpp>
#include <queue>
#include <spdlog/spdlog.h>
#include <thread>
#include <type_traits>
#include <utility>

using dlib::mat;
using mimkl::definitions::Index;
using mimkl::kernels_handler::KernelsHandler;
using mimkl::utilities::sort_indices_decending;

namespace mimkl
{
namespace models
{

static std::shared_ptr<spdlog::logger> logger_average =
spdlog::stdout_color_mt("AverageMKL");

//! average kernel weights
template <typename Scalar, typename Kernel>
class AverageMKL : public Model<Scalar, Kernel>
{

    private:
    // inheritance of templatized base members
    using Model<Scalar, Kernel>::_kernels_handler;
    using Model<Scalar, Kernel>::_trained;
    using Model<Scalar, Kernel>::_precompute;
    using Model<Scalar, Kernel>::_trace_normalization;
    using Model<Scalar, Kernel>::_number_of_support_vectors;
    using Model<Scalar, Kernel>::_number_of_kernels;

    std::shared_ptr<spdlog::logger> _logger = spdlog::get("AverageMKL");

    typedef MATRIX(Scalar) Matrix;
    typedef COLUMN(Scalar) Column;
    typedef Eigen::Map<const Column> MapColumn;
    typedef dlib::matrix<Scalar, 0, 1, dlib::default_memory_manager, dlib::column_major_layout>
    DlibColumn;

    Column _weights;

    void fit();

    public:
    AverageMKL(const std::vector<Kernel> & = std::vector<Kernel>(),
               const bool precompute = true,
               const bool trace_normalization = true);
    AverageMKL(const std::vector<Matrix> & = std::vector<Matrix>(),
               const bool precompute = true,
               const bool trace_normalization = true);

    void fit(const Matrix &);
    void fit(const std::vector<Matrix> &);
    Matrix predict(const Matrix &);
    Matrix predict(const std::vector<Matrix> &);

    Matrix get_optimal_kernel();

    Column get_weights() const;
    void set_weights(const Column &);
};

template <typename Scalar, typename Kernel>
AverageMKL<Scalar, Kernel>::AverageMKL(const std::vector<Kernel> &kernel_functions,
                                       const bool precompute,
                                       const bool trace_normalization)
: Model<Scalar, Kernel>(kernel_functions, precompute, trace_normalization)
{
    _weights =
    Column::Constant(_number_of_kernels, (double)1. / _number_of_kernels);
}

template <typename Scalar, typename Kernel>
AverageMKL<Scalar, Kernel>::AverageMKL(const std::vector<Matrix> &kernel_matrices,
                                       const bool precompute,
                                       const bool trace_normalization)
: Model<Scalar, Kernel>(kernel_matrices, precompute, trace_normalization)
{
    _weights =
    Column::Constant(_number_of_kernels, (double)1. / _number_of_kernels);
}

template <typename Scalar, typename Kernel>
void AverageMKL<Scalar, Kernel>::fit()
{
    _number_of_support_vectors = _kernels_handler.get_lhs_size();
    _number_of_kernels = _kernels_handler.get_number_of_kernels();

    if (_weights.size() != _number_of_kernels)
    {
        _weights =
        Column::Constant(_number_of_kernels, (double)1. / _number_of_kernels);
    }
    _trained = true;
    _logger->debug("all done");
}

template <typename Scalar, typename Kernel>
void AverageMKL<Scalar, Kernel>::fit(const std::vector<Matrix> &kernel_matrices)
{
    _kernels_handler.set_matrices(kernel_matrices, true);
    fit();
}

template <typename Scalar, typename Kernel>
void AverageMKL<Scalar, Kernel>::fit(const Matrix &X)
{
    _kernels_handler.set_lhs(X);
    fit();
}

template <typename Scalar, typename Kernel>
typename AverageMKL<Scalar, Kernel>::Matrix
AverageMKL<Scalar, Kernel>::predict(const Matrix &X)
{
    if (!_trained)
        throw std::logic_error("The model should be trained first (after "
                               "instantiation or change in parameters)");
    _kernels_handler.set_rhs(X);
    return get_optimal_kernel();
}

template <typename Scalar, typename Kernel>
typename AverageMKL<Scalar, Kernel>::Matrix
AverageMKL<Scalar, Kernel>::predict(const std::vector<Matrix> &kernel_matrices)
{
    if (!_trained)
        throw std::logic_error("The model should be trained first (after "
                               "instantiation or change in parameters)");
    if (kernel_matrices[0].rows() != _number_of_support_vectors)
        throw std::length_error("Similarities must be provided for all support "
                                "vectors; matrices have wrong number of rows.");
    if (kernel_matrices.size() != _number_of_kernels)
        throw std::length_error(
        "Same number of kernels as on training is required");
    _kernels_handler.set_matrices(kernel_matrices);
    return get_optimal_kernel();
}

template <typename Scalar, typename Kernel>
typename AverageMKL<Scalar, Kernel>::Column
AverageMKL<Scalar, Kernel>::get_weights() const
{
    if (!_trained)
        _logger->debug(
        "The model has not been fit. Maybe the parameters were changed?");
    return _weights;
}

template <typename Scalar, typename Kernel>
typename AverageMKL<Scalar, Kernel>::Matrix
AverageMKL<Scalar, Kernel>::get_optimal_kernel()
{
    return _kernels_handler.sum(_weights);
}

template <typename Scalar, typename Kernel>
void AverageMKL<Scalar, Kernel>::set_weights(const Column &weights)
{
    if (weights.rows() != _number_of_kernels)
        throw std::length_error(
        "passed weights does not have one weight for each kernel");
    _weights = weights;
}

} // namespace models
} // namespace mimkl

#endif /* INCLUDE_MIMKL_MODELS_AVERAGE_HPP_ */


#include "in_process_pipe.hpp"
#include "byte_counter.hpp"
#include <benchmark/benchmark.h>
#include "benchmark_interfaces.hpp"
#include <silicium/error_or.hpp>
#include <test/checkpoint.hpp>
#include <boost/asio/io_service.hpp>
#include <queue>

namespace
{
    struct my_hello_service_a : async_benchmark_service_a
    {
        void evaluate(std::tuple<std::uint64_t, std::uint64_t> argument,
                      std::function<void(Si::error_or<std::uint64_t>)> on_result) override
        {
            on_result(std::get<0>(argument) * std::get<1>(argument));
        }
    };

    struct my_hello_service_b : async_benchmark_service_b
    {
        void evaluate(std::string argument,
                      std::function<void(Si::error_or<std::uint64_t>)> on_result) override
        {
            on_result(argument.size());
        }
    };

    template <class ClientServerProvider, class LoopBody>
    void BenchmarkWithInProcessPipe(benchmark::State &state, ClientServerProvider const &provider,
                                    LoopBody const &body)
    {
        boost::asio::io_service io;
        warpcoil::byte_counter<warpcoil::in_process_pipe> client_to_server(io);
        warpcoil::byte_counter<warpcoil::in_process_pipe> server_to_client(io);

        provider(client_to_server, server_to_client,
                 [&io, &state, &body](auto &client, auto &server)
                 {
                     while (state.KeepRunning())
                     {
                         body(io, client, server);
                     }
                 });

        std::uint64_t const total_transferred = client_to_server.read + client_to_server.written;
        if (total_transferred > (std::numeric_limits<std::size_t>::max)())
        {
            boost::throw_exception(std::logic_error("Transferred bytes overflowed"));
        }
        state.SetBytesProcessed(static_cast<size_t>(total_transferred));
    }

    void TinyRequestAndResponseOverInProcessPipe(benchmark::State &state)
    {
        BenchmarkWithInProcessPipe(
            state,
            [](warpcoil::byte_counter<warpcoil::in_process_pipe> &client_to_server,
               warpcoil::byte_counter<warpcoil::in_process_pipe> &server_to_client, auto &&user)
            {
                my_hello_service_a server_impl;
                warpcoil::cpp::message_splitter<decltype(client_to_server)> server_splitter(
                    client_to_server);
                warpcoil::cpp::buffered_writer<decltype(server_to_client)> server_writer(
                    server_to_client);
                async_benchmark_service_a_server<decltype(server_impl), decltype(client_to_server),
                                                 decltype(server_to_client)> server(server_impl,
                                                                                server_splitter,
                                                                                server_writer);

                warpcoil::cpp::message_splitter<decltype(server_to_client)> client_splitter(
                    server_to_client);
                warpcoil::cpp::buffered_writer<decltype(client_to_server)> client_writer(
                    client_to_server);
                async_benchmark_service_a_client<
                    decltype(server_to_client), decltype(client_to_server)> client(client_writer,
                                                                                client_splitter);

                user(client, server);
            },
            [](boost::asio::io_service &io, auto &client, auto &server)
            {
                warpcoil::checkpoint client_side;
                client.evaluate(std::make_tuple(34, 45),
                                [&client_side](Si::error_or<std::uint64_t> result)
                                {
                                    client_side.enter();
                                    if (result.get() != (34u * 45u))
                                    {
                                        boost::throw_exception(std::logic_error("wrong result"));
                                    }
                                });
                warpcoil::checkpoint server_side;
                server.serve_one_request([&server_side](boost::system::error_code ec)
                                         {
                                             server_side.enter();
                                             Si::throw_if_error(ec);
                                         });
                server_side.enable();
                client_side.enable();
                io.reset();
                io.run();
            });
    }
    BENCHMARK(TinyRequestAndResponseOverInProcessPipe);

    void MediumRequestAndResponseOverInProcessPipe(benchmark::State &state)
    {
        std::string const message_content(state.range_x(), 'a');
        BenchmarkWithInProcessPipe(
            state,
            [](warpcoil::byte_counter<warpcoil::in_process_pipe> &client_to_server,
               warpcoil::byte_counter<warpcoil::in_process_pipe> &server_to_client, auto &&user)
            {
                my_hello_service_b server_impl;
                warpcoil::cpp::message_splitter<decltype(client_to_server)> server_splitter(
                    client_to_server);
                warpcoil::cpp::buffered_writer<decltype(server_to_client)> server_writer(
                    server_to_client);
                async_benchmark_service_b_server<decltype(server_impl), decltype(client_to_server),
                                                 decltype(server_to_client)> server(server_impl,
                                                                                server_splitter,
                                                                                server_writer);

                warpcoil::cpp::message_splitter<decltype(server_to_client)> client_splitter(
                    server_to_client);
                warpcoil::cpp::buffered_writer<decltype(client_to_server)> client_writer(
                    client_to_server);
                async_benchmark_service_b_client<
                    decltype(server_to_client), decltype(client_to_server)> client(client_writer,
                                                                                client_splitter);

                user(client, server);
            },
            [&message_content](boost::asio::io_service &io, auto &client, auto &server)
            {
                warpcoil::checkpoint client_side;
                client.evaluate(message_content,
                                [&client_side, &message_content](Si::error_or<std::uint64_t> result)
                                {
                                    client_side.enter();
                                    if (result.get() != message_content.size())
                                    {
                                        boost::throw_exception(std::logic_error("wrong result"));
                                    }
                                });
                warpcoil::checkpoint server_side;
                server.serve_one_request([&server_side](boost::system::error_code ec)
                                         {
                                             server_side.enter();
                                             Si::throw_if_error(ec);
                                         });
                server_side.enable();
                client_side.enable();
                io.reset();
                io.run();
            });
    }
    BENCHMARK(MediumRequestAndResponseOverInProcessPipe)->Range(1, 1 << 20);
}


// Copyright 2021 Jan Niklas Hasse <jhasse@bixense.com>
// For conditions of distribution and use, see copyright notice in LICENSE.txt
/// Contains jngl::Mat3 class
/// @file
#pragma once

#if !defined(__has_include) || __has_include("boost/version.hpp")
#include <boost/version.hpp>
#endif
#include <initializer_list>

namespace jngl {

class Vec2;

/// 3x3 matrix
///
/// identical to boost::qvm::mat<float, 3, 3>, but stores the elements column-major in memory
class Mat3 {
public:
	/// creates identity matrix
	Mat3() = default;

	/// construct matrix from row-major array with 9 elements
	Mat3(std::initializer_list<float>);

	/// Multiplies the matrix with a translation matrix generated from v
	///
	/// \return *this
	Mat3& translate(const Vec2& v);

	/// Multiplies the matrix by a scaling matrix
	///
	/// Equivalent to calling `Mat3::scale(factor, factor)`.
	/// \return *this
	Mat3& scale(float factor);

	/// Multiplies the matrix by a scaling matrix
	///
	/// \return *this
	///
	/// If you want to draw a sprite with double the width, but the correct height:
	/// \code
	/// yourSprite.draw(jngl::modelview().scale(2, 1));
	/// \endcode
	Mat3& scale(float xfactor, float yfactor);

	/// Multiplies the matrix with a rotation matrix
	///
	/// \return *this
	Mat3& rotate(float radian);

	/// column-major
	float data[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
};

} // namespace jngl

#if BOOST_VERSION >= 106200 || __has_include("boost/qvm/mat_traits.hpp")
#include <boost/qvm/mat_traits.hpp>

namespace boost::qvm {
template <> struct mat_traits<jngl::Mat3> {
	static int const rows = 3;
	static int const cols = 3;
	using scalar_type = float;

	template <int R, int C> static scalar_type read_element(const jngl::Mat3& m) {
		return m.data[C * 3 + R];
	}
	template <int R, int C> static scalar_type& write_element(jngl::Mat3& m) {
		return m.data[C * 3 + R];
	}
};
} // namespace boost::qvm
#endif


// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef INT_PART_HH
#define INT_PART_HH

#include "config.h"

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

#include "cache.hh"

namespace graph_tool
{
using namespace boost;

void init_q_cache(size_t n_max);
double q_rec(int n, int k);
double q_rec_memo(int n, int k);
double log_q_approx(size_t n, size_t k);
double log_q_approx_big(size_t n, size_t k);
double log_q_approx_small(size_t n, size_t k);

extern boost::multi_array<double, 2> __q_cache;

template <class T>
double log_q(T n, T k)
{
    if (n <= 0 || k < 1)
        return 0;
    if (k > n)
        k = n;
    if (n < T(__q_cache.shape()[0]))
        return __q_cache[n][k];
    return log_q_approx(n, k);
}

}
#endif // INT_PART_HH


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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 __LIBV_LMA_OPTIMISATION2_CONTAINER_BLAZE_HPP__
#define __LIBV_LMA_OPTIMISATION2_CONTAINER_BLAZE_HPP__

#include "tag.hpp"
#include <blaze/Blaze.h>
#include <libv/lma/ttt/traits/naming.hpp>
#include <libv/lma/ttt/traits/wrap.hpp>
#include <libv/lma/string/string_utiliy.hpp>
#include <boost/fusion/include/pair.hpp>
#include "container.hpp"
#include <libv/lma/ttt/traits/unroll1.hpp>
#include <boost/mpl/if.hpp>

namespace std
{
  template<int I, int J, size_t N, class Float> inline const Float& get(const blaze::StaticVector<Float,N>& mat)
  {
    static_assert( I < N , "std::get<I,J>(blaze)");
    static_assert( J == 0 , "std::get<I,J>(toon)");
    return mat[I];
  }

  template<int I, int J, size_t N, size_t M, class Float> inline const Float& get(const blaze::StaticMatrix<Float,N,M>& mat)
  {
    static_assert( I < N , "std::get<I,J>(blaze)");
    static_assert( I < M , "std::get<I,J>(blaze)");
    return mat(I,J);
  }

  template<int I, int J, size_t N, class Float> inline Float& get(blaze::StaticVector<Float,N>& mat)
  {
    static_assert( I < N , "std::get<I,J>(blaze)");
    static_assert( J == 0 , "std::get<I,J>(blaze)");
    return mat[I];
  }

  template<int I, int J, size_t N, size_t M, class Float> inline Float& get(blaze::StaticMatrix<Float,N,M>& mat)
  {
    static_assert( I < N , "std::get<I,J>(blaze)");
    static_assert( I < M , "std::get<I,J>(blaze)");
    return mat(I,J);
  }
}

namespace lma
{
  template<std::size_t I, std::size_t J, class flt> struct ContainerOption<boost::fusion::pair<Blaze,flt>,I,J>
  {
    typedef flt Float;
    typedef typename
      boost::mpl::if_c<
                        J==1,
                        blaze::StaticVector<Float,I>,
                        blaze::StaticMatrix<Float,I,J>
                      >::type Matrix;

    static Matrix Zero() { return Matrix(0); }
    typedef blaze::DynamicMatrix<Float> MatrixDD;
    typedef blaze::DynamicVector<Float> MatrixD1;
  };

  template<class Float, int N> struct Size<blaze::StaticVector<Float,N>> { enum { value = N }; };
  template<class Float, int N> struct Size<blaze::StaticMatrix<Float,N,1>> { enum { value = N }; };

  template<class Float, int Rows, int Cols> struct Rows<blaze::StaticMatrix<Float,Rows,Cols>>
  {
    enum { value = Rows };
  };

  template<class Float, int Rows, int Cols> struct Cols<blaze::StaticMatrix<Float,Rows,Cols>>
  {
    enum { value = Cols };
  };

  template<class Float, std::size_t I, std::size_t J>
  bool is_invalid(const blaze::StaticMatrix<Float,I,J>& mat)
  {
    for(int i = 0 ; i < I ; ++i)
      for(int j = 0 ; j < J ; ++j)
      if (is_invalid(mat(i,j)))
        return true;
    return false;
  }

  template<class Float>
  bool is_invalid(const blaze::DynamicVector<Float>& mat)
  {
    for(size_t i = 0 ; i < mat.size() ; ++i)
      if (is_invalid(mat[i]))
        return true;
    return false;
  }

  template<class Float, std::size_t N> const Float& at(const blaze::StaticMatrix<Float,N,N>& m, int i, int j)
  {
    return m(i,j);
  }

  template<class Float, std::size_t N> Float& at(blaze::StaticMatrix<Float,N,N>& m, int i, int j)
  {
    return m(i,j);
  }

  template<class Float, std::size_t N> Float& at(blaze::StaticVector<Float,N>& m, int i, int j)
  {
    plz_no_warning(j);
    assert(j==0);
    return m[i];
  }

  template<class Float, std::size_t N> const Float& at(const blaze::StaticVector<Float,N>& m, int i, int j)
  {
    plz_no_warning(j);
    assert(j==0);
    return m[i];
  }

  template<class Float> void set_zero(blaze::DynamicMatrix<Float>& mat)
  {
    mat.reset();
  }

  template<class Float, size_t I> void set_zero(blaze::StaticVector<Float,I>& mat)
  {
    mat.reset();
  }

  template<class Float, size_t I, size_t J> void set_zero(blaze::StaticMatrix<Float,I,J>& mat)
  {
    mat.reset();
  }

  template<class Float, size_t I>
  const Float* get_ptr(const blaze::StaticVector<Float,I>& m)
  {
    return m.data();
  }

  template<class Float, size_t I, size_t J, bool b>
  auto transpose(const blaze::StaticMatrix<Float,I,J,b>& mat)
  {
    return blaze::trans(mat);
  }

  template<class Float> void inverse_in_place(blaze::StaticMatrix<Float,3,3>& mat)
  {
    const Float
      a = mat(0,0),
      b = mat(0,1),
      c = mat(0,2),
      e = mat(1,1),
      f = mat(1,2),
      i = mat(2,2),
      f2 = f*f,
      fc = f*c,
      ae = a*e,
      b_2 = b*b,
      c_2 = c*c,
      det = 1.0 / (ae*i + b*fc * 2.0 - c_2*e - f2*a - i*b_2);

    mat(0,0) = (e*i - f2) * det;
    mat(1,0) = mat(0,1) = (fc - b*i) * det;
    mat(1,1) = (a*i - c_2) * det;
    mat(2,1) = mat(1,2) = (b*c - a*f) * det;
    mat(2,0) = mat(0,2) = (b*f - e*c) * det;
    mat(2,2) = (ae - b_2) * det;
  }
}

namespace ttt
{
  template<> struct Name<lma::Blaze>
  { static std::string name() { return "Blaze"; } };

  template<class Float, size_t I, size_t J>
  struct Name<blaze::StaticMatrix<Float,I,J>>
  { static std::string name() { return "Blaze::Matrix<" + lma::to_string(I) + "," + lma::to_string(J) + "," + ttt::name<Float>() + ">"; } };

  template<class Float, size_t I>
  struct Name<blaze::StaticVector<Float,I>>
  { static std::string name() { return "Blaze::Vector<" + lma::to_string(I) + "," + ttt::name<Float>() + ">"; } };
}

#endif


#include "autotune/execution_wrapper.hpp"
#include "autotune/queue_thread_pool.hpp"
#include "parameters.hpp"
#include <boost/align/aligned_allocator.hpp>
#include <chrono>
#include <iostream>
#include <omp.h>
#include <vector>

using high_resolution_clock = std::chrono::high_resolution_clock;
using time_point = std::chrono::high_resolution_clock::time_point;

#include "opttmp/vectorization/register_tiling.hpp"
using reg_arr =
    opttmp::vectorization::register_array<Vc::double_v, REG_BLOCKING>;
using align = boost::alignment::aligned_allocator<double, 64>;

void print_bandwidth(double bytes_traffic, double duration_s_avr) {
  double bandwidth_gb = (bytes_traffic / duration_s_avr) * 1E-9;
  std::cout << "bandwidth GB/s: " << bandwidth_gb << std::endl;
}

AUTOTUNE_EXPORT void triad(const size_t N_per_task, const size_t giga_bytes_min,
                           double &duration_kernel) {
  size_t bytes_transfer = N_per_task * 8 * 3 * KERNEL_THREADS;
  std::cout << "bytes overall: " << (bytes_transfer * 1E-9) << "GB"
            << std::endl;
  size_t repeat = giga_bytes_min * 1E9 / bytes_transfer;
  repeat += 1;
  if (repeat < 100) {
    repeat = 100;
  }
  std::cout << "calculated repeat: " << repeat << std::endl;

  // const size_t N = N_per_task * KERNEL_THREADS;
  std::array<std::vector<double, align>, KERNEL_THREADS> a;
  std::array<std::vector<double, align>, KERNEL_THREADS> b;
  std::array<std::vector<double, align>, KERNEL_THREADS> c;
  const double q = 135.0;
  const Vc::double_v q_vec = q;

  autotune::queue_thread_pool<KERNEL_THREADS> pool;
  pool.set_affinity(static_cast<autotune::affinity_type_t>(AFFINITY_POLICY));
  pool.start();

  std::function<void(size_t)> f_init = [&](size_t thread_id) {
    a[thread_id].resize(N_per_task);
    for (size_t i = 0; i < N_per_task; i += 1) {
      a[thread_id][i] = 0.0;
    }
    b[thread_id].resize(N_per_task);
    for (size_t i = 0; i < N_per_task; i += 1) {
      b[thread_id][i] = static_cast<double>(i);
    }
    c[thread_id].resize(N_per_task);
    for (size_t i = 0; i < N_per_task; i += 1) {
      c[thread_id][i] = static_cast<double>(i);
    }
  };

  for (size_t i = 0; i < KERNEL_THREADS; i += 1) {
    pool.enqueue_work_id(f_init, static_cast<size_t>(0));
  }

  {
    time_point start = high_resolution_clock::now();
    for (size_t r = 0; r < repeat; r += 1) {
      std::function<void(size_t)> f = [&](size_t thread_id) {
        std::vector<double, align> &a_thread = a[thread_id];
        std::vector<double, align> &b_thread = b[thread_id];
        std::vector<double, align> &c_thread = c[thread_id];
        for (size_t i = 0; i < N_per_task;
             i += REG_BLOCKING * Vc::double_v::size()) {
          reg_arr temp(&b_thread[i], Vc::flags::vector_aligned);
          reg_arr temp2(&c_thread[i], Vc::flags::vector_aligned);
          temp = temp + q_vec * temp2;
          temp.memstore(&a_thread[i], Vc::flags::vector_aligned);
        }
      };

      for (size_t i = 0; i < KERNEL_THREADS; i += 1) {
        pool.enqueue_work_id(f, static_cast<size_t>(0));
      }
    }
    pool.finish();
    time_point end = high_resolution_clock::now();
    double duration_s_avr =
        std::chrono::duration<double>(end - start).count() / repeat;
    std::cout << "duration_s_avr: " << duration_s_avr << std::endl;
    double bytes_copied = 8.0 * 4.0 * N_per_task * KERNEL_THREADS;
    std::cout << "triad: 3 read, 1 write" << std::endl;
    print_bandwidth(bytes_copied, duration_s_avr);
    duration_kernel = duration_s_avr / (bytes_copied * 1E-9); // s per GB
  }
}


#ifndef TVMTL_TVMINIMIZER_PRPT_HPP
#define TVMTL_TVMINIMIZER_PRPT_HPP

//System includes
#include <iostream>
#include <map>
#include <vector>
#include <chrono>

#ifdef TVMTL_TVMIN_DEBUG
    #include <string>
#endif

//Eigen includes
#include <Eigen/Sparse>
#include <Eigen/Core>
#include <unsupported/Eigen/Splines>

//CGAL includes For linear Interpolation
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Interpolation_traits_2.h>
#include <CGAL/natural_neighbor_coordinates_2.h>
#include <CGAL/interpolation_functions.h>

//vpp includes
#include <vpp/vpp.hh>

namespace tvmtl {

template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
    class TV_Minimizer< PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR > {

	public:
	    // Manifold typedefs
	    typedef typename MANIFOLD::scalar_type scalar_type;
	    typedef typename MANIFOLD::value_type value_type;

	    // Functional typedefs
	    typedef typename FUNCTIONAL::weights_mat weights_mat;
	    typedef typename FUNCTIONAL::weights_type weights_type;

	    // Data acccess typedefs
	    typedef vpp::box_nbh2d<value_type,3,3> nbh_type;
	    typedef typename DATA::storage_type img_type;

	    // Constructor
	    TV_Minimizer(FUNCTIONAL& func, DATA& dat):
		func_(func),
		data_(dat)
	    {
		static_assert(FUNCTIONAL::disc_type == ANISO, "Proximal point is only possible with anisotropic weights!");

		for(int i = 0; i < 5; i++){
		    proximal_mappings_[i] = img_type(data_.img_.domain());
		}
		use_approximate_mean_ = false;
		max_prpt_steps_ = 50;
		tolerance_ = 1e-4;
		max_runtime_ = 1000.0;
	    }

	    void use_approximate_mean(bool u) { use_approximate_mean_ = u; }

	    void first_guess();

	    void updateFidelity(double muk);
	    void updateTV(double muk, int dim, const weights_mat& W);

	    void geod_mean();
	    void approx_mean();

	    void prpt_step(double muk);
	    void minimize();

	    void setMax_runtime(int t) { max_runtime_ = t; }
	    void setMax_prpt_steps(int n) { max_prpt_steps_ = n; }
	    void setTolerance(double t) {tolerance_ =t; }

	    int max_runtime(int t) const { return max_runtime_; }
	    int max_prpt_steps(int n) const { return max_prpt_steps_; }
	    int tolerance(double t) const { return tolerance_; }

	private:
	    FUNCTIONAL& func_;
	    DATA& data_;

	    img_type proximal_mappings_[5];

	    bool use_approximate_mean_;

	    int prpt_step_;
	    int max_prpt_steps_;
	    double tolerance_;
	    double max_runtime_;

	    std::vector< std::chrono::duration<double> > Ts_;
	    std::vector< typename FUNCTIONAL::result_type > Js_;
    };

/*----- IMPLEMENTATION PRPT------*/
template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
void TV_Minimizer<PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR>::updateFidelity(double muk){
    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t Fidelity Update with muk: " << muk << std::endl;
	    func_.output_img(data_.img_, "prptImg.csv");
	    func_.output_img(data_.noise_img_,"prptNoiseImg.csv");
	    func_.output_img(proximal_mappings_[0],"prptProxMap.csv");
    #endif

    double tau = muk / (muk + 1.0);

    if(data_.doInpaint()){
	auto proximal_mapF = [&] (value_type& p, const value_type& i, const value_type& n, const bool inp ) {
	    value_type temp;
	    MANIFOLD::convex_combination(i, n, tau, temp);
	    p = temp * (1-inp) + i * inp;
	};

	vpp::pixel_wise(proximal_mappings_[0], data_.img_, data_.noise_img_, data_.inp_) | proximal_mapF;
    }
    else{
    	auto proximal_mapF = [&] (value_type& p, const value_type& i, const value_type& n) {
	    MANIFOLD::convex_combination(i, n, tau, p);
	};

	vpp::pixel_wise(proximal_mappings_[0], data_.img_, data_.noise_img_) | proximal_mapF;
    }

}

template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
void TV_Minimizer<PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR>::updateTV(double muk, int dim, const weights_mat& W){
    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t TV Update with muk: " << muk << std::endl;
	    std::cout << "\t\t... Dimension: " << dim << std::endl;
    #endif
    double lmuk = muk * func_.getlambda();
    weights_mat tau = vpp::pixel_wise(W) | [&] (const weights_type& w) {return std::min(lmuk * w, 0.5); };

    // 2D
    int nr = data_.img_.nrows();
    int nc = data_.img_.ncols();

    // Dimensions of slice
    vpp::vint2 dims(nr, nc); dims(dim) = 2; // y-dim = 0, x-dim = 1
    vpp::vint2 loopdims(nr, nc); loopdims(dim) = 1; // y-dim = 0, x-dim = 1
    //vpp::vint3 b(nz, ny, nx); dims(dim) =2;

    // Relative neighbor coordinates
    vpp::vint2 n(0,0); n(dim) = 1;

    // Correct Subimage
    vpp::box2d subimage = data_.img_.domain();
    if(dim == 0  && (nr % 2 != 0)) // dim = y
	    subimage =  vpp::box2d(vpp::vint2(0,0), vpp::vint2(nr-2, nc-1)); // subdomain without last row

    if(dim == 1  &&  (nc % 2 != 0)) // dim =x
	    subimage = vpp::box2d(vpp::vint2(0,0), vpp::vint2(nr-1, nc-2)); // subdomain without last columns

    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t\t...Odd Pairs..., dims = (" << dims(0) << "," << dims(1) << "), n = (" << n(0) << "," << n(1) << ")" <<std::endl;
	    int num_blocks = 0;
    #endif

    auto blockwise_proximal_map = [&] (const auto I, const auto T, auto P) {
	for(int r = 0; r < loopdims(0); ++r ){
	    for(int c = 0; c < loopdims(1); ++c){
	    #ifdef TVMTL_TVMIN_DEBUG_VERBOSE
		    std::cout << "\t\t\tBlock: " << num_blocks << ", Row: " << r << ", Col: " << c << std::endl;
	    #endif
		value_type l;
		MANIFOLD::log(I(r,c), I(vpp::vint2(r,c) + n), l);
		MANIFOLD::exp(I(r,c), l * T(r,c), P(r,c));
		MANIFOLD::exp(I(r,c), l - l * T(r,c), P(vpp::vint2(r,c) + n));
	    }
	}
    #ifdef TVMTL_TVMIN_DEBUG
	num_blocks++;
    #endif
    };

    proximal_mappings_[1 + 2 * dim] = vpp::clone(data_.img_);
    vpp::block_wise(dims, data_.img_ | subimage, tau | subimage, proximal_mappings_[1 + 2 * dim] | subimage) | blockwise_proximal_map;
    #ifdef TVMTL_TVMIN_DEBUG
	std::cout << "\t\tBlock processed: " << num_blocks << std::endl;
    #endif

    if(dim == 0 /* y */)
	if(nr % 2 == 0)
	    subimage =  vpp::box2d(vpp::vint2(1,0), vpp::vint2(nr-2, nc-1)); // subdomain without first and last row
	else
	    subimage =  vpp::box2d(vpp::vint2(1,0), vpp::vint2(nr-1, nc-1)); // subdomain without first row

    if(dim == 1 /* x */)
	if(nc % 2 == 0)
	    subimage = vpp::box2d(vpp::vint2(0,1), vpp::vint2(nr-1, nc-2)); // subdomain without first and last column
	else
	    subimage = vpp::box2d(vpp::vint2(0,1), vpp::vint2(nr-1, nc-1)); // subdomain without first column

    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t\t...Even Pairs..." << std::endl;
	    num_blocks = 0;
    #endif
    proximal_mappings_[2 + 2 * dim] = vpp::clone(data_.img_);
    vpp::block_wise(dims, data_.img_ | subimage, tau | subimage, proximal_mappings_[2 + 2 * dim] | subimage) | blockwise_proximal_map;

    #ifdef TVMTL_TVMIN_DEBUG
	std::cout << "\t\tBlocks processed: " << num_blocks << std::endl;
    #endif

}

template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
void TV_Minimizer<PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR>::geod_mean(){
    typedef value_type& vtr;
    typedef const vtr cvtr;

    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t Calculate Geodesic mean: "<< std::endl;
	    std::cout << "\t\t...first guess euclidiean mean: "<< std::endl;
    #endif

    // First estimate
    auto euclidian_mean = [&] (vtr i, cvtr p0, cvtr p1, cvtr p2, cvtr p3, cvtr p4) {
	i = (p0 + p1 + p2 + p3 + p4) / 5.0;
    };
    vpp::pixel_wise(data_.img_, proximal_mappings_[0], proximal_mappings_[1], proximal_mappings_[2], proximal_mappings_[3], proximal_mappings_[4]) | euclidian_mean;

    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t\t...Karcher mean Iteration: "<< std::endl;
    #endif

    //TODO: Put in Algo traits
    double tol = 1e-10;
    double max_karcher_iterations = 15;

    auto karcher_mean = [&] (vtr i, cvtr p0, cvtr p1, cvtr p2, cvtr p3, cvtr p4) {

	/* Slow version, - needs to copy p_i for insertion in value_list
	typename MANIFOLD::value_list v;
	v.push_back(p0);
	v.push_back(p1);
	v.push_back(p2);
	v.push_back(p3);
	v.push_back(p4);
	MANIFOLD::karcher_mean(i, v, tol, max_karcher_iterations); */

	MANIFOLD::karcher_mean(i, p0, p1, p2, p3, p4);
    };

    vpp::pixel_wise(data_.img_, proximal_mappings_[0], proximal_mappings_[1], proximal_mappings_[2], proximal_mappings_[3], proximal_mappings_[4]) | karcher_mean;

}

template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
void TV_Minimizer<PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR>::approx_mean(){
    typedef value_type& vtr;
    typedef const vtr cvtr;

    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t Calculate approximate mean via convex combinations: "<< std::endl;
    #endif

    auto convex_comb_mean = [&] (vtr i, cvtr p0, cvtr p1, cvtr p2, cvtr p3, cvtr p4) {
	value_type p01, p23;
	MANIFOLD::convex_combination(p0, p1, 0.5, p01);
	MANIFOLD::convex_combination(p2, p3, 0.5, p23);
	MANIFOLD::convex_combination(p01, p23, 0.5, i);
	MANIFOLD::convex_combination(i, p4, 0.2, i);
    };

    vpp::pixel_wise(data_.img_, proximal_mappings_[0], proximal_mappings_[1], proximal_mappings_[2], proximal_mappings_[3], proximal_mappings_[4]) | convex_comb_mean;

}

template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
void TV_Minimizer<PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR>::prpt_step(double muk){
    #ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t Proximal step with muk: " << muk << std::endl;
    #endif
    weights_mat X = func_.getweightsX();
    weights_mat Y = func_.getweightsY();

    updateFidelity(muk);

    updateTV(muk, 0, Y);
    updateTV(muk, 1, X);

    if(use_approximate_mean_)
	approx_mean();
    else
	geod_mean();
}

template <class FUNCTIONAL, class MANIFOLD, class DATA, enum PARALLEL PAR>
void TV_Minimizer<PRPT, FUNCTIONAL, MANIFOLD, DATA, PAR>::minimize(){
    std::cout << "Starting Proximal Point Algorithm with..." << std::endl;
    std::cout << "\t Lambda = \t" << func_.getlambda() << std::endl;
    std::cout << "\t Max Steps = \t" << max_prpt_steps_ << std::endl;

    prpt_step_ = 1;

    std::chrono::time_point<std::chrono::system_clock> start, end;
    std::chrono::duration<double> t = std::chrono::duration<double>::zero();
    start = std::chrono::system_clock::now();

    // PRPT Iteration Loop
    while(prpt_step_ <= max_prpt_steps_ && t.count() < max_runtime_){

	std::cout << "PRPT Step #" << prpt_step_ << std::endl;
	typename FUNCTIONAL::result_type J = func_.evaluateJ();
	Js_.push_back(J);
	std::cout << "\t Value of Functional J: " << J << std::endl;

	double muk = 3.0 * std::pow(static_cast<double>(prpt_step_),-0.95);
	#ifdef TVMTL_TVMIN_DEBUG
	    std::cout << "\t Value of muk: " << muk << std::endl;
	#endif

	prpt_step(muk);

	end = std::chrono::system_clock::now();
	t = end - start;
	std::cout << "\t Elapsed time: " << t.count() << " seconds." << std::endl;
	prpt_step_++;
	Ts_.push_back(t);
    }

    std::cout << "Minimization in " << t.count() << " seconds." << std::endl;
}

} // end namespace tvmtl

#endif