#pragma once

#include <chrono>

#include <elle/windows.hh>
#include <boost/date_time/posix_time/posix_time.hpp>

namespace elle
{
  // https://stackoverflow.com/a/4918873/1353549
  template <typename Clock, typename Duration>
  inline static
  boost::posix_time::ptime
  to_boost(const std::chrono::time_point<Clock, Duration>& from)
  {
    using duration_t = std::chrono::nanoseconds;
    using rep_t = duration_t::rep;
    rep_t d = std::chrono::duration_cast<duration_t>(from.time_since_epoch()).count();
    rep_t sec = d / 1000000000;
    rep_t nsec = d % 1000000000;
    return boost::posix_time::from_time_t(0)
      + boost::posix_time::seconds(static_cast<long>(sec))
#ifdef BOOST_DATE_TIME_HAS_NANOSECONDS
      + boost::posix_time::nanoseconds(nsec);
#else
      + boost::posix_time::microseconds((nsec+500)/1000);
#endif
  }

  template <typename Clock, typename Duration>
  inline static
  std::chrono::time_point<Clock, Duration>
  from_boost(const boost::posix_time::ptime& from)
  {
    boost::posix_time::time_duration const time_since_epoch
      = from - boost::posix_time::from_time_t(0);
    std::chrono::time_point<Clock, Duration> t
      = std::chrono::system_clock::from_time_t(time_since_epoch.total_seconds());
    long nsec
      = time_since_epoch.fractional_seconds()
      * (1000000000 / time_since_epoch.ticks_per_second());
    return t + std::chrono::nanoseconds(nsec);
  };
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/swar/include/functions/reverse.hpp>
#include <boost/simd/include/constants/signmask.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/dispatch/functor/meta/call.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL ( reverse, BOOST_SIMD_SIMD_TYPES)
{
  using boost::simd::reverse;
  using boost::simd::tag::reverse_;
  using boost::simd::native;

  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>               vT;

  NT2_TEST_TYPE_IS(typename boost::dispatch::meta::call<reverse_(vT)>::type, vT);

  vT origin, reversed;

  for(std::size_t i=0; i < vT::static_size;++i)
  {
    origin[i]   = T(i);
    reversed[i] = T(vT::static_size-1-i);
  }

  origin[0] = reversed[vT::static_size-1] = boost::simd::Signmask<T>();
  origin[vT::static_size-1] = reversed[0] = boost::simd::Valmax<T>();

  NT2_TEST_EQUAL(reverse(origin),reversed);
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_SDK_COMPLEX_DRY_HPP_INCLUDED
#define NT2_SDK_COMPLEX_DRY_HPP_INCLUDED

#include <nt2/sdk/complex/details/dry/meta.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/details/aliasing.hpp>

namespace nt2
{
  template<class T>
  struct BOOST_SIMD_MAY_ALIAS dry
  {
    typedef T type;
    T value_;

    dry() {}
    explicit dry(T const& v) : value_(v)
    {
    }
    operator T () const {return value_; }
    operator T&()       {return value_; }

    T  value() const  { return value_; }
    T& value()        { return value_; }

    bool operator == (const dry<T>&a) const{return value_ == a.value_; }
    bool operator != (const dry<T>&a) const{return value_ != a.value_; }

    T&       operator()()       { return value_; }
    T const& operator()() const { return value_; }
  };
    ////////////////////////////////////////////////////////////////////////////
  // Stream insertion for dry<T>
  ////////////////////////////////////////////////////////////////////////////
  template<class T>
  BOOST_FORCEINLINE
  std::ostream& operator<<(std::ostream& os, dry<T> const& v )
  {
    return os << "(" << v() << ", _ )";
  }
}

#include <nt2/include/functions/extract.hpp>

namespace boost { namespace simd
{
  template<class T, class Ext>
  BOOST_FORCEINLINE
  std::ostream& operator<<(std::ostream& os, boost::simd::native<nt2::dry<T>,Ext> const& v )
  {
    const std::size_t size = meta::cardinal_of<boost::simd::native<T, Ext> >::value;
    os << "{";
    for(std::size_t i = 0;  i < size; ++i)
    {
      os << v[i] << ((i+1 == size) ? "}" :", ");
    }
    return os;
  }
} }

#endif


/* ============================================================================
* Copyright (c) 2009-2016 BlueQuartz Software, LLC
*
* 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 BlueQuartz Software, the US Air Force, nor the names of its
* contributors may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
* USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
* The code contained herein was partially funded by the followig contracts:
*    United States Air Force Prime Contract FA8650-07-D-5800
*    United States Air Force Prime Contract FA8650-10-D-5210
*    United States Prime Contract Navy N00173-07-C-2068
*
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#include <limits>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigen>

#include "EbsdLib/Math/EbsdMatrixMath.h"
#include "EbsdLib/EbsdLib.h"
#include "EbsdLib/Core/Quaternion.hpp"

#include "UnitTestSupport.hpp"

#include "EbsdLib/Test/EbsdLibTestFileLocations.h"

#define DREAM3D_PASSIVE 1.0
#define DREAM3D_ACTIVE -1.0

class QuaternionTest
{
public:
  QuaternionTest() = default;
  virtual ~QuaternionTest() = default;

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void RemoveTestFiles()
  {
#if REMOVE_TEST_FILES
// QFile::remove();
#endif
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void TestEbsdMatrixMath()
  {
    using VectorMapType = Eigen::Map<Eigen::Vector3f>;

    {
      float dir[3] = {1.0f, 2.0f, 3.0f};

      VectorMapType array(const_cast<float*>(dir));
      array.normalize();

      array = array * -1.0f;
    }

    {
      float dir[3] = {1.0f, 2.0f, 3.0f};
      EbsdMatrixMath::Normalize3x1(dir);
      EbsdMatrixMath::Multiply3x1withConstant(dir, -1.0f);
    }
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void TestQuat_t()
  {
    using QuatF = Quaternion<float>;
    QuatF p(1.0f, 0.0f, 0.0f, 1.0f);
    QuatF q(0.0f, 1.0f, 0.0f, 2.0f);
    QuatF out(0.0f, 0.0f, 0.0f, 0.0f);
    QuatF out2(10.0f, 20.0f, 30.0f, 40.0f);

    out2.negate();
    DREAM3D_REQUIRE_EQUAL(out2.x(), -10.0)
    DREAM3D_REQUIRE_EQUAL(out2.y(), -20.0)
    DREAM3D_REQUIRE_EQUAL(out2.z(), -30.0)
    DREAM3D_REQUIRE_EQUAL(out2.w(), -40.0)

    out = p;
    DREAM3D_REQUIRE_EQUAL(p.x(), out.x())
    DREAM3D_REQUIRE_EQUAL(p.y(), out.y())
    DREAM3D_REQUIRE_EQUAL(p.z(), out.z())
    DREAM3D_REQUIRE_EQUAL(p.w(), out.w())

    out.identity();
    DREAM3D_REQUIRE_EQUAL(out.x(), 0.0)
    DREAM3D_REQUIRE_EQUAL(out.y(), 0.0)
    DREAM3D_REQUIRE_EQUAL(out.z(), 0.0)
    DREAM3D_REQUIRE_EQUAL(out.w(), 1.0)

    out = QuatF(-10.5f, -1.5f, -30.66f, -40.987f);
    out.elementWiseAbs();
    DREAM3D_REQUIRE_EQUAL(out.x(), 10.5f)
    DREAM3D_REQUIRE_EQUAL(out.y(), 1.5f)
    DREAM3D_REQUIRE_EQUAL(out.z(), 30.66f)
    DREAM3D_REQUIRE_EQUAL(out.w(), 40.987f)

    out = QuatF(10.0f, 20.0f, 30.0f, 40.0f);
    out.scalarMultiply(-1.0f);
    DREAM3D_REQUIRE_EQUAL(out.x(), -10.0)
    DREAM3D_REQUIRE_EQUAL(out.y(), -20.0)
    DREAM3D_REQUIRE_EQUAL(out.z(), -30.0)
    DREAM3D_REQUIRE_EQUAL(out.w(), -40.0)

    out.scalarDivide(-1.0f);
    out.scalarAdd(50.0f);
    DREAM3D_REQUIRE_EQUAL(out.x(), 60.0)
    DREAM3D_REQUIRE_EQUAL(out.y(), 70.0)
    DREAM3D_REQUIRE_EQUAL(out.z(), 80.0)
    DREAM3D_REQUIRE_EQUAL(out.w(), 90.0)

    out.elementWiseAssign(5.0f);
    DREAM3D_REQUIRE_EQUAL(out.x(), 5.0)
    DREAM3D_REQUIRE_EQUAL(out.y(), 5.0)
    DREAM3D_REQUIRE_EQUAL(out.z(), 5.0)
    DREAM3D_REQUIRE_EQUAL(out.w(), 5.0)

    out.negate();
    out = p + q;
    out = p - q;

    // Conjugate Tests where conjugate of a Quaternion is q*
    // (q*)* = q

    p.x() = 1.0f;
    p.y() = 2.0f;
    p.z() = 3.0f;
    p.w() = 1.0f;

    p = p.conjugate();
    DREAM3D_REQUIRE_EQUAL(p.x(), -1.0)
    DREAM3D_REQUIRE_EQUAL(p.y(), -2.0)
    DREAM3D_REQUIRE_EQUAL(p.z(), -3.0)
    DREAM3D_REQUIRE_EQUAL(p.w(), 1.0)
    p = p.conjugate();
    DREAM3D_REQUIRE_EQUAL(p.x(), 1.0)
    DREAM3D_REQUIRE_EQUAL(p.y(), 2.0)
    DREAM3D_REQUIRE_EQUAL(p.z(), 3.0)
    DREAM3D_REQUIRE_EQUAL(p.w(), 1.0)

    // (pq)* = q*p*
    q.x() = 1.0f;
    q.y() = 0.0f;
    q.z() = 1.0f;
    q.w() = 1.0f;
    out = p * q;
    out = out.conjugate();
    p = p.conjugate();
    q = q.conjugate();
    out2 = q * p;
    DREAM3D_REQUIRE_EQUAL(out.x(), out2.x())
    DREAM3D_REQUIRE_EQUAL(out.y(), out2.y())
    DREAM3D_REQUIRE_EQUAL(out.z(), out2.z())
    DREAM3D_REQUIRE_EQUAL(out.w(), out2.w())

    //(p+q)* = p*+q*
    p.x() = 1.0f;
    p.y() = 2.0f;
    p.z() = 3.0f;
    p.w() = 1.0f;
    q.x() = 1.0f;
    q.y() = 0.0f;
    q.z() = 1.0f;
    q.w() = 1.0f;
    out = p + q;
    out = out.conjugate();
    p = p.conjugate();
    q = q.conjugate();
    out2 = p + q;

    DREAM3D_REQUIRE_EQUAL(out.x(), out2.x())
    DREAM3D_REQUIRE_EQUAL(out.y(), out2.y())
    DREAM3D_REQUIRE_EQUAL(out.z(), out2.z())
    DREAM3D_REQUIRE_EQUAL(out.w(), out2.w())

    // Multiplication Test
    // pq != qp
    p.x() = 1.0f;
    p.y() = 0.0f;
    p.z() = 0.0f;
    p.w() = 1.0f;

    q.x() = 0.0f;
    q.y() = 1.0f;
    q.z() = 0.0f;
    q.w() = 2.0f;

    out = p * q;
    DREAM3D_REQUIRE_EQUAL(out.x(), 2.0)
    DREAM3D_REQUIRE_EQUAL(out.y(), 1.0)
    DREAM3D_REQUIRE_EQUAL(out.z(), 1.0)
    DREAM3D_REQUIRE_EQUAL(out.w(), 2.0)

    out = q * p;
    DREAM3D_REQUIRE_EQUAL(out.x(), 2.0)
    DREAM3D_REQUIRE_EQUAL(out.y(), 1.0)
    DREAM3D_REQUIRE_EQUAL(out.z(), -1.0)
    DREAM3D_REQUIRE_EQUAL(out.w(), 2.0)

    // Norm Test
    // N(q*) = N(q)
    p.x() = 1.0f;
    p.y() = 0.0f;
    p.z() = 0.0f;
    p.w() = 1.0f;
    q.x() = 0.0f;
    q.y() = 1.0f;
    q.z() = 0.0f;
    q.w() = 2.0f;
    float norm = p.norm();
    p = p.conjugate();
    float cnorm = p.norm();
    DREAM3D_REQUIRE_EQUAL(norm, cnorm)

    // Length and Unit Quaternion Tests
    p.x() = 2.0f;
    p.y() = 2.0f;
    p.z() = 2.0f;
    p.w() = 2.0f;
    float length = p.length();
    DREAM3D_REQUIRE_EQUAL(length, 4.0);
    p = p.unitQuaternion();
    DREAM3D_REQUIRE_EQUAL(p.x(), 0.5)
    DREAM3D_REQUIRE_EQUAL(p.y(), 0.5)
    DREAM3D_REQUIRE_EQUAL(p.z(), 0.5)
    DREAM3D_REQUIRE_EQUAL(p.w(), 0.5)

    p.x() = 1.0;
    p.y() = 2.0;
    p.z() = 3.0;
    p.w() = 0.0;
    std::array<float, 3> vec = {0.0f, 0.0f, 0.0f};
    std::array<float, 3> ovec = {0.0f, 0.0f, 0.0f};

    vec = p.getMisorientationVector();
    bool pass = EbsdLibMath::closeEnough(vec[0], 3.14159274f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true);

    pass = EbsdLibMath::closeEnough(vec[1], 6.28318548f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    pass = EbsdLibMath::closeEnough(vec[2], 9.42477798f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    // Test point r=[100] rotated 120Deg @ [111] axis
    vec[0] = 1.0;
    vec[1] = 0.0;
    vec[2] = 0.0;

    // The quaternion Representation for 120@[111]
    q.x() = 0.5f;
    q.y() = 0.5f;
    q.z() = 0.5f;
    q.w() = 0.5f;

    // Passive Rotation
    ovec = q.multiplyByVector(vec.data());

    pass = EbsdLibMath::closeEnough(ovec[0], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true);

    pass = EbsdLibMath::closeEnough(ovec[1], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    pass = EbsdLibMath::closeEnough(ovec[2], 1.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    // Active Rotation
    q = q.conjugate();
    ovec = q.multiplyByVector(vec.data());

    pass = EbsdLibMath::closeEnough(ovec[0], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true);

    pass = EbsdLibMath::closeEnough(ovec[1], 1.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    pass = EbsdLibMath::closeEnough(ovec[2], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    // 90@[100]
    q.x() = 0.7071067811865475f;
    q.y() = 0.0f;
    q.z() = 0.0f;
    q.w() = 0.7071067811865476f;

    vec[0] = 0.0;
    vec[1] = 1.0;
    vec[2] = 0.0;
    // Passive Rotation
    ovec = q.multiplyByVector(vec.data());

    pass = EbsdLibMath::closeEnough(ovec[0], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true);

    pass = EbsdLibMath::closeEnough(ovec[1], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    pass = EbsdLibMath::closeEnough(ovec[2], -1.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    // active Rotation
    q = q.conjugate();
    ovec = q.multiplyByVector(vec.data());

    pass = EbsdLibMath::closeEnough(ovec[0], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true);

    pass = EbsdLibMath::closeEnough(ovec[1], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    pass = EbsdLibMath::closeEnough(ovec[2], 1.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void operator()()
  {
    std::cout << "#### QuaternionTest Starting ####" << std::endl;

    int err = EXIT_SUCCESS;
    DREAM3D_REGISTER_TEST(TestEbsdMatrixMath())
    DREAM3D_REGISTER_TEST(TestQuat_t())
    DREAM3D_REGISTER_TEST(RemoveTestFiles())
  }

public:
  QuaternionTest(const QuaternionTest&) = delete;            // Copy Constructor Not Implemented
  QuaternionTest(QuaternionTest&&) = delete;                 // Move Constructor Not Implemented
  QuaternionTest& operator=(const QuaternionTest&) = delete; // Copy Assignment Not Implemented
  QuaternionTest& operator=(QuaternionTest&&) = delete;      // Move Assignment Not Implemented
};


/*=========================================================================
 *
 *  Copyright Insight Software Consortium
 *
 *  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.txt
 *
 *  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 "stdlib.h"
#include <sstream>
#include <iostream>
#include <string>

#include "itkImage.h"

#include "itkResampleImageFilter.h"
#include "itkImageFileWriter.h"
#include "itkIdentityTransform.h"
#include "itkLinearInterpolateImageFunction.h"
#include "itkImageFileReader.h"
#include "itkCartesianToToroidalTransform.h"
#include "itkToroidalToCartesianTransform.h"
#include "itkKretzImageIO.h"
#include "itkNearestNeighborInterpolateImageFunction.h"
#include <itkCastImageFilter.h>
#include <itkNormalizeImageFilter.h>
#include <itkInterpolateImageFunction.h>

#include <itkPointSetToImageFilter.h>
#include <itkMeshSpatialObject.h>
#include <itkBoundingBox.h>

#include <ctime>
#include <boost/program_options.hpp>
#include <iostream>
#include <string>
#include <itkRescaleIntensityImageFilter.h>

#include <unistd.h>    /* for getopt */

using namespace std;
namespace po = boost::program_options;

const unsigned int Dimension = 3;
typedef itk::Image<unsigned char, Dimension> ImageType;
typedef itk::Image<double, Dimension> DoubleImageType;
typedef itk::ResampleImageFilter<ImageType,ImageType> ResampleImageFilterType;
typedef itk::ImageFileWriter<ImageType> ImageWriterType;
typedef itk::LinearInterpolateImageFunction<ImageType, double> LinearInterpolatorType;
typedef itk::IdentityTransform<double, Dimension> TransformType;
typedef itk::ImageFileReader<ImageType> ImageFileReaderType;
typedef itk::ToroidalToCartesianTransform<double, Dimension> T2CTransformType;
typedef itk::CartesianToToroidalTransform<double, Dimension> C2TTransformType;
typedef itk::Mesh< double, 3 > MeshType;
typedef itk::PointSet<ImageType::PixelType, Dimension> PointSetType;
typedef PointSetType::PointsContainerPointer PointsContainerPointer;
typedef itk::BoundingBox<PointSetType::PointIdentifier, Dimension, double, PointSetType::PointsContainer> BoundingBoxType;

/*
 * Used to find a binary mask of a toroidal volume in cartesian coordinates
 */
ImageType::Pointer createMaskImage(ImageType::Pointer image)
{
  ImageType::Pointer returnImage = ImageType::New();
  returnImage->SetOrigin(image->GetOrigin());
  returnImage->SetDirection(image->GetDirection());
  returnImage->SetSpacing(image->GetSpacing());
  returnImage->SetRegions(image->GetLargestPossibleRegion());
  returnImage->Allocate();
  returnImage->FillBuffer(1);

  return returnImage;
}

int execute(std::string filename, std::string filename_out, std::vector<int> size_vec,std::vector<float> resol_vec, bool flagMask, bool flagNormalise, bool IsDoppler, std::string filename_toroidal_nii)
{
  typedef itk::ImageFileReader< ImageType > ReaderType;
  ReaderType::Pointer reader = ReaderType::New();
  reader->SetFileName( filename );

  typedef itk::KretzImageIO           ImageIOType;
  ImageIOType::Pointer kretzImageIO = ImageIOType::New();
  reader->SetImageIO( kretzImageIO );
  reader->Update();

  ImageType::Pointer toroidalImage = reader->GetOutput();
  T2CTransformType::Pointer t2c = T2CTransformType::New();
  t2c->SetBModeRadius(kretzImageIO->GetRadiusD());
  t2c->SetSweepRadius(kretzImageIO->GetRadiusBStart());
  t2c->SetResolution(kretzImageIO->GetResolution());
  t2c->SetTableTheta(kretzImageIO->m_TableAnglesTheta);
  t2c->SetTablePhi(kretzImageIO->m_TableAnglesPhi);

  //Find the bounds of the toroidal volume in cartesian coordinates
  BoundingBoxType::BoundsArrayType bounds = T2CTransformType::ComputeBounds(toroidalImage.GetPointer(), t2c.GetPointer());
  if(IsDoppler){
    reader = ReaderType::New();
    reader->SetFileName( filename );
    kretzImageIO = ImageIOType::New();
    kretzImageIO->SetIsDoppler(IsDoppler);
    reader->SetImageIO( kretzImageIO );
    reader->Update();
    toroidalImage = reader->GetOutput();
  }
  if(filename_toroidal_nii != ""){
    reader = ReaderType::New();
    reader->SetFileName( filename_toroidal_nii );
    reader->Update();
    toroidalImage = reader->GetOutput();
  }

  std::cout << "size " << size_vec.size() << std::endl;
  std::cout << "resol " << resol_vec.size() << std::endl;

  if(size_vec[0]==0) //if the resolution is provided set the size
  {
    size_vec[0]=(int) ((bounds[1]-bounds[0])/resol_vec[0]);
    size_vec[1]=(int) ((bounds[3]-bounds[2])/resol_vec[1]);
    size_vec[2]=(int) ((bounds[5]-bounds[4])/resol_vec[2]);
  }
  else if(resol_vec[0]==0) //if the size is provided set the resolution
  {
    resol_vec[0]=(bounds[1]-bounds[0])/(size_vec[0]-1);
    resol_vec[1]=(bounds[3]-bounds[2])/(size_vec[1]-1);
    resol_vec[2]=(bounds[5]-bounds[4])/(size_vec[2]-1);
  }
  else
  {
    std::cerr << "Error: both resolution and size provided" <<  std::endl;

    return EXIT_FAILURE;
  }
  std::cout << "resol " << resol_vec.at(0) << " " << resol_vec.at(1) << " " << resol_vec.at(2) << std::endl;
  std::cout << "size " << size_vec[0] << " " << size_vec[1] << " " << size_vec[2]  << std::endl;

  //if a mask is required create a binary image with ones everywhere
  //and same dimensions as input file
  if(flagMask) toroidalImage = createMaskImage(toroidalImage);

  if(flagNormalise){ //if normalised images wanted outpu double image type

    typedef itk::ImageFileWriter<DoubleImageType> ImageWriterType;
    typedef itk::CastImageFilter<ImageType,DoubleImageType> CastImageFilterType;
    typedef itk::NormalizeImageFilter<DoubleImageType, DoubleImageType> NormaliseImageFilterType;

    CastImageFilterType::Pointer castImgFilter = CastImageFilterType::New();
    castImgFilter->SetInput(toroidalImage);
    castImgFilter->Update();

    DoubleImageType::Pointer castImage = castImgFilter->GetOutput();

    NormaliseImageFilterType::Pointer normaliseFilter = NormaliseImageFilterType::New();
    normaliseFilter->SetInput(castImage);
    normaliseFilter->Update();
    DoubleImageType::Pointer normImage = normaliseFilter->GetOutput();

    DoubleImageType::PointType origin;
    origin[0] = bounds[0];
    origin[1] = bounds[2];
    origin[2] = bounds[4];

    C2TTransformType::Pointer c2t = C2TTransformType::New();
    c2t->SetBModeRadius(kretzImageIO->GetRadiusD());
    c2t->SetSweepRadius(kretzImageIO->GetRadiusBStart());
    c2t->SetResolution(kretzImageIO->GetResolution());
    c2t->SetTableTheta(kretzImageIO->m_TableAnglesTheta);
    c2t->SetTablePhi(kretzImageIO->m_TableAnglesPhi);
    typedef itk::ResampleImageFilter<DoubleImageType,DoubleImageType> ResampleFilterType;

    typedef DoubleImageType::SpacingType SpacingType;
    SpacingType spacing;
    spacing[0] = resol_vec.at(0);
    spacing[1] = resol_vec.at(1);
    spacing[2] = resol_vec.at(2);

    typedef typename DoubleImageType::SizeType SizeType;
    SizeType size;
    size[0]= size_vec[0];
    size[1]= size_vec[1];
    size[2]= size_vec[2];

    typename ResampleFilterType::Pointer resampleFilter = ResampleFilterType::New();
          resampleFilter->SetInput(normImage);
    resampleFilter->SetTransform(c2t);
    resampleFilter->SetSize(size);
    resampleFilter->SetOutputOrigin(origin);
    resampleFilter->SetOutputSpacing(spacing);
    resampleFilter->Update();
    DoubleImageType::Pointer output = resampleFilter->GetOutput();

    ImageWriterType::Pointer ITKImageWriter = ImageWriterType::New();

    ITKImageWriter->SetFileName(filename_out.c_str());
    ITKImageWriter->SetInput(output);
    ITKImageWriter->Write();

  }
  else //If non normalised images required can revert to original image type
  {
    ImageType::PointType origin;
    origin[0] = bounds[0];
    origin[1] = bounds[2];
    origin[2] = bounds[4];

    typedef itk::ImageFileWriter<ImageType> ImageWriterType;
    C2TTransformType::Pointer c2t = C2TTransformType::New();
    c2t->SetBModeRadius(kretzImageIO->GetRadiusD());
    c2t->SetSweepRadius(kretzImageIO->GetRadiusBStart());
    c2t->SetResolution(kretzImageIO->GetResolution());
    c2t->SetTableTheta(kretzImageIO->m_TableAnglesTheta);
    c2t->SetTablePhi(kretzImageIO->m_TableAnglesPhi);
    typedef itk::ResampleImageFilter<ImageType,ImageType> ResampleFilterType;

    typedef ImageType::SpacingType SpacingType;
    SpacingType spacing;
    spacing[0] = resol_vec[0];
    spacing[1] = resol_vec[1];
    spacing[2] = resol_vec[2];

    typedef typename ImageType::SizeType SizeType;
    SizeType size;
    size[0]= size_vec[0];
    size[1]= size_vec[1];
    size[2]= size_vec[2];

    typedef itk::NearestNeighborInterpolateImageFunction<ImageType,  double
	    >  NearestNeighbourInterpolatorType;
    typename ResampleFilterType::Pointer resampleFilter = ResampleFilterType::New();
    resampleFilter->SetInput(toroidalImage);
    auto interpolator = NearestNeighbourInterpolatorType::New();
    resampleFilter->SetInterpolator(interpolator);

    resampleFilter->SetTransform(c2t);
    resampleFilter->SetSize(size);
    resampleFilter->SetOutputOrigin(origin);
    resampleFilter->SetOutputSpacing(spacing);
    resampleFilter->Update();
    ImageType::Pointer output = resampleFilter->GetOutput();

    ImageWriterType::Pointer ITKImageWriter = ImageWriterType::New();

    ITKImageWriter->SetFileName(filename_out.c_str());
    ITKImageWriter->SetInput(output);
    ITKImageWriter->Write();
  }
  return EXIT_SUCCESS;

}

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

    std::string filename, filename_out, filename_toroidal_nii;
    std::vector<int> size_vec(3);
    std::vector<float> resol_vec(3);

    po::options_description desc("Allowed options");

    bool flagMask = false;
    bool flagNormalise = false;
    bool flagDoppler = false;

    desc.add_options()
            ("help,h", "produce help message")
            ("inputfile,i", po::value<std::string>(&filename)->required(), "set input file")
            ("outputfile,o", po::value<std::string>(&filename_out)->required(), "set output file")
            ("resol,r", po::value<std::vector<float>>(&resol_vec)->multitoken(), "set resolution")
            ("size,s", po::value<std::vector<int>>(&size_vec)->multitoken(), "set size")
            ("mask,m", po::bool_switch(&flagMask), "mask volume")
            ("normalise,n", po::bool_switch(&flagNormalise), "normalise volume")
            ("IsDoppler,d", po::bool_switch(&flagDoppler), "output power Doppler")
            ("toroidal_image_nii,t", po::value<std::string>(&filename_toroidal_nii), "set toroidal input file")
            ;

    try
    {

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

        if (vm.count("help") || vm.empty() || (vm.count("size")==0 && vm.count("resol")==0)) {
            cout << desc << "\n";
            return EXIT_FAILURE;
        }

        po::notify(vm);

        return execute(filename,filename_out,size_vec,resol_vec,flagMask,flagNormalise,flagDoppler,filename_toroidal_nii);

    }
    catch(std::exception& e)
    {

        std::cerr << "Error: " << e.what() << "\n";

        cout << desc << "\n";

        return EXIT_FAILURE;
    }
    catch(...)
    {
        std::cerr << "Unknown error!" << "\n";
        return EXIT_FAILURE;
    }

}


// Copyright 2017 Yahoo Holdings. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.

#include <vespa/vespalib/testkit/testapp.h>
#include <vespa/vespalib/util/bits.h>
#include <boost/crc.hpp>
#include <boost/version.hpp>

#if BOOST_VERSION < 106900
    #define REFLECT reflect
#else
    #define REFLECT reflect_q
#endif

using namespace vespalib;

class Test : public TestApp
{
public:
    int Main() override;
    template <typename T>
    void testFixed(const T * v, size_t sz);
    void testBuffer();
};

int
Test::Main()
{
    TEST_INIT("bits_test");
    uint8_t u8[5] = { 0, 1, 127, 135, 255 };
    testFixed(u8, sizeof(u8)/sizeof(u8[0]));
    uint16_t u16[5] = { 0, 1, 127, 135, 255 };
    testFixed(u16, sizeof(u16)/sizeof(u16[0]));
    uint32_t u32[5] = { 0, 1, 127, 135, 255 };
    testFixed(u32, sizeof(u32)/sizeof(u32[0]));
    uint64_t u64[5] = { 0, 1, 127, 135, 255 };
    testFixed(u64, sizeof(u64)/sizeof(u64[0]));
    testBuffer();
    TEST_DONE();
}

template <typename T>
void Test::testFixed(const T * v, size_t sz)
{
    EXPECT_EQUAL(0u, Bits::reverse(static_cast<T>(0)));
    EXPECT_EQUAL(1ul << (sizeof(T)*8 - 1), Bits::reverse(static_cast<T>(1)));
    EXPECT_EQUAL(static_cast<T>(-1), Bits::reverse(static_cast<T>(-1)));
    for (size_t i(0); i < sz; i++) {
        EXPECT_EQUAL(Bits::reverse(v[i]), boost::detail::reflector<sizeof(T)*8>::REFLECT(v[i]));
        EXPECT_EQUAL(Bits::reverse(Bits::reverse(v[i])), v[i]);
    }
}

void Test::testBuffer()
{
    uint64_t a(0x0102040810204080ul);
    uint64_t b(a);
    Bits::reverse(&a, sizeof(a));
    EXPECT_EQUAL(a, Bits::reverse(b));
    Bits::reverse(&a, sizeof(a));
    EXPECT_EQUAL(a, b);
}

TEST_APPHOOK(Test)


/*
 * SPDX-FileCopyrightText: 2019-2020 Smart Robotics Lab, Imperial College London
 * SPDX-FileCopyrightText: 2019-2020 Sotiris Papatheodorou, Imperial College London
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <gtest/gtest.h>

#include <cstdint>
#include <cstring>
#include <memory>

#include <Eigen/Dense>

#include <se/image_utils.hpp>

class DepthImageIO : public ::testing::Test {
  protected:
    DepthImageIO() {
      depth_image_data_16_ = std::unique_ptr<uint16_t[]>(new uint16_t[num_pixels_]());
      depth_image_data_f_ = std::unique_ptr<float[]>(new float[num_pixels_]());

      // Initialize the test images (one in scaled uint16_t, the other in float
      // metres) with a black (B), white (W) and gray (G) pattern.
      // G B B B
      // W G B B
      // W W G B
      // W W W G
      for (size_t x = 0; x < depth_image_width_; ++x) {
        for (size_t y = 0; y < depth_image_height_; ++y) {
          uint16_t depth_value_scaled;
          if (x > y) {
            depth_value_scaled = 0;
          } else if (x == y) {
            depth_value_scaled = UINT16_MAX / 2;
          } else {
            depth_value_scaled = UINT16_MAX;
          }
          depth_image_data_16_[x + depth_image_width_ * y] = depth_value_scaled;
          depth_image_data_f_[x + depth_image_width_ * y] = depth_inverse_scale_ * depth_value_scaled;
        }
      }
    }

    std::unique_ptr<uint16_t[]> depth_image_data_16_;
    std::unique_ptr<float[]> depth_image_data_f_;
    const size_t depth_image_width_  = 64;
    const size_t depth_image_height_ = 64;
    const float depth_scale_ = 5000.0f;
    const float depth_inverse_scale_ = 1.0f / depth_scale_;
    const size_t num_pixels_ = depth_image_width_ * depth_image_height_;
    const size_t depth_16_size_bytes_ = sizeof(uint16_t) * num_pixels_;
    const size_t depth_f_size_bytes_ = sizeof(float) * num_pixels_;
    const Eigen::Vector2i depth_image_res_
        = Eigen::Vector2i(depth_image_width_, depth_image_height_);
};

TEST_F(DepthImageIO, PNGSave16Load16) {
  // Save the image.
  const int save_ok = se::save_depth_png(depth_image_data_16_.get(), depth_image_res_, "/tmp/depth_16.png");
  EXPECT_EQ(save_ok, 0);

  // Load the image.
  uint16_t* loaded_depth_16;
  Eigen::Vector2i loaded_depth_image_res (0, 0);
  const int load_ok = se::load_depth_png(&loaded_depth_16, loaded_depth_image_res, "/tmp/depth_16.png");
  EXPECT_EQ(load_ok, 0);

  // Compare the loaded image with the saved one.
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.x()), depth_image_width_);
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.y()), depth_image_height_);
  EXPECT_EQ(memcmp(loaded_depth_16, depth_image_data_16_.get(), depth_16_size_bytes_), 0);

  free(loaded_depth_16);
}

TEST_F(DepthImageIO, PNGSaveFLoad16) {
  // Save the image.
  const int save_ok = se::save_depth_png(depth_image_data_f_.get(), depth_image_res_, "/tmp/depth_f.png", depth_scale_);
  EXPECT_EQ(save_ok, 0);

  // Load the image.
  uint16_t* loaded_depth_16;
  Eigen::Vector2i loaded_depth_image_res (0, 0);
  const int load_ok = se::load_depth_png(&loaded_depth_16, loaded_depth_image_res, "/tmp/depth_f.png");
  EXPECT_EQ(load_ok, 0);

  // Compare the loaded image with the saved one.
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.x()), depth_image_width_);
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.y()), depth_image_height_);
  EXPECT_EQ(memcmp(loaded_depth_16, depth_image_data_16_.get(), depth_16_size_bytes_), 0);

  free(loaded_depth_16);
}

TEST_F(DepthImageIO, PNGSave16LoadF) {
  // Save the image.
  const int save_ok = se::save_depth_png(depth_image_data_16_.get(), depth_image_res_, "/tmp/depth_16.png");
  EXPECT_EQ(save_ok, 0);

  // Load the image.
  float* loaded_depth_f;
  Eigen::Vector2i loaded_depth_image_res (0, 0);
  const int load_ok = se::load_depth_png(&loaded_depth_f, loaded_depth_image_res, "/tmp/depth_16.png", depth_inverse_scale_);
  EXPECT_EQ(load_ok, 0);

  // Compare the loaded image with the saved one.
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.x()), depth_image_width_);
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.y()), depth_image_height_);
  EXPECT_EQ(memcmp(loaded_depth_f, depth_image_data_f_.get(), depth_f_size_bytes_), 0);

  free(loaded_depth_f);
}

TEST_F(DepthImageIO, PNGSaveFLoadF) {
  // Save the image.
  const int save_ok = se::save_depth_png(depth_image_data_f_.get(), depth_image_res_, "/tmp/depth_f.png", depth_scale_);
  EXPECT_EQ(save_ok, 0);

  // Load the image.
  float* loaded_depth_f;
  Eigen::Vector2i loaded_depth_image_res (0, 0);
  const int load_ok = se::load_depth_png(&loaded_depth_f, loaded_depth_image_res, "/tmp/depth_f.png", depth_inverse_scale_);
  EXPECT_EQ(load_ok, 0);

  // Compare the loaded image with the saved one.
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.x()), depth_image_width_);
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.y()), depth_image_height_);
  EXPECT_EQ(memcmp(loaded_depth_f, depth_image_data_f_.get(), depth_f_size_bytes_), 0);

  free(loaded_depth_f);
}

TEST_F(DepthImageIO, PGMSave16Load16) {
  // Save the image.
  const int save_ok = se::save_depth_pgm(depth_image_data_16_.get(), depth_image_res_, "/tmp/depth_16.pgm");
  EXPECT_EQ(save_ok, 0);

  // Load the image.
  uint16_t* loaded_depth_16;
  Eigen::Vector2i loaded_depth_image_res (0, 0);
  const int load_ok = se::load_depth_pgm(&loaded_depth_16, loaded_depth_image_res, "/tmp/depth_16.pgm");
  EXPECT_EQ(load_ok, 0);

  // Compare the loaded image with the saved one.
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.x()), depth_image_width_);
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.y()), depth_image_height_);
  EXPECT_EQ(memcmp(loaded_depth_16, depth_image_data_16_.get(), depth_16_size_bytes_), 0);

  free(loaded_depth_16);
}

TEST_F(DepthImageIO, PGMSaveFLoad16) {
  // Save the image.
  const int save_ok = se::save_depth_pgm(depth_image_data_f_.get(), depth_image_res_, "/tmp/depth_f.pgm", depth_scale_);
  EXPECT_EQ(save_ok, 0);

  // Load the image.
  uint16_t* loaded_depth_16;
  Eigen::Vector2i loaded_depth_image_res (0, 0);
  const int load_ok = se::load_depth_pgm(&loaded_depth_16, loaded_depth_image_res, "/tmp/depth_f.pgm");
  EXPECT_EQ(load_ok, 0);

  // Compare the loaded image with the saved one.
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.x()), depth_image_width_);
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.y()), depth_image_height_);
  EXPECT_EQ(memcmp(loaded_depth_16, depth_image_data_16_.get(), depth_16_size_bytes_), 0);

  free(loaded_depth_16);
}

TEST_F(DepthImageIO, PGMSave16LoadF) {
  // Save the image.
  const int save_ok = se::save_depth_pgm(depth_image_data_16_.get(), depth_image_res_, "/tmp/depth_16.pgm");
  EXPECT_EQ(save_ok, 0);

  // Load the image.
  float* loaded_depth_f;
  Eigen::Vector2i loaded_depth_image_res (0, 0);
  const int load_ok = se::load_depth_pgm(&loaded_depth_f, loaded_depth_image_res, "/tmp/depth_16.pgm", depth_inverse_scale_);
  EXPECT_EQ(load_ok, 0);

  // Compare the loaded image with the saved one.
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.x()), depth_image_width_);
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.y()), depth_image_height_);
  EXPECT_EQ(memcmp(loaded_depth_f, depth_image_data_f_.get(), depth_f_size_bytes_), 0);

  free(loaded_depth_f);
}

TEST_F(DepthImageIO, PGMSaveFLoadF) {
  // Save the image.
  const int save_ok = se::save_depth_pgm(depth_image_data_f_.get(), depth_image_res_, "/tmp/depth_f.pgm", depth_scale_);
  EXPECT_EQ(save_ok, 0);

  // Load the image.
  float* loaded_depth_f;
  Eigen::Vector2i loaded_depth_image_res (0, 0);
  const int load_ok = se::load_depth_pgm(&loaded_depth_f, loaded_depth_image_res, "/tmp/depth_f.pgm", depth_inverse_scale_);
  EXPECT_EQ(load_ok, 0);

  // Compare the loaded image with the saved one.
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.x()), depth_image_width_);
  EXPECT_EQ(static_cast<unsigned>(loaded_depth_image_res.y()), depth_image_height_);
  EXPECT_EQ(memcmp(loaded_depth_f, depth_image_data_f_.get(), depth_f_size_bytes_), 0);

  free(loaded_depth_f);
}


#include "point_neighborhood.h"

#include <Eigen/Dense>
#include <Eigen/StdVector>

  PointNeighborhood::PointNeighborhood(PointXYZRGBNormalCloud::ConstPtr cloudptr,const Conf & conf)
  {
    const PointXYZRGBNormalCloud & cloud = *cloudptr;
    const uint64 cloud_size = cloud.size();

    m_conf = conf;

    m_index.resize(cloud_size);

    Uint64Vector temporary_indices_vector(cloud_size);

    KdTree::Ptr kdtree(new KdTree);

    kdtree->setInputCloud(cloudptr);

    uint64 counter = 0;

    std::vector<int> idxs;
    std::vector<float> dsts;
    for (uint64 i = 0; i < cloud_size; i++)
    {
      const PointXYZRGBNormal & pt = cloud[i];
      conf.searcher->Search(*kdtree,pt,idxs,dsts);
      const uint64 size = idxs.size();

      m_index[i].size = 0;

      if (size == 0)
        continue;
      m_neighbors.resize(counter + size);
      m_total_dists.resize(counter + size);
      m_position_dists.resize(counter + size);
      uint64 inc = 0;
      for (uint64 h = 0; h < size; h++)
            if (uint64(idxs[h]) != i) // do not add self
            {
              m_neighbors[counter + inc] = idxs[h];
              m_total_dists[counter + inc] = TotalDistance(cloud[i],cloud[idxs[h]],conf);
              m_position_dists[counter + inc] = std::sqrt(dsts[h]);
              inc++;
            }

            temporary_indices_vector[i] = counter;
            counter += inc;
            m_index[i].size = inc;
          }

        // if requested by the searcher, remove all non-symmetric links
          if (conf.searcher->IsPostProcessingRequired(PointNeighborhoodSearch::PPTYPE_REMOVE_UNIDIRECTIONAL_LINKS))
            RemoveUnidirectionalLinks(temporary_indices_vector);

          if (conf.searcher->IsPostProcessingRequired(PointNeighborhoodSearch::PPTYPE_COMPLETE_UNIDIRECTIONAL_LINKS))
            AddUnidirectionalLinks(temporary_indices_vector);

        // update pointers
          for (uint64 i = 0; i < cloud_size; i++)
          {
            m_index[i].neighbors = &(m_neighbors[temporary_indices_vector[i]]);
            m_index[i].total_dists = &(m_total_dists[temporary_indices_vector[i]]);
            m_index[i].position_dists = &(m_position_dists[temporary_indices_vector[i]]);
          }
        }

        void PointNeighborhood::RemoveUnidirectionalLinks(Uint64Vector & temporary_indices_vector)
        {
          const uint64 cloud_size = temporary_indices_vector.size();
          Uint64Vector temporary_indices_vector_in(cloud_size);
          NeighsVector index_in(cloud_size);
          Uint64Vector neighbors_in;
          FloatVector total_dists_in;
          FloatVector position_dists_in;
          temporary_indices_vector.swap(temporary_indices_vector_in);
          m_index.swap(index_in);
          m_neighbors.swap(neighbors_in);
          m_total_dists.swap(total_dists_in);
          m_position_dists.swap(position_dists_in);

          m_neighbors.reserve(neighbors_in.size());
          m_total_dists.reserve(total_dists_in.size());
          m_position_dists.reserve(position_dists_in.size());

          uint64 counter = 0;
          for (uint64 i = 0; i < cloud_size; i++)
          {
            const uint64 size = index_in[i].size;
            const uint64 start = temporary_indices_vector_in[i];

            uint64 inc = 0;

            for (uint64 h = 0; h < size; h++)
            {
              const uint64 i2 = neighbors_in[start + h];
              const float td = total_dists_in[start + h];
              const float pd = position_dists_in[start + h];

              const uint64 size2 = index_in[i2].size;
              const uint64 start2 = temporary_indices_vector_in[i2];

              for (uint64 k = 0; k < size2; k++)
                if (neighbors_in[start2 + k] == i)
                {
                  m_neighbors.push_back(i2);
                  m_total_dists.push_back(td);
                  m_position_dists.push_back(pd);
                  inc++;
                  break;
                }
              }

              temporary_indices_vector[i] = counter;
              counter += inc;
              m_index[i].size = inc;
            }
          }

          void PointNeighborhood::AddUnidirectionalLinks(Uint64Vector & temporary_indices_vector)
          {
            const uint64 cloud_size = temporary_indices_vector.size();
            Uint64Vector temporary_indices_vector_in(cloud_size);
            NeighsVector index_in(cloud_size);
            Uint64Vector neighbors_in;
            FloatVector total_dists_in;
            FloatVector position_dists_in;
            temporary_indices_vector.swap(temporary_indices_vector_in);
            m_index.swap(index_in);
            m_neighbors.swap(neighbors_in);
            m_total_dists.swap(total_dists_in);
            m_position_dists.swap(position_dists_in);

            Uint64VectorVector dv_neighbors(cloud_size);
            FloatVectorVector dv_total_dists(cloud_size);
            FloatVectorVector dv_position_dists(cloud_size);

            uint64 counter = 0;
            for (uint64 i = 0; i < cloud_size; i++)
            {
              const uint64 size = index_in[i].size;
              const uint64 start = temporary_indices_vector_in[i];

              Uint64Vector & v_neighbors = dv_neighbors[i];
              FloatVector & v_total_dists = dv_total_dists[i];
              FloatVector & v_position_dists = dv_position_dists[i];

              for (uint64 h = 0; h < size; h++)
              {
                const uint64 i2 = neighbors_in[start + h];
                const float td = total_dists_in[start + h];
                const float pd = position_dists_in[start + h];

                if (std::find(v_neighbors.begin(),v_neighbors.end(),i2) == v_neighbors.end())
                {
                  v_neighbors.push_back(i2);
                  v_total_dists.push_back(td);
                  v_position_dists.push_back(pd);
                  counter++;
                }

                Uint64Vector & v2_neighbors = dv_neighbors[i2];
                FloatVector & v2_total_dists = dv_total_dists[i2];
                FloatVector & v2_position_dists = dv_position_dists[i2];

                if (std::find(v2_neighbors.begin(),v2_neighbors.end(),i) == v2_neighbors.end())
                {
                  v2_neighbors.push_back(i);
                  v2_total_dists.push_back(td);
                  v2_position_dists.push_back(pd);
                  counter++;
                }
              }
            }

            m_neighbors.resize(counter);
            m_total_dists.resize(counter);
            m_position_dists.resize(counter);

            counter = 0;
            for (uint64 i = 0; i < cloud_size; i++)
            {
              const uint64 size = dv_neighbors[i].size();
              m_index[i].size = size;
              const uint64 start = counter;
              temporary_indices_vector[i] = counter;
              for (uint64 h = 0; h < size; h++)
              {
                m_neighbors[start + h] = dv_neighbors[i][h];
                m_total_dists[start + h] = dv_total_dists[i][h];
                m_position_dists[start + h] = dv_position_dists[i][h];
              }
              counter += size;
            }
          }

          float PointNeighborhood::TotalDistance(const PointXYZRGBNormal & a,const PointXYZRGBNormal & b,const Conf & conf) const
          {
            const Eigen::Vector3f epta(a.x,a.y,a.z);
            const Eigen::Vector3f eptb(b.x,b.y,b.z);
            const Eigen::Vector3f eca(a.r,a.g,a.b);
            const Eigen::Vector3f ecb(b.r,b.g,b.b);
            const Eigen::Vector3f ena(a.normal_x,a.normal_y,a.normal_z);
            const Eigen::Vector3f enb(b.normal_x,b.normal_y,b.normal_z);

            float spatial_dist = (epta - eptb).norm() / conf.max_distance;
            float color_dist = (eca - ecb).norm() / (255.0f * std::sqrt(3.0f));
            float cos_angle_normal = 1.0f - ena.dot(enb);
            return spatial_dist * conf.position_importance + cos_angle_normal * conf.normal_importance + color_dist * conf.color_importance;
          }


//------------------------------------------------------------------------------
/*
This file is part of paysharesd: https://github.com/payshares/paysharesd
Copyright (c) 2014 Stellar Development Foundation.

Permission to use, copy, modify, and/or distribute this software for any
purpose  with  or without fee is hereby granted, provided that the above
copyright notice and this permission notice appear in all copies.

THE  SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
WITH  REGARD  TO  THIS  SOFTWARE  INCLUDING  ALL  IMPLIED  WARRANTIES  OF
MERCHANTABILITY  AND  FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
ANY  SPECIAL ,  DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
WHATSOEVER  RESULTING  FROM  LOSS  OF USE, DATA OR PROFITS, WHETHER IN AN
ACTION  OF  CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
//==============================================================================
#include "ripple_app/ripple_app.h"
#include "InflationTransactor.h"
#include <boost/multiprecision/cpp_int.hpp>
#include "ripple_data/protocol/TER.h"
#include "ripple_basics/log/LogPartition.h"
#include "ripple_basics/log/Log.h"
#include "ripple_app/tx/TransactionEngine.h"
#include "ripple_basics/types/BasicTypes.h"
#include "ripple_app/main/Application.h"
#include "ripple_app/data/DatabaseCon.h"

using namespace std;

#define INFLATION_FREQUENCY			(60*60*24*7)  // every 7 days
//inflation is .000190721 per 7 days, or 1% a year
#define INFLATION_RATE_TRILLIONTHS      190721000L
#define TRILLION                    1000000000000L
#define INFLATION_WIN_MIN_PERCENT	  15000000000L   // 1.5%
#define INFLATION_NUM_WINNERS		50
#define INFLATION_START_TIME		(1403900503-946684800) // seconds since 1/1/2000  start july 1st
#ifndef MIN
#define MIN(x,y)  ((x)<(y) ? (x) : (y))
#endif

/*
What about when an account that wins a dole is now gone?
*/

namespace ripple {

	bool voteSorter(const std::pair<uint160, uint64>  &p1, const std::pair<uint160, uint64> &p2)
	{
		return p1.second > p2.second;
	}

	SETUP_LOG(InflationTransactor)

	TER InflationTransactor::checkSig()
	{
		return tesSUCCESS;
	}

    TER InflationTransactor::payFee()
    {
        STAmount saPaid = mTxn.getTransactionFee();
        if (saPaid == zero)
            return tesSUCCESS;
        if (LedgerDump::enactHistoricalQuirk (QuirkNonzeroInflationFees))
            return Transactor::payFee ();
        else
            return temBAD_FEE;
    }

    TER InflationTransactor::precheckAgainstLedger()
    {
        // make sure it is time to apply inflation
        // make sure the seq number of this inflation transaction is correct

        uint32 seq = mTxn.getFieldU32(sfInflateSeq);
        if (seq != mEngine->getLedger()->getInflationSeq())
        {
            WriteLog(lsINFO, InflationTransactor) << "doInflation: Invalid Seq number.";

            return telNOT_TIME;
        }

        uint32 closeTime = mEngine->getLedger()->getParentCloseTimeNC();
        uint32 nextTime = (INFLATION_START_TIME + seq*INFLATION_FREQUENCY);
        if (closeTime < nextTime)
        {
            WriteLog(lsINFO, InflationTransactor) << "doInflation: Too early.";

            return telNOT_TIME;
        }
        return tesSUCCESS;
    }

	TER InflationTransactor::doApply()
	{
		WriteLog(lsWARNING, InflationTransactor) << "InflationTransactor::doApply() ****************************** ";

		TER terResult = tesSUCCESS;

        /*

        Inflation is calculated using the following

        1. calculate tally of votes based on "inflationDest" set on each account
        2. take the top accounts (by vote) that exceed 1.5% (INFLATION_WIN_MIN_PERCENT) of votes, up to 50 accounts (INFLATION_NUM_WINNERS)
            exception:
                if no account crosses the INFLATION_WIN_MIN_PERCENT, the top 50 is used
        3. share the coins between those accounts proportionally to the number of votes they got

        */

		// don't bother to process on the first apply
		if (!mEngine->mClosingLedger) return tesSUCCESS;

		boost::multiprecision::cpp_int minBalance(mEngine->getLedger()->getTotalCoins());
		boost::multiprecision::cpp_int minWinMultiplier(INFLATION_WIN_MIN_PERCENT);
		boost::multiprecision::cpp_int inflRateDivider(TRILLION);
		boost::multiprecision::cpp_int totalVoted;

		minBalance *= minWinMultiplier;
		minBalance /= inflRateDivider;

		WriteLog(lsDEBUG, InflationTransactor) << "minBalance: " << minBalance;

        // limit to large accounts for now, will need different logic to perform well
        boost::format topAccountsSQL("SELECT sum(balance) as votes,inflationDest from Accounts where inflationDest is not NULL AND inflationDest is NOT 'ggggggggggggggggggggghoLvTs' AND balance > 1000000000 group by inflationDest order by votes desc limit %d");
		string sql(boost::str(topAccountsSQL % INFLATION_NUM_WINNERS));

		vector< pair<uint160, boost::multiprecision::cpp_int> > winners;

		{
            // lock is already taken as we're running inside processor
			Database* db = getApp().getWorkingLedgerDB()->getDB();

			if(db->executeSQL(sql, true) && db->startIterRows())
			{
				totalVoted = db->getBigInt("votes");
				uint160 destAccount=db->getAccountID("inflationDest");
				winners.push_back(pair<uint160, boost::multiprecision::cpp_int>(destAccount, totalVoted));

				WriteLog(lsWARNING, InflationTransactor) << "totalVoted: " << totalVoted << " minBalance: " << minBalance;

				if(totalVoted <= minBalance)
				{  // need to take top 50 as nobody crossed the threshold
					minBalance = 0;
				}

				while(db->getNextRow())
				{
					boost::multiprecision::cpp_int votes = db->getBigInt("votes");
					destAccount = db->getAccountID("inflationDest");
					if(votes>minBalance)
					{
						totalVoted += votes;
						winners.push_back(pair<uint160, boost::multiprecision::cpp_int>(destAccount, votes));
					}
					else
						break;
				};
				db->endIterRows();
			}
			else
			{
				WriteLog(lsERROR, InflationTransactor) << "SELECT failed";
			}
		}

		boost::multiprecision::cpp_int biCoinsToDole( mEngine->getLedger()->getTotalCoins() );
		boost::multiprecision::cpp_int inflRateMultiplier( INFLATION_RATE_TRILLIONTHS );
		boost::multiprecision::cpp_int feePool( mEngine->getLedger()->getFeePool() );

		/// coinsToDole = totalCoins * INFLATION_RATE + feePool
		biCoinsToDole *= inflRateMultiplier;
		biCoinsToDole /= inflRateDivider;
		biCoinsToDole += feePool;

		WriteLog(lsWARNING, InflationTransactor) << "totalVoted: " << totalVoted << " coinsToDole: " << biCoinsToDole;

		for (auto &thisWinner : winners)
		{
			/// coinsDoled = coinToDole * votes / totalVoted
			boost::multiprecision::cpp_int biCoinsDoled=biCoinsToDole;
			biCoinsDoled *= thisWinner.second;
			biCoinsDoled /= totalVoted;

			uint64 coinsDoled = static_cast<uint64>(biCoinsDoled);

			WriteLog(lsWARNING, InflationTransactor) << "coinsDoled: " << coinsDoled;

			SLE::pointer account = mEngine->entryCache(ltACCOUNT_ROOT, Ledger::getAccountRootIndex(thisWinner.first));

			if (account)
			{
				mEngine->entryModify(account);
				account->setFieldAmount(sfBalance, account->getFieldAmount(sfBalance) + coinsDoled);
				mEngine->getLedger()->inflateCoins(coinsDoled);
			}
			else
			{
                // SANITY: maybe we should just redistribute votes to the actual accounts in that case?
				RippleAddress tempAddr;
				tempAddr.setAccountID(thisWinner.first);
				WriteLog(lsERROR, InflationTransactor) << "Inflation dest account not found: " << tempAddr.ToString();
			}
		}

		mEngine->getLedger()->incrementInflationSeq();

		return terResult;
	}

}


#include <iostream>
#include <memory>
#include "amcl_3d/mcl.hpp"
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#define EIGEN_MPL2_ONLY
#include <Eigen/Core>

#include <Eigen/Geometry>
int main(int argc, char *argv[])
{

    amcl_3d::Amcl amcl;
    // amcl param
    {
        amcl_3d::AmclParam amcl_param;
        amcl_param.augmented_mcl.alpha_fast = 0.95;
        amcl_param.augmented_mcl.alpha_slow = 0.05;
        amcl_param.augmented_mcl.w_fast = 1.0;
        amcl_param.augmented_mcl.w_slow = 1.0;
        amcl_param.augmented_mcl.noise_x_var = 1.0;
        amcl_param.augmented_mcl.noise_y_var = 1.0;
        amcl_param.augmented_mcl.noise_z_var = 0.2;
        amcl_param.augmented_mcl.noise_roll_var = 0.1;
        amcl_param.augmented_mcl.noise_pitch_var = 0.3;
        amcl_param.augmented_mcl.noise_yaw_var = 1.0;
        amcl_param.resample_timing.ess_ratio_threshold = 0.95;
        amcl_param.kld_sampling.min_particle_num = 10;
        amcl_param.kld_sampling.delta = 0.5;
        amcl_param.kld_sampling.epsilon = 0.5;
        amcl_param.kld_sampling.x_bin_width = 0.1;
        amcl_param.kld_sampling.y_bin_width = 0.1;
        amcl_param.kld_sampling.z_bin_width = 0.1;
        amcl.setParam(amcl_param);
        // prediction_model_ = std::make_shared<>(); // todo
    }
    std::cout << "test" << std::endl;
    pcl::PointCloud<pcl::PointXYZ>::Ptr pc_map(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointXYZ map_point(0, 1, 0);
    pc_map->push_back(map_point);pcl::PointXYZ map_point2(1, 0, 0);
    pc_map->push_back(map_point2);
    amcl.setMap(pc_map);

    Position position(0, 0, 0);
    Quat quat;
    quat = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ());
    PoseCovariance covariance;
    covariance(/*x*/ 0, /*x*/ 0) = 1.0;         // x var
    covariance(/*y*/ 1, /*y*/ 1) = 0.0;         // y var
    covariance(/*z*/ 2, /*z*/ 2) = 0.0;         // z var
    covariance(/*roll*/ 3, /*roll*/ 3) = 0.0;   // roll var
    covariance(/*pitch*/ 4, /*pitch*/ 4) = 0.0; // pitch var
    covariance(/*yaw*/ 5, /*yaw*/ 5) = 0.0;     // yaw var

    // set initial pose
    amcl.setInitialPose(position, quat, covariance);

    pcl::PointCloud<pcl::PointXYZ>::Ptr pc_measurement(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointXYZ measurement_point(0, 1, 0);
    pc_measurement->push_back(measurement_point); pcl::PointXYZ measurement_point2(1, 0, 0);
    pc_measurement->push_back(measurement_point2);
    amcl.measureLidar(pc_measurement);
    return 0;
}


#ifndef _GAMEPLAY_H
#define _GAMEPLAY_H

#include <random>
#include <iostream>
#include <sstream>
#include <string>
#include <iostream>
#include <stdexcept>
#include <cmath>
#include <vector>
#include <assert.h>
#include <set>
#include <bitset>
#include <unordered_set>
#include <unordered_map>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <SDL2/SDL.h>
#include <SDL2/SDL_pixels.h>
#include <SDL2/SDL_render.h>
#include <SDL2/SDL2_gfxPrimitives.h>
#include "Events.hpp"
#include "Wrapper.hpp"

SDL_Point operator+(SDL_Point left, SDL_Point right);

SDL_Point operator-(SDL_Point left, SDL_Point right);

SDL_Point operator*(SDL_Point left, SDL_Point right);

SDL_Point operator/(SDL_Point left, SDL_Point right);

SDL_Point operator/(SDL_Point left, SDL_Point right);

SDL_Point operator*(double left, SDL_Point right);

SDL_Point operator*(SDL_Point left, double right);

SDL_Point operator/(SDL_Point left, double right);

double operator!(SDL_Point left);

struct Orientation
{
    // cubic to point
    const double f0, f1, f2, f3;
    // point to cubic
    const double b0, b1, b2, b3;
    // in multiples of 60 deg
    const double start_angle;

    Orientation(double f0_, double f1_, double f2_, double f3_, double b0_, double b1_, double b2_, double b3_,
                double start_angle_)
            : f0(f0_), f1(f1_), f2(f2_), f3(f3_), b0(b0_), b1(b1_), b2(b2_), b3(b3_),
              start_angle(start_angle_) { }
};

const Orientation pointy_orientation = Orientation(sqrt(3.0), sqrt(3.0) / 2.0, 0.0, 3.0 / 2.0, sqrt(3.0) / 3.0,
                                                   -1.0 / 3.0, 0.0, 2.0 / 3.0, 0.5);
const Orientation flat_orientation = Orientation(3.0 / 2.0, 0.0, sqrt(3.0) / 2.0, sqrt(3.0), 2.0 / 3.0, 0.0, -1.0 / 3.0,
                                                 sqrt(3.0) / 3.0, 0);

struct Layout
{
    const Orientation orientation;
    Sint16 size;
    SDL_Point origin;
    SDL_Rect box;

    Layout(Orientation orientation_, Sint16 size_, SDL_Point origin_, SDL_Rect box_)
            : orientation(orientation_), size(size_), origin(origin_), box(box_) { }
};

struct Field;

struct Point
{
    double x;
    double y;

    Point(double x_, double y_) : x(x_), y(y_) { }

    bool operator==(const Point &rhs) const
    {
        return (this->x == rhs.x && this->y == rhs.y);
    }

    inline bool operator!=(const Point &rhs) const
    {
        return !(*this == rhs);
    }

    Point &operator+=(const Point &rhs)
    {
        this->x += rhs.x;
        this->y += rhs.y;
        return *this;
    }

    friend Point operator+(Point lhs, const Point &rhs)
    {
        lhs += rhs;
        return lhs;
    }

    Point &operator-=(const Point &rhs)
    {
        this->x -= rhs.x;
        this->y -= rhs.y;
        return *this;
    }

    friend Point operator-(Point lhs, const Point &rhs)
    {
        lhs -= rhs;
        return rhs;
    }

    Point &operator*=(const Point &rhs)
    {
        this->x *= rhs.x;
        this->y *= rhs.y;
        return *this;
    }

    friend Point operator*(Point lhs, const Point &rhs)
    {
        lhs *= rhs;
        return rhs;
    }

    Point &operator/=(const Point &rhs)
    {
        this->x /= rhs.x;
        this->y /= rhs.y;
        return *this;
    }

    friend Point operator/(Point lhs, const Point &rhs)
    {
        lhs /= rhs;
        return lhs;
    }

    Point &operator*=(const double &rhs)
    {
        this->x *= rhs;
        this->y *= rhs;
        return *this;
    }

    friend Point operator*(Point lhs, const double &rhs)
    {
        lhs *= rhs;
        return lhs;
    }

    Point &operator/=(const double &rhs)
    {
        this->x /= rhs;
        this->y /= rhs;
        return *this;
    }

    friend Point operator/(Point lhs, const double &rhs)
    {
        lhs /= rhs;
        return lhs;
    }

    double operator!()
    {
        return std::sqrt(this->x * this->x + this->y * this->y);
    }

    Field point_to_field(const Layout *layout) const;
};

struct Field
{
    Sint16 x, y, z;

    Field(Sint16 x_, Sint16 y_, Sint16 z_) : x(x_), y(y_), z(z_)
    {
        assert(x + y + z == 0);
    }

    inline bool operator==(const Field &rhs) const
    {
        return (this->x == rhs.x && this->y == rhs.y);
    }

    inline bool operator!=(const Field &rhs) const
    {
        return !(*this == rhs);
    }

    Field &operator+=(const Field &rhs)
    {
        this->x += rhs.x;
        this->y += rhs.y;
        this->z += rhs.z;
        return *this;
    }

    friend Field operator+(Field lhs, const Field &rhs)
    {
        lhs += rhs;
        return lhs;
    }

    Field &operator-=(const Field &rhs)
    {
        this->x -= rhs.x;
        this->y -= rhs.y;
        this->z -= rhs.z;
        return *this;
    }

    friend Field operator-(Field lhs, const Field &rhs)
    {
        lhs -= rhs;
        return rhs;
    }

    Field &operator*=(const Field &rhs)
    {
        this->x *= rhs.x;
        this->y *= rhs.y;
        this->z *= rhs.z;
        return *this;
    }

    friend Field operator*(Field lhs, const Field &rhs)
    {
        lhs *= rhs;
        return rhs;
    }

    Field &operator/=(const Field &rhs)
    {
        double x = this->x / rhs.x;
        double y = this->y / rhs.y;
        double z = this->z / rhs.z;
        Field f = cubic_round(x, y, z);
        *this = f;
        return *this;
    }

    friend Field operator/(Field lhs, const Field &rhs)
    {
        lhs /= rhs;
        return lhs;
    }

    Field &operator*=(const double &rhs)
    {
        double x = this->x * rhs;
        double y = this->y * rhs;
        double z = this->z * rhs;
        Field f = cubic_round(x, y, z);
        *this = f;
        return *this;
    }

    friend Field operator*(Field lhs, const double &rhs)
    {
        lhs *= rhs;
        return lhs;
    }

    Field &operator/=(const double &rhs)
    {
        double x = this->x / rhs;
        double y = this->y / rhs;
        double z = this->z / rhs;
        Field f = cubic_round(x, y, z);
        *this = f;
        return *this;
    }

    friend Field operator/(Field lhs, const double &rhs)
    {
        lhs /= rhs;
        return lhs;
    }

    int operator&(const Field &rhs)
    {
        return (abs(this->x - rhs.x) + abs(this->y - rhs.y) + abs(this->z - rhs.z)) / 2;
    }

    Field get_neighbor(Uint8 direction) const;

    Point field_to_point(const Layout *layout) const;

    std::vector<Point> field_to_polygon_normalized(const Layout *layout) const;

    std::vector<Point> field_to_polygon(const Layout *layout) const;

    std::vector<SDL_Point> field_to_polygon_sdl(const Layout *layout) const;

    static Field cubic_round(double x, double y, double z);

    static Field hex_direction(Uint8 direction);
};

// from upper right corner
const std::vector<Field> hex_directions = {{+1, -1, 0},
                                           {+1, 0,  -1},
                                           {0,  +1, -1},
                                           {-1, +1, 0},
                                           {-1, 0,  +1},
                                           {0,  -1, +1}};

Point field_corner_offset(Uint8 corner, const Layout *layout);

namespace std
{
    template<>
    struct hash<Field>
    {
        size_t operator()(const Field &f) const
        {
            hash<Sint16> int_hash;
            size_t hx = int_hash(f.x);
            size_t hy = int_hash(f.y);
            // hz would be redundant, since f.z is redundant
            // combine hashes
            return hx ^ (hy + 0x9e3779b9 + (hx << 6) + (hx >> 2));
        }
    };
}

inline std::ostream &operator<<(std::ostream &os, const Field &rhs)
{
    os << "(" << rhs.x << "," << rhs.y << ",";
    return os;
}

struct Resource
{
    Uint32 circle;
    Uint32 triangle;
    Uint32 square;

    Resource &operator+=(const Resource &rhs)
    {
        this->circle += rhs.circle;
        this->triangle += rhs.triangle;
        this->square += rhs.square;
        return *this;
    }

    friend Resource operator+(Resource lhs, const Resource &rhs)
    {
        lhs += rhs;
        return lhs;
    }

    Resource &operator-=(const Resource &rhs)
    {
        if (this->circle < rhs.circle)
            this->circle = 0;
        else
            this->circle -= rhs.circle;

        if (this->triangle < rhs.triangle)
            this->triangle = 0;
        else
            this->triangle -= rhs.triangle;

        if (this->square < rhs.square)
            this->square = 0;
        else
            this->square -= rhs.square;

        return *this;
    }

    friend Resource operator-(Resource lhs, const Resource &rhs)
    {
        lhs -= rhs;
        return lhs;
    }

    Resource &operator*=(const Resource &rhs)
    {
        this->circle *= rhs.circle;
        this->triangle *= rhs.triangle;
        this->square *= rhs.square;
        return *this;
    }

    friend Resource operator*(Resource lhs, const Resource &rhs)
    {
        lhs *= rhs;
        return lhs;
    }

    Resource &operator*=(const double rhs)
    {
        this->circle *= rhs;
        this->triangle *= rhs;
        this->square *= rhs;
        return *this;
    }

    friend Resource operator*(const double lhs, Resource rhs)
    {
        rhs *= lhs;
        return rhs;
    }

    friend Resource operator*(Resource lhs, const double rhs)
    {
        return rhs * lhs;
    }

    bool operator<(const Resource &rhs) const
    {
        return (this->circle < rhs.circle || this->triangle < rhs.triangle || this->square < rhs.square);
    }

    inline bool operator>(const Resource &rhs) const { return rhs < *this; }

    inline bool operator<=(const Resource &rhs) const { return !(*this > rhs); }

    inline bool operator>=(const Resource &rhs) const { return !(*this < rhs); }

    bool operator==(const Resource &rhs) const
    {
        return (this->circle == rhs.circle && this->triangle == rhs.triangle && this->square == rhs.square);
    }

    inline bool operator!=(const Resource &rhs) const { return !(*this == rhs); }
};

enum Upgrade
{
    Regeneration_1,
    Regeneration_2,
    Regeneration_3,
    Reproduction_1,
    Reproduction_2,
    Reproduction_3,
    Offense_1,
    Offense_2,
    Offense_3,
    Defense_1,
    Defense_2,
    Defense_3,
};

const std::vector<Upgrade> UPGRADES = {Regeneration_1, Regeneration_2, Regeneration_3, Reproduction_1, Reproduction_2,
                                       Reproduction_3, Offense_1, Offense_2, Offense_3, Defense_1, Defense_2, Defense_3
};

const int NUM_UPGRADES = 12;

typedef std::bitset<NUM_UPGRADES> UpgradeFlags;

namespace std
{
    template<>
    struct hash<Upgrade>
    {
        size_t operator()(const Upgrade &f) const
        {
            int i = static_cast<int>(f);
            hash<int> int_hash;
            return int_hash(f);
        }
    };
}

const std::unordered_map<Upgrade, Resource> UPGRADE_COSTS(
        {
                {Regeneration_1, {4,  4,  4}},
                {Regeneration_2, {8,  8,  8}},
                {Regeneration_3, {16, 16, 16}},
                {Reproduction_1, {4,  4,  4}},
                {Reproduction_2, {8,  8,  8}},
                {Reproduction_3, {16, 16, 16}},
                {Offense_1,      {4,  4,  4}},
                {Offense_2,      {8,  8,  8}},
                {Offense_3,      {16, 16, 16}},
                {Defense_1,      {4,  4,  4}},
                {Defense_2,      {8,  8,  8}},
                {Defense_3,      {16, 16, 16}}
        }
);

const std::unordered_map<Upgrade, std::string> UPGRADE_NAMES(
        {
                {Regeneration_1, "Regeneration 1"},
                {Regeneration_2, "Regeneration 2"},
                {Regeneration_3, "Regeneration 3"},
                {Reproduction_1, "Reproduction 1"},
                {Reproduction_2, "Reproduction 2"},
                {Reproduction_3, "Reproduction 3"},
                {Offense_1,      "Offense 1"},
                {Offense_2,      "Offense 2"},
                {Offense_3,      "Offense 3"},
                {Defense_1,      "Defense 1"},
                {Defense_2,      "Defense 2"},
                {Defense_3,      "Offense 3"}
        }
);

const std::unordered_map<Upgrade, std::string> UPGRADE_TEXTS(
        {
                {Regeneration_1, "Resources yield 2x their base resources per turn."},
                {Regeneration_2, "Resources yield 4x their base resources per turn."},
                {Regeneration_3, "Resources yield 8x their base resources per turn."},
                {Reproduction_1, "Increase the chance for expanding to a new field at the end of the turn by 5%."},
                {Reproduction_2, "Increase the chance for expanding to a new field at the end of the turn by 10%."},
                {Reproduction_3, "Increase the chance for expanding to a new field at the end of the turn by 20%."},
                {Offense_1,      "Double your offense."},
                {Offense_2,      "Double your offense."},
                {Offense_3,      "Double your offense."},
                {Defense_1,      "Double your defense."},
                {Defense_2,      "Double your defense."},
                {Defense_3,      "Double your defense."}
        }
);

class FieldMeta;

class Player
{
public:
    Player()
            : name("Default Player"), uuid(boost::uuids::nil_uuid()) { }

    Player(std::string name_)
            : name(name_), uuid(boost::uuids::basic_random_generator<boost::mt19937>()())
    {
        // use the last 24 bits of the tag for the color
        boost::uuids::uuid id = this->uuid;
        uint8_t *data = id.data;
        this->color = {data[13], data[14], data[15], 0xff};
    }

    SDL_Color get_color() { return this->color; }

    std::string get_name()
    {
        std::ostringstream descriptor;
        int number = (this->uuid.data[0] << 8) | this->uuid.data[1];
        descriptor << this->name << " (" << std::hex << number << ")";
        return descriptor.str();
    }

    bool fight(FieldMeta *field);

    boost::uuids::uuid get_id() { return this->uuid; }

    inline bool operator==(const Player &rhs) const
    {
        return this->uuid == rhs.uuid;
    }

    inline bool operator!=(const Player &rhs) const
    {
        return !(*this == rhs);
    }

    void handle_event(SDL_Event *event);
private:
    boost::uuids::uuid uuid;
    SDL_Color color;
    std::string name;
};

class Grid;

class HexagonGrid;

class FieldMeta
{
public:
    FieldMeta(HexagonGrid *grid_, Field field_, Player &owner_)
            : grid(grid_), field(field_), owner(owner_), changed(true)
    {
        this->upgrades = 0;
        static std::random_device rd;
        std::mt19937 rng(rd());
        std::uniform_int_distribution<Uint8> distro(0, 1);
        this->resources_base.circle = distro(rng);
        this->resources_base.triangle = distro(rng);
        this->resources_base.square = distro(rng);
        this->offense = 0;
        this->defense = 0;
        this->regenerate_resources();
    }

    HexagonGrid *get_grid() { return this->grid; }

    int get_offense()
    {
        int factor = 1;
        if (this->get_upgrades()[Offense_1])
            factor *= 2;
        if (this->get_upgrades()[Offense_2])
            factor *= 2;
        if (this->get_upgrades()[Offense_3])
            factor *= 2;
        return this->offense * factor;
    }

    int get_defense()
    {
        int factor = 1;
        if (this->upgrades[Defense_1])
            factor *= 2;
        if (this->get_upgrades()[Defense_2])
            factor *= 2;
        if (this->get_upgrades()[Defense_3])
            factor *= 2;
        return this->offense * factor;
    }

    void set_offense(int off) { this->offense = off; }
    void set_defense(int def) { this->defense = def; }
    Field get_field() { return this->field; }

    Player &get_owner() { return this->owner; }

    void set_owner(Player &player) { this->owner = player; }
    void load(SDL_Renderer *renderer, Layout *layout);
    Resource get_resources() { return this->resources; }
    UpgradeFlags get_upgrades() { return this->upgrades; }
    void consume_resources(Resource costs);
    void regenerate_resources();
    bool upgrade(Upgrade upgrade);
    void handle_event(const SDL_Event *event);
    FieldMeta *get_neighbor(Uint8 direction);
    double get_reproduction()
    {
        return upgrades[Reproduction_1] * 0.05 + upgrades[Reproduction_2] * 0.1 + upgrades[Reproduction_3] * 0.2 + 0.01;
    }
private:
    bool changed;
    const Field field;
    HexagonGrid *grid;
    Player owner;
    UpgradeFlags upgrades;
    Resource resources_base; // without upgrades applied, used as basis of regeneration
    Resource resources; // actual current resources
    int offense;
    int defense;
};

typedef std::unordered_set<FieldMeta *> Cluster;

class HexagonGrid;

class PlayerManager
{
public:
    PlayerManager()
    {
        players = std::vector<Player>();
        current_player = players.begin();
        default_player = Player();
    }

    Player &get_current();

    void next_turn();

    void shuffle();

    void surrender(Player &player, HexagonGrid *grid);

    void add_player(Player &player);

    long get_num_players() { return players.size(); }

    Player default_player;
    static PlayerManager *pm;

    static bool init();

    static bool destroy();

private:
    std::vector<Player> players;
    std::vector<Player>::iterator current_player;
};

class HexagonGrid
{
public:
    HexagonGrid(Sint16 grid_radius, Layout *layout_, Renderer *renderer_)
            : layout(layout_), radius(grid_radius), renderer(renderer_)
    {
        this->attack_marker = nullptr;
        this->texture = nullptr;
        this->panning = false;
        std::unordered_map<Field, FieldMeta *> fields = std::unordered_map<Field, FieldMeta *>();
        // first lower half, then upper half
        Field new_field = {0, 0, 0};
        for (Sint16 x = -grid_radius; x <= grid_radius; x++)
        {
            Sint16 y_l = (-grid_radius > -x - grid_radius) ? -grid_radius : -x - grid_radius;
            Sint16 y_u = (grid_radius < -x + grid_radius) ? grid_radius : -x + grid_radius;
            for (Sint16 y = y_l; y <= y_u; y++)
            {
                Sint16 z = -x - y;
                new_field = {x, y, z};
                FieldMeta *meta = new FieldMeta(this, new_field, PlayerManager::pm->default_player);
                this->fields.insert({new_field, meta});
            }
        }
        this->marker = new FieldMeta(this, new_field, PlayerManager::pm->default_player);
        this->load();
    }

    ~HexagonGrid()
    {
        for (auto const &elem : this->fields)
        {
            delete elem.second;
        }
        SDL_DestroyTexture(this->texture);
    }
    FieldMeta *get_neighbor(FieldMeta *field, Uint8 direction);
    Cluster get_cluster(FieldMeta *field);
    void render(Renderer *renderer);
    void load();
    Sint16 get_radius() { return radius * layout->size; }
    void move(SDL_Point move);
    void update_marker();
    void update_dimensions(SDL_Point dimensions);
    Resource get_resources_of_cluster(Cluster *cluster);
    Resource consume_resources_of_cluster(Cluster *cluster, Resource costs);
    FieldMeta *point_to_field(const Point p);
    Point field_to_point(FieldMeta *field);
    FieldMeta *get_field(Field field);
    void handle_event(SDL_Event *event);

    bool place(Player &player, FieldMeta *center);
    void set_selecting(bool state) { this->placing = state; }
    FieldMeta *get_attack_marker() { return this->attack_marker; }

    void free(Player &player);
private:
    bool changed;
    bool placing;
    FieldMeta *attack_marker;
    Renderer *renderer;
    SDL_Texture *texture;
    std::unordered_map<Field, FieldMeta *> fields;
    Layout *layout;
    FieldMeta *marker;
    bool panning;
    Sint16 radius;
    bool on_rectangle(SDL_Rect *rect);
};

bool inside_target(const SDL_Rect *target, const SDL_Point *position);

#endif


#include <math.h>
#include <ros/ros.h>

#include <boost/thread.hpp>

//ROS libraries
#include <tf/transform_datatypes.h>
#include <dynamic_reconfigure/server.h>
#include <dynamic_reconfigure/client.h>

//ROS messages
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Range.h>
#include <std_msgs/UInt8.h>
#include <std_srvs/Empty.h>

// Project messages
#include <bridge/PidState.h>
#include <swarmie_msgs/SwarmieIMU.h>

//Package include
#include <usbSerial.h>

#include "pid.h"
#include <bridge/pidConfig.h>

using namespace std;

//aBridge functions
void driveCommandHandler(const geometry_msgs::Twist::ConstPtr& message);
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void serialActivityTimer(const ros::TimerEvent& e);
void publishRosTopics();
void parseData(string data);
void initialconfig();

//Globals
geometry_msgs::QuaternionStamped fingerAngle;
geometry_msgs::QuaternionStamped wristAngle;
swarmie_msgs::SwarmieIMU imuRaw;
nav_msgs::Odometry odom;
double odomTheta = 0;
sensor_msgs::Range sonarLeft;
sensor_msgs::Range sonarCenter;
sensor_msgs::Range sonarRight;
USBSerial usb;
const int baud = 115200;
char dataCmd[] = "d\n";
char moveCmd[16];
char host[128];
const float deltaTime = 0.1; //abridge's update interval
int currentMode = 0;
geometry_msgs::Twist speedCommand;

// Allowing messages to be sent to the arduino too fast causes a disconnect
// This is the minimum time between messages to the arduino in microseconds.
// Only used with the gripper commands to fix a manual control bug.
unsigned int min_usb_send_delay = 100;

float heartbeat_publish_interval = 2;

const double wheelBase = 0.278; //distance between left and right wheels (in M)
const double leftWheelCircumference = 0.3651; // avg for 3 rovers (in M)
const double rightWheelCircumference = 0.3662; // avg for 3 rovers (in M)
const int cpr = 8400; //"cycles per revolution" -- number of encoder increments per one wheel revolution

// running counts of encoder ticks
double leftTicks = 0;
double rightTicks = 0;
// wheel velocities in ticks/sec
double leftTickVel = 0;
double rightTickVel = 0;
double odomTS = 0;

// Immobilize robot until the first PID configuration.
PID left_pid(0, 0, 0, 0, 120, -120, 0, -1);
PID right_pid(0, 0, 0, 0, 120, -120, 0, -1);

//Publishers
ros::Publisher fingerAnglePublish;
ros::Publisher wristAnglePublish;
ros::Publisher imuRawPublish;
ros::Publisher odomPublish;
ros::Publisher sonarLeftPublish;
ros::Publisher sonarCenterPublish;
ros::Publisher sonarRightPublish;
ros::Publisher infoLogPublisher;
ros::Publisher heartbeatPublisher;

ros::Publisher debugPIDPublisher;
bridge::PidState pid_state;

//Subscribers
ros::Subscriber driveControlSubscriber;
ros::Subscriber fingerAngleSubscriber;
ros::Subscriber wristAngleSubscriber;
ros::Subscriber modeSubscriber;

//Timers
ros::Timer publishTimer;
ros::Timer publish_heartbeat_timer;

// Feed-forward constants
double ff_l;
double ff_r;

//Callback handlers
void publishHeartBeatTimerEventHandler(const ros::TimerEvent& event);
void reconfigure(bridge::pidConfig &cfg, uint32_t level);
void modeHandler(const std_msgs::UInt8::ConstPtr& message);

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

    ros::init(argc, argv, "abridge");

    string devicePath;
    ros::param::param("~device", devicePath, string("/dev/ttyUSB0"));
    usb.openUSBPort(devicePath, baud);
    void modeHandler(const std_msgs::UInt8::ConstPtr& message);

    sleep(5);

    ros::NodeHandle aNH;

    fingerAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>("fingerAngle/prev_cmd", 10);
    wristAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>("wristAngle/prev_cmd", 10);
    imuRawPublish = aNH.advertise<swarmie_msgs::SwarmieIMU>("imu/raw", 10);
    odomPublish = aNH.advertise<nav_msgs::Odometry>("odom", 10);
    sonarLeftPublish = aNH.advertise<sensor_msgs::Range>("sonarLeft", 10);
    sonarCenterPublish = aNH.advertise<sensor_msgs::Range>("sonarCenter", 10);
    sonarRightPublish = aNH.advertise<sensor_msgs::Range>("sonarRight", 10);
    infoLogPublisher = aNH.advertise<std_msgs::String>("/infoLog", 1, true);
    debugPIDPublisher = aNH.advertise<bridge::PidState>("bridge/debugPID", 1, false);
    heartbeatPublisher = aNH.advertise<std_msgs::String>("bridge/heartbeat", 1, true);

    driveControlSubscriber = aNH.subscribe("driveControl", 10, driveCommandHandler);
    fingerAngleSubscriber = aNH.subscribe("fingerAngle/cmd", 1, fingerAngleHandler);
    wristAngleSubscriber = aNH.subscribe("wristAngle/cmd", 1, wristAngleHandler);
    modeSubscriber = aNH.subscribe("mode", 1, modeHandler);

    publishTimer = aNH.createTimer(ros::Duration(deltaTime), serialActivityTimer);
    publish_heartbeat_timer = aNH.createTimer(ros::Duration(heartbeat_publish_interval), publishHeartBeatTimerEventHandler);

    ros::param::param<std::string>("odom_frame", odom.header.frame_id, "odom");
    ros::param::param<std::string>("base_link_frame", odom.child_frame_id, "base_link");
    ros::param::param<std::string>("sonar_left_frame", sonarLeft.header.frame_id, "us_left_link");
    ros::param::param<std::string>("sonar_center_frame", sonarCenter.header.frame_id, "us_center_link");
    ros::param::param<std::string>("sonar_right_frame", sonarRight.header.frame_id, "us_right_link");
    imuRaw.header.frame_id = odom.child_frame_id;

    // configure dynamic reconfiguration
    dynamic_reconfigure::Server<bridge::pidConfig> config_server;
    dynamic_reconfigure::Server<bridge::pidConfig>::CallbackType f;
    f = boost::bind(&reconfigure, _1, _2);
    config_server.setCallback(f);

    boost::thread t(initialconfig);
    ros::spin();

    return EXIT_SUCCESS;
}

void reconfigure(bridge::pidConfig &cfg, uint32_t level) {
	double p, i, d, db, st, wu;
	p = cfg.groups.pid.Kp * cfg.groups.pid.scale;
	i = cfg.groups.pid.Ki * cfg.groups.pid.scale;
	d = cfg.groups.pid.Kd * cfg.groups.pid.scale;
	db = cfg.groups.pid.db;
	st = cfg.groups.pid.st;
	wu = cfg.groups.pid.wu;
	ff_l = cfg.groups.pid.ff_l;
	ff_r = cfg.groups.pid.ff_r;

	left_pid.reconfig(p, i, d, db, st, wu);
	right_pid.reconfig(p, i, d, db, st, wu);

	std_msgs::String msg;
	msg.data = "PID reconfigure is done.";
	infoLogPublisher.publish(msg);
}

void driveCommandHandler(const geometry_msgs::Twist::ConstPtr& message) {
  speedCommand.linear.x = message->linear.x;
  speedCommand.angular.z = message->angular.z;
  speedCommand.angular.y = message->angular.y;
}

// The finger and wrist handlers receive gripper angle commands in floating point
// radians, write them to a string and send that to the arduino
// for processing.
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle) {

  // To throttle the message rate so we don't lose connection to the arduino
  usleep(min_usb_send_delay);

  char cmd[16]={'\0'};

  // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'f' indicates this is a finger command to the arduino
    sprintf(cmd, "f,0\n");
  } else {
    sprintf(cmd, "f,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
  // To throttle the message rate so we don't lose connection to the arduino
  usleep(min_usb_send_delay);

    char cmd[16]={'\0'};

    // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'w' indicates this is a wrist command to the arduino
    sprintf(cmd, "w,0\n");
  } else {
    sprintf(cmd, "w,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

double leftTicksToMeters(double leftTicks) {
	return (leftWheelCircumference * leftTicks) / cpr;
}

double rightTicksToMeters(double rightTicks) {
	return (rightWheelCircumference * rightTicks) / cpr;
}

double metersToTicks(double meters) {
    return (meters * cpr) / ((leftWheelCircumference + rightWheelCircumference) / 2);
}

double leftMetersToTicks(double meters) {
    return (meters * cpr) / leftWheelCircumference;
}

double rightMetersToTicks(double meters) {
    return (meters * cpr) / rightWheelCircumference;
}

double diffToTheta(double right, double left) {
	return (right - left) / wheelBase;
}

double thetaToDiff(double theta) {
	return theta * wheelBase;
}

void serialActivityTimer(const ros::TimerEvent& e) {

	int cmd_mode = round(speedCommand.angular.y);

	if (cmd_mode == DRIVE_MODE_STOP) {
		left_pid.reset();
		right_pid.reset();

		sprintf(moveCmd, "s\n");
		usb.sendData(moveCmd);
		memset(&moveCmd, '\0', sizeof (moveCmd));
	}
	else {
		// Calculate tick-wise velocities.
		double linear_sp = metersToTicks(speedCommand.linear.x);
		double angular_sp = metersToTicks(thetaToDiff(speedCommand.angular.z));

        double left_sp = leftMetersToTicks(speedCommand.linear.x) - angular_sp;
        double right_sp = rightMetersToTicks(speedCommand.linear.x) + angular_sp;

		int l = round(left_pid.step(left_sp, leftTickVel, odomTS));
		int r = round(right_pid.step(right_sp, rightTickVel, odomTS));

		// Feed forward
		l += ff_l * left_sp;
		r += ff_r * right_sp;

		// Debugging: Report PID performance for tuning.
		// Output of the PID is in Linear:
		pid_state.header.stamp = ros::Time::now();
		pid_state.left_wheel.output = l; // sp_linear * ff
		pid_state.right_wheel.output = r;
		pid_state.left_wheel.error = left_sp - leftTickVel; // sp - feedback
		pid_state.right_wheel.error = right_sp - rightTickVel; // sp - feedback
		pid_state.left_wheel.p_term = left_pid.getP();
		pid_state.right_wheel.p_term = right_pid.getP();
		pid_state.left_wheel.i_term = left_pid.getI();
		pid_state.right_wheel.i_term = right_pid.getI();
		pid_state.left_wheel.d_term = left_pid.getD();
		pid_state.right_wheel.d_term = right_pid.getD();
		pid_state.linear_setpoint = linear_sp;
		pid_state.left_wheel.setpoint = left_sp;
		pid_state.right_wheel.setpoint = right_sp;

		// Feedback function is in Angular:
		pid_state.angular_setpoint = angular_sp;
		pid_state.left_wheel.feedback = leftTickVel;
		pid_state.right_wheel.feedback = rightTickVel;
		debugPIDPublisher.publish(pid_state);

		sprintf(moveCmd, "v,%d,%d\n", l, r); //format data for arduino into c string
		usb.sendData(moveCmd);                      //send movement command to arduino over usb
		memset(&moveCmd, '\0', sizeof (moveCmd));   //clear the movement command string
	}

	usb.sendData(dataCmd);
	try {
		parseData(usb.readData());
		publishRosTopics();
	} catch (std::exception &e) {
		ROS_WARN("Exception while parsing Arduino data. Probably IMU.");
	}
}

void publishRosTopics() {
	/*
    fingerAnglePublish.publish(fingerAngle);
    wristAnglePublish.publish(wristAngle);
    imuRawPublish.publish(imuRaw);
    odomPublish.publish(odom);
    sonarLeftPublish.publish(sonarLeft);
    sonarCenterPublish.publish(sonarCenter);
    sonarRightPublish.publish(sonarRight);
    */
}

void parseData(string str) {
    istringstream oss(str);
    string sentence;
    static double lastOdomTS = 0;

    while (getline(oss, sentence, '\n')) {
		istringstream wss(sentence);
		string word;

		vector<string> dataSet;
		while (getline(wss, word, ',')) {
			dataSet.push_back(word);
		}

		if (dataSet.size() >= 3 && dataSet.at(1) == "1") {

			if (dataSet.at(0) == "GRF") {
				fingerAngle.header.stamp = ros::Time::now();
				fingerAngle.quaternion = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(2).c_str()), 0.0, 0.0);
            }
			else if (dataSet.at(0) == "GRW") {
				wristAngle.header.stamp = ros::Time::now();
				wristAngle.quaternion = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(2).c_str()), 0.0, 0.0);
			}
			else if (dataSet.at(0) == "IMU") {
				imuRaw.header.stamp = ros::Time::now();
				imuRaw.accelerometer.x = atof(dataSet.at(2).c_str());
				imuRaw.accelerometer.y  = atof(dataSet.at(3).c_str());
				imuRaw.accelerometer.z = atof(dataSet.at(4).c_str());
				imuRaw.magnetometer.x = atof(dataSet.at(5).c_str());
				imuRaw.magnetometer.y = atof(dataSet.at(6).c_str());
				imuRaw.magnetometer.z = atof(dataSet.at(7).c_str());
				imuRaw.angular_velocity.x = atof(dataSet.at(8).c_str());
				imuRaw.angular_velocity.y = atof(dataSet.at(9).c_str());
				imuRaw.angular_velocity.z = atof(dataSet.at(10).c_str());

			    imuRawPublish.publish(imuRaw);
			}
			else if (dataSet.at(0) == "ODOM") {
				leftTicks = atoi(dataSet.at(2).c_str());
				rightTicks = atoi(dataSet.at(3).c_str());
				odomTS = atof(dataSet.at(4).c_str()) / 1000; // Seconds

                double rightWheelDistance = rightTicksToMeters(rightTicks);

                double leftWheelDistance = leftTicksToMeters(leftTicks);

			    //Calculate relative angle that robot has turned
				double dtheta = diffToTheta(rightWheelDistance, leftWheelDistance);

			    //Accumulate angles to calculate absolute heading
			    odomTheta += dtheta;

			    //Decompose linear distance into its component values
			    double meanWheelDistance = (rightWheelDistance + leftWheelDistance) / 2;
                //Twist is in base_link frame, use relative heading
			    double twistX = meanWheelDistance * cos(dtheta);
			    double twistY = meanWheelDistance * sin(dtheta);
                //Pose is in the odom frame, use absolute heading
                double poseX = meanWheelDistance * cos(odomTheta);
                double poseY = meanWheelDistance * sin(odomTheta);

                // Calculate velocities if possible.
			    double vtheta = 0;
			    double vx = 0;
			    double vy = 0;
			    if (lastOdomTS > 0) {
			    	double deltaT = odomTS - lastOdomTS;
			    	vtheta = dtheta / deltaT;
			    	vx = twistX / deltaT;
			    	vy = twistY / deltaT;

			    	// Normalize ticks to ticks/s
			    	leftTickVel = leftTicks / deltaT;
			    	rightTickVel = rightTicks / deltaT;
			    }
		    	lastOdomTS = odomTS;

				odom.header.stamp = ros::Time::now();
				odom.pose.pose.position.x += poseX;
				odom.pose.pose.position.y += poseY;
				odom.pose.pose.position.z = 0;
				odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(odomTheta);

				odom.twist.twist.linear.x = vx;
				odom.twist.twist.linear.y = vy;
				odom.twist.twist.angular.z = vtheta;

			    odomPublish.publish(odom);
			}
			else if (dataSet.at(0) == "USL") {
				sonarLeft.header.stamp = ros::Time::now();
				sonarLeft.range = atof(dataSet.at(2).c_str()) / 100.0;
			    sonarLeftPublish.publish(sonarLeft);
			}
			else if (dataSet.at(0) == "USC") {
				sonarCenter.header.stamp = ros::Time::now();
				sonarCenter.range = atof(dataSet.at(2).c_str()) / 100.0;
			    sonarCenterPublish.publish(sonarCenter);
			}
			else if (dataSet.at(0) == "USR") {
				sonarRight.header.stamp = ros::Time::now();
				sonarRight.range = atof(dataSet.at(2).c_str()) / 100.0;
			    sonarRightPublish.publish(sonarRight);
			}

		}
	}
}

void modeHandler(const std_msgs::UInt8::ConstPtr& message) {
	currentMode = message->data;
}

void publishHeartBeatTimerEventHandler(const ros::TimerEvent&) {
    std_msgs::String msg;
    msg.data = "";
    heartbeatPublisher.publish(msg);
}

void initialconfig() {
    // Set PID parameters from the launch configuration
    bridge::pidConfig initial_config;
	ros::NodeHandle nh("~");

	nh.getParam("scale", initial_config.scale);
	nh.getParam("Kp", initial_config.Kp);
	nh.getParam("Ki", initial_config.Ki);
	nh.getParam("Kd", initial_config.Kd);
	nh.getParam("db", initial_config.db);
	nh.getParam("st", initial_config.st);
	nh.getParam("wu", initial_config.wu);
	nh.getParam("ff_l", initial_config.ff_l);
	nh.getParam("ff_r", initial_config.ff_r);

	// Announce the configuration to the server
	dynamic_reconfigure::Client<bridge::pidConfig> dyn_client("abridge");
	dyn_client.setConfiguration(initial_config);

	cout << "Initial configuration sent." << endl;
}