///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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)

// This file provides two traits:
//  boost::multiprecision::complex<Real>                 - creates a coplex type supported by Boost
//  boost::multiprecision::is_boost_complex<T>::value    - a bool telling whether the type is a complex number supported by Boost
//                                                         it has nothing to do with boost::is_complex<T> which recognizes only std::complex<T>
//
// TODO: this file should be split apart and each specializetion should go to the end of the relevant file in Boost.Multiprecision
//   --> specialization for float128      --> complex128    goes to <boost/multiprecision/complex128.hpp>
//   --> specialization for cpp_bin_float --> cpp_complex   goes to <boost/multiprecision/cpp_complex.hpp>
//   --> specialization for mpfr_float    --> mpc_complex   goes to <boost/multiprecision/mpc.hpp>

#ifndef BOOST_MATH_FFT_MULTIPRECISION_COMPLEX
  #define BOOST_MATH_FFT_MULTIPRECISION_COMPLEX
  #include <boost/config.hpp>
  #include <boost/cstdfloat.hpp>
  #if defined(BOOST_MATH_USE_FLOAT128)
  #include <boost/multiprecision/float128.hpp>
  #endif
  #include <boost/multiprecision/cpp_bin_float.hpp>
  #include <boost/multiprecision/cpp_dec_float.hpp>
  #if defined(__GNUC__)
  //#include <boost/multiprecision/mpfr.hpp>
  #endif
  #include <boost/core/demangle.hpp>
  #include <boost/core/enable_if.hpp>
  // now include complex types
  #if defined(BOOST_MATH_USE_FLOAT128)
  #include <boost/multiprecision/complex128.hpp>
  #endif
  #include <boost/multiprecision/cpp_complex.hpp>
  #if defined(__GNUC__)
  //#include <boost/multiprecision/mpc.hpp>
  #endif

  namespace boost { namespace multiprecision { namespace detail {

    template<typename T>
    struct make_boost_complex {
      using type = std::complex<T>;
    };

    // float128 --> <boost/multiprecision/complex128.hpp>
    #if defined(BOOST_MATH_USE_FLOAT128)
    template<>
    struct make_boost_complex<boost::multiprecision::float128> {
      using type = boost::multiprecision::complex128;
    };
    #endif

    // cpp_bin_float --> <boost/multiprecision/cpp_complex.hpp>
    template <unsigned Digits, backends::digit_base_type DigitBase, class Allocator, class Exponent, Exponent MinExponent, Exponent MaxExponent, expression_template_option ExpressionTemplates>
    struct make_boost_complex< number< cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinExponent, MaxExponent>, ExpressionTemplates> > {
      using type = number<complex_adaptor<cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinExponent, MaxExponent>>, ExpressionTemplates>;
    };

    #if defined(__GNUC__)
    // mpc_complex --> <boost/multiprecision/mpc.hpp>
    //template <unsigned Digits, mpfr_allocation_type AllocationType, expression_template_option ExpressionTemplates>
    //struct make_boost_complex< number< backends::mpfr_float_backend<Digits, AllocationType>, ExpressionTemplates> > {
    //  using type = number<mpc_complex_backend<Digits>, ExpressionTemplates>;
    //};
    #endif

    #if __cplusplus < 201700L
    template<typename... Ts> struct make_void { typedef void type;};
    template<typename... Ts> using void_t = typename make_void<Ts...>::type;
    #else
    using std::void_t;
    #endif

    template< class, class = void > struct has_value_type : std::false_type { };
    template< class T > struct has_value_type<T, void_t<typename T::value_type>> : std::true_type { };

    template <typename T, bool = has_value_type<T>::value > struct is_boost_complex {
      static constexpr bool value = false;
    };

    template <typename T> struct is_boost_complex<T, true> {
      static constexpr bool value = std::is_same<
                                      typename make_boost_complex<typename T::value_type>::type
                                    , typename std::decay<T>::type
                                    >::value;
    };

  }  // boost::multiprecision::detail
  template<typename T> using complex          = typename detail::make_boost_complex<T>::type;
  template<typename T> using is_boost_complex = detail::is_boost_complex<T>; // may be a duplicate of https://github.com/BoostGSoC21/math/pull/8#discussion_r660549430

  }} // boost::multiprecision

#endif


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__FEEDBACK__NLP_HPP_
#define SMOOTH__FEEDBACK__NLP_HPP_

/**
 * @file
 * @brief Nonlinear program definition.
 */

#include <Eigen/Core>
#include <Eigen/Sparse>

#include <limits>
#include <optional>

namespace smooth::feedback {

/**
 * @brief Nonlinear Programming Problem
 * \f[
 *  \begin{cases}
 *   \min_{x}    & f(x)                    \\
 *   \text{s.t.} & x_l \leq x \leq x_u     \\
 *               & g_l \leq g(x) \leq g_u
 *  \end{cases}
 * \f]
 * for \f$ f : \mathbb{R}^n \rightarrow \mathbb{R} \f$ and
 * \f$ g : \mathbb{R}^n \rightarrow \mathbb{R}^m \f$.
 */
template<typename T>
concept NLP = requires(
  std::decay_t<T> & nlp,
  const Eigen::Ref<const Eigen::VectorXd> x,
  const Eigen::Ref<const Eigen::VectorXd> lambda)
{
  // clang-format off
  {nlp.n()} -> std::convertible_to<std::size_t>;
  {nlp.m()} -> std::convertible_to<std::size_t>;

  // variable bounds
  {nlp.xl()} -> std::convertible_to<Eigen::VectorXd>;
  {nlp.xu()} -> std::convertible_to<Eigen::VectorXd>;

  // objective and its derivatives
  {nlp.f(x)}     -> std::convertible_to<double>;
  {nlp.df_dx(x)} -> std::convertible_to<Eigen::SparseMatrix<double>>;

  // constraint, its bounds, and its derivatives
  {nlp.g(x)}     -> std::convertible_to<Eigen::VectorXd>;
  {nlp.gl()}     -> std::convertible_to<Eigen::VectorXd>;
  {nlp.gu()}     -> std::convertible_to<Eigen::VectorXd>;
  {nlp.dg_dx(x)} -> std::convertible_to<Eigen::SparseMatrix<double>>;
  // clang-format on
};

/**
 * @brief Nonlinear Programming Problem with Hessian information
 */
template<typename T>
concept HessianNLP =
  NLP<T> && requires(std::decay_t<T> & nlp, Eigen::VectorXd x, Eigen::VectorXd lambda)
{
  // clang-format off
  {nlp.d2f_dx2(x)}         -> std::convertible_to<Eigen::SparseMatrix<double>>;
  {nlp.d2g_dx2(x, lambda)} -> std::convertible_to<Eigen::SparseMatrix<double>>;
  // clang-format on
};

/**
 * @brief Solution to a Nonlinear Programming Problem
 */
struct NLPSolution
{
  /// @brief Solver status
  enum class Status {
    Optimal,
    PrimalInfeasible,
    DualInfeasible,
    MaxIterations,
    MaxTime,
    Unknown,
  };

  /// @brief Solver status
  Status status;

  /// @brief Number of iterations
  std::size_t iter{0};

  /// @brief Variable values
  Eigen::VectorXd x;

  ///@{
  /// @brief Inequality multipliers
  Eigen::VectorXd zl, zu;
  ///@}

  /// @brief Constraint multipliers
  Eigen::VectorXd lambda;

  /// @brief Objective
  double objective{0};
};

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__NLP_HPP_


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include <boost/make_shared.hpp>

#include "rttbDVHCalculator.h"
#include "rttbNullPointerException.h"
#include "rttbInvalidParameterException.h"

namespace rttb
{
	namespace core
	{

		DVHCalculator::DVHCalculator(DoseIteratorPointer aDoseIterator, const IDType& aStructureID,
		                             const IDType& aDoseID,
		                             DoseTypeGy aDeltaD, const int aNumberOfBins)
		{
			if (aDoseIterator == nullptr)
			{
				throw NullPointerException("aDoseIterator must not be nullptr! ");
			}

			_doseIteratorPtr = aDoseIterator;
			_structureID = aStructureID;
			_doseID = aDoseID;

			if (aNumberOfBins <= 0 || aDeltaD < 0)
			{
				throw InvalidParameterException("aNumberOfBins/aDeltaD must be >0! ");
			}

			_numberOfBins = aNumberOfBins;
			_deltaD = aDeltaD;

			if (_deltaD == 0)
			{
				aDoseIterator->reset();
				DoseTypeGy max = 0;

				while (aDoseIterator->isPositionValid())
				{
					DoseTypeGy currentVal = 0;
					currentVal = aDoseIterator->getCurrentDoseValue();

					if (currentVal > max)
					{
						max = currentVal;
					}

					aDoseIterator->next();
				}

				_deltaD = (max * 1.5 / _numberOfBins);

				if (_deltaD == 0)
				{
					_deltaD = 0.1;
				}
			}

		}

		DVHCalculator::~DVHCalculator() = default;

		DVH::Pointer DVHCalculator::generateDVH()
		{

			std::deque<DoseCalcType> dataDifferential(_numberOfBins, 0);

			// calculate DVH
			_doseIteratorPtr->reset();

			while (_doseIteratorPtr->isPositionValid())
			{
				DoseTypeGy currentVal = 0;
				FractionType voxelProportion = _doseIteratorPtr->getCurrentRelevantVolumeFraction();
				currentVal = _doseIteratorPtr->getCurrentDoseValue();

				auto dose_bin = static_cast<int>(currentVal / _deltaD);

				if (dose_bin < _numberOfBins)
				{
					dataDifferential[dose_bin] += voxelProportion;
				}
				else
				{
					throw InvalidParameterException("_numberOfBins is too small: dose bin out of bounds! ");
				}

				_doseIteratorPtr->next();
			}

			if (boost::dynamic_pointer_cast<MaskedDoseIteratorPointer>(_doseIteratorPtr))
			{
				_dvh = boost::make_shared<DVH>(dataDifferential, _deltaD, _doseIteratorPtr->getCurrentVoxelVolume(),
				                               _structureID,
				                               _doseID, _doseIteratorPtr->getVoxelizationID());
			}
			else
			{
				_dvh = boost::make_shared<DVH>(dataDifferential, _deltaD, _doseIteratorPtr->getCurrentVoxelVolume(),
				                               _structureID,
				                               _doseID);
			}

			return _dvh;
		}

	}//end namespace core
}//end namespace rttb


//
// Created by LEI XU on 4/27/19.
//

#ifndef RASTERIZER_TEXTURE_H
#define RASTERIZER_TEXTURE_H
#include "global.hpp"
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
class Texture{
private:
    cv::Mat image_data;

public:
    Texture(const std::string& name)
    {
        image_data = cv::imread(name);
        cv::cvtColor(image_data, image_data, cv::COLOR_RGB2BGR);
        width = image_data.cols;
        height = image_data.rows;
    }

    int width, height;

    Eigen::Vector3f getColor(float u, float v)
    {
        if(u<0)u = 0;
        if(u>1)u = 1;
        if(v<0)v = 0;
        if(v>1)v = 1;
        auto u_img = u * width;
        auto v_img = (1 - v) * height;
        auto u_min = floor(u_img),v_min = floor(v_img);
        auto u_max = std::min(width*1.0f,ceil(u_img)),v_max = std::min(height*1.0f,ceil(v_img));
        auto rs = (u_img - u_min)/(u_max - u_min);
        auto rt = (v_img - v_min)/(v_max - v_min);
        auto ctop = image_data.at<cv::Vec3b>(v_max,u_min)*(1-rs) + image_data.at<cv::Vec3b>(v_max,u_max)*rs;
        auto cbot = image_data.at<cv::Vec3b>(v_min,u_min)*(1-rs) + image_data.at<cv::Vec3b>(v_min, u_max)*rs;
//        auto u_img2 = (1-u)*width;
//        auto v_img2 = v*height;
//        auto u_img = u*u_img1+(1-u)*u_img2;
//        auto v_img = v*v_img1+(1-v)*v_img2;
       // auto color = cbot*(1-rt)+ctop*rt;
        auto color = image_data.at<cv::Vec3b>(v_img,u_img);
        return Eigen::Vector3f(color[0], color[1], color[2]);
    }

};
#endif //RASTERIZER_TEXTURE_H


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#include <boost/simd/function/scalar/sqrs.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/simd/function/sqrs.hpp>

#endif


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

  Program:   Visualization Toolkit
  Module:    TestBoostAlgorithms.cxx

  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.

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

#include "vtkActor.h"
#include "vtkBoostBetweennessClustering.h"
#include "vtkDataSetAttributes.h"
#include "vtkIntArray.h"
#include "vtkMutableUndirectedGraph.h"
#include "vtkPoints.h"
#include "vtkSmartPointer.h"
#include "vtkVertexListIterator.h"

#include <boost/version.hpp>

#include <map>

int TestBoostBetweennessClustering(int vtkNotUsed(argc),
                                   char* vtkNotUsed(argv)[])
{
  // Create the test graph
  vtkSmartPointer<vtkMutableUndirectedGraph> g
    (vtkSmartPointer<vtkMutableUndirectedGraph>::New());

  vtkSmartPointer<vtkIntArray> weights (
    vtkSmartPointer<vtkIntArray>::New());
  weights->SetName("weights");

  g->GetEdgeData()->AddArray(weights);

  vtkSmartPointer<vtkPoints> pts (vtkSmartPointer<vtkPoints>::New());
  g->AddVertex();
  pts->InsertNextPoint(1, 1, 0);

  g->AddVertex();
  pts->InsertNextPoint(1, 0, 0);

  g->AddVertex();
  pts->InsertNextPoint(1, -1, 0);

  g->AddVertex();
  pts->InsertNextPoint(2, 0, 0);

  g->AddVertex();
  pts->InsertNextPoint(3, 0, 0);

  g->AddVertex();
  pts->InsertNextPoint(2.5, 1, 0);

  g->AddVertex();
  pts->InsertNextPoint(4, 1, 0);

  g->AddVertex();
  pts->InsertNextPoint(4, 0, 0);

  g->AddVertex();
  pts->InsertNextPoint(4, -1, 0);

  g->SetPoints(pts);

  vtkEdgeType e = g->AddEdge(0, 3);
  weights->InsertTuple1(e.Id, 10);

  e = g->AddEdge(1, 3);
  weights->InsertTuple1(e.Id, 10);

  e = g->AddEdge(2, 3);
  weights->InsertTuple1(e.Id, 10);

  e = g->AddEdge(3, 4);
  weights->InsertTuple1(e.Id, 1);

  e = g->AddEdge(3, 5);
  weights->InsertTuple1(e.Id, 10);

  e = g->AddEdge(5, 4);
  weights->InsertTuple1(e.Id, 10);

  e = g->AddEdge(6, 4);
  weights->InsertTuple1(e.Id, 10);

  e = g->AddEdge(7, 4);
  weights->InsertTuple1(e.Id, 10);

  e = g->AddEdge(8, 4);
  weights->InsertTuple1(e.Id, 10);

  // Test centrality
  vtkSmartPointer<vtkBoostBetweennessClustering> bbc (
    vtkSmartPointer<vtkBoostBetweennessClustering>::New());
  bbc->SetInputData(g);
  bbc->SetThreshold(4);
  bbc->SetEdgeWeightArrayName("weights");
  bbc->SetEdgeCentralityArrayName("bbc_centrality");
  bbc->UseEdgeWeightArrayOn();
  bbc->Update();

  vtkGraph* og = bbc->GetOutput();

  if(!og)
    {
    return 1;
    }

  vtkIntArray* compArray = vtkIntArray::SafeDownCast(og->GetVertexData()->
                                                     GetArray("component"));
  if(!compArray)
    {
    return 1;
    }

  // Now lets create the correct mapping so that we can compare the results
  // against it.
  std::map<int, int> expResults;
  expResults[0] = 0;
  expResults[1] = 0;
  expResults[2] = 0;
  expResults[3] = 0;
  expResults[4] = 1;
  expResults[5] = 1;
  expResults[6] = 1;
  expResults[7] = 1;
  expResults[8] = 2;

  vtkSmartPointer<vtkVertexListIterator> vlItr (
    vtkSmartPointer<vtkVertexListIterator>::New());
  vlItr->SetGraph(og);

  while(vlItr->HasNext())
    {
    vtkIdType id = vlItr->Next();

    if(expResults[id] != compArray->GetVariantValue(id).ToInt())
      {
      return 1;
      }
    }

    return 0;
}


/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014, Daichi Yoshikawa
 *  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 Daichi Yoshikawa 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 OWNER 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.
 *
 * Author: Daichi Yoshikawa
 *
 *********************************************************************/

#ifndef __GL_WRAPPER_RENDER_CAMERA_HPP
#define __GL_WRAPPER_RENDER_CAMERA_HPP

#include <boost/shared_ptr.hpp>
#include <Eigen/Dense>

namespace gl_wrapper
{

  class Camera
  {
  public:
    Camera(
      bool p, double fovy, double z_near, double z_far,
      const Eigen::Vector3d& pp, const Eigen::Vector3d& cc, const Eigen::Vector3d& uu,
      double zoom_rate, double translate_rate, double rotate_rate);
    void look(int w, int h);
    void reset();

    void zoomin();
    void zoomout();
    void translate(int dx, int dy);
    void rotate(int dx, int dy);

    const Eigen::Vector3d& getPos() const
    {
      return pos_;
    }

    const Eigen::Vector3d& getCenter() const
    {
      return center_;
    }

    const Eigen::Vector3d& getUp() const
    {
      return up_;
    }

  private:
    void calcOrthoParams(int w, int h);

    bool perspective_;

    double fovy_;
    double z_near_;
    double z_far_;

    double left_;
    double right_;
    double bottom_;
    double top_;

    Eigen::Vector3d pos_;
    Eigen::Vector3d center_;
    Eigen::Vector3d up_;

    Eigen::Vector3d pre_pos_;
    Eigen::Vector3d pre_center_;
    Eigen::Vector3d pre_up_;

    Eigen::Vector3d init_pos_;
    Eigen::Vector3d init_center_;
    Eigen::Vector3d init_up_;

    double zoom_rate_;
    double translate_rate_;
    double rotate_rate_;
  };

  typedef boost::shared_ptr<Camera> CameraPtr;
}

#endif /* __GL_WRAPPER_RENDER_CAMERA_HPP */


#pragma once
#include <vector>
#include <unordered_map>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>

namespace kt84 {
    template <class TMesh>
    inline std::vector<TMesh> get_connected_components(const TMesh& mesh) {
        assert(mesh.has_vertex_status() && mesh.has_face_status());

        std::vector<TMesh> result;

        boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> graph;
        for (auto e : mesh.edges()) {
            int f0 = mesh.face_handle(mesh.halfedge_handle(e, 0)).idx();
            int f1 = mesh.face_handle(mesh.halfedge_handle(e, 1)).idx();
            boost::add_edge(f0, f1, graph);
        }

        std::vector<int> f_component_id(mesh.n_faces());
        int n_components = boost::connected_components(graph, &f_component_id[0]);

        // generate array of meshes according to component id
        for (int i = 0; i < n_components; ++i) {
            TMesh component = mesh;
            for (auto f : component.faces()) {
                if (f_component_id[f.idx()] != i)
                    component.delete_face(f, true);
            }
            component.garbage_collection();
            result.push_back(component);
        }

        return result;
    }
}


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

#include <memory>

#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>

#include "action_msgs/msg/goal_status.hpp"
#include "rclcpp/rclcpp.hpp"
#include "unique_identifier_msgs/msg/uuid.hpp"

#include "halodi_msgs/msg/whole_body_state.hpp"
#include "halodi_msgs/msg/joint_name.hpp"
#include "halodi_msgs/msg/joint_space_command.hpp"
#include "halodi_msgs/msg/whole_body_trajectory.hpp"
#include "halodi_msgs/msg/whole_body_trajectory_point.hpp"
#include "halodi_msgs/msg/whole_body_controller_command.hpp"

#include "eve_ros2_examples/utils.h"

namespace eve_ros2_examples {

using halodi_msgs::msg::JointName;
using halodi_msgs::msg::JointSpaceCommand;
using halodi_msgs::msg::TrajectoryInterpolation;
using halodi_msgs::msg::WholeBodyTrajectory;
using halodi_msgs::msg::WholeBodyTrajectoryPoint;
using halodi_msgs::msg::WholeBodyControllerCommand;
using std::placeholders::_1;

using namespace std::chrono_literals;

class WholeBodyPublisher : public rclcpp::Node {
 public:
  WholeBodyPublisher() : Node("whole_body_publisher") {
    // Create a latching QoS to make sure the first message arrives at the trajectory manager, even if the connection is not up when
    // publishTrajectory is called the first time. Note: If the trajectory manager starts after this node, it'll execute immediatly.
    rclcpp::QoS latching_qos(1);
    latching_qos.transient_local();

    // set up publisher to trajectory topic
    publisher_ = this->create_publisher<WholeBodyControllerCommand>("/eve/whole_body_command", latching_qos);

    // publish at 500Hz
    timer_ = this->create_wall_timer(2ms, [this]() {
      // publishVelocityInt();
      publishAccelerationInt();
    });
  }
  rclcpp::Publisher<WholeBodyControllerCommand>::SharedPtr publisher_;

  void publishVelocityInt() {
    // Start from x_0 with constant velocity v, integrate to get the target position x
    double x_0 = 0;
    double v = 0.1;
    static uint64_t i = 0;
    double t = i*(double)0.002;
    double x = x_0 + t*v;

    WholeBodyControllerCommand torque_msg;
    torque_msg.joint_space_commands.push_back(generateJointSpaceCommand(JointName::LEFT_ELBOW_PITCH, -1.5, 0, 0));
    torque_msg.joint_space_commands.push_back(generateJointSpaceCommand(JointName::RIGHT_ELBOW_PITCH, -x, -v, 0));
    torque_msg.sequence_id = 13;

    RCLCPP_INFO(this->get_logger(), "Sending velocity, t = %f, v = %f, x = %f..", t, v, x);
    i++;
    publisher_->publish(torque_msg);
  }

  void publishAccelerationInt() {
    // Start from x_0, v_0 and with constant acceleration a, integrate to get the target x, v
    double x_0 = 0;
    double v_0 = 0;
    double a = 0.1;
    static uint64_t i = 0;
    double t = i*(double)0.002;
    double v = v_0 + t*a;
    double x = x_0 + t*v;

    WholeBodyControllerCommand torque_msg;
    halodi_msgs::msg::JointSpaceCommand ret_msg;
    halodi_msgs::msg::JointName name;
    name.joint_id = JointName::LEFT_ELBOW_PITCH; // Negative for flexion
    name.joint_id = JointName::RIGHT_WRIST_PITCH; // Negative for upward
    name.joint_id = JointName::RIGHT_WRIST_ROLL; // Negative for outward
    name.joint_id = JointName::RIGHT_SHOULDER_PITCH; // Positive for arm backwards
    name.joint_id = JointName::RIGHT_SHOULDER_YAW; // Negative to twist arm outward
    name.joint_id = JointName::RIGHT_SHOULDER_ROLL; // Negative for abduction
    ret_msg.joint = name;
    ret_msg.q_desired = -x;
    ret_msg.qd_desired = -v;
    ret_msg.qdd_desired = -a;
    ret_msg.use_default_gains = false;

    // Ive removed the initializzation of x and v in the underlying classes, but doesnt move without x or v for some reason
    torque_msg.joint_space_commands.push_back(ret_msg);

    RCLCPP_INFO(this->get_logger(), "Sending acceleration, t = %f, v = %f, x = %f..", t, v, x);
    i++;
    publisher_->publish(torque_msg);
  }

  rclcpp::TimerBase::SharedPtr timer_;
};

}  // namespace eve_ros2_examples

int main(int argc, char* argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<eve_ros2_examples::WholeBodyPublisher>());

  rclcpp::shutdown();
  return 0;
}


#include <algorithm>
#include <array>
#include <iostream>
#include <string_view>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include <boost/range/irange.hpp>

#include "input.hpp"

struct ParseStats {
  int num_lines,
      med_size;
};

constexpr auto NEWLINE = '\n';
constexpr auto UPPERCASE_ALPHABET = "ABCDEFGHIJKLMNOPQRSTUVWXYZ";

constexpr auto parse_stats(std::string_view input) {

  auto stats = ParseStats{};

  auto& num_lines = stats.num_lines;

  for(auto c : input) {
    num_lines += (c == NEWLINE);
  }
  ++num_lines;

  auto& med_size = stats.med_size;
  auto pos = input.rfind(NEWLINE) + 1;

  while(pos != input.npos) {
      auto next = input.find_first_of(UPPERCASE_ALPHABET, (pos + 1));
      ++med_size;
      pos = next;
  }
  ++med_size;

  return stats;
}

constexpr auto PARSE_STATS = parse_stats(puzzle_input);

constexpr auto NUM_LINES = PARSE_STATS.num_lines;
constexpr auto MED_SIZE = PARSE_STATS.med_size;

auto split_input(std::string_view input) {

  auto lines = std::array<decltype(input), NUM_LINES>{};

  auto index = 0;
  auto pos = decltype(input.size()){};

  while(pos != input.npos) {
    pos = input.find(NEWLINE);
    lines[index++] = input.substr(0, pos);
    input.remove_prefix(pos+1);
  }

  return lines;
}

using Rules = std::unordered_map<std::string_view, std::vector<std::string_view>>;
using Medicine = std::array<std::string_view, MED_SIZE>;

struct Machine {
  Rules rules;
  Medicine medicine;
};

/*
 * Molecules always start with an uppercase letter. If we represent the medicine
 * as a sequence of separate molecules, we can detect duplicates very easily
 * later on.
 */
auto parse_medicine(std::string_view input) {

  auto medicine = Medicine{};

  auto index = 0;
  auto pos = decltype(input.size()){};

  while(pos != input.npos) {
    const auto next = input.find_first_of(UPPERCASE_ALPHABET, (pos + 1));
    medicine[index++] = input.substr(pos, (next - pos));
    pos = next;
  }

  return medicine;
}

auto parse(std::string_view input) {

  auto rules = Rules{};

  const auto& lines = split_input(input);

  const auto& medicine = parse_medicine(lines.back());

  constexpr auto SPACE = ' ';

  for(const auto i : boost::irange(NUM_LINES - 2)) {

    auto line = lines[i];

    auto key = line.substr(0, line.find(SPACE));
    auto value = line.substr((line.rfind(SPACE) + 1), line.npos);

    rules[key].push_back(value);
  }

  return Machine{rules, medicine};
}

auto num_unique_molecules(const Machine& machine) {

  const auto& rules = machine.rules;

  /*
   * The duplicate lists contains the rules for which the input is part of the
   * output.
   *
   * Front duplicates are the rules where a molecule is both input and first part
   * of the output.
   *
   * Back duplicates are the rules where a molecule is both input and last part of
   * the output.
   *
   * Example:
   *
   * H => HCa   (front duplicate)
   * P => CaP   (back duplicate)
   * Ti => TiTi (front & back duplicate)
   *
   * For rule "H => HCa", "Ca" will be stored in the list of front duplicates
   * for "H", so "H" will map to a list that contains "Ca".
   */
  auto front_duplicates = std::unordered_map<std::string_view, std::unordered_set<std::string_view>>{};
  auto back_duplicates = decltype(front_duplicates){};

  for(const auto& rule : rules) {

    auto input = rule.first;
    const auto& outputs = rule.second;

    const auto in_size = input.size();

    for(auto output : outputs) {

      const auto out_size = output.size();
      const auto output_is_big_enough = (out_size >= in_size);

      const auto output_is_equal_to = [&] (auto&&... args) {
        return (output.compare(std::forward<decltype(args)>(args)...) == 0);
      };

      if(output_is_big_enough and output_is_equal_to(0, in_size, input)) {
        front_duplicates[input].insert(output.substr(in_size));
      }

      if(output_is_big_enough and output_is_equal_to((out_size - in_size), output.npos, input)) {
        output.remove_suffix(in_size);
        back_duplicates[input].insert(output);
      }
    }
  }

  const auto& medicine = machine.medicine;

  const auto pairwise_iteration = [] (const auto& container, auto&& callback) {
    auto first1 = container.begin();
    auto first2 = (first1 + 1);
    const auto last = container.end();
    while(first2 != last) {
      callback(*first1++,*first2++);
    }
  };

  /*
   * We do a pairwise iteration through the medicine molecules and discard the
   * rules for the second element of the pair that have already been generated
   * by the rule for the first element.
   *
   * Example Rules (general | duplicate):
   *
   * P => PTi | P => Ti (front duplicate)
   * C => TiC | C => Ti (back duplicate)
   *
   * If our pair consists of molecules P and C (PC, in short), then the combination
   * PTiC will have been generated by the rules for P.
   *
   * At that point we can count all the duplicates found in both the front and back
   * lists and subtract it from the number of rules found for C and add it to the
   * number of unique molecules we have already found.
   */
  auto count_unique_molecules = [&, acc = rules.at(medicine.front()).size()] () mutable {

    pairwise_iteration(medicine, [&] (auto a, auto b) {

      if(rules.find(b) != rules.end()) {

        const auto& fda = front_duplicates[a];
        const auto& bdb = back_duplicates[b];

        const auto num_duplicates = std::count_if(bdb.begin(), bdb.end(), [&] (auto duplicate) {
          return fda.find(duplicate) != fda.end();
        });

        acc += (rules.at(b).size() - num_duplicates);
      }
    });

    return acc;
  };

  return count_unique_molecules();
}

auto solution(std::string_view input) {

  auto machine = parse(input);

  return num_unique_molecules(machine);
}

int main() {
  std::cout << solution(puzzle_input) << std::endl;
}


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#ifndef STAPL_SKELETONS_EXPLICIT_NEW_MAP_TRANSFORMATIONS_HPP
#define STAPL_SKELETONS_EXPLICIT_NEW_MAP_TRANSFORMATIONS_HPP

#include <stapl/skeletons/utility/tags.hpp>
#include <stapl/views/metadata/coarseners/default.hpp>
#include <boost/utility/result_of.hpp>

namespace stapl {

namespace prototype {

namespace functional {

struct inner_product;

} } // namespace prototype::functional

namespace composition {

//////////////////////////////////////////////////////////////////////
/// @brief Workfunction that is one possible algorithmic coarsening of the
/// nested skeleton map_func(functional::map_reduce(), ...).
///
/// Transforms the fine grain specification into a map_reduce operating
/// on a transposed version of the input two dimensional array view, with
/// custom coarsened map and reduce workfunctions.
///
/// Implemented with matrix vector multiplication as driving example.
///
/// @todo This needs to be cleaned and become a default transformation
///       for the case as it is likely always better than the original
///       specification. Generalize naming and assumptions from current
///       use case where the @p map_reduce is an @p inner_product call.
///
/// @ingroup skeletonsExplicitFactoriesInternal
//////////////////////////////////////////////////////////////////////
struct m_mr_transform_wf
{
  typedef std::vector<double>* result_type;

  //////////////////////////////////////////////////////////////////////
  /// @brief Coarsened binary workfunction which is map operator for
  /// @ref m_mr_transform_wf.
  //////////////////////////////////////////////////////////////////////
  struct map_wf
  {
    typedef std::vector<double>* result_type;

    //////////////////////////////////////////////////////////////////////
    /// @param row_pieces        A row from the transposed two dimensional
    ///                          array view input.
    /// @param rep_vector_piece The row input to the @ref m_mr_transform_wf
    ///                         workfunction.
    ///
    /// @return A heap allocated one dimensional container with result of the
    ///         operator (currently fixed to @p inner_product).
    //////////////////////////////////////////////////////////////////////
    template<typename V0, typename V1>
    result_type
    operator()(V0& row_pieces, V1& rep_vector_piece)
    {
      const size_t num_rows = rep_vector_piece.size();
      const size_t num_cols = (*rep_vector_piece.begin()).size();

      stapl_assert(num_rows > 0 && num_cols > 0,
        "m_mr_transform_wf: rows or cols equal 0");

      result_type tmp = new std::vector<double>(num_rows);

      typename V0::iterator v0_iter          = row_pieces.begin();
      typename V1::iterator v1_iter          = rep_vector_piece.begin();
      std::vector<double>::iterator res_iter = tmp->begin();

      for (std::size_t i = 0; i < num_rows;
           ++i, ++v0_iter, ++v1_iter, ++res_iter)
        *res_iter = std::inner_product(
                      v0_iter->begin(), v0_iter->end(),
                      v1_iter->begin(),
                      std::multiplies<double>(), std::plus<double>()
                    );

      return tmp;
    }
  };

  //////////////////////////////////////////////////////////////////////
  /// @brief Coarsened binary reduction workfunction defined for result type
  /// of @ref map_wf.
  ///
  /// @return Mutates storage of lhs with result of reduction and returns it.
  ///
  /// Frees underlying storage of rhs.
  //////////////////////////////////////////////////////////////////////
  struct reduce_wf
  {
    typedef std::vector<double>* result_type;

    result_type
    operator()(result_type lhs, result_type rhs)
    {
      stapl_assert(lhs->size() == rhs->size(),
        "ip_transform::reduce_wf::operator(), lhs / rhs sizes differ");

      std::transform(lhs->begin(), lhs->end(), rhs->begin(),
                     lhs->begin(), std::plus<int>());

      delete rhs;

      return lhs;
    }
  };

  //////////////////////////////////////////////////////////////////////
  /// @brief Function operator.
  ///
  /// @param mat_columns A two dimensional array view.
  /// @param vector_piece A one dimensional array view.
  ///
  /// @return A one dimensional array view containing result of the
  /// @p map_reduce operation.
  //////////////////////////////////////////////////////////////////////
  template<typename V0, typename V1>
  result_type operator()(V0& mat_columns, V1& vector_piece) const
  {
    return map_reduce(is_coarse_wf(map_wf()), reduce_wf(),
                      transpose(mat_columns),
                      repeat_view<V1>(vector_piece));
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief A coarsened workfunction for matrix vector multiplication that
/// operates on a block of a two dimensional matrix and the vector portion
/// corresponding to it.
///
/// @ingroup skeletonsExplicitFactoriesInternal
//////////////////////////////////////////////////////////////////////
struct block_matvec_wf
{
  // typedef cm_res_view<std::vector<double> > res_t;

  typedef cm_res_view<my_container<double> > res_t;

  //////////////////////////////////////////////////////////////////////
  /// @brief Nested struct which reflects the return type of the function
  /// operator based on the types of the input views.
  //////////////////////////////////////////////////////////////////////
  template<typename Signature>
  struct result;

  template<typename SubMatrix, typename SubVector>
  struct result<block_matvec_wf(SubMatrix, SubVector)>
  {
    // Using SubVector would imply map_subview usage.
    // May want make that cm_res_view instead.
    // typedef SubVector type;

    typedef res_t type;
  };

  //////////////////////////////////////////////////////////////////////
  /// @brief Function operator.
  ///
  /// @param mv A view of a sub-block of the input matrix.
  /// @param sv_view A view of a piece of the input vector.
  /// @return A view over a container with the result of matrix
  ///   vector multiplication.
  //////////////////////////////////////////////////////////////////////
  template<typename SubMatrix, typename SubVector>
  res_t operator()(SubMatrix& mv, SubVector& sv_view) const
  {
    typedef typename SubMatrix::container_t::b_container_t SequentialMatrix;

    SequentialMatrix* mtl_a = &mv.m_ct->bcontainer();

    res_t result_vw(mtl_a->nrows(), 0);

    typename SequentialMatrix::iterator i        = mtl_a->begin();
    typename SequentialMatrix::iterator i_end    = mtl_a->end();
    typename SubVector::iterator        res_iter = result_vw.begin();

    for (; i != i_end; ++i, ++res_iter)
    {
      double sum = 0.0l;

      typename SequentialMatrix::OneD::iterator j;
      typename SequentialMatrix::OneD::iterator j_end;

      j_end = (*i).end();

      for (j = (*i).begin(); j != j_end; ++j)
        sum += *j * sv_view[j.column()];

      *res_iter = sum;
    }

    return result_vw;
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Custom key mapper passed to the @ref directory when using the
/// @ref nest_map_mr_factory factory within a PARAGRAPH.
///
/// @ingroup skeletonsExplicitFactoriesInternal
//////////////////////////////////////////////////////////////////////
struct nest_map_mr_task_mapper
{
private:
  /// @brief The number of processor columns. (nest_map_mr_factory uses
  /// a two dimensional logical processor layout).
  std::size_t m_n_pcols;

  /// @brief True if transparent computation replication has enabled.
  bool        m_b_replicated;

public:
  nest_map_mr_task_mapper(std::size_t n_pcols, bool b_replicated)
    : m_n_pcols(n_pcols), m_b_replicated(b_replicated)
  { }

  //////////////////////////////////////////////////////////////////////
  /// @brief Maps the task identifier @p tid to the location where it will
  /// execute.
  //////////////////////////////////////////////////////////////////////
  size_t operator()(size_t tid) const
  {
    const size_t n_locs = stapl::get_num_locations();

    if (m_b_replicated)
      return tid % n_locs;

    // phase 1 tids...
    if (tid < m_n_pcols)
      return tid;

    // phase 2 tids...
    tid -= m_n_pcols;

    if (tid < n_locs)
      return tid;

    // phase 3 tids...
    tid -= n_locs;

    if (tid < n_locs)
      return tid;

    // phase 4 tids
    tid -= n_locs;

    if (tid < n_locs)
      return tid;

    // unknown tid
    stapl_assert(0, "nest_map_mr_task_mapper: unexpected tid lookup request");

    return 0;
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Customized implementation of a map_func(map_reduce()...)
/// skeleton that is used for matrix vector multiplication.
///
/// The key implementation features are the use of logical two dimension
/// processor layout to reduce communication and the replication of computation
/// across subportions of processors to further reduce communication caused by
/// vector updates.
///
/// @todo Generalize and extend usage beyond matrix vector multiplication.
///
/// @ingroup skeletonsExplicitFactories
//////////////////////////////////////////////////////////////////////
struct nest_map_mr_factory
  : public task_factory_base
{
private:
  typedef composition::coarse_map_wf<identity<double> >    coarse_map_wf_t;
  typedef composition::coarse_map_wf<plus<double> >        coarse_map_plus_wf_t;
  typedef skeletons::tags::nested_map_reduce                tag_type;
public:
//  typedef cm_res_view<std::vector<double> >                sv_t;
  typedef cm_res_view<my_container<double> >  sv_t;
  using coarsener_type = default_coarsener;

  default_coarsener get_coarsener() const { return default_coarsener(); }

private:
  //////////////////////////////////////////////////////////////////////
  /// @brief Factory implementation called by function operator if computation
  /// replication is disabled.
  ///
  /// @param tgv The @ref paragraph_view.
  /// @param mv A view over the input matrix.
  /// @param vv A view over the input vector.
  //////////////////////////////////////////////////////////////////////
  template<typename TGV, typename Matrix, typename Vector>
  void matvec_no_replication(TGV const& tgv, Matrix& mv, Vector& vv)
  {
    stapl_assert(0, "don't expect to get here");

    const std::size_t nprows     =  mv.m_ct.nprows();
    const std::size_t npcols     =  mv.m_ct.npcols();

    stapl_assert(nprows == npcols,
      "Double sanity check of nprows==npcols in nest_map_mr_factory failed");

    const std::size_t my_prow    =  mv.m_ct.my_prow();
    const std::size_t my_pcol    =  mv.m_ct.my_pcol();

    const size_t n_locs          = vv.get_num_subviews();
    const std::size_t num_succs  = 1;

    //
    // (1) Identity add_task to allow consumption of col subvector piece by
    //     allows locations in col
    //
    const std::size_t phase_one_tid = my_pcol;
////
////    if (my_prow == 0)
////    {
////      // std::cout << get_location_id() << ": Phase 1 - " << phase_one_tid
////                   << "\n";
////      //
////      tgv.add_task(phase_one_tid,
////                       coarse_map_wf_t(identity<double>(), n_locs),
////                       nprows,
////                       std::make_pair(&vv, get_location_id()));
////    }

    //
    // (2) Local Matmult.
    //
    const std::size_t phase_two_tid = npcols + get_location_id();

    // std::cout << get_location_id() << ": Phase 2 - << " << phase_two_tid
    //           << " consuming " << phase_one_tid << "\n";

    tgv.add_task(phase_two_tid,
                 block_matvec_wf(),
                 std::vector<std::size_t>(),
                 1,
                 std::make_pair(&mv, 0),
                 consume<sv_t>(tgv, phase_one_tid));

    //
    // (3) Reduction across location row tasks
    //
    const std::size_t phase_three_tid   =
      npcols + get_num_locations() + get_location_id();

    const std::size_t phase_3_num_succs =
      vv.get_num_local_subviews() == 0 ? 2 : 2;

    if (my_pcol == 0)
    {
      // std::cout << get_location_id() << ": Phase 3 -
      //           << " << phase_three_tid << " consuming "
      //           << phase_two_tid << "\n";

      tgv.add_task(phase_three_tid,
                   coarse_map_wf_t(identity<double>()),
                   std::vector<std::size_t>(),
                   phase_3_num_succs,
                   consume<sv_t>(tgv, phase_two_tid));
    }
    else
    {
      const std::size_t left_neighbor = phase_three_tid - 1;

      // std::cout << get_location_id() << ": Phase 3 - << " << phase_three_tid
      //           << " consuming " << phase_two_tid << " and "
      //           << left_neighbor << "\n";

      tgv.add_task(phase_three_tid,
                   coarse_map_plus_wf_t(plus<double>()),
                   std::vector<std::size_t>(),
                   phase_3_num_succs,
                   consume<sv_t>(tgv, phase_two_tid),
                   consume<sv_t>(tgv, left_neighbor));
    }

    //
    // (4) Transpose value back add_task..
    //
    const std::size_t phase_four_pred_tid =
      npcols + get_num_locations() + my_pcol * npcols + npcols - 1;

    const std::size_t result_tid =
      npcols + 2 * get_num_locations() + get_location_id();

    if (vv.get_num_local_subviews() == 0)
    {
      // NOTE - we make this task depend on phase 3, so that we safely say local
      // computation is done when the result_tid task is done.  That way we can
      // use result based TD and avoid the general terminator algorithm.
      //
      tgv.add_task(result_tid, simple_wf(), num_succs,
                   localize_object(sv_t(0, n_locs)),
                   consume<sv_t>(tgv, phase_three_tid));

      //std::cout << get_location_id() << ": Phase 4 - << "
      //          << result_tid << " consuming " << phase_four_pred_tid << "\n";
    }
    else
    {
      stapl_assert(my_prow == 0, "Error in nested_map_mr_factory");

      //std::cout << get_location_id() << ": Phase 4 - << result_tid consuming "
      //          << phase_four_pred_tid << "\n";

      // NOTE - setting the result_tid as commented out below is sufficient.
      // The only reason we create a local task is for the dependence on the
      // phase 3 task so that we can guaranteed all local tasks have finished
      // when result fires, so that we can use result based TD.  A better TD
      // would relax this unnecssarily strict ordering.
      //
      // result_tid = phase_four_pred_tid;
      //
      tgv.add_task(result_tid,
                   coarse_map_wf_t(identity<double>()),
                   std::vector<std::size_t>(1, phase_three_tid),
                   num_succs,
                   consume<sv_t>(tgv, phase_four_pred_tid));
    }

    // std::cout << get_location_id() << ": Result Tid is "
    //           << result_tid << "\n";

    tgv.set_result(result_tid);
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Factory implementation called by function operator if computation
  /// replication is enabled.
  ///
  /// @param tgv The @ref paragraph_view.
  /// @param mv A view over the input matrix.
  /// @param vv A view over the input vector.
  //////////////////////////////////////////////////////////////////////
  template<typename TGV, typename Matrix, typename Vector>
  void matvec_replication(TGV const& tgv, Matrix& mv, Vector& vv)
  {
    const std::size_t nprows  = mv.m_ct.nprows();
    const std::size_t npcols  = mv.m_ct.npcols();

    const std::size_t my_prow = mv.m_ct.my_prow();
    const std::size_t my_pcol = mv.m_ct.my_pcol();

    const size_t n_locs       = get_num_locations();
    const size_t my_loc       = get_location_id();

    stapl_assert(nprows == npcols,
      "Double sanity check of nprows==npcols in nest_map_mr_factory failed");

    // (1) Local Matmult.
    //
    const std::size_t phase_one_tid = my_loc;

    tgv.add_task(phase_one_tid,
                 block_matvec_wf(),
                 std::vector<std::size_t>(),
                 2,
                 std::make_pair(&mv, 0),
                 std::make_pair(&vv, vv.get_local_component(0).get_id()));

    // (2) Reduction across location rows.
    //
    size_t l2_npcols = 0;

    for (size_t i = npcols; i > 1; i /= 2)
      ++l2_npcols;

    plus<double> plus_wf;

    coarse_map_plus_wf_t phase2_wf(plus_wf);

    size_t phase2_base_tid  = n_locs;
    size_t div_factor       = 2;

    // size_t level = 0;

    for (; div_factor <= npcols; phase2_base_tid += n_locs)
    {
      size_t new_div_factor = div_factor * 2;

      size_t j = (my_pcol + div_factor/2) % div_factor
        + my_pcol / div_factor * div_factor;

      const size_t exchange_location = my_prow * npcols + j;

      const size_t my_tid     = phase2_base_tid + my_loc;
      //const size_t n_succs    = div_factor == npcols ? 1 : 2;
      const size_t n_succs    = div_factor == npcols ? 2 : 2;
      const size_t left_pred  = my_tid - n_locs;
      const size_t right_pred = phase2_base_tid + exchange_location - n_locs;

      // ++level;

      tgv.add_task(my_tid, phase2_wf, std::vector<std::size_t>(), n_succs,
                   consume<sv_t>(tgv, left_pred),
                   consume<sv_t>(tgv, right_pred));

      div_factor = new_div_factor;
    }

    // (3) Transpose value back add_task..
    //
    size_t phase3_base_tid   = n_locs * (l2_npcols);
    size_t exchange_location = my_loc % nprows * nprows + my_loc / nprows;
    size_t predecessor_tid   = phase3_base_tid + exchange_location;

    tgv.set_result(predecessor_tid);
  }

public:
  typedef map_view<sv_t>                                   result_type;
  typedef nest_map_mr_task_mapper                          task_id_mapper_type;

  //////////////////////////////////////////////////////////////////////
  /// @brief Return a copy of the custom task to location mapper used to
  /// specialize the key distribution of the PARAGRAPH's @ref directory.
  ///
  /// @return A key mapper which knows how tasks for this PARAGRAPH will be
  /// distributed among locations.
  ///
  /// @param mv A view over the input matrix.
  /// @param vv A view over the input vector.
  //////////////////////////////////////////////////////////////////////
  template<typename Matrix, typename Vector>
  task_id_mapper_type
  get_task_id_mapper(Matrix& mv, Vector& vv) const
  {
    return task_id_mapper_type(mv.m_ct.npcols(), true);
//    return task_id_mapper_type(mv.m_ct.npcols(), vv.get_num_copies() > 1);
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Function operator a factory called to populate the PARAGRAPH
  /// with tasks.
  ///
  /// @param tgv The @ref paragraph_view.
  /// @param mv A view over the input matrix.
  /// @param vv A view over the input vector.
  //////////////////////////////////////////////////////////////////////
  template<typename TGV, typename Matrix, typename Vector>
  void operator()(TGV const& tgv, Matrix& mv, Vector& vv)
  {
//    if (vv.get_num_copies() > 1)
      matvec_replication(tgv, mv, vv);
//    else
//      matvec_no_replication(tgv, mv, vv);
  }

}; // struct nest_map_mr_factory

} // namespace composition

namespace detail {

//////////////////////////////////////////////////////////////////////
/// @brief Custom PARAGRAPH termination detection which signals termination
/// after a predefined number of invocations of the @p receive_notify method.
///
/// Used by @ref composition::nest_map_mr_factory to tie termination to the
/// completion of a predefined set of PARAGRAPH task completions.
///
/// @ingroup skeletonsExplicitFactories
//////////////////////////////////////////////////////////////////////
class counter_based_terminator
  : public terminator_base
{
private:
  /// @brief The number of times @p receive_notify should be called before
  /// signaling PARAGRAPH termination.
  size_type m_count;

public:
  explicit counter_based_terminator(size_type count = 2)
    : m_count(count)
  { }

  //////////////////////////////////////////////////////////////////////
  /// @brief Starting function required by executor for terminators.
  /// Noop for this terminator.
  ///
  /// @return @c true if @ref receive_notify() has been called the required
  ///         number of times, otherwise @c false.
  //////////////////////////////////////////////////////////////////////
  bool operator()(void)
  { return (m_count==0); }

  //////////////////////////////////////////////////////////////////////
  /// @brief Invokes base class method @ref terminator_base::call_notifier()
  ///  after the defined number of invocations.
  //////////////////////////////////////////////////////////////////////
  void receive_notify(void)
  {
    //stapl_assert(!m_executor_notifier.empty(),
    //  "result_based_terminator::receive_notify(): executor notifier not set");

    if (--m_count == 0)
      call_notifier();
  }

  size_type iterations(void) const noexcept override
  { return 0; }
}; // struct counter_based_terminator

//////////////////////////////////////////////////////////////////////
/// @brief Notifier used by custom termination detection for
/// @ref composition::nest_map_mr_factory.
///
/// Invokes @p receive_notify member on pointer to forward notification to
/// to underlying terminator.  Used instead of bind / function idiom to
/// avoid heap allocations.
///
/// @ingroup skeletonsExplicitFactoriesInternal
//////////////////////////////////////////////////////////////////////
class counter_receive_notify_callback
{
private:
  /// @brief Pointer to underlying terminator to forward events to.
  counter_based_terminator* m_terminator_ptr;

public:
  counter_receive_notify_callback(counter_based_terminator* terminator_ptr)
    : m_terminator_ptr(terminator_ptr)
  { }

  void operator()(void) const
  {
    m_terminator_ptr->receive_notify();
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Partial specialization to enable custom termination detection
/// for @ref composition::nest_map_mr_factory.
///
/// @ingroup skeletonsExplicitFactoriesInternal
//////////////////////////////////////////////////////////////////////
template<>
struct terminator_initializer<composition::nest_map_mr_factory>
{
  typedef counter_based_terminator terminator_t;

  template<typename Result, typename SVS>
  terminator_t*
  operator()(Result const& result, task_graph& tg, SVS& svs) const
  {
    terminator_t* t_ptr = new terminator_t();

    size_t l2_npcols = 0;

    const size_t npcols = get<0>(svs).m_ct.npcols();

    for (size_t i = npcols; i > 1; i /= 2)
      ++l2_npcols;

    const size_t n_locs            = get_num_locations();
    const size_t my_loc            = get_location_id();
    const size_t final_reduce_tid  = l2_npcols * n_locs + my_loc;

    stapl_assert(!find_accessor<Result>()(result).available(),
      "terminator_initializer found available result");

    find_accessor<Result>()(result).request_notify(
      counter_receive_notify_callback(t_ptr)
    );

    typedef composition::nest_map_mr_factory::sv_t edge_t;

    // std::cout << get_location_id() << ": setting terminator to result and "
    //           << final_reduce_tid << "\n";

    tg.request_notify<edge_t>(
      final_reduce_tid,
      counter_receive_notify_callback(t_ptr)
    );

    return t_ptr;
  }
}; // struct terminator_initializer

} // namespace detail

namespace composition {

namespace result_of {

///////////////////////////////////////////////////////////////////////
/// @brief Reflect return type of map_func specialization when
/// workfunction is @ref prototype::functional::inner_product.
///
/// @ingroup skeletonsExplicitFactories
//////////////////////////////////////////////////////////////////////
template<typename V0, typename V1>
struct map_func<prototype::functional::inner_product, V0, V1>
  : public boost::result_of<
      paragraph<
        nest_map_mr_factory,
        typename V0::base_view_type,
        typename V1::base_view_type
      >()
    >
{ };

} // namespace result_of

//////////////////////////////////////////////////////////////////////
/// @brief Signature of function template @ref map_func that matches
/// cases when the workfunction is @ref prototype::functional::inner_product.
///
/// Specializes the implementation for this case, creating a PARAGRAPH using
/// the @ref nest_map_mr_factory factory.
///
/// @param wf An instance of the @ref prototype::functional::inner_product
///           with initialized elementary operations.
/// @param v0 A one dimensional view input.
/// @param v1 A one dimensional view input.
///
/// @return A view of the results of the spawned PARAGRAPH computation.
///
/// @ingroup skeletonsExplicitFactories
//////////////////////////////////////////////////////////////////////
template<typename V0, typename V1>
inline
typename result_of::map_func<
  prototype::functional::inner_product, V0, V1
>::type
map_func(prototype::functional::inner_product const& wf,
         V0 const& v0,
         V1 const& v1 //,
         /*unsigned int num_succs = 0*/)
{
  typedef typename V0::base_view_type v0_t;
  typedef typename V1::base_view_type v1_t;

  typedef paragraph<nest_map_mr_factory, v0_t, v1_t> tg_t;

  return (*new tg_t(nest_map_mr_factory(), v0_t(v0.m_ct), v1.m_view))((int) 0);

//  typedef ip_transform_coarse_wf coarse_wf_t;
//
//  return composition::map_func(is_coarse_wf(coarse_wf_t()), v0, v1);
//
  //typedef composition::m_mr_transform_wf coarse_wf_t;
//
  // v1 is repeated.  However, we're going to work with coarse for now,
  // so don't repeat it since m_mr_transform_wf will deal with it.
  //
  //return composition::map_func(
  //  is_coarse_wf(coarse_wf_t()), transpose(v0), v1.m_view);
}

// If the functor is the result of a bind2nd invocation, unwind it, passing
// the underlying functor as the wf and transforming m_value (fixed view)
// to a repeated, top level argument.  Normalizes structure for future
// transforms and make view visible to task placement policy.
//
namespace result_of {

///////////////////////////////////////////////////////////////////////
/// @brief Reflect return type of map_func specialization when unary
/// workfunction is an instantiation of the @ref binder2nd class template.
///
/// @ingroup skeletonsExplicitFactories
//////////////////////////////////////////////////////////////////////
template<typename Operation, typename Stored, typename V0>
struct map_func<binder2nd<Operation, Stored, false>, V0>
  : public map_func<Operation, V0, repeat_view<Stored> >
{ };

///////////////////////////////////////////////////////////////////////
/// @brief Reflect return type of map_func specialization when unary
/// workfunction is an instantiation of the @ref binder1st class template.
///
/// @ingroup skeletonsExplicitFactories
//////////////////////////////////////////////////////////////////////
template<typename Operation, typename Stored, typename V0>
struct map_func<binder1st<Operation, Stored, true>, V0>
  : public map_func<Operation, V0, repeat_view<Stored> >
{ };

} // namespace result_of

//////////////////////////////////////////////////////////////////////
/// @brief Converts @ref map_func call with @ref stapl::binder2nd as
/// the workfunction to one with the base workfunction of type
/// @p Operation and a @ref repeat_view in place of the bound view
/// of type @p Stored.
///
/// A replicated view and a view bound to a workfunction with a bind operation
/// are equivalent.  Normalize to a common form that makes the PARAGRAPH aware
/// of all views.
///
/// @ingroup skeletonsExplicitFactories
//////////////////////////////////////////////////////////////////////
template<typename Operation, typename Stored, typename V0>
inline
typename result_of::map_func<binder2nd<Operation, Stored, false>, V0>::type
map_func(binder2nd<Operation, Stored, false> const& wf, V0 const& v0)
{
  return composition::map_func(wf.op(), v0, repeat_view<Stored>(wf.m_value));
}

//////////////////////////////////////////////////////////////////////
/// @brief Converts @ref map_func call with @ref stapl::binder1st as
/// the workfunction to one with the base workfunction of type
/// @p Operation and a @ref repeat_view in place of the bound view
/// of type @p Stored.
///
/// A replicated view and a view bound to a workfunction with a bind operation
/// are equivalent.  Normalize to a common form that makes the PARAGRAPH aware
/// of all views.
///
/// @ingroup skeletonsExplicitFactories
//////////////////////////////////////////////////////////////////////
template<typename Operation, typename Stored, typename V0>
inline
typename result_of::map_func<binder1st<Operation, Stored, true>, V0>::type
map_func(binder1st<Operation, Stored, true> const& wf, V0 const& v0)
{
  return composition::map_func(wf.op(), v0, repeat_view<Stored>(wf.m_value));
}

} // namespace composition

} // namespace stapl

#endif // ifndef STAPL_SKELETONS_EXPLICIT_NEW_MAP_TRANSFORMATIONS_HPP


/* -*- c++ -*- */
/*
 * Copyright 2009-2013 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * 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 GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <cstdio>
#include <cmath>
#include <algorithm>

#include "pfb_clock_sync_ccf_impl.h"
#include <gnuradio/io_signature.h>
#include <gnuradio/math.h>
#include <boost/format.hpp>
#include <boost/math/special_functions/round.hpp>

namespace gr {
  namespace digital {

    pfb_clock_sync_ccf::sptr
    pfb_clock_sync_ccf::make(double sps, float loop_bw,
			     const std::vector<float> &taps,
			     unsigned int filter_size,
			     float init_phase,
			     float max_rate_deviation,
			     int osps)
    {
      return gnuradio::get_initial_sptr
	(new pfb_clock_sync_ccf_impl(sps, loop_bw, taps,
				     filter_size,
				     init_phase,
				     max_rate_deviation,
				     osps));
    }

    static int ios[] = {sizeof(gr_complex), sizeof(float), sizeof(float), sizeof(float)};
    static std::vector<int> iosig(ios, ios+sizeof(ios)/sizeof(int));
    pfb_clock_sync_ccf_impl::pfb_clock_sync_ccf_impl(double sps, float loop_bw,
						     const std::vector<float> &taps,
						     unsigned int filter_size,
						     float init_phase,
						     float max_rate_deviation,
						     int osps)
      : block("pfb_clock_sync_ccf",
		  io_signature::make(1, 1, sizeof(gr_complex)),
		  io_signature::makev(1, 4, iosig)),
	d_updated(false), d_nfilters(filter_size),
	d_max_dev(max_rate_deviation),
	d_osps(osps), d_error(0), d_out_idx(0)
    {
      if(taps.size() == 0)
        throw std::runtime_error("pfb_clock_sync_ccf: please specify a filter.\n");

      // Let scheduler adjust our relative_rate.
      //enable_update_rate(true);
      set_tag_propagation_policy(TPP_DONT);

      d_nfilters = filter_size;
      d_sps = floor(sps);

      // Set the damping factor for a critically damped system
      d_damping = 2*d_nfilters;

      // Set the bandwidth, which will then call update_gains()
      set_loop_bandwidth(loop_bw);

      // Store the last filter between calls to work
      // The accumulator keeps track of overflow to increment the stride correctly.
      // set it here to the fractional difference based on the initial phaes
      d_k = init_phase;
      d_rate = (sps-floor(sps))*(double)d_nfilters;
      d_rate_i = (int)floor(d_rate);
      d_rate_f = d_rate - (float)d_rate_i;
      d_filtnum = (int)floor(d_k);

      d_filters = std::vector<kernel::fir_filter_ccf*>(d_nfilters);
      d_diff_filters = std::vector<kernel::fir_filter_ccf*>(d_nfilters);

      // Create an FIR filter for each channel and zero out the taps
      std::vector<float> vtaps(1,0);
      for(int i = 0; i < d_nfilters; i++) {
	d_filters[i] = new kernel::fir_filter_ccf(1, vtaps);
	d_diff_filters[i] = new kernel::fir_filter_ccf(1, vtaps);
      }

      // Now, actually set the filters' taps
      std::vector<float> dtaps;
      create_diff_taps(taps, dtaps);
      set_taps(taps, d_taps, d_filters);
      set_taps(dtaps, d_dtaps, d_diff_filters);

      d_old_in = 0;
      d_new_in = 0;
      d_last_out = 0;

      set_relative_rate((float)d_osps/(float)d_sps);
    }

    pfb_clock_sync_ccf_impl::~pfb_clock_sync_ccf_impl()
    {
      for(int i = 0; i < d_nfilters; i++) {
	delete d_filters[i];
	delete d_diff_filters[i];
      }
    }

    bool
    pfb_clock_sync_ccf_impl::check_topology(int ninputs, int noutputs)
    {
      return noutputs == 1 || noutputs == 4;
    }

    void
    pfb_clock_sync_ccf_impl::forecast(int noutput_items,
                                      gr_vector_int &ninput_items_required)
    {
      unsigned ninputs = ninput_items_required.size ();
      for(unsigned i = 0; i < ninputs; i++)
        ninput_items_required[i] = (noutput_items + history()) * (d_sps/d_osps);
    }

    void
    pfb_clock_sync_ccf_impl::update_taps(const std::vector<float> &taps)
    {
      d_updated_taps = taps;
      d_updated = true;
    }

    /*******************************************************************
     SET FUNCTIONS
    *******************************************************************/

    void
    pfb_clock_sync_ccf_impl::set_loop_bandwidth(float bw)
    {
      if(bw < 0) {
	throw std::out_of_range("pfb_clock_sync_ccf: invalid bandwidth. Must be >= 0.");
      }

      d_loop_bw = bw;
      update_gains();
    }

    void
    pfb_clock_sync_ccf_impl::set_damping_factor(float df)
    {
      if(df < 0 || df > 1.0) {
	throw std::out_of_range("pfb_clock_sync_ccf: invalid damping factor. Must be in [0,1].");
      }

      d_damping = df;
      update_gains();
    }

    void
    pfb_clock_sync_ccf_impl::set_alpha(float alpha)
    {
      if(alpha < 0 || alpha > 1.0) {
	throw std::out_of_range("pfb_clock_sync_ccf: invalid alpha. Must be in [0,1].");
      }
      d_alpha = alpha;
    }

    void
    pfb_clock_sync_ccf_impl::set_beta(float beta)
    {
      if(beta < 0 || beta > 1.0) {
	throw std::out_of_range("pfb_clock_sync_ccf: invalid beta. Must be in [0,1].");
      }
      d_beta = beta;
    }

    /*******************************************************************
     GET FUNCTIONS
    *******************************************************************/

    float
    pfb_clock_sync_ccf_impl::loop_bandwidth() const
    {
      return d_loop_bw;
    }

    float
    pfb_clock_sync_ccf_impl::damping_factor() const
    {
      return d_damping;
    }

    float
    pfb_clock_sync_ccf_impl::alpha() const
    {
      return d_alpha;
    }

    float
    pfb_clock_sync_ccf_impl::beta() const
    {
      return d_beta;
    }

    float
    pfb_clock_sync_ccf_impl::clock_rate() const
    {
      return d_rate_f;
    }

    float
    pfb_clock_sync_ccf_impl::error() const
    {
      return d_error;
    }

    float
    pfb_clock_sync_ccf_impl::rate() const
    {
      return d_rate_f;
    }

    float
    pfb_clock_sync_ccf_impl::phase() const
    {
      return d_k;
    }

    /*******************************************************************
     *******************************************************************/

    void
    pfb_clock_sync_ccf_impl::update_gains()
    {
      float denom = (1.0 + 2.0*d_damping*d_loop_bw + d_loop_bw*d_loop_bw);
      d_alpha = (4*d_damping*d_loop_bw) / denom;
      d_beta = (4*d_loop_bw*d_loop_bw) / denom;
    }

    void
    pfb_clock_sync_ccf_impl::set_taps(const std::vector<float> &newtaps,
				      std::vector< std::vector<float> > &ourtaps,
				      std::vector<kernel::fir_filter_ccf*> &ourfilter)
    {
      int i,j;

      unsigned int ntaps = newtaps.size();
      d_taps_per_filter = (unsigned int)ceil((double)ntaps/(double)d_nfilters);

      // Create d_numchan vectors to store each channel's taps
      ourtaps.resize(d_nfilters);

      // Make a vector of the taps plus fill it out with 0's to fill
      // each polyphase filter with exactly d_taps_per_filter
      std::vector<float> tmp_taps;
      tmp_taps = newtaps;
      while((float)(tmp_taps.size()) < d_nfilters*d_taps_per_filter) {
	tmp_taps.push_back(0.0);
      }

      // Partition the filter
      for(i = 0; i < d_nfilters; i++) {
	// Each channel uses all d_taps_per_filter with 0's if not enough taps to fill out
	ourtaps[i] = std::vector<float>(d_taps_per_filter, 0);
	for(j = 0; j < d_taps_per_filter; j++) {
	  ourtaps[i][j] = tmp_taps[i + j*d_nfilters];
	}

	// Build a filter for each channel and add it's taps to it
	ourfilter[i]->set_taps(ourtaps[i]);
      }

      // Set the history to ensure enough input items for each filter
      set_history(d_taps_per_filter + d_sps + d_sps);

      // Make sure there is enough output space for d_osps outputs/input.
      set_output_multiple(d_osps);
    }

    void
    pfb_clock_sync_ccf_impl::create_diff_taps(const std::vector<float> &newtaps,
					      std::vector<float> &difftaps)
    {
      std::vector<float> diff_filter(3);
      diff_filter[0] = -1;
      diff_filter[1] = 0;
      diff_filter[2] = 1;

      float pwr = 0;
      difftaps.clear();
      difftaps.push_back(0);
      for(unsigned int i = 0; i < newtaps.size()-2; i++) {
	float tap = 0;
	for(unsigned int j = 0; j < diff_filter.size(); j++) {
	  tap += diff_filter[j]*newtaps[i+j];
	}
	difftaps.push_back(tap);
        pwr += fabsf(tap);
      }
      difftaps.push_back(0);

      // Normalize the taps
      for(unsigned int i = 0; i < difftaps.size(); i++) {
        difftaps[i] *= d_nfilters/pwr;
        if(difftaps[i] != difftaps[i]) {
          throw std::runtime_error("pfb_clock_sync_ccf::create_diff_taps produced NaN.");
        }
      }
    }

    std::string
    pfb_clock_sync_ccf_impl::taps_as_string() const
    {
      int i, j;
      std::stringstream str;
      str.precision(4);
      str.setf(std::ios::scientific);

      str << "[ ";
      for(i = 0; i < d_nfilters; i++) {
	str << "[" << d_taps[i][0] << ", ";
	for(j = 1; j < d_taps_per_filter-1; j++) {
	  str << d_taps[i][j] << ", ";
	}
	str << d_taps[i][j] << "],";
      }
      str << " ]" << std::endl;

      return str.str();
    }

    std::string
    pfb_clock_sync_ccf_impl::diff_taps_as_string() const
    {
      int i, j;
      std::stringstream str;
      str.precision(4);
      str.setf(std::ios::scientific);

      str << "[ ";
      for(i = 0; i < d_nfilters; i++) {
	str << "[" << d_dtaps[i][0] << ", ";
	for(j = 1; j < d_taps_per_filter-1; j++) {
	  str << d_dtaps[i][j] << ", ";
	}
	str << d_dtaps[i][j] << "],";
      }
      str << " ]" << std::endl;

      return str.str();
    }

    std::vector< std::vector<float> >
    pfb_clock_sync_ccf_impl::taps() const
    {
      return d_taps;
    }

    std::vector< std::vector<float> >
    pfb_clock_sync_ccf_impl::diff_taps() const
    {
      return d_dtaps;
    }

    std::vector<float>
    pfb_clock_sync_ccf_impl::channel_taps(int channel) const
    {
      std::vector<float> taps;
      for(int i = 0; i < d_taps_per_filter; i++) {
	taps.push_back(d_taps[channel][i]);
      }
      return taps;
    }

    std::vector<float>
    pfb_clock_sync_ccf_impl::diff_channel_taps(int channel) const
    {
      std::vector<float> taps;
      for(int i = 0; i < d_taps_per_filter; i++) {
	taps.push_back(d_dtaps[channel][i]);
      }
      return taps;
    }

    int
    pfb_clock_sync_ccf_impl::general_work(int noutput_items,
					  gr_vector_int &ninput_items,
					  gr_vector_const_void_star &input_items,
					  gr_vector_void_star &output_items)
    {
      gr_complex *in = (gr_complex *) input_items[0];
      gr_complex *out = (gr_complex *) output_items[0];

      if(d_updated) {
        std::vector<float> dtaps;
        create_diff_taps(d_updated_taps, dtaps);
        set_taps(d_updated_taps, d_taps, d_filters);
        set_taps(dtaps, d_dtaps, d_diff_filters);
	d_updated = false;
	return 0;		     // history requirements may have changed.
      }

      float *err = NULL, *outrate = NULL, *outk = NULL;
      if(output_items.size() == 4) {
	err = (float *) output_items[1];
	outrate = (float*)output_items[2];
	outk = (float*)output_items[3];
      }

      std::vector<tag_t> tags;
      get_tags_in_window(tags, 0, 0,
                        d_sps*noutput_items,
                        pmt::intern("time_est"));

      int i = 0, count = 0;
      float error_r, error_i;

      // produce output as long as we can and there are enough input samples
      while(i < noutput_items) {
        if(tags.size() > 0) {
          size_t offset = tags[0].offset-nitems_read(0);
          if((offset >= (size_t)count) && (offset < (size_t)(count + d_sps))) {
            float center = (float)pmt::to_double(tags[0].value);
            d_k = d_nfilters*(center + (offset - count));

            tags.erase(tags.begin());
          }
        }

	while(d_out_idx < d_osps) {

	  d_filtnum = (int)floor(d_k);

	  // Keep the current filter number in [0, d_nfilters]
	  // If we've run beyond the last filter, wrap around and go to next sample
	  // If we've gone below 0, wrap around and go to previous sample
	  while(d_filtnum >= d_nfilters) {
	    d_k -= d_nfilters;
	    d_filtnum -= d_nfilters;
	    count += 1;
	  }
	  while(d_filtnum < 0) {
	    d_k += d_nfilters;
	    d_filtnum += d_nfilters;
	    count -= 1;
	  }

	  out[i+d_out_idx] = d_filters[d_filtnum]->filter(&in[count+d_out_idx]);
	  d_k = d_k + d_rate_i + d_rate_f; // update phase

          // Manage Tags
          std::vector<tag_t> xtags;
          std::vector<tag_t>::iterator itags;
          d_new_in = nitems_read(0) + count + d_out_idx + d_sps;
          get_tags_in_range(xtags, 0, d_old_in, d_new_in);
          for(itags = xtags.begin(); itags != xtags.end(); itags++) {
            tag_t new_tag = *itags;
            //new_tag.offset = d_last_out + d_taps_per_filter/(2*d_sps) - 2;
            new_tag.offset = d_last_out + d_taps_per_filter/4 - 2;
            add_item_tag(0, new_tag);
          }
          d_old_in = d_new_in;
          d_last_out = nitems_written(0) + i + d_out_idx;

          d_out_idx++;

	  if(output_items.size() == 4) {
	    err[i] = d_error;
	    outrate[i] = d_rate_f;
	    outk[i] = d_k;
	  }

	  // We've run out of output items we can create; return now.
	  if(i+d_out_idx >= noutput_items) {
	    consume_each(count);
	    return i;
	  }
	}

	// reset here; if we didn't complete a full osps samples last time,
	// the early return would take care of it.
	d_out_idx = 0;

	// Update the phase and rate estimates for this symbol
	gr_complex diff = d_diff_filters[d_filtnum]->filter(&in[count]);
	error_r = out[i].real() * diff.real();
	error_i = out[i].imag() * diff.imag();
	d_error = (error_i + error_r) / 2.0;       // average error from I&Q channel

        // Run the control loop to update the current phase (k) and
        // tracking rate estimates based on the error value
        // Interpolating here to update rates for ever sps.
        for(int s = 0; s < d_sps; s++) {
          d_rate_f = d_rate_f + d_beta*d_error;
          d_k = d_k + d_rate_f + d_alpha*d_error;
        }

	// Keep our rate within a good range
	d_rate_f = gr::branchless_clip(d_rate_f, d_max_dev);

	i+=d_osps;
	count += (int)floor(d_sps);
      }

      consume_each(count);
      return i;
    }

    void
    pfb_clock_sync_ccf_impl::setup_rpc()
    {
#ifdef GR_CTRLPORT
      // Getters
      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_get<pfb_clock_sync_ccf, float>(
	      alias(), "error",
	      &pfb_clock_sync_ccf::error,
	      pmt::mp(-2.0f), pmt::mp(2.0f), pmt::mp(0.0f),
	      "", "Error signal of loop", RPC_PRIVLVL_MIN,
              DISPTIME | DISPOPTSTRIP)));

      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_get<pfb_clock_sync_ccf, float>(
	      alias(), "rate",
	      &pfb_clock_sync_ccf::rate,
	      pmt::mp(-2.0f), pmt::mp(2.0f), pmt::mp(0.0f),
	      "", "Rate change of phase", RPC_PRIVLVL_MIN,
              DISPTIME | DISPOPTSTRIP)));

      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_get<pfb_clock_sync_ccf, float>(
	      alias(), "phase",
	      &pfb_clock_sync_ccf::phase,
	      pmt::mp(0), pmt::mp((int)d_nfilters), pmt::mp(0),
	      "", "Current filter phase arm", RPC_PRIVLVL_MIN,
              DISPTIME | DISPOPTSTRIP)));

      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_get<pfb_clock_sync_ccf, float>(
	      alias(), "loop bw",
	      &pfb_clock_sync_ccf::loop_bandwidth,
	      pmt::mp(0.0f), pmt::mp(1.0f), pmt::mp(0.0f),
	      "", "Loop bandwidth",
	      RPC_PRIVLVL_MIN, DISPNULL)));

      // Setters
      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_set<pfb_clock_sync_ccf, float>(
	      alias(), "loop bw",
	      &pfb_clock_sync_ccf::set_loop_bandwidth,
	      pmt::mp(0.0f), pmt::mp(1.0f), pmt::mp(0.0f),
	      "", "Loop bandwidth",
	      RPC_PRIVLVL_MIN, DISPNULL)));
#endif /* GR_CTRLPORT */
    }

  } /* namespace digital */
} /* namespace gr */