#if !defined(_WIN32)
  #define BOOST_TEST_DYN_LINK
#endif

#define BOOST_TEST_MAIN

#include <ctime>
#include <cmath>
#include <iostream>
#include <boost/test/unit_test.hpp>
#include "../src/cxxConvolver.hpp"

BOOST_AUTO_TEST_CASE(convolver_getters) {
 CxxConvolver cc;
 cc.initializePartitioning(11, 13, 11200, 523);
 BOOST_CHECK_EQUAL(cc.getNumOutputs(), 13);
 BOOST_CHECK_EQUAL(cc.getNumInputs(), 11);
 BOOST_CHECK_EQUAL(cc.getFilterLen(), 11200);
 BOOST_CHECK_EQUAL(cc.getBufferLen(), 523);

 BOOST_CHECK_EQUAL(cc.getTransformLen(), 523*2-1);

 unsigned int num_filter_parts = ceil(11200.f/523.f);
 BOOST_CHECK_EQUAL(cc.getNumFilterParts(), num_filter_parts);
 BOOST_CHECK_EQUAL(cc.getNumFilters(), 11*13);

 // Number of float values in single filter container in time domain
 BOOST_CHECK_EQUAL(cc.getFilterContainerSizeT(), num_filter_parts*(523*2-1));

 // Freq domain the values are complex, hence container size is N/2+1
 BOOST_CHECK_EQUAL(cc.getFilterContainerSizeF(), num_filter_parts*((523*2-1)/2+1));
 cc.destroyPartitioning();
}

BOOST_AUTO_TEST_CASE(convolver_filter_assign_long_simple) {
 CxxConvolver cc;
 cc.initializePartitioning(5, 7, 10, 5);

 unsigned int test_len = 20;
 unsigned int count = 0;

 std::vector< std::vector<float> > filters;
 for(unsigned int i = 0; i < cc.getNumFilters(); i++) {
   filters.push_back(std::vector<float>());
   for(unsigned int j = 0; j < test_len; j++ ) {
     filters.at(i).push_back((float)count);
     count++;
   }
 }

 count = 0;
 for(unsigned int i = 0; i < cc.getNumInputs(); i++) {
   for(unsigned int j = 0; j < cc.getNumOutputs(); j++) {
     cc.updateFilter(i, j, filters.at(count));
     count++;
   }
 }

 count=0;
 for(unsigned int i = 0; i < cc.getNumInputs(); i++) {
   for(unsigned int j = 0; j < cc.getNumOutputs(); j++) {
     float* cur_filter = cc.getTimeFilterAt(i, j);
     for(unsigned int k = 0; k<cc.getFilterContainerSizeT(); k++) {
       unsigned int idx = k%cc.getTransformLen();
       unsigned int f_idx = k/cc.getTransformLen();
       if(idx<cc.getBufferLen()) {
         BOOST_CHECK_EQUAL(cur_filter[idx+f_idx*cc.getTransformLen()], (float)count);
         count++;
       }
       else{
         BOOST_CHECK_EQUAL(cur_filter[idx+f_idx*cc.getTransformLen()], 0.f);
       }
     }
     count+=test_len-cc.getBufferLen()*cc.getNumFilterParts();
   }
 }

 cc.destroyPartitioning();
}

BOOST_AUTO_TEST_CASE(convolver_filter_assign_long) {
 CxxConvolver cc;
 cc.initializePartitioning(2, 4, 2117, 513);

 unsigned int test_len = 5000;
 unsigned int count = 0;

 std::vector< std::vector<float> > filters;
 for(unsigned int i = 0; i < cc.getNumFilters(); i++) {
   filters.push_back(std::vector<float>());
   for(unsigned int j = 0; j < test_len; j++ ) {
     filters.at(i).push_back((float)count);
     count++;
   }
 }

 count = 0;
 for(unsigned int i = 0; i < cc.getNumInputs(); i++) {
   for(unsigned int j = 0; j < cc.getNumOutputs(); j++) {
     cc.updateFilter(i, j, filters.at(count));
     count++;
   }
 }

 count=0;
 for(unsigned int i = 0; i < cc.getNumInputs(); i++) {
   for(unsigned int j = 0; j < cc.getNumOutputs(); j++) {
     float* cur_filter = cc.getTimeFilterAt(i, j);
     for(unsigned int k = 0; k<cc.getFilterContainerSizeT(); k++) {
       unsigned int idx = k%cc.getTransformLen();
       unsigned int f_idx = k/cc.getTransformLen();
       if(idx<cc.getBufferLen()) {
         BOOST_CHECK_EQUAL(cur_filter[idx+f_idx*cc.getTransformLen()], (float)count);
         count++;
       }
       else{
         BOOST_CHECK_EQUAL(cur_filter[idx+f_idx*cc.getTransformLen()], 0.f);
       }
     }
     count+=test_len-cc.getBufferLen()*cc.getNumFilterParts();
   }
 }

 cc.destroyPartitioning();
}

BOOST_AUTO_TEST_CASE(convolver_fft_ifft) {
 CxxConvolver cc;
 unsigned int filter_len = 6889;
 unsigned int buffer_len = 1882;
 unsigned int transform_len = buffer_len*2-1;

 cc.initializePartitioning(1, 1, filter_len, buffer_len);

 float* t_input = (float*)calloc(transform_len, sizeof(float));
 float* t_output = (float*)calloc(transform_len, sizeof(float));
 fftwf_complex* f_output = (fftwf_complex*)calloc(transform_len/2+1, sizeof(fftwf_complex));

 t_input[0] = 1.f;

 cc.fft(t_input, f_output);

 for(int i = 0; i < transform_len/2+1; i++) {
   BOOST_CHECK_EQUAL((float)f_output[i][0], 1.0f);
   BOOST_CHECK_EQUAL((float)f_output[i][1], 0.0f);
 }

 cc.ifft(f_output, t_output);

 for (int i = 0; i<transform_len; i++) {
   if(i==0)
     BOOST_CHECK_EQUAL((float)t_output[i]/(float)transform_len, 1.0f);
   else {
     BOOST_CHECK((bool)(abs(t_output[i]/(float)transform_len)<1.0e-7f));
   }
 }

 for (int i = 0; i<transform_len; i++)
   if (i<buffer_len)
     t_input[i] = ((float)rand()/(float)RAND_MAX)*2.0f-1.0f;
   else
     t_input[i] = 0.f;

 cc.fft(t_input, f_output);

 // Test if the real to complex dump random crap in the trunctaion part
 for (int i = 0; i<transform_len; i++)
   if (i>=buffer_len)
     BOOST_CHECK_EQUAL(t_input[i], 0.f);

 free(t_input);
 free(t_output);
 free(f_output);
 cc.destroyPartitioning();
}

BOOST_AUTO_TEST_CASE(add_buffer) {
 CxxConvolver cc;
 unsigned int filter_len = 24000;
 unsigned int buffer_len = 512;
 unsigned int transform_len = buffer_len*2-1;
 unsigned int num_inputs = 2;
 unsigned int num_outputs = 6;

 cc.initializePartitioning(num_inputs, num_outputs, filter_len, buffer_len);

 unsigned int num_filter_parts = cc.getNumFilterParts();

 std::vector< std::vector<float> > buffers;

 // Initialize generic buffer sequence, long enough to fill the whole fdl,
 // in this case 2 buffers longer
 unsigned int test_container_len = buffer_len*(num_filter_parts+2);

 for (int i = 0; i<num_inputs; i++) {
   BOOST_CHECK_EQUAL(cc.getFdlPosition(i), 0);

   std::vector<float> input;
   input.assign(test_container_len+buffer_len, 0.f);

   // Assign value input*1e5 + buffer_index*1e3 + buffer_sample_index
   for (int j = 0; j<test_container_len; j++)
     input.at(j) = 1e5*i+j/buffer_len*1e3f;//+j%buffer_len;

   buffers.push_back(input);
 }

 // Test adding buffers to the convolver
 float* cur_timedomain = (float*)calloc(cc.getTransformLen(), sizeof(float));

 // Go through the generic buffers and add them one by one
 // Test on the fly that the partition index works, and that
 // the frequency domain version is correct at right position
 for (int part = 0; part<cc.getNumFilterParts()+2; part++) {

   for (int i = 0; i<num_inputs; i++) {
     cc.addNewBuffer(i, &(buffers.at(i)[part*buffer_len]));
     for(int k = 0; k<buffer_len;k++)
       BOOST_CHECK_EQUAL(cc.getInBuffers(i)[k], buffers.at(i)[part*buffer_len+k]);
   }

   // Add new buffer increments the fdl_position counter for each input
   for (int i = 0; i<num_inputs; i++) {
     BOOST_CHECK_EQUAL(cc.getFdlPosition(i), MOD((part+1), cc.getNumFilterParts()));

     // Added buffer in frequency domain (with time domain padding)
     fftwf_complex* cur = cc.getFdlAt(i, part);
     // ifft
     cc.ifft(cur, cur_timedomain);

     // Check that input index is correct
     BOOST_CHECK(cur_timedomain[0]/transform_len-buffers.at(i)[part*buffer_len]<1e-1);
     // Seems like with very large numbers (>1e5) the error is aroud 20 dB

     // std::cout<<cur_timedomain[0]/transform_len<<" , "<<1e5*i<<", "<<<<std::endl;
     // std::cout<<cur_timedomain[0]/transform_len-buffers.at(i)[part*buffer_len]<<std::endl;
   }
 }

 free(cur_timedomain);
 cc.destroyPartitioning();
}

BOOST_AUTO_TEST_CASE(add_buffer_more_parts) {
  CxxConvolver cc;
  unsigned int filter_len = 45001;
  unsigned int buffer_len = 1289;
  unsigned int transform_len = buffer_len*2-1;
  unsigned int num_inputs = 5;
  unsigned int num_outputs = 11;

  cc.initializePartitioning(num_inputs, num_outputs, filter_len, buffer_len);

  unsigned int num_filter_parts = cc.getNumFilterParts();

  std::vector< std::vector<float> > buffers;

  // Initialize generic buffer sequence, long enough to fill the whole fdl,

  unsigned int num_extra_buffers=10;
  unsigned int test_container_len = buffer_len*(num_filter_parts+num_extra_buffers);

  for (int i = 0; i<num_inputs; i++) {
    BOOST_CHECK_EQUAL(cc.getFdlPosition(i), 0);

    std::vector<float> input;
    input.assign(test_container_len+buffer_len, 0.f);

    // Assign value input*1e5 + buffer_index*1e3 + buffer_sample_index
    for (int j = 0; j<test_container_len; j++)
      input.at(j) = (1e5*i+j/buffer_len*1e3f)*1e-5;//+j%buffer_len;

    for(int i = 0; i<cc.getBufferLen()/2-1; i++)
      input.insert(input.begin(), 0.f);

    buffers.push_back(input);
  }

  // Test adding buffers to the convolver
  float* cur_timedomain = (float*)calloc(cc.getTransformLen(), sizeof(float));

  // Go through the generic buffers and add them one by one
  // Test on the fly that the partition index works, and that
  // the frequency domain version is correct at right position
  for (int part = 0; part < cc.getNumFilterParts()+num_extra_buffers; part++) {

    for (int i = 0; i<num_inputs; i++) {
      cc.addNewBuffer(i, &(buffers.at(i)[part*buffer_len]));

      for(int k = 0; k<buffer_len;k++)
        BOOST_CHECK_EQUAL(cc.getInBuffers(i)[k], buffers.at(i)[part*buffer_len+k]);
    }

    // Adding a new buffer increments the fdl_position counter for each input
    for (int i = 0; i<num_inputs; i++) {
      BOOST_CHECK_EQUAL(cc.getFdlPosition(i), MOD((part+1), cc.getNumFilterParts()));

      // Added buffer (with time domain padding) in frequency domain
      fftwf_complex* cur = cc.getFdlAt(i, cc.getFdlPosition(i));
      // ifft
      cc.ifft(cur, cur_timedomain);

      // Check that input index is correct
      BOOST_CHECK(std::abs(cur_timedomain[0]/transform_len-buffers.at(i)[part*buffer_len])<1e-4);

    }
  }

  free(cur_timedomain);
  cc.destroyPartitioning();
}

// BOOST_AUTO_TEST_CASE(process_input_output) {
//   CxxConvolver cc;
//   std::ifstream file("reference_data.json");
//   json j;
//   j<<file;
//
//   std::vector< float > input = j["input"].get< std::vector<float> >();
//   std::vector< float > output = j["output"].get< std::vector<float> >();
//   std::vector< float > filters = j["filters"].get< std::vector<float> >();
//
//   unsigned int num_inputs = 1;
//   unsigned int num_outputs = j["num_outputs"].get< unsigned int >();
//   unsigned int buffer_len = j["buffer_len"].get< unsigned int >();
//   unsigned int filter_len = j["filter_len"].get< unsigned int >();
//   unsigned int num_buffers = j["num_buffers"].get< unsigned int >();
//
//   cc.initializePartitioning(num_inputs, num_outputs, filter_len, buffer_len);
//   std::vector<float> output_buffers(buffer_len*num_buffers*num_outputs);
//
//   BOOST_CHECK_EQUAL(input.size(), num_buffers*buffer_len);
//   for(int i = 0; i < num_outputs; i++) {
//     float* cur_filt = &(filters[i*filter_len]);
//     cc.updateFilter(0, i, cur_filt, filter_len);
//   }
//
//
//   for(int i = 0; i < cc.getBufferLen()-1; i++)
//     input.insert(input.begin(), 0.f);
//
//   unsigned int temp_num = num_buffers;
//
//   for(int i = 0; i < temp_num; i++) {
//     cc.addNewBuffer(0, &(input[i*buffer_len]));
//     BOOST_CHECK_EQUAL(cc.getFdlPosition(0), MOD((i+1), cc.getNumFilterParts()));
//
//     for(int j = 0; j < num_outputs; j++) {
//       cc.processInputOutput(0, j);
//       float* cur = cc.getOutputAt(j);
//       unsigned int out_buf_idx = j*buffer_len+i*buffer_len*num_outputs;
//       memcpy(&(output_buffers[out_buf_idx]), cur+cc.getBufferLen()-1, buffer_len*sizeof(float));
//     }
//   }
//
//   file.close();
//   cc.destroyPartitioning();
// }

BOOST_AUTO_TEST_CASE(swap_filters) {
  CxxConvolver cc;
  unsigned int filter_len = 45001;
  unsigned int buffer_len = 1289;
  unsigned int transform_len = buffer_len*2-1;
  unsigned int num_inputs = 5;
  unsigned int num_outputs = 11;

  cc.initializePartitioning(num_inputs, num_outputs, filter_len, buffer_len);

  std::vector<float> filters_1(num_inputs*num_outputs*filter_len);
  std::vector<float> filters_2(num_inputs*num_outputs*filter_len);
  int count = 0;
  for(int i = 0; i < num_inputs; i++) {
    for(int j = 0; j < num_outputs; j++) {
      int f_address = (j+num_outputs*i)*filter_len;
      for(int k = 0; k < filter_len; k++) {
        filters_1.at(f_address+k) = count;
        filters_2.at(f_address+k) = -count;
        count++;
      }
    }
  }

  for(int i = 0; i < num_inputs; i++) {
    for(int j = 0; j < num_outputs; j++) {
      int f_address = (j+num_outputs*i)*filter_len;
      cc.updateFilter(i, j, &(filters_1[f_address]), filter_len);
    }
  }

  int swapped_input = 2;
  int swapped_output = 3;
  int swapped_part = 4;
  int swapped_address = (swapped_output+swapped_input*num_outputs)*filter_len;
  cc.swapPart(swapped_input, swapped_output,
              &(filters_2[swapped_address]), filter_len, swapped_part);

  float* cc_ptr = cc.getTimeSwapFilterAt(swapped_input, swapped_output);
  int f_address = (swapped_output+num_outputs*swapped_input)*filter_len;

  for(int i = 0; i < cc.getBufferLen(); i++) {
    BOOST_CHECK_EQUAL(cc_ptr[i+transform_len*(swapped_part-1)],
                      0.f);
    BOOST_CHECK_EQUAL(cc_ptr[i+transform_len*(swapped_part+1)],
                      0.f);
    BOOST_CHECK_EQUAL(cc_ptr[i+transform_len*swapped_part],
                      filters_2[f_address+buffer_len*swapped_part+i]);
  }

  cc.setAllSwap(true);
  for(int i = 0; i < num_inputs; i++)
    for(int j = 0; j < num_outputs; j++)
      BOOST_CHECK_EQUAL(cc.isSwapping(i, j), true);

  int num_filter_parts = cc.getNumFilterParts();

  // Swap more filter parts that there is in the class
  for(int count = 0; count < 50; count++) {
    if(count < num_filter_parts) {
      for(int i = 0; i < num_inputs; i++) {
        for(int j = 0; j < num_outputs; j++) {
          BOOST_CHECK_EQUAL(cc.getSwapPartPosition(i, j), count);
          BOOST_CHECK_EQUAL(cc.isSwapping(i, j), true);
          BOOST_CHECK_EQUAL(cc.getNumSwapping(), num_outputs*num_inputs);
        }
      }
    }
    else {
      for(int i = 0; i < num_inputs; i++) {
        for(int j = 0; j < num_outputs; j++) {
          BOOST_CHECK_EQUAL(cc.isSwapping(i, j), false);
        }
      }
    }

    for(int i = 0; i < num_inputs; i++) {
      for(int j = 0; j < num_outputs; j++) {
        int f_address = (j+num_outputs*i)*filter_len;
        cc.execPartSwap(i,j, &filters_2[f_address], filter_len);
      }
    }
  }

  // Check swapped filter
  for(int i = 0; i < num_inputs; i++) {
    for(int j = 0; j < num_outputs; j++) {
      float* cc_ptr = cc.getTimeSwapFilterAt(i, j);
      int f_address = (j+num_outputs*i)*filter_len;
      for(int k = 0; k < cc.getBufferLen(); k++)
        BOOST_CHECK_EQUAL(cc_ptr[k], filters_2[f_address+k]);
    }
  }
  cc.destroyPartitioning();
}


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if !defined(DIRICHLET_DISTRIBUTION_HPP)
#define DIRICHLET_DISTRIBUTION_HPP

#if defined(_MSC_VER)
#	pragma warning(disable: 4267)	// warning about loss of data when converting size_t to int
#endif

//#include <cmath>
//#include "ncl/nxsdefs.h"

//#include "phycas/src/states_patterns.hpp"

//#include <boost/shared_ptr.hpp>
//#include <boost/format.hpp>
#include "multivariate_probability_distribution.hpp"
//#include "phycas/src/basic_cdf.hpp"
//#include "phycas/src/basic_lot.hpp"
//#include "phycas/src/phycas_string.hpp"
//#if defined(PYTHON_ONLY) && defined(USING_NUMARRAY)
//#	include <boost/python/tuple.hpp>
//#	include <boost/python/numeric.hpp>
//#	include "thirdparty/num_util/num_util.h"
//#endif
//#include "phycas/src/xprobdist.hpp"

namespace phycas
{

/*------------------------------------------------------------------------------------------------------------------------------------------------------------------
|	The Dirichlet distribution, with the number of parameters determined by the number of params supplied to the constructor.
*/
class DirichletDistribution : public MultivariateProbabilityDistribution
	{
	public:
                                                    DirichletDistribution();
                                                    DirichletDistribution(const std::vector<double> & params);
                                                    DirichletDistribution(const DirichletDistribution & other);
		virtual                                     ~DirichletDistribution();

        DirichletDistribution *                     cloneAndSetLot(Lot * other) const;
        DirichletDistribution *                     Clone() const;
		virtual void								SetLot(Lot * other);
		virtual void								ResetLot();
		virtual void								SetSeed(unsigned rnseed);

		virtual bool								IsDiscrete() const;
		virtual std::string 						GetDistributionName() const;
		virtual std::string 						GetDistributionDescription() const;
		std::string                                 GetDescriptionForPython() const;
		virtual std::vector<double>					GetMean() const;
		virtual std::vector<double> 				GetVar() const;
		virtual std::vector<double> 				GetStdDev() const;
		virtual std::vector<double>					Sample() const;
		virtual double								ApproxCDF(const std::vector<double> &x, unsigned nsamples = 10000) const;
		virtual double								GetLnPDF(const std::vector<double> &x) const;
		virtual double								GetRelativeLnPDF(const std::vector<double> &x) const;
		virtual void 								SetMeanAndVariance(const std::vector<double> &m, const std::vector<double> &v);
#		if defined(PYTHON_ONLY)
#			if defined(USING_NUMARRAY)
				void                                AltSetMeanAndVariance(boost::python::numeric::array m, boost::python::numeric::array v);
				boost::python::numeric::array       GetVarCovarMatrix();
#			else
				void                                AltSetMeanAndVariance(std::vector<double> m, std::vector<double> v);
				std::vector<double>					GetVarCovarMatrix();
#			endif
#		endif

		virtual unsigned							GetNParams() const;
		const GammaDistribution &                   GetDistributionOnParameter(unsigned i) const;

    protected:

		void                                        initialize(const double_vect_t & params);

    protected:

		std::vector<double>                         dirParams;
		std::vector<GammaDistribution>              paramDistributions;
		mutable std::vector<double>                 scratchSpace;
	};

} // namespace phycas

#endif


#pragma once

#include <utility>
#include <vector>

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

#include <perception/kinematics/Parameters.hpp>
#include <perception/kinematics/Pose.hpp>
#include <perception/vision/Camera.hpp>

#include <utils/matrix_helpers.hpp>
#include <types/RRCoord.hpp>
#include <types/JointValues.hpp>
#include <types/SensorValues.hpp>

#include "CKF.hpp"

#define CAMERA_DH_CHAIN_LEN 12
#define HEAD_DH_CHAIN_LEN 3
#define ARM_DH_CHAIN_LEN 14
#define LEG_DH_CHAIN_LEN 19

class Kinematics {
   public:
      friend class KinematicsAdapter;
      friend class KinematicsCalibrationSkill;
      friend class CameraPoseTab;
      Kinematics();

      enum Link {
         FOOT = 0,
         BODY = 8,
         CAMERA = 12,
         NECK = 9
      };

      enum Chain {
         LEFT_CHAIN = 0,
         RIGHT_CHAIN = 1
      };

      /* Creates a Pose with the current evaluated DH Chain */
      Pose getPose();

      void updateDHChain();

      boost::numeric::ublas::matrix<float>
      evaluateDHChain(Link from, Link to, Chain foot, bool top = true);

      boost::numeric::ublas::matrix<float> evaluateMassChain();

      boost::numeric::ublas::matrix<float>
      createBodyToFootOnGroundTransform(Chain foot, boost::numeric::ublas::matrix<float> b2f);

      boost::numeric::ublas::matrix<float>
      createCameraToFootTransform(Chain foot, bool top);

      boost::numeric::ublas::matrix<float>
      createNeckToFootTransform(Chain foot);

      boost::numeric::ublas::matrix<float>
      createFootToWorldTransform(Chain foot, bool top = true);

      boost::numeric::ublas::matrix<float>
      createCameraToWorldTransform(Chain foot, bool top);

      boost::numeric::ublas::matrix<float>
      createNeckToWorldTransform(Chain foot);

      boost::numeric::ublas::matrix<float>
      createWorldToFOVTransform(
         const boost::numeric::ublas::matrix<float> &m);

      boost::numeric::ublas::matrix<float>
      fovToImageSpaceTransform(
         const boost::numeric::ublas::matrix<float> &transform,
         const boost::numeric::ublas::matrix<float> &point, bool top);

      void determineBodyExclusionArray(
         const boost::numeric::ublas::matrix<float> &m,
         int16_t *points, bool top);

      Chain determineSupportChain();

      void setSensorValues(SensorValues sensorValues);

      std::pair<int, int> calculateHorizon(
         const boost::numeric::ublas::matrix<float> &m);
//   private:
      // CKF is not really tuned and not being used at the moment
      //CKF ckf;
      SensorValues sensorValues;
      Chain supportChain;

      boost::numeric::ublas::matrix<float> transformLTop[CAMERA_DH_CHAIN_LEN];
      boost::numeric::ublas::matrix<float> transformLBot[CAMERA_DH_CHAIN_LEN];
      boost::numeric::ublas::matrix<float> transformRTop[CAMERA_DH_CHAIN_LEN];
      boost::numeric::ublas::matrix<float> transformRBot[CAMERA_DH_CHAIN_LEN];

      boost::numeric::ublas::matrix<float> cameraPanInverseHack;

      // DH matrices for mass
      boost::numeric::ublas::matrix<float> transformHB[HEAD_DH_CHAIN_LEN];
      boost::numeric::ublas::matrix<float> transformRAB[ARM_DH_CHAIN_LEN];
      boost::numeric::ublas::matrix<float> transformLAB[ARM_DH_CHAIN_LEN];
      boost::numeric::ublas::matrix<float> transformRFB[LEG_DH_CHAIN_LEN];
      boost::numeric::ublas::matrix<float> transformLFB[LEG_DH_CHAIN_LEN];

      // Contains the masses and centre position of each joint
      std::vector<float> masses;
      std::vector<boost::numeric::ublas::matrix<float> > massesCom;

      Parameters<float> parameters;

      std::vector<std::vector<boost::numeric::ublas::matrix<float> > >
      bodyParts;
};


///1
#include <iostream>
#include <vector>
#include <cmath>
#include <tuple>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/pending/disjoint_sets.hpp>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_2.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef std::size_t                                            Index;
typedef CGAL::Triangulation_vertex_base_with_info_2<Index,K>   Vb;
typedef CGAL::Triangulation_face_base_2<K>                     Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>            Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds>                  Delaunay;
typedef std::pair<K::Point_2,Index> IPoint;
typedef std::tuple<int,int> Edge;
typedef std::vector<Edge> EdgeV;
typedef Tds::Vertex_handle Vh;

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> graph;
typedef boost::graph_traits<graph>::vertex_descriptor vertex_desc;
typedef boost::graph_traits<graph>::edge_iterator edge_it;

using namespace std;

// Strategy:
// - Binary search for the best k
//   - For a given k, construct a Graph that contains only points p_k...p_{n-1}
//   - Find the largest connected component
void solve() {
  int n;
  long r;
  cin >> n >> r;
  long r2 = r * r;

  vector<IPoint> points;
  points.reserve(n);
  int x, y;
  for (int i = 0; i < n; ++i) {
    cin >> x >> y;
    points.emplace_back(K::Point_2(x, y), i);
  }

  // Start binary search
  int a = 1;
  int b = n;
  while (a < b - 1) {

    int empireSize = (a + b) / 2;

    // Use delaunay to quickly construct graph of points outside the empire
    Delaunay dt;
    dt.insert(points.begin() + empireSize, points.end());

    graph G(n - empireSize);
    for (auto e = dt.finite_edges_begin(); e != dt.finite_edges_end(); ++e) {
      int u = e->first->vertex((e->second+1)%3)->info();
      int v = e->first->vertex((e->second+2)%3)->info();
      if (dt.segment(e).squared_length() <= r2) {
        boost::add_edge(u - empireSize, v - empireSize, G);
      }
    }

    // Compute largest connected component
    std::vector<int> component_map(n);
    int ncc = boost::connected_components(G, boost::make_iterator_property_map(component_map.begin(), boost::get(boost::vertex_index, G)));

    int largestComponent = 0;
    vector<int> componentSize(ncc, 0);
    for (int i = 0; i < n - empireSize; ++i) {
      int c = component_map[i];
      componentSize[c]++;
      largestComponent = max(largestComponent, int(componentSize[c]));
    }

    // Continue binary search
    if (empireSize <= largestComponent) {
      // Invalid solution, the empire must be at least as large as the rebel alliance (both expand equally fast)
      // => Expand empire
      a = empireSize;
    }
    else {
      b = empireSize;
    }
  }
  cout << a << endl;
}

int main() {
    ios_base::sync_with_stdio(false);
    int t; cin >> t;
    while (t--) {
        solve();
    }
    return 0;
}


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

#include <boost/simd/arithmetic/functions/hypot.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>
#include <boost/simd/include/functions/simd/exponent.hpp>
#include <boost/simd/include/functions/simd/ldexp.hpp>
#include <boost/simd/include/functions/simd/sqr.hpp>
#include <boost/simd/include/functions/simd/sqrt.hpp>
#include <boost/simd/include/constants/maxexponentm1.hpp>
#include <boost/simd/include/constants/minexponent.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/logical_and.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/simd/include/functions/simd/is_inf.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>
#include <boost/simd/include/constants/inf.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( hypot_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                      ((simd_<floating_<A0>,X>))
                                    )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::as_integer<result_type>::type   iA0;
      result_type r =  boost::simd::abs(a0);
      result_type i =  boost::simd::abs(a1);
      iA0 e =  exponent(boost::simd::max(i, r));
      e = boost::simd::min(boost::simd::max(e,Minexponent<A0>()),Maxexponentm1<A0>());
      result_type res =  ldexp(sqrt(sqr(ldexp(r, -e))+sqr(ldexp(i, -e))), e);
      #ifndef BOOST_SIMD_NO_INVALIDS
      typedef typename meta::as_logical<result_type>::type             lA0;
      lA0 test = logical_or(logical_and(is_nan(a0), is_inf(a1)),
                            logical_and(is_nan(a1), is_inf(a0)));
      return if_else(test, Inf<result_type>(), res);
      #else
      return res;
      #endif
    }
  };
} } }

#endif


#include <boost/foreach.hpp>

#include <cctbx/maptbx/structure_factors.h>
#include <scitbx/array_family/tiny_algebra.h>
#include <cctbx/maptbx/asymmetric_map.h>
#include <iotbx/xplor/map_writer.h>
#include <scitbx/util/cpu_timer.h>

namespace cctbx { namespace maptbx
{

namespace af = scitbx::af;
using std::vector;
using scitbx::int3;

using scitbx::util::cpu_timer;

af::shared< std::complex<double > > asymmetric_map::structure_factors(
  af::const_ref< miller::index<> > indices) const
{
  typedef af::c_grid_padded<3> grid_t;
  typedef std::complex<double> cmplx;
  scitbx::fftpack::real_to_complex_3d<double> fft(this->fft_grid().focus());
  cpu_timer timer;
  fft_map_t fftmap = this->map_for_fft();
  this->map_for_fft_times_ = timer.format();
  CCTBX_ASSERT( fftmap.accessor().all().all_eq(fft.m_real()) );
  CCTBX_ASSERT( fftmap.accessor().focus().all_eq(fft.n_real()) );
  fft.forward(fftmap);
  grid_t cmplxpad(fft.n_complex(), fft.n_complex());
  CCTBX_ASSERT( cmplxpad.size_1d()*2UL == fftmap.accessor().size_1d() );
  af::versa< cmplx, grid_t > cmap( fftmap.handle(), cmplxpad);
  bool anomalous_flag = false, conjugate_flag = true;
  structure_factors::from_map<double> frommap(
    this->space_group(),
    anomalous_flag,
    indices,
    cmap.const_ref(),
    conjugate_flag);
  af::shared< cmplx > fcalc = frommap.data();
  double vol = 1. / fftmap.accessor().focus_size_1d();
  // BOOST_FOREACH(cmplx & fc, fcalc.ref())
  // for(auto &fc : fcalc.ref())
  for(cmplx *fc = fcalc.ref().begin(); fc!=fcalc.ref().end(); ++fc)
  {
    *fc *= vol;
  }
  CCTBX_ASSERT( indices.size() == fcalc.size() );
  return fcalc;
}

// prepare grid adapted integer symmetry operators
//! @todo code duplication, see atom_mask.h
vector<sgtbx::grid_symop> asymmetric_map::grid_symops() const
{
  sgtbx::space_group group = this->space_group();
  unsigned short order = group.order_z();
  CCTBX_ASSERT( order>0 );
  const int3 n = this->unit_cell_grid_size();
  CCTBX_ASSERT( n[0]>0 && n[1]>0 && n[2] >0 );
  vector<sgtbx::grid_symop> symops;
  symops.reserve(order);
  for(size_t i=0; i<order; ++i)
  {
    sgtbx::grid_symop grsym( group(i), n );
    symops.push_back(grsym);
  }
  CCTBX_ASSERT( symops.size() == order );
  return symops;
}

asymmetric_map::fft_map_t asymmetric_map::map_for_fft() const
{
  int3 map_size = this->unit_cell_grid_size();
  // auto symops = this->grid_symops();
  vector<sgtbx::grid_symop> symops = this->grid_symops();
  unsigned short order = this->space_group().order_z();
  fft_map_t result(this->fft_grid(), 0.);
  int3 ebox_min, ebox_max;
  bool has_enclosed_box=asu_.enclosed_box_corners(ebox_min, ebox_max, map_size);
  bool is_asu_inside_cell = this->box_begin().as_tiny().all_ge(0)
    && this->box_end().as_tiny().all_le(map_size+1);
  // CCTBX_ASSERT( is_asu_inside_cell ); No: 48 fails
  // CCTBX_ASSERT( has_enclosed_box ); No: 99
  int3 padded_grid_size( result.accessor().all() );
  cpu_timer timer;
  if( has_enclosed_box && is_asu_inside_cell )
  {
    // auto result_ref = result.ref();
    af::ref<double, af::c_grid_padded<3> > result_ref = result.ref();
    const double *d = data_.begin();
    for(mapped_iterator_t i3=this->mapped_begin(padded_grid_size); !i3.over();
      i3.advance(), ++d)
    {
      int3 pos = i3();
      af::int3 t = pos.as_tiny();
      if( t.all_ge(ebox_min) && t.all_le(ebox_max) )
        result_ref[i3.mapped_index_1d()] = *d;
      else
      {
        short ww = optimized_asu_.where_is(pos);
        if( ww!=0 )
        {
          if( ww==-1 ) // inside on the face
          {
            int3 pos_in_cell(pos);
            translate_into_cell(pos_in_cell, map_size);
            unsigned short ns=site_symmetry_order(symops,pos_in_cell,map_size);
            result_ref(pos_in_cell) = *d / ns;
          }
          else
            result_ref[i3.mapped_index_1d()] = *d;
        }
      }
    }
    CCTBX_ASSERT( d==data_.end() );
  }
  else
  {
    // slower generic version
    for(grid_iterator_t i3=this->grid_begin(); !i3.over(); i3.incr() )
    {
      int3 pos = i3();
      short ww = optimized_asu_.where_is(pos);
      if( ww!=0 )
      {
        int3 pos_in_cell(pos);
        translate_into_cell(pos_in_cell, map_size);
        double map_value = data_(pos);
        if( ww==-1 ) // inside on the face
        {
          unsigned short ns=site_symmetry_order(symops, pos_in_cell, map_size);
          map_value /= ns;
        }
        result(pos_in_cell) = map_value;
      }
    }
  }
  this->fill_fft_times_ = timer.format();
  return result;
}

asymmetric_map::fft_map_t asymmetric_map::symmetry_expanded_map() const
{
  int3 map_size = this->unit_cell_grid_size();
  //! @todo if map is not padded for FFT, do not initialize with 0.
  fft_map_t result(this->fft_grid(), 0.);
  vector<sgtbx::grid_symop> symops = this->grid_symops();
  int3 ebox_min, ebox_max;
  bool has_enclosed_box=asu_.enclosed_box_corners(ebox_min, ebox_max, map_size);
  const double *d = data_.begin();
  if( has_enclosed_box )
  {
    for(grid_iterator_t i3=this->grid_begin(); !i3.over(); i3.incr(), ++d )
    {
      int3 asym_pos = i3();
      af::int3 t = asym_pos.as_tiny();
      bool in_asu = t.all_ge(ebox_min) && t.all_le(ebox_max);
      if( !in_asu )
        in_asu = optimized_asu_.where_is(asym_pos) !=0;
      if( in_asu )
      {
        double map_value = *d;
        for(size_t i=0; i<symops.size(); ++i)
        {
          int3 sym_pos = symops[i].apply_to(asym_pos);
          int3 pos_in_cell(sym_pos);
          translate_into_cell(pos_in_cell, map_size);
          result(pos_in_cell) = map_value;
        }
      }
    }
  }
  else
  {
    for(grid_iterator_t i3=this->grid_begin(); !i3.over(); i3.incr(), ++d )
    {
      int3 asym_pos = i3();
      if( optimized_asu_.where_is(asym_pos) !=0 )
      {
        double map_value = *d;
        for(size_t i=0; i<symops.size(); ++i)
        {
          int3 sym_pos = symops[i].apply_to(asym_pos);
          int3 pos_in_cell(sym_pos);
          translate_into_cell(pos_in_cell, map_size);
          result(pos_in_cell) = map_value;
        }
      }
    }
  }
  return result;
}

asymmetric_map::asu_grid_t asymmetric_map::asu_grid(const int3 &map_size) const
{
  //! @todo: test grid for compatiblity with the group, see
  //     crystal_form::determine_grid
  int3 grid = map_size;
  CCTBX_ASSERT( this->unit_cell_grid_size().as_tiny().all_eq(grid) );
  asu::rvector3_t box_min, box_max;
  asu_.box_corners(box_min, box_max);
  int3 ebox_min, ebox_max;
  scitbx::mul(box_min, grid);
  scitbx::mul(box_max, grid);
  int3 ibox_min = scitbx::floor(box_min), ibox_max = scitbx::ceil(box_max);
  ibox_max += int3(1,1,1); // open range: [box_min,box_max)
  return asu_grid_t(ibox_min, ibox_max);
}

void asymmetric_map::copy_to_asu_box(const int3 &map_size,
  const int3 &padded_map_size, const double *cell_data)
{
  //! @todo: code duplication, see atom_mask
  //! @todo: test grid for compatiblity with the group, see
  //     crystal_form::determine_grid
  int3 grid = map_size;
  asu::rvector3_t box_min, box_max;
  asu_.box_corners(box_min, box_max);
  int3 ebox_min, ebox_max;
  bool has_enclosed_box = asu_.enclosed_box_corners(ebox_min, ebox_max, grid);
  scitbx::mul(box_min, grid);
  scitbx::mul(box_max, grid);
  int3 ibox_min = scitbx::floor(box_min),
    ibox_max = scitbx::ceil(box_max);
  if( has_enclosed_box )
  {
    CCTBX_ASSERT( scitbx::gt_all( ebox_min, ibox_min ) );
    CCTBX_ASSERT( scitbx::lt_all( ebox_max, ibox_max ) );
  }
  bool is_asu_inside_cell = ibox_min.as_tiny().all_ge(0)
    && ibox_max.as_tiny().all_le(grid);
  data_.resize(this->asu_grid(grid), 0.); //! @todo optimize
  cpu_timer timer;
  int3 padded_grid_size = padded_map_size;
  CCTBX_ASSERT( padded_grid_size.as_tiny().all_ge(grid) );
  if( has_enclosed_box && is_asu_inside_cell )
  {
    double *d = data_.begin();
    for(mapped_iterator_t l3=this->mapped_begin(padded_grid_size); !l3.over();
      l3.advance(), ++d)
    {
      int3 pos = l3();
      if( scitbx::ge_all(pos,ebox_min) && scitbx::le_all(pos,ebox_max) )
          *d = cell_data[l3.mapped_index_1d()];
      else
      {
        short ww = optimized_asu_.where_is(pos);
        if( ww==-1 ) // inside on the face
        {
          int3 pos_in_cell(pos);
          translate_into_cell(pos_in_cell, grid);
          *d = cell_data[ l3.mapped_index_1d(pos_in_cell) ];
        }
        else if( ww!=0 ) // inside
          *d = cell_data[l3.mapped_index_1d()];
      }
    }
    CCTBX_ASSERT( d==data_.end() );
  }
  else
  {
    // slower generic version
    double *d = data_.begin();
    mapped_iterator_t indexer = this->mapped_begin(padded_grid_size);
    for(grid_iterator_t l3=this->grid_begin(); !l3.over(); l3.incr(), ++d)
    {
      int3 pos = l3();
      if(optimized_asu_.where_is(pos)!=0 )
      {
        int3 pos_in_cell(pos);
        translate_into_cell(pos_in_cell, grid);
        *d = cell_data[ indexer.mapped_index_1d(pos_in_cell) ];
      }
    }
    CCTBX_ASSERT( d==data_.end() );
  }
  this->fill_density_times_ = timer.format();
}

void asymmetric_map::save(const std::string &file_name,
    const uctbx::unit_cell &unit_cell, format f) const
{
  if( f!=xplor )
    throw error("unsupported file format");
  af::const_ref<double, af::flex_grid<> > flex_data(data_.begin(),
    data_.accessor().as_flex_grid());
  std::string remark = "SPACE GROUP HALL: " + this->hall_symbol();
  iotbx::xplor::map_writer(file_name, unit_cell, flex_data,
    this->unit_cell_grid_size(), remark);
}

}}


//==============================================================================
//         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 <nt2/table.hpp>
#include <nt2/elliptic/include/functions/scalar/ellipke.hpp>
#include <nt2/elliptic/include/functions/simd/ellipke.hpp>
#include <nt2/include/functions/ellipke.hpp>
#include <nt2/include/functions/sin.hpp>
#include <nt2/include/functions/cos.hpp>
#include <nt2/include/functions/linspace.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/constants/pi.hpp>
#include <boost/fusion/include/vector_tie.hpp>

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

NT2_TEST_CASE_TPL(ellipke_table, NT2_REAL_TYPES)
{
  using nt2::ellipke;
  using nt2::table;
  typedef table<T> tab_t;
  static const size_t nb = 60;
  tab_t a = nt2::linspace(T(0), T(1), nb);
  tab_t  s(nt2::of_size(1, nb)), c(nt2::of_size(1, nb));
  {
    ellipke(a, nt2::Eps<T>(), s, c);
    for(size_t i =  1; i <= nb; ++i)
    {
      T s1, c1;
      boost::fusion::vector_tie(s1, c1) = nt2::ellipke(a(i));
      NT2_TEST_EQUAL(s(i), s1);
      NT2_TEST_EQUAL(c(i), c1);
    }
  }

  {
    s = ellipke(a, nt2::Eps<T>(), c);
    for(size_t i =  1; i <= nb; ++i)
    {
      T s1, c1;
      boost::fusion::vector_tie(s1, c1) = nt2::ellipke(a(i));
      NT2_TEST_EQUAL(s(i), s1);
      NT2_TEST_EQUAL(c(i), c1);
    }
  }

  {
    nt2::tie(s, c) = ellipke(a);
    for(size_t i =  1; i <= nb; ++i)
    {
      T s1, c1;
      boost::fusion::vector_tie(s1, c1) = nt2::ellipke(a(i));
      NT2_TEST_EQUAL(s(i), s1);
      NT2_TEST_EQUAL(c(i), c1);
    }
  }
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * @file testDiscreteMarginals.cpp
 * @date Jun 7, 2012
 * @author Abhijit Kundu
 * @author Richard Roberts
 * @author Frank Dellaert
 */

#include <gtsam/discrete/DiscreteMarginals.h>

#include <boost/assign/std/vector.hpp>
using namespace boost::assign;

#include <CppUnitLite/TestHarness.h>

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST_UNSAFE( DiscreteMarginals, UGM_small ) {
  size_t nrStates = 2;
  DiscreteKey Cathy(1, nrStates), Heather(2, nrStates), Mark(3, nrStates),
      Allison(4, nrStates);
  DiscreteFactorGraph graph;

  // add node potentials
  graph.add(Cathy, "1 3");
  graph.add(Heather, "9 1");
  graph.add(Mark, "1 3");
  graph.add(Allison, "9 1");

  // add edge potentials
  graph.add(Cathy & Heather, "2 1 1 2");
  graph.add(Heather & Mark, "2 1 1 2");
  graph.add(Mark & Allison, "2 1 1 2");

  DiscreteMarginals marginals(graph);
  DiscreteFactor::shared_ptr actualC = marginals(Cathy.first);
  DiscreteValues values;

  values[Cathy.first] = 0;
  EXPECT_DOUBLES_EQUAL( 0.359631, (*actualC)(values), 1e-6);

  Vector actualCvector = marginals.marginalProbabilities(Cathy);
  EXPECT(assert_equal(Vector2(0.359631, 0.640369), actualCvector, 1e-6));

  actualCvector = marginals.marginalProbabilities(Mark);
  EXPECT(assert_equal(Vector2(0.48628, 0.51372), actualCvector, 1e-6));
}

/* ************************************************************************* */
TEST_UNSAFE( DiscreteMarginals, UGM_chain ) {

  const int nrNodes = 10;
  const size_t nrStates = 7;

  // define variables
  vector<DiscreteKey> key;
  for (int i = 0; i < nrNodes; i++) {
    DiscreteKey key_i(i, nrStates);
    key.push_back(key_i);
  }

  // create graph
  DiscreteFactorGraph graph;

  // add node potentials
  graph.add(key[0], ".3 .6 .1 0 0 0 0");
  for (int i = 1; i < nrNodes; i++)
    graph.add(key[i], "1 1 1 1 1 1 1");

  const std::string edgePotential =   ".08 .9 .01 0 0 0 .01 "
                                      ".03 .95 .01 0 0 0 .01 "
                                      ".06 .06 .75 .05 .05 .02 .01 "
                                      "0 0 0 .3 .6 .09 .01 "
                                      "0 0 0 .02 .95 .02 .01 "
                                      "0 0 0 .01 .01 .97 .01 "
                                      "0 0 0 0 0 0 1";

  // add edge potentials
  for (int i = 0; i < nrNodes - 1; i++)
    graph.add(key[i] & key[i + 1], edgePotential);

  DiscreteMarginals marginals(graph);
  DiscreteFactor::shared_ptr actualC = marginals(key[2].first);
  DiscreteValues values;

  values[key[2].first] = 0;
  EXPECT_DOUBLES_EQUAL( 0.03426, (*actualC)(values), 1e-4);
}

/* ************************************************************************* */
TEST_UNSAFE( DiscreteMarginals, truss ) {

  const int nrNodes = 5;
  const size_t nrStates = 2;

  // define variables
  vector<DiscreteKey> key;
  for (int i = 0; i < nrNodes; i++) {
    DiscreteKey key_i(i, nrStates);
    key.push_back(key_i);
  }

  // create graph and add three truss potentials
  DiscreteFactorGraph graph;
  graph.add(key[0] & key[2] & key[4],"2 2 2 2 1 1 1 1");
  graph.add(key[1] & key[3] & key[4],"1 1 1 1 2 2 2 2");
  graph.add(key[2] & key[3] & key[4],"1 1 1 1 1 1 1 1");
  DiscreteBayesTree::shared_ptr bayesTree = graph.eliminateMultifrontal();
//  bayesTree->print("Bayes Tree");
  typedef DiscreteBayesTreeClique Clique;

  Clique expected0(boost::make_shared<DiscreteConditional>((key[0] | key[2], key[4]) = "2/1 2/1 2/1 2/1"));
  Clique::shared_ptr actual0 = (*bayesTree)[0];
//  EXPECT(assert_equal(expected0, *actual0)); // TODO, correct but fails

  Clique expected1(boost::make_shared<DiscreteConditional>((key[1] | key[3], key[4]) = "1/2 1/2 1/2 1/2"));
  Clique::shared_ptr actual1 = (*bayesTree)[1];
//  EXPECT(assert_equal(expected1, *actual1)); // TODO, correct but fails

  // Create Marginals instance
  DiscreteMarginals marginals(graph);

  // test 0
  DecisionTreeFactor expectedM0(key[0],"0.666667 0.333333");
  DiscreteFactor::shared_ptr actualM0 = marginals(0);
  EXPECT(assert_equal(expectedM0, *boost::dynamic_pointer_cast<DecisionTreeFactor>(actualM0),1e-5));

  // test 1
  DecisionTreeFactor expectedM1(key[1],"0.333333 0.666667");
  DiscreteFactor::shared_ptr actualM1 = marginals(1);
  EXPECT(assert_equal(expectedM1, *boost::dynamic_pointer_cast<DecisionTreeFactor>(actualM1),1e-5));
}

/* ************************************************************************* */
// Second truss example with non-trivial factors
TEST_UNSAFE(DiscreteMarginals, truss2) {
  const int nrNodes = 5;
  const size_t nrStates = 2;

  // define variables
  vector<DiscreteKey> key;
  for (int i = 0; i < nrNodes; i++) {
    DiscreteKey key_i(i, nrStates);
    key.push_back(key_i);
  }

  // create graph and add three truss potentials
  DiscreteFactorGraph graph;
  graph.add(key[0] & key[2] & key[4], "1 2 3 4 5 6 7 8");
  graph.add(key[1] & key[3] & key[4], "1 2 3 4 5 6 7 8");
  graph.add(key[2] & key[3] & key[4], "1 2 3 4 5 6 7 8");

  // Calculate the marginals by brute force
  auto allPosbValues =
      cartesianProduct(key[0] & key[1] & key[2] & key[3] & key[4]);
  Vector T = Z_5x1, F = Z_5x1;
  for (size_t i = 0; i < allPosbValues.size(); ++i) {
    DiscreteValues x = allPosbValues[i];
    double px = graph(x);
    for (size_t j = 0; j < 5; j++)
      if (x[j])
        T[j] += px;
      else
        F[j] += px;
  }

  // Check all marginals given by a sequential solver and Marginals
  //  DiscreteSequentialSolver solver(graph);
  DiscreteMarginals marginals(graph);
  for (size_t j = 0; j < 5; j++) {
    double sum = T[j] + F[j];
    T[j] /= sum;
    F[j] /= sum;

    // Marginals
    vector<double> table;
    table += F[j], T[j];
    DecisionTreeFactor expectedM(key[j], table);
    DiscreteFactor::shared_ptr actualM = marginals(j);
    EXPECT(assert_equal(
        expectedM, *boost::dynamic_pointer_cast<DecisionTreeFactor>(actualM)));
  }
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


// test_nc_t.cpp

// Copyright John Maddock 2008, 2012.
// Copyright Paul A. Bristow 2012.

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

#include <pch.hpp> // Need to include lib/math/test in path.

#ifdef _MSC_VER
#pragma warning (disable:4127 4512)
#endif

#include <boost/math/tools/test.hpp>
#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/math/distributions/non_central_t.hpp> // for chi_squared_distribution.
#include <boost/math/distributions/normal.hpp> // for normal distribution (for comparison).
#include <boost/multiprecision/cpp_bin_float.hpp> // for normal distribution (for comparison).

#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp> // for test_main
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp> // for BOOST_CHECK_CLOSE

#define SC_(x) static_cast<T>(BOOST_STRINGIZE(x))

#include "functor.hpp"
#include "handle_test_result.hpp"
#include "table_type.hpp"
#include "test_nc_t.hpp"

#include <iostream>
#include <iomanip>
using std::cout;
using std::endl;
#include <limits>
using std::numeric_limits;

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      "cpp_bin_float_quad",             // test type(s)
      ".*large parameters.*",           // test data group
      "[^|]*", 300000, 100000);         // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      "cpp_bin_float_quad",             // test type(s)
      "[^|]*",                          // test data group
      "[^|]*", 250, 50);                // test function

   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

BOOST_AUTO_TEST_CASE( test_main )
{
  BOOST_MATH_CONTROL_FP;
   // Basic sanity-check spot values.
   expected_results();
#if !BOOST_WORKAROUND(BOOST_MSVC, < 1920)
   test_spots(boost::multiprecision::cpp_bin_float_quad(0));
#endif
   test_accuracy(boost::multiprecision::cpp_bin_float_quad(0), "cpp_bin_float_quad");
   // double precision tests only:
   //test_big_df(boost::multiprecision::cpp_bin_float_quad(0));
} // BOOST_AUTO_TEST_CASE( test_main )


/** Test cases for Segment class.
 *
 */

#include "tmfsc/device.h"

#ifndef LINK_STATIC
#define BOOST_TEST_DYN_LINK
#endif
#define BOOST_TEST_MODULE DeviceTest
#include <boost/test/unit_test.hpp>

#include <iostream>

using namespace quest::tmfsc;
using namespace std;

Device create_simple_device (const point& A, const point& B, const point& C,
const point& D) {
    Device dev;
    int ptA = dev.addPoint(A);
    int ptB = dev.addPoint(B);
    int ptC = dev.addPoint(C);
    int ptD = dev.addPoint(D);

    dev.addEdge(ptA, ptB);
    dev.addEdge(ptB, ptC);
    dev.addEdge(ptC, ptD);
    dev.addEdge(ptD, ptA);

    return dev;
}

BOOST_AUTO_TEST_CASE(basics)
{
    Device dev = create_simple_device ({0, 0}, {20, 0}, {20, 10}, {0, 10});
    dev.edgeType(1, Edge::EDGE_ABSORB);

    BOOST_CHECK(dev.numEdges() == 4);
    BOOST_CHECK(dev.numConts() == 1);
    BOOST_CHECK(dev.isAbsorbEdge(1));
    BOOST_CHECK(dev.isReflectEdge(2));
    BOOST_CHECK(dev.contToEdgeIndx(0) == 1);
}

BOOST_AUTO_TEST_CASE(vectors)
{
    Device dev = create_simple_device ({0, 0}, {20, 0}, {20, 10}, {0, 10});
    dev.edgeType(1, Edge::EDGE_ABSORB);

    svec u = dev.edgeUnitVect(0);
    BOOST_CHECK_CLOSE(u(0), 1, 1E-4);
    BOOST_CHECK_CLOSE(u(1), 0, 1E-4);

    u = dev.edgeUnitVect(1);
    BOOST_CHECK_CLOSE(u(0), 0, 1E-4);
    BOOST_CHECK_CLOSE(u(1), 1, 1E-4);

    u = dev.edgeUnitVect(2);
    BOOST_CHECK_CLOSE(u(0), -1, 1E-4);
    BOOST_CHECK_CLOSE(u(1), 0, 1E-4);

    u = dev.edgeUnitVect(3);
    BOOST_CHECK_CLOSE(u(0), 0, 1E-4);
    BOOST_CHECK_CLOSE(u(1), -1, 1E-4);

    u = dev.edgeVect(3);
    BOOST_CHECK_CLOSE(u(0), 0, 1E-4);
    BOOST_CHECK_CLOSE(u(1), -10, 1E-4);
}

BOOST_AUTO_TEST_CASE(intersection)
{
    Device dev = create_simple_device ({0, 0}, {20, 0}, {20, 10}, {0, 10});
    dev.edgeType(1, Edge::EDGE_ABSORB);

    point ctr = {10, 5};
    point q = {30, 5};

    BOOST_CHECK(dev.intersects(ctr, q) == 1);
    point ints = dev.intersection(1, ctr, q);
    BOOST_CHECK_CLOSE(ints(0), 20, 1E-4);
    BOOST_CHECK_CLOSE(ints(1), 5, 1E-4);

    q(0) = 20; q(1) = 5;
    BOOST_CHECK(dev.intersects(ctr, q) == 1);

    q(0) = 20-0.0001; q(1) = 5;
    BOOST_CHECK(dev.intersects(ctr, q) == -1);

    point p1 = {20-0.01, 10-0.01};
    point q1 = {20-0.01, 10+0.01};
    BOOST_CHECK(dev.intersects(p1, q1) == 2);
    ints = dev.intersection(2, p1, q1);
    BOOST_CHECK_CLOSE(ints(0), 20-0.01, 1E-4);
    BOOST_CHECK_CLOSE(ints(1), 10, 1E-4);
}

BOOST_AUTO_TEST_CASE(pointsOnEdge)
{
    Device dev = create_simple_device ({0, 0}, {20, 0}, {20, 10}, {0, 10});
    dev.edgeType(1, Edge::EDGE_ABSORB);

    Edge e = dev.edge(1);
    cout << "Edge#0 p=(" << e.p()[0] << "," << e.p()[1] << ")"
                 << " q=(" << e.q()[0] << "," << e.q()[1] << ")"
                 << endl << endl;

    vector<point> pts = dev.createPointsOnCont(0, 11);
    for (int i = 0; i < pts.size(); i += 1) {
        cout << "pt#" << i << pts[i];
    }
}


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
//using namespace std;

#include <boost/mpl/for_each.hpp>
#include <boost/mpl/tag.hpp>
#include <boost/type_index.hpp>

#include <boost/core/ignore_unused.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/string.hpp>
#include <boost/mpl/copy.hpp>

using namespace boost;
using namespace boost::mpl;

///////////////////////////////////////

#include <boost/mpl/back_inserter.hpp>
#include <boost/mpl/front_inserter.hpp>
#include <boost/mpl/copy.hpp>

void case1()
{
    typedef mpl::vector<char, int , long> vec;

    typedef copy<vec,
        mpl::back_inserter<vec>>::type vec2;
    assert((size<vec2>::value == 6));

    typedef mpl::string<> str1;
    typedef copy<
        mpl::string<'time'>,
        mpl::front_inserter<str1>>::type str2;
    std::cout << c_str<str2>::value << std::endl;

}

///////////////////////////////////////

#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/find.hpp>
#include <boost/mpl/count.hpp>
#include <boost/mpl/contains.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/size.hpp>

void case2()
{
    typedef range_c<int, 0, 10> rc;
    assert((size<rc>::value == 10));

    #define INTV(n) integral_c<rc::value_type, n>

    assert((deref<
        find<rc, INTV(0)>::type
        >::type::value == 0));

    assert((contains<rc, INTV(9)>::value));
    assert((!contains<rc, INTV(10)>::value));

    assert((count<rc, INTV(5)>::value == 1));
    assert((count<rc, INTV(-5)>::value == 0));

    typedef vector_c<int, 0,1,2,3,4,5,6,7,8,9> vec;
    assert((equal<rc, vec>::value));
    assert((!is_same<rc, vec>::value));

}

///////////////////////////////////////
#include <boost/mpl/reverse.hpp>
#include <boost/mpl/replace.hpp>
#include <boost/mpl/remove.hpp>

void case3()
{
    typedef mpl::vector<char, short, int, long> vec;
    typedef replace<vec,
        int, char>::type vec2;

    assert((count<vec2, char>::value == 2));
    assert((is_same<deref<
                advance<begin<vec2>::type, int_<2>>::type>::type,
                char>::value));

    typedef mpl::remove<vec2, short>::type vec3;
    assert((!contains<vec3, short>::value));

    assert((equal<
        reverse<vec3>::type,
        mpl::vector<long, char, char>
        >::value));

}

///////////////////////////////////////

struct mpl_func1
{
    template<typename T>
    void operator()(T t)
    {   std::cout << typeindex::type_id<T>().pretty_name() << std::endl;   }
    //{   std::cout << typeid(t).name() << std::endl;   }
};

struct mpl_func2
{
    template<typename T>
    void operator()(T t)
    {
        if (is_same<typename tag<T>::type, integral_c_tag>::value)
        {   std::cout << t << ',';}
    }
};

void case4()
{
    typedef range_c<int, 0, 5> rc;

    typedef mpl::vector<> vec;

    typedef mpl::copy<rc,
        mpl::back_inserter<vec>>::type vec2;

    typedef push_front<vec2, float>::type vec3;

    mpl::for_each<vec3>(mpl_func1());

    std::cout << std::endl;

    mpl::for_each<vec3>(mpl_func2());
}

///////////////////////////////////////

int main()
{
    std::cout << "hello container" << std::endl;

    case1();
    case2();
    case3();
    case4();
}