// Copyright (c) 2020 - present Advanced Micro Devices, Inc. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "accuracy_test.h"
#include "../../rocFFT/library/include/gpubuf.h"
#include <boost/scope_exit.hpp>
#include <hipfft.h>

void hipfft_transform(const std::vector<size_t>                                  length,
                      const size_t                                               nbatch,
                      const rocfft_precision                                     precision,
                      const rocfft_transform_type                                transformType,
                      const std::vector<size_t>&                                 cpu_istride,
                      const std::vector<size_t>&                                 cpu_ostride,
                      const size_t                                               cpu_idist,
                      const size_t                                               cpu_odist,
                      const rocfft_array_type                                    cpu_itype,
                      const rocfft_array_type                                    cpu_otype,
                      const std::vector<std::vector<char, fftwAllocator<char>>>& cpu_input_copy,
                      const std::vector<std::vector<char, fftwAllocator<char>>>& cpu_output,
                      const std::pair<double, double>& cpu_output_L2Linfnorm,
                      std::thread*                     cpu_output_thread)

{
    const size_t dim      = length.size();
    const size_t istride0 = 1;
    const size_t ostride0 = 1;
    hipfftHandle plan;
    hipfftResult fft_status;
    hipError_t   hip_status;

    rocfft_array_type itype, otype;

    std::stringstream info;
    info << "\nGPU params:\n";

    auto olength = length;
    if(transformType == rocfft_transform_type_real_forward)
        olength[dim - 1] = olength[dim - 1] / 2 + 1;

    auto ilength = length;
    if(transformType == rocfft_transform_type_real_inverse)
        ilength[dim - 1] = ilength[dim - 1] / 2 + 1;

    auto gpu_istride
        = compute_stride(ilength, istride0, transformType == rocfft_transform_type_real_forward);
    auto gpu_ostride
        = compute_stride(olength, ostride0, transformType == rocfft_transform_type_real_inverse);
    const auto gpu_idist = set_idist(rocfft_placement_inplace, transformType, length, gpu_istride);
    const auto gpu_odist = set_odist(rocfft_placement_inplace, transformType, length, gpu_ostride);

    info << "\tilength:";
    for(auto i : ilength)
        info << " " << i;
    info << "\n\tnbatch: " << nbatch;
    info << "\n\tolength:";
    for(auto i : olength)
        info << " " << i;
    info << "\n\tcpu_istride:";
    for(auto i : cpu_istride)
        info << " " << i;
    info << "\n\tcpu_idist: " << cpu_idist;
    info << "\n\tcpu_ostride:";
    for(auto i : cpu_ostride)
        info << " " << i;
    info << "\n\tcpu_odist: " << cpu_odist;
    info << "\n\tgpu_istride:";
    for(auto i : gpu_istride)
        info << " " << i;
    info << "\n\tgpu_idist: " << gpu_idist;
    info << "\n\tgpu_ostride:";
    for(auto i : gpu_ostride)
        info << " " << i;
    info << "\n\tgpu_odist: " << gpu_odist;
    if(precision == rocfft_precision_single)
        info << "\n\tsingle-precision\n";
    else
        info << "\n\tdouble-precision\n";

    if(transformType == rocfft_transform_type_complex_forward
       || transformType == rocfft_transform_type_complex_inverse)
    {
        itype = rocfft_array_type_complex_interleaved;
        otype = rocfft_array_type_complex_interleaved;
        if(precision == rocfft_precision_single)
        {
            if(nbatch > 1)
            {
                int n[3];
                for(int i = 0; i < dim; ++i)
                    n[i] = (int)length[i];
                fft_status = hipfftPlanMany(&plan,
                                            dim,
                                            n,
                                            nullptr,
                                            1,
                                            gpu_idist,
                                            nullptr,
                                            1,
                                            gpu_odist,
                                            HIPFFT_C2C,
                                            nbatch);
            }
            else if(dim == 1)
                fft_status = hipfftPlan1d(&plan, length[0], HIPFFT_C2C, 1);
            else if(dim == 2)
                fft_status = hipfftPlan2d(&plan, length[0], length[1], HIPFFT_C2C);
            else if(dim == 3)
                fft_status = hipfftPlan3d(&plan, length[0], length[1], length[2], HIPFFT_C2C);
            info << "\ttype: C2C\n";
        }
        if(precision == rocfft_precision_double)
        {
            if(nbatch > 1)
            {
                int n[3];
                for(int i = 0; i < dim; ++i)
                    n[i] = (int)length[i];
                fft_status = hipfftPlanMany(&plan,
                                            dim,
                                            n,
                                            nullptr,
                                            1,
                                            gpu_idist,
                                            nullptr,
                                            1,
                                            gpu_odist,
                                            HIPFFT_Z2Z,
                                            nbatch);
            }
            else if(dim == 1)
                fft_status = hipfftPlan1d(&plan, length[0], HIPFFT_Z2Z, 1);
            else if(dim == 2)
                fft_status = hipfftPlan2d(&plan, length[0], length[1], HIPFFT_Z2Z);
            else if(dim == 3)
                fft_status = hipfftPlan3d(&plan, length[0], length[1], length[2], HIPFFT_Z2Z);
            info << "\ttype: Z2Z\n";
        }
    }
    else if(transformType == rocfft_transform_type_real_forward)
    {
        itype = rocfft_array_type_real;
        otype = rocfft_array_type_hermitian_interleaved;
        if(precision == rocfft_precision_single)
        {
            if(nbatch > 1)
            {
                int n[3];
                for(int i = 0; i < dim; ++i)
                    n[i] = (int)length[i];
                fft_status = hipfftPlanMany(&plan,
                                            dim,
                                            n,
                                            nullptr,
                                            1,
                                            gpu_idist,
                                            nullptr,
                                            1,
                                            gpu_odist,
                                            HIPFFT_R2C,
                                            nbatch);
            }
            else if(dim == 1)
                fft_status = hipfftPlan1d(&plan, length[0], HIPFFT_R2C, 1);
            else if(dim == 2)
                fft_status = hipfftPlan2d(&plan, length[0], length[1], HIPFFT_R2C);
            else if(dim == 3)
                fft_status = hipfftPlan3d(&plan, length[0], length[1], length[2], HIPFFT_R2C);
            info << "\ttype: R2C\n";
        }
        if(precision == rocfft_precision_double)
        {
            if(nbatch > 1)
            {
                int n[3];
                for(int i = 0; i < dim; ++i)
                    n[i] = (int)length[i];
                fft_status = hipfftPlanMany(&plan,
                                            dim,
                                            n,
                                            nullptr,
                                            1,
                                            gpu_idist,
                                            nullptr,
                                            1,
                                            gpu_odist,
                                            HIPFFT_D2Z,
                                            nbatch);
            }
            else if(dim == 1)
                fft_status = hipfftPlan1d(&plan, length[0], HIPFFT_D2Z, 1);
            else if(dim == 2)
                fft_status = hipfftPlan2d(&plan, length[0], length[1], HIPFFT_D2Z);
            else if(dim == 3)
                fft_status = hipfftPlan3d(&plan, length[0], length[1], length[2], HIPFFT_D2Z);
            info << "\ttype: D2Z\n";
        }
    }
    else if(transformType == rocfft_transform_type_real_inverse)
    {
        itype = rocfft_array_type_hermitian_interleaved;
        otype = rocfft_array_type_real;
        if(precision == rocfft_precision_single)
        {
            if(nbatch > 1)
            {
                int n[3];
                for(int i = 0; i < dim; ++i)
                    n[i] = (int)length[i];
                fft_status = hipfftPlanMany(&plan,
                                            dim,
                                            n,
                                            nullptr,
                                            1,
                                            gpu_idist,
                                            nullptr,
                                            1,
                                            gpu_odist,
                                            HIPFFT_C2R,
                                            nbatch);
            }
            else if(dim == 1)
                fft_status = hipfftPlan1d(&plan, length[0], HIPFFT_C2R, 1);
            else if(dim == 2)
                fft_status = hipfftPlan2d(&plan, length[0], length[1], HIPFFT_C2R);
            else if(dim == 3)
                fft_status = hipfftPlan3d(&plan, length[0], length[1], length[2], HIPFFT_C2R);
            info << "\ttype: C2R\n";
        }
        if(precision == rocfft_precision_double)
        {
            if(nbatch > 1)
            {
                int n[3];
                for(int i = 0; i < dim; ++i)
                    n[i] = (int)length[i];
                fft_status = hipfftPlanMany(&plan,
                                            dim,
                                            n,
                                            nullptr,
                                            1,
                                            gpu_idist,
                                            nullptr,
                                            1,
                                            gpu_odist,
                                            HIPFFT_Z2D,
                                            nbatch);
            }
            else if(dim == 1)
                fft_status = hipfftPlan1d(&plan, length[0], HIPFFT_Z2D, 1);
            else if(dim == 2)
                fft_status = hipfftPlan2d(&plan, length[0], length[1], HIPFFT_Z2D);
            else if(dim == 3)
                fft_status = hipfftPlan3d(&plan, length[0], length[1], length[2], HIPFFT_Z2D);
            info << "\ttype: Z2D\n";
        }
    }
    EXPECT_TRUE(fft_status == HIPFFT_SUCCESS) << "hipFFT plan creation failure";

    auto gpu_input = allocate_host_buffer<fftwAllocator<char>>(
        precision, itype, length, gpu_istride, gpu_idist, nbatch);

    copy_buffers(cpu_input_copy,
                 gpu_input,
                 ilength,
                 nbatch,
                 precision,
                 cpu_itype,
                 cpu_istride,
                 cpu_idist,
                 itype,
                 gpu_istride,
                 gpu_idist);

    auto gpu_buffer_size = buffer_sizes(precision, otype, gpu_idist, nbatch)[0];
    gpu_buffer_size      = std::max(gpu_buffer_size, cpu_input_copy[0].size());

    gpubuf gpu_buffer;
    hip_status = gpu_buffer.alloc(gpu_buffer_size);
    ASSERT_TRUE(hip_status == hipSuccess)
        << "hipMalloc failure for input buffer size " << gpu_buffer_size;

    hip_status = hipMemcpy(
        gpu_buffer.data(), gpu_input[0].data(), gpu_input[0].size(), hipMemcpyHostToDevice);
    EXPECT_TRUE(hip_status == hipSuccess) << "hipMemcpy failure";

    switch(transformType)
    {
    case(rocfft_transform_type_complex_forward):
    case(rocfft_transform_type_complex_inverse):
        if(precision == rocfft_precision_single)
            hipfftExecC2C(plan,
                          (hipfftComplex*)gpu_buffer.data(),
                          (hipfftComplex*)gpu_buffer.data(),
                          transformType == rocfft_transform_type_complex_forward ? HIPFFT_FORWARD
                                                                                : HIPFFT_BACKWARD);
        else
            hipfftExecZ2Z(plan,
                          (hipfftDoubleComplex*)gpu_buffer.data(),
                          (hipfftDoubleComplex*)gpu_buffer.data(),
                          transformType == rocfft_transform_type_complex_forward ? HIPFFT_FORWARD
                                                                                : HIPFFT_BACKWARD);
        break;
    case(rocfft_transform_type_real_forward):
        if(precision == rocfft_precision_single)
            hipfftExecR2C(plan, (hipfftReal*)gpu_buffer.data(), (hipfftComplex*)gpu_buffer.data());
        else
            hipfftExecD2Z(plan,
                          (hipfftDoubleReal*)gpu_buffer.data(),
                          (hipfftDoubleComplex*)gpu_buffer.data());
        break;
    case(rocfft_transform_type_real_inverse):
        if(precision == rocfft_precision_single)
            hipfftExecC2R(plan, (hipfftComplex*)gpu_buffer.data(), (hipfftReal*)gpu_buffer.data());
        else
            hipfftExecZ2D(plan,
                          (hipfftDoubleComplex*)gpu_buffer.data(),
                          (hipfftDoubleReal*)gpu_buffer.data());
        break;
    }
    EXPECT_TRUE(fft_status == HIPFFT_SUCCESS) << "hipFFT plan execution failure";

    auto gpu_output = allocate_host_buffer<fftwAllocator<char>>(
        precision, otype, olength, gpu_istride, gpu_odist, nbatch);
    hip_status = hipMemcpy(
        gpu_output[0].data(), gpu_buffer.data(), gpu_output[0].size(), hipMemcpyDeviceToHost);
    EXPECT_TRUE(hip_status == hipSuccess) << "hipMemcpy failure";

    info << "\tbuffer size: " << cpu_input_copy[0].size() << " " << gpu_buffer_size << " "
         << gpu_output[0].size() << "\n";

    // Compute the Linf and L2 norm of the GPU output:
    std::pair<double, double> L2LinfnormGPU;
    std::thread               normthread([&]() {
        L2LinfnormGPU
            = LinfL2norm(gpu_output, olength, nbatch, precision, otype, gpu_ostride, gpu_odist);
    });
    if(cpu_output_thread && cpu_output_thread->joinable())
        cpu_output_thread->join();

    // std::cout << info.str() << std::endl;
    // std::cout << "cpu_input:" << std::endl;
    // printbuffer(precision, itype, cpu_input_copy, ilength, cpu_istride, nbatch, cpu_idist);
    // std::cout << "cpu_output:" << std::endl;
    // printbuffer(precision, otype, cpu_output, olength, cpu_ostride, nbatch, cpu_odist);
    // std::cout << "gpu_output:" << std::endl;
    // printbuffer(precision, otype, gpu_output, olength, gpu_ostride, nbatch, gpu_odist);

    // Compute the Linf and L2 distance between the CPU and GPU output:
    std::vector<std::pair<size_t, size_t>> linf_failures;
    const auto                             total_length
        = std::accumulate(length.begin(), length.end(), 1, std::multiplies<size_t>());
    const double linf_cutoff
        = type_epsilon(precision) * cpu_output_L2Linfnorm.first * log(total_length);
    auto linfl2diff = LinfL2diff(cpu_output,
                                 gpu_output,
                                 olength,
                                 nbatch,
                                 precision,
                                 cpu_otype,
                                 cpu_ostride,
                                 cpu_odist,
                                 otype,
                                 gpu_ostride,
                                 gpu_odist,
                                 linf_failures,
                                 linf_cutoff);
    normthread.join();

    EXPECT_TRUE(std::isfinite(L2LinfnormGPU.first))
        << L2LinfnormGPU.first << " " << cpu_output_L2Linfnorm.first;
    EXPECT_TRUE(std::isfinite(L2LinfnormGPU.second))
        << L2LinfnormGPU.second << " " << cpu_output_L2Linfnorm.second;
    EXPECT_TRUE(linfl2diff.second / cpu_output_L2Linfnorm.second
                < sqrt(log(total_length)) * type_epsilon(precision))
        << "L2 diff failure " << linfl2diff.second / cpu_output_L2Linfnorm.second << " "
        << info.str();

    fft_status = hipfftDestroy(plan);
    EXPECT_TRUE(fft_status == HIPFFT_SUCCESS) << "hipFFT plan destroy failure";
}

TEST_P(hipfft_accuracy_test, vs_fftw)
{
    const std::vector<size_t>   length        = std::get<0>(GetParam());
    const size_t                nbatch        = std::get<1>(GetParam());
    const rocfft_precision      precision     = std::get<2>(GetParam());
    const rocfft_transform_type transformType = std::get<3>(GetParam());

    const size_t dim = length.size();

    // Input cpu parameters
    auto ilength = length;
    if(transformType == rocfft_transform_type_real_inverse)
        ilength[dim - 1] = ilength[dim - 1] / 2 + 1;
    const auto cpu_istride = compute_stride(ilength, 1);
    const auto cpu_itype   = contiguous_itype(transformType);
    const auto cpu_idist
        = set_idist(rocfft_placement_notinplace, transformType, length, cpu_istride);

    // Output cpu parameters
    auto olength = length;
    if(transformType == rocfft_transform_type_real_forward)
        olength[dim - 1] = olength[dim - 1] / 2 + 1;
    const auto cpu_ostride = compute_stride(olength, 1);
    const auto cpu_odist
        = set_odist(rocfft_placement_notinplace, transformType, length, cpu_ostride);
    auto cpu_otype = contiguous_otype(transformType);

    // Generate input
    auto cpu_input = compute_input<fftwAllocator<char>>(
        precision, cpu_itype, length, cpu_istride, cpu_idist, nbatch);
    auto cpu_input_copy = cpu_input; // copy of input (might get overwritten by FFTW).

    // FFTW computation
    decltype(cpu_input)       cpu_output;
    std::pair<double, double> cpu_output_L2Linfnorm;
    std::thread               cpu_output_thread([&]() {
        cpu_output = fftw_via_rocfft(length,
                                     cpu_istride,
                                     cpu_ostride,
                                     nbatch,
                                     cpu_idist,
                                     cpu_odist,
                                     precision,
                                     transformType,
                                     cpu_input);

        cpu_output_L2Linfnorm
            = LinfL2norm(cpu_output, olength, nbatch, precision, cpu_otype, cpu_ostride, cpu_odist);
    });

    // Clean up threads if transform throws...
    BOOST_SCOPE_EXIT_ALL(&cpu_output_thread)
    {
        if(cpu_output_thread.joinable())
            cpu_output_thread.join();
    };

    // GPU computation and comparison
    hipfft_transform(length,
                     nbatch,
                     precision,
                     transformType,
                     cpu_istride,
                     cpu_ostride,
                     cpu_idist,
                     cpu_odist,
                     cpu_itype,
                     cpu_otype,
                     cpu_input_copy,
                     cpu_output,
                     cpu_output_L2Linfnorm,
                     &cpu_output_thread);

    if(cpu_output_thread.joinable())
        cpu_output_thread.join();
    ASSERT_TRUE(std::isfinite(cpu_output_L2Linfnorm.first));
    ASSERT_TRUE(std::isfinite(cpu_output_L2Linfnorm.second));

    SUCCEED();
}


#ifndef RASTER_SOURCE_HPP
#define RASTER_SOURCE_HPP

#include "util/coordinate.hpp"
#include "util/exception.hpp"

#include <boost/algorithm/string/trim.hpp>
#include <boost/assert.hpp>
#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/qi_int.hpp>
#include <storage/io.hpp>

#include <iterator>
#include <unordered_map>

namespace osrm
{
namespace extractor
{

/**
    \brief Small wrapper around raster source queries to optionally provide results
    gracefully, depending on source bounds
*/
struct RasterDatum
{
    static std::int32_t get_invalid() { return std::numeric_limits<std::int32_t>::max(); }

    std::int32_t datum = get_invalid();

    RasterDatum() = default;

    RasterDatum(std::int32_t _datum) : datum(_datum) {}
};

class RasterGrid
{
  public:
    RasterGrid(const boost::filesystem::path &filepath, std::size_t _xdim, std::size_t _ydim)
    {
        xdim = _xdim;
        ydim = _ydim;
        _data.reserve(ydim * xdim);

        storage::io::FileReader file_reader(filepath, storage::io::FileReader::HasNoFingerprint);

        std::string buffer;
        buffer.resize(file_reader.Size());

        BOOST_ASSERT(buffer.size() > 1);

        file_reader.ReadInto(&buffer[0], buffer.size());

        boost::algorithm::trim(buffer);

        auto itr = buffer.begin();
        auto end = buffer.end();

        bool r = false;
        try
        {
            r = boost::spirit::qi::parse(
                itr, end, +boost::spirit::qi::int_ % +boost::spirit::qi::space, _data);
        }
        catch (std::exception const &ex)
        {
            throw util::exception("Failed to read from raster source " + filepath.string() + ": " +
                                  ex.what() + SOURCE_REF);
        }

        if (!r || itr != end)
        {
            throw util::exception("Failed to parse raster source: " + filepath.string() +
                                  SOURCE_REF);
        }
    }

    RasterGrid(const RasterGrid &) = default;
    RasterGrid &operator=(const RasterGrid &) = default;

    RasterGrid(RasterGrid &&) = default;
    RasterGrid &operator=(RasterGrid &&) = default;

    std::int32_t operator()(std::size_t x, std::size_t y) { return _data[y * xdim + x]; }
    std::int32_t operator()(std::size_t x, std::size_t y) const { return _data[(y)*xdim + (x)]; }

  private:
    std::vector<std::int32_t> _data;
    std::size_t xdim, ydim;
};

/**
    \brief Stores raster source data in memory and provides lookup functions.
*/
class RasterSource
{
  private:
    const float xstep;
    const float ystep;

    float CalcSize(int min, int max, std::size_t count) const;

  public:
    RasterGrid raster_data;

    const std::size_t width;
    const std::size_t height;
    const int xmin;
    const int xmax;
    const int ymin;
    const int ymax;

    RasterDatum GetRasterData(const int lon, const int lat) const;

    RasterDatum GetRasterInterpolate(const int lon, const int lat) const;

    RasterSource(RasterGrid _raster_data,
                 std::size_t width,
                 std::size_t height,
                 int _xmin,
                 int _xmax,
                 int _ymin,
                 int _ymax);
};

class SourceContainer
{
  public:
    SourceContainer() = default;

    int LoadRasterSource(const std::string &path_string,
                         double xmin,
                         double xmax,
                         double ymin,
                         double ymax,
                         std::size_t nrows,
                         std::size_t ncols);

    RasterDatum GetRasterDataFromSource(unsigned int source_id, double lon, double lat);

    RasterDatum GetRasterInterpolateFromSource(unsigned int source_id, double lon, double lat);

  private:
    std::vector<RasterSource> LoadedSources;
    std::unordered_map<std::string, int> LoadedSourcePaths;
};
}
}

#endif /* RASTER_SOURCE_HPP */


/**
 * @example studies/dead_reckoning/dead_reckon_test.cc
 */

#include <usml/ocean/boundary_flat.h>
#include <usml/ocean/ocean_shared.h>
#include <usml/sensors/beams.h>
#include <usml/sensors/sensors.h>
#include <usml/eigenverb/wavefront_generator.h>
#include <list>
#include <boost/timer.hpp>
#include <boost/foreach.hpp>

using namespace usml::sensors ;
using namespace usml::eigenverb ;

/**
 * This scenario compute reverberation for a scenario that has a simple
 * analytic solution.  This is a bi-static scenario that uses two sensors,
 * one source, one receiver, a flat bottom, and a constant sound speed.
 * Reverberation is only computed for the ocean bottom.
 */
class dead_reckon_test {

public:

	/**
	 * Initializes the scenario, starts the reverberation calculation,
	 * and waits for the results.
	 */
	dead_reckon_test() {
		define_ocean_characteristics();
		define_sensor_characteristics();
		deploy_sensor_instance();
		wait_for_results();
		cout << "== test complete ==" << endl;
	}

	/**
	 * Cleans up singletons so that valgrind can be used to find
	 * other memory leaks.
	 */
	~dead_reckon_test() {
	}

private:

	/**
	 * Creates a simple ocean for comparison to the analytic solution
	 * for bi-static reverberation.
	 *
	 *

	 * profile:             constant 1500 m/s
	 * attenuation:         none
	 * surface loss:        none
	 * surface scattering:  none
	 * bottom depth:        constant 505 meters
	 * bottom loss:         Rayleigh sand
	 * bottom scattering:   Lambert -27 dB
	 *

	 *
	 * Generates the ocean model and then updates the ocean_shared singleton.
	 * Uses profile_lock and boundary_lock classes so that the new ocean
	 * can be shared by multiple threads.
	 */
	void define_ocean_characteristics() {
		cout << "== define ocean characteristics ==" << endl;
		profile_model* water = new profile_linear(1500.0);
		water->attenuation(new attenuation_constant(0.0));
		profile_lock* profile = new profile_lock(water);

		boundary_model* surf = new boundary_flat();
		boundary_lock* surface = new boundary_lock(surf);

		/****************** BOTTOM DEPTH *****************************/
		boundary_model* btm = new boundary_flat(505); // default 1657 ft.
		btm->reflect_loss(
				new reflect_loss_rayleigh(reflect_loss_rayleigh::SAND));
		btm->scattering(new scattering_lambert());
		boundary_lock* bottom = new boundary_lock(btm);

		ocean_shared::reference ocean(
				new ocean_model(surface, bottom, profile));
		ocean_shared::update(ocean);
	}

	/**
	 * Defines the a source and a receiver parameters for a simple
	 * sensor.
	 *
	 *

	 * multistatic:			true
	 * source strength:     200 dB
	 * pulse length:		250 msec
	 * reverb duration:		7.0 sec
	 * transmit frequency:  2k - 12k Hz
	 * src beam pattern:    omni directional
	 * rcv beam pattern: 	omni directional
	 *

	 *
	 * Generates the sensor parameter then updates the sensor parameter
	 * singletons.
	 */
	void define_sensor_characteristics() {
		cout << "== define sensor characteristics ==" << endl;
		sensor_params::id_type paramsID = 10;
		bool multistatic = true;
		vector<double> source_level(1, 200.0);

		// ParamsID = 10
        // Source frequencies 2.0K, 5.3K, 8.6K, 11.9 K
        seq_linear source_frequencies(2000.0, 3300.0, 4);

        beam_pattern_model::id_type omni = 0;
        std::list<beam_pattern_model::id_type> source_beams;
        source_beams.push_back(omni);

        // Receiver frequencies 500, 750
        seq_linear receiver_frequencies(500.0, 250.0, 2);

		// define source parameters

		source_params::reference source(
				new source_params(paramsID, source_level,
				0.250,						// pulse_length
				7.0,						// reverb_duration
                10.0, 50000.0,       		// min, max active freq
                source_frequencies, source_beams, multistatic));
		source_params_map::instance()->insert(source->paramsID(), source);

		// define receiver parameters

		std::list<beam_pattern_model::id_type> receiver_beams;
		receiver_beams.push_back(omni);

		paramsID = 13;
		receiver_params::reference receiver(
            new receiver_params(paramsID,
            10.0, 50000.0,  // min, max active freq
            receiver_frequencies, receiver_beams, multistatic));
		receiver_params_map::instance()->insert(receiver->paramsID(), receiver);
	}

	/**
	 * Creates a new instance of a fast-moving target and an
	 * omni-direction receiver, and updates it with a position of
	 * (55.0, 149.0, 1.0).
	 */
	void deploy_sensor_instance() {

		cout << "== deploy source instance ==" << endl;
		sensor_model::id_type sensorID = 3;
		sensor_params::id_type paramsID = 10;
		wposition1 source_pos(54.955, 149.0, -15.240);         // 5km from receiver
		orientation orient(0.0, 0.0);	 // default orientation

		sensor_manager::instance()->add_sensor(sensorID, paramsID, "Fast Moving Target");

		// Set wave_queue attributes
        wavefront_generator::time_maximum =  7.0/2.0 + 0.5; // reverb_duration/2 + 1/2 sec
        wavefront_generator::intensity_threshold = 150.0; //dB

        /*************************** TIME STEP ********************************/
        wavefront_generator::time_step = 0.1;
        wavefront_generator::extra_rays = 4;
        //wavefront_generator::number_de = 181;
        //wavefront_generator::max_bottom = 0;              // Max number of bottom bounces.
        //wavefront_generator::max_surface = 0;             // Max number of surface bounces.

        // Update sensor data and run wave_queue.
        sensor_manager::instance()->update_sensor(sensorID, source_pos, orient);

        cout << "== deploy receiver instance ==" << endl;
		// Receiver
		sensorID = 1;
        paramsID = 13;
        wposition1 receiver_pos(55.0, 149.0, -1.0);  // locate close to ocean surface
        // default orientation

        sensor_manager::instance()->add_sensor(sensorID, paramsID, "ADAR");

		// Update sensor data and run wave_queue.
		sensor_manager::instance()->update_sensor(sensorID, receiver_pos, orient);
	}

	/**
	 * Wait for the reverberation model to compute results.
	 *
	 * Retrieve fathometers and envelopes from sensor_pair_manager.
	 */
	void wait_for_results() {

    	cout << "== wait for results ==" << endl ;

    	sensor_pair_manager* sp_manager = sensor_pair_manager::instance() ;
        fathometer_collection::fathometer_package fathometers ;
        envelope_collection::envelope_package envelopes ;

        // Build query for fathometers
        sensor_data_map query;
        sensor_data sensor;
    	orientation orient(0.0, 0.0);  // default orientation

    	// Insert Receiver
    	sensor._sensorID = 1; // ADAR
    	sensor._mode = usml::sensors::RECEIVER;
    	sensor._orient = orient;
    	wposition1 pos(55.0, 149.0, -1.0);
        sensor._position = pos;

    	std::pair<sensor_model::id_type, sensor_data> pair;
    	pair.first = sensor._sensorID;
    	pair.second = sensor;
    	query.insert(pair);

    	// Set up source data
    	sensor._sensorID = 3; // Fast moving target
		sensor._mode = usml::sensors::SOURCE;
		sensor._orient = orient;

    	double latitude = 54.955;      // Starting longitude set earlier
    	double movement = 0.009;	   // 1 km latitude, velocity equivalent of 20 knots = ~ 97.297 sec/km

    	int request_id = 0;
        // Sleep enough for Waveq3d to run under debugger - 6.5 sec
        boost::this_thread::sleep(boost::posix_time::milliseconds(6800));

        while (true) {

        	request_id++;
        	// Only run three times
        	if ( request_id >= 4 ) break;

        	// Update source position
            wposition1 pos(latitude, 149.0, -15.240);
            sensor._position = pos;
            pair.first = sensor._sensorID;
            pair.second = sensor;
            query.insert(pair);

//            cout << " Fathometers Request ID = " << request_id << endl;
            fathometers = sp_manager->get_fathometers(const_cast<sensor_data_map&>(query));

            if ( fathometers.size() > 0 ) {
                write_fathometers(fathometers, request_id);
            }

            query.erase(sensor._sensorID);

			// Reposition for velocity equivalent of 20 knots = ~ 97.297 sec/km
			latitude = latitude + movement;
        }
        cout << "== wait completed for fathometers ==" << endl ;
        cout << "== run matlab dead_reckon_compare.m to verify results ==" << endl ;

        cout << "== wait for envelopes started ==" << endl ;
        boost::timer timer;
        request_id = 0;
        envelopes = sp_manager->get_envelopes(const_cast<sensor_data_map&>(query));
#ifdef USML_DEBUG
        double t_time = 250.0;
#else
        double t_time = 6.0;
#endif
        while ( timer.elapsed() < t_time ) {
            if ( envelopes.size() > 0 ) {
                write_envelopes(envelopes, request_id);
            }
	    }
        cout << "== wait for envelopes completed time elapsed " << timer.elapsed() << " ==" << endl ;
	}

	void write_fathometers( fathometer_collection::fathometer_package& fathometers, int request_id ) {

	    std::string ncname_fathometers = USML_STUDIES_DIR "/dead_reckoning/fathometer_";
        fathometer_collection::fathometer_package::iterator iter_fathometers;
        for ( iter_fathometers = fathometers.begin();
            iter_fathometers != fathometers.end(); ++iter_fathometers )
        {
            fathometer_collection* collection = ( *iter_fathometers );
            sensor_model::id_type src_id = collection->source_id();
            sensor_model::id_type rcv_id = collection->receiver_id();
            std::stringstream ss ;
            ss << "src_" << src_id << "_rcv_" << rcv_id << "_request_" << request_id ;
            ncname_fathometers += ss.str();
            ncname_fathometers += ".nc";
            collection->write_netcdf(ncname_fathometers.c_str());

//            cout << "Wrote fathometers to " << ncname_fathometers << endl ;
        }
    }

	void write_envelopes( envelope_collection::envelope_package& envelopes, int request_id ) {

		std::string ncname_envelopes = USML_STUDIES_DIR "/dead_reckoning/envelope_";
		envelope_collection::envelope_package::iterator iter_envelopes;
		for ( iter_envelopes = envelopes.begin();
			iter_envelopes != envelopes.end(); ++iter_envelopes )
		{
			envelope_collection* collection = ( *iter_envelopes );
			sensor_model::id_type src_id = collection->source_id();
			sensor_model::id_type rcv_id = collection->receiver_id();
			std::stringstream ss ;
			ss << "src_" << src_id << "_rcv_" << rcv_id << "_request_" << request_id ;
			ncname_envelopes += ss.str();
			ncname_envelopes += ".nc";
			collection->write_netcdf(ncname_envelopes.c_str());

			cout << "Wrote envelopes to " << ncname_envelopes << endl ;
		}
	}
};

/**
 * Command line interface.
 */
int main(int argc, char* argv[]) {
	dead_reckon_test test;
	return 0;
}


/*

Copyright (c) 2005-2016, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford 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.

*/

#ifndef SPHEREGEOMETRYBOUNDARYCONDITION_HPP_
#define SPHEREGEOMETRYBOUNDARYCONDITION_HPP_

#include "AbstractCellPopulationBoundaryCondition.hpp"

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/vector.hpp>

/**
 * A spherical cell population boundary condition class, which restricts nodes to lie
 * on the surface of a sphere in the domain. Although the name of this class suggests
 * it is specific to 3D, it is actually also implemented in 2D, for which it is really
 * a circle geometry boundary condition.
 */
template<unsigned DIM>
class SphereGeometryBoundaryCondition : public AbstractCellPopulationBoundaryCondition<DIM>
{
private:

    /** The centre of the sphere. */
    c_vector<double, DIM> mCentreOfSphere;

    /** The radius of the sphere. */
    double mRadiusOfSphere;

    /** The maximum distance from the surface of the sphere that cells may be. */
    double mMaximumDistance;

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Serialize the object.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractCellPopulationBoundaryCondition<DIM> >(*this);
        archive & mMaximumDistance;
    }

public:

    /**
     * Constructor.
     *
     * @param pCellPopulation pointer to the cell population
     * @param centre the centre of the sphere
     * @param radius the radius of the sphere
     * @param distance the maximum distance from the surface of the sphere that cells may be (defaults to 1e-5)
     */
    SphereGeometryBoundaryCondition(AbstractCellPopulation<DIM>* pCellPopulation,
                                    c_vector<double, DIM> centre,
                                    double radius,
                                    double distance=1e-5);

    /**
     * @return #mCentreOfSphere.
     */
    const c_vector<double, DIM>& rGetCentreOfSphere() const;

    /**
     * @return #mRadiusOfSphere.
     */
    double GetRadiusOfSphere() const;

    /**
     * Overridden ImposeBoundaryCondition() method.
     *
     * Apply the cell population boundary conditions.
     *
     * @param rOldLocations the node locations before any boundary conditions are applied
     */
    void ImposeBoundaryCondition(const std::map<Node<DIM>*, c_vector<double, DIM> >& rOldLocations);

    /**
     * Overridden VerifyBoundaryCondition() method.
     * Verify the boundary conditions have been applied.
     * This is called after ImposeBoundaryCondition() to ensure the condition is still satisfied.
     *
     * @return whether the boundary conditions are satisfied.
     */
    bool VerifyBoundaryCondition();

    /**
     * Overridden OutputCellPopulationBoundaryConditionParameters() method.
     * Output cell population boundary condition parameters to file.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    void OutputCellPopulationBoundaryConditionParameters(out_stream& rParamsFile);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(SphereGeometryBoundaryCondition)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a SphereGeometryBoundaryCondition.
 */
template<class Archive, unsigned DIM>
inline void save_construct_data(
    Archive & ar, const SphereGeometryBoundaryCondition<DIM>* t, const unsigned int file_version)
{
    // Save data required to construct instance
    const AbstractCellPopulation<DIM>* const p_cell_population = t->GetCellPopulation();
    ar << p_cell_population;

    // Archive c_vectors one component at a time
    c_vector<double, DIM> point = t->rGetCentreOfSphere();
    for (unsigned i=0; i<DIM; i++)
    {
        ar << point[i];
    }

    // Archive other member variables
    double radius = t->GetRadiusOfSphere();
    ar << radius;
}

/**
 * De-serialize constructor parameters and initialize a SphereGeometryBoundaryCondition.
 */
template<class Archive, unsigned DIM>
inline void load_construct_data(
    Archive & ar, SphereGeometryBoundaryCondition<DIM>* t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    AbstractCellPopulation<DIM>* p_cell_population;
    ar >> p_cell_population;

    // Retrieve c_vectors one component at a time
    c_vector<double, DIM> point;
    for (unsigned i=0; i<DIM; i++)
    {
        ar >> point[i];
    }

    // Retrieve other member variables
    double radius;
    ar >> radius;

    // Invoke inplace constructor to initialise instance
    ::new(t)SphereGeometryBoundaryCondition<DIM>(p_cell_population, point, radius);
}
}
} // namespace ...

#endif /*SPHEREGEOMETRYBOUNDARYCONDITION_HPP_*/


//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
//   license.txt   (http://opensource.org/licenses/MIT)

#pragma once

#include "geospatial/network/Point.hpp"
#include "util/GeomHelpers.hpp"
#include "util/LangHelpers.hpp"
#include <map>
#include <vector>
#include <string>
#include <ostream>

#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/unordered_map.hpp>
#include <boost/utility.hpp>
#include <boost/thread.hpp>

#include "entities/params/PT_NetworkEntities.hpp"
#include "StreetDirectory.hpp"

namespace sim_mob {

class A_StarPublicTransitShortestPathImpl : public StreetDirectory::PublicTransitShortestPathImpl {
public:
    A_StarPublicTransitShortestPathImpl(const std::map<int,PT_NetworkEdge>& ,const std::map<std::string,PT_NetworkVertex>&);
    virtual ~A_StarPublicTransitShortestPathImpl() {}

public:
    /**
     * search shortest path in public transit network
     * @param from is original Id
     * @param to is destination Id
     * @param cost indicate the cost type when searching
     * @return searching result
     */
    std::vector<PT_NetworkEdge> searchShortestPath(const StreetDirectory::PT_VertexId& from,const StreetDirectory::PT_VertexId& to,const PT_CostLabel cost);

    /**
     * search k-shortest path in public transit network
     * @param k indicate how many times to repeat to search
     * @param from is original Id
     * @param to is destination Id
     * @param outPathList hold searching result
     */
    void searchK_ShortestPaths(uint32_t k, const StreetDirectory::PT_VertexId& from, const StreetDirectory::PT_VertexId& to, std::vector< std::vector<PT_NetworkEdge> > & outPathList);

    /**
     * search shortest path in public transit network
     * @param from is original Id
     * @param to is destination Id
     * @param blackList include black list of edges
     * @param pathCost indicate the cost type when searching
     * @return searching result
     */
    std::vector<PT_NetworkEdge> searchShortestPathWithBlacklist(const StreetDirectory::PT_VertexId& from,const StreetDirectory::PT_VertexId& to, const std::set<StreetDirectory::PT_EdgeId>& blackList, double& pathCost);

private:
    /**
     * initialize the public transit graph from public transit network
     * @param ptEdges hold all edges in public network graph
     * @param ptVertexes hold all vertices in public network graph
     */
    void initPublicNetwork(const std::map<int,PT_NetworkEdge>& ptEdges ,const std::map<std::string,PT_NetworkVertex>& ptVertexes);
    /**
     * process adding vertices to public graph from public transit vertex
     * @param graph is public transit graph
     * @param ptVertex is a vertex in public transit network
     */
    void procAddPublicNetworkVertices(StreetDirectory::PublicTransitGraph& graph,const PT_NetworkVertex& ptVertex);
    /**
     * process to add public transit edges to the graph
     * @param graph is public transit graph
     * @param ptEdge hold edge information in public transit network
     */
    void procAddPublicNetworkEdges(StreetDirectory::PublicTransitGraph& graph,const PT_NetworkEdge& ptEdge);
    /**
     * process to add weight value to the public transit graph
     * @param edge is the edge of public transit graph
     * @param graph is public transit graph
     * @param ptEdge hold edge information in public transit network
     */
    void procAddPublicNetworkEdgeWeights(const StreetDirectory::PT_Edge& edge,StreetDirectory::PublicTransitGraph& graph,const PT_NetworkEdge& ptEdge);
    /**
     * get cost for k-shortest path
     * @param ptEdge hold edge information in public transit network
     */
    double getK_ShortestPathCost(const PT_NetworkEdge& ptEdge);
    /**
     * get weight value for labeling approach
     * @param label is the index in the group of labeling approach
     * @param ptEdge hold edge information in public transit network
     */
    double getLabelingApproachWeights(int label,PT_NetworkEdge ptEdge);
    /**
     * get weight value for simulation approach
     * @param ptEdge hold edge information in public transit network
     */
    double getSimulationApproachWeights(PT_NetworkEdge ptEdge);

private:
    /**public transit graph*/
    StreetDirectory::PublicTransitGraph publicTransitMap;

    /**store the map from the index to the edge*/
    std::map<const int,StreetDirectory::PT_Edge> edgeMap;

    /**store the map from the index to the vertex*/
    std::map<const std::string,StreetDirectory::PT_Vertex> vertexMap;

    /**Used to terminate our search*/
    struct PT_FoundGoal {};

    /**
     * Goal visitor: terminates when a goal has been found.
     */
    class PT_GoalVisitor : public boost::default_astar_visitor {
    public:
        PT_GoalVisitor(StreetDirectory::PT_Vertex goal) : goal(goal) {}

        template <class Graph>
        void examine_vertex(StreetDirectory::PT_Vertex u, const Graph& g) {
            if(u == goal){
                throw PT_FoundGoal();
            }
        }
    private:
        StreetDirectory::PT_Vertex goal;
    };
    /**
     * black list constraint for our A* search algorithm
     */
    struct PT_EdgeConstraint {
        PT_EdgeConstraint(const std::set<StreetDirectory::PT_Edge>& blacklist =
                std::set<StreetDirectory::PT_Edge>()) : blacklist(blacklist) {
        }

        bool operator()(const StreetDirectory::PT_Edge& e) const {
            /**Include the edge if it's not in the blacklist.*/
            return blacklist.find(e) == blacklist.end();
        }

    private:
        std::set<StreetDirectory::PT_Edge> blacklist;
    };
    /**
     * Distance heuristic for our A* search algorithm
     */
    class PT_HeuristicGraph: public boost::astar_heuristic<
            StreetDirectory::PublicTransitGraph, double> {
    public:
        PT_HeuristicGraph(const StreetDirectory::PublicTransitGraph* graph, StreetDirectory::PT_Vertex goal) :
                graph(graph), goal(goal) {
        }
        double operator()(StreetDirectory::PT_Vertex v) {
            return 0.0;
        }
    private:
        const StreetDirectory::PublicTransitGraph* graph;
        StreetDirectory::PT_Vertex goal;
    };

};
}


/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2011, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <iostream>
#include <algorithm>

#include <pcl/common/time.h>
#include <pcl/gpu/kinfu/kinfu.h>
#include "internal.h"

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>

#ifdef HAVE_OPENCV
#include <opencv2/opencv.hpp>
#include <opencv2/gpu/gpu.hpp>
#include <pcl/gpu/utils/timers_opencv.hpp>
#endif

using namespace std;
using namespace pcl::device;
using namespace pcl::gpu;

using Eigen::AngleAxisf;
using Eigen::Array3f;
using Eigen::Vector3f;
using Eigen::Vector3i;

namespace pcl {
namespace gpu {
Eigen::Vector3f rodrigues2(const Eigen::Matrix3f &matrix);
}
} // namespace pcl

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
pcl::gpu::KinfuTracker::KinfuTracker(int rows, int cols)
    : rows_(rows), cols_(cols), global_time_(0), max_icp_distance_(0),
      integration_metric_threshold_(0.f) {
    const Vector3f volume_size = Vector3f::Constant(VOLUME_SIZE);
    const Vector3i volume_resolution(VOLUME_X, VOLUME_Y, VOLUME_Z);

    tsdf_volume_ = TsdfVolume::Ptr(new TsdfVolume(volume_resolution));
    tsdf_volume_->setSize(volume_size);

    setDepthIntrinsics(525.f, 525.f); // default values, can be overwritten

    init_Rcam_ =
        Eigen::Matrix3f::Identity(); // * AngleAxisf(-30.f/180*3.1415926,
                                     // Vector3f::UnitX());
    init_tcam_ = volume_size * 0.5f - Vector3f(0, 0, volume_size(2) / 2 * 1.2f);

    const int iters[] = {10, 5, 4};
    std::copy(iters, iters + LEVELS, icp_iterations_);

    const float default_distThres = 0.10f; // meters
    const float default_angleThres = sin(20.f * 3.14159254f / 180.f);
    const float default_tranc_dist = 0.03f; // meters

    setIcpCorespFilteringParams(default_distThres, default_angleThres);
    tsdf_volume_->setTsdfTruncDist(default_tranc_dist);

    allocateBufffers(rows, cols);

    rmats_.reserve(30000);
    tvecs_.reserve(30000);

    reset();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void pcl::gpu::KinfuTracker::setDepthIntrinsics(float fx, float fy, float cx,
                                                float cy) {
    fx_ = fx;
    fy_ = fy;
    cx_ = (cx == -1) ? cols_ / 2 - 0.5f : cx;
    cy_ = (cy == -1) ? rows_ / 2 - 0.5f : cy;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void pcl::gpu::KinfuTracker::setInitalCameraPose(const Eigen::Affine3f &pose) {
    init_Rcam_ = pose.rotation();
    init_tcam_ = pose.translation();
    reset();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void pcl::gpu::KinfuTracker::setDepthTruncationForICP(float max_icp_distance) {
    max_icp_distance_ = max_icp_distance;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void pcl::gpu::KinfuTracker::setCameraMovementThreshold(float threshold) {
    integration_metric_threshold_ = threshold;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void pcl::gpu::KinfuTracker::setIcpCorespFilteringParams(float distThreshold,
                                                         float sineOfAngle) {
    distThres_ = distThreshold; // mm
    angleThres_ = sineOfAngle;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int pcl::gpu::KinfuTracker::cols() { return (cols_); }

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int pcl::gpu::KinfuTracker::rows() { return (rows_); }

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void pcl::gpu::KinfuTracker::reset() {
    if (global_time_)
        cout << "Reset" << endl;

    global_time_ = 0;
    rmats_.clear();
    tvecs_.clear();

    rmats_.push_back(init_Rcam_);
    tvecs_.push_back(init_tcam_);

    tsdf_volume_->reset();

    if (color_volume_) // color integration mode is enabled
        color_volume_->reset();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void pcl::gpu::KinfuTracker::allocateBufffers(int rows, int cols) {
    depths_curr_.resize(LEVELS);
    vmaps_g_curr_.resize(LEVELS);
    nmaps_g_curr_.resize(LEVELS);

    vmaps_g_prev_.resize(LEVELS);
    nmaps_g_prev_.resize(LEVELS);

    vmaps_curr_.resize(LEVELS);
    nmaps_curr_.resize(LEVELS);

    coresps_.resize(LEVELS);

    for (int i = 0; i < LEVELS; ++i) {
        int pyr_rows = rows >> i;
        int pyr_cols = cols >> i;

        depths_curr_[i].create(pyr_rows, pyr_cols);

        vmaps_g_curr_[i].create(pyr_rows * 3, pyr_cols);
        nmaps_g_curr_[i].create(pyr_rows * 3, pyr_cols);

        vmaps_g_prev_[i].create(pyr_rows * 3, pyr_cols);
        nmaps_g_prev_[i].create(pyr_rows * 3, pyr_cols);

        vmaps_curr_[i].create(pyr_rows * 3, pyr_cols);
        nmaps_curr_[i].create(pyr_rows * 3, pyr_cols);

        coresps_[i].create(pyr_rows, pyr_cols);
    }
    depthRawScaled_.create(rows, cols);
    // see estimate tranform for the magic numbers
    gbuf_.create(27, 20 * 60);
    sumbuf_.create(27);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool pcl::gpu::KinfuTracker::operator()(const DepthMap &depth_raw) {
    device::Intr intr(fx_, fy_, cx_, cy_);
    {
        // ScopeTime time(">>> Bilateral, pyr-down-all, create-maps-all");
        // depth_raw.copyTo(depths_curr[0]);
        device::bilateralFilter(depth_raw, depths_curr_[0]);

        if (max_icp_distance_ > 0)
            device::truncateDepth(depths_curr_[0], max_icp_distance_);

        for (int i = 1; i < LEVELS; ++i)
            device::pyrDown(depths_curr_[i - 1], depths_curr_[i]);

        for (int i = 0; i < LEVELS; ++i) {
            device::createVMap(intr(i), depths_curr_[i], vmaps_curr_[i]);
            // device::createNMap(vmaps_curr_[i], nmaps_curr_[i]);
            computeNormalsEigen(vmaps_curr_[i], nmaps_curr_[i]);
        }
        pcl::device::sync();
    }

    // can't perform more on first frame
    if (global_time_ == 0) {
        Matrix3frm init_Rcam = rmats_[0]; //  [Ri|ti] - pos of camera, i.e.
        Vector3f init_tcam = tvecs_[0]; //  transform from camera to global coo
                                        //  space for (i-1)th camera pose

        Mat33 &device_Rcam = device_cast<Mat33>(init_Rcam);
        float3 &device_tcam = device_cast<float3>(init_tcam);

        Matrix3frm init_Rcam_inv = init_Rcam.inverse();
        Mat33 &device_Rcam_inv = device_cast<Mat33>(init_Rcam_inv);
        float3 device_volume_size =
            device_cast<const float3>(tsdf_volume_->getSize());

        // integrateTsdfVolume(depth_raw, intr, device_volume_size,
        // device_Rcam_inv, device_tcam, tranc_dist, volume_);
        device::integrateTsdfVolume(depth_raw, intr, device_volume_size,
                                    device_Rcam_inv, device_tcam,
                                    tsdf_volume_->getTsdfTruncDist(),
                                    tsdf_volume_->data(), depthRawScaled_);

        for (int i = 0; i < LEVELS; ++i)
            device::tranformMaps(vmaps_curr_[i], nmaps_curr_[i], device_Rcam,
                                 device_tcam, vmaps_g_prev_[i],
                                 nmaps_g_prev_[i]);

        ++global_time_;
        return (false);
    }

    ///////////////////////////////////////////////////////////////////////////////////////////
    // Iterative Closest Point
    Matrix3frm Rprev =
        rmats_[global_time_ - 1]; //  [Ri|ti] - pos of camera, i.e.
    Vector3f tprev =
        tvecs_[global_time_ - 1]; //  tranfrom from camera to global coo space
                                  //  for (i-1)th camera pose
    Matrix3frm Rprev_inv = Rprev.inverse(); // Rprev.t();

    // Mat33&  device_Rprev     = device_cast<Mat33> (Rprev);
    Mat33 &device_Rprev_inv = device_cast<Mat33>(Rprev_inv);
    float3 &device_tprev = device_cast<float3>(tprev);

    Matrix3frm Rcurr = Rprev; // tranform to global coo for ith camera pose
    Vector3f tcurr = tprev;
    {
        // ScopeTime time("icp-all");
        for (int level_index = LEVELS - 1; level_index >= 0; --level_index) {
            int iter_num = icp_iterations_[level_index];

            MapArr &vmap_curr = vmaps_curr_[level_index];
            MapArr &nmap_curr = nmaps_curr_[level_index];

            // MapArr& vmap_g_curr = vmaps_g_curr_[level_index];
            // MapArr& nmap_g_curr = nmaps_g_curr_[level_index];

            MapArr &vmap_g_prev = vmaps_g_prev_[level_index];
            MapArr &nmap_g_prev = nmaps_g_prev_[level_index];

            // CorespMap& coresp = coresps_[level_index];

            for (int iter = 0; iter < iter_num; ++iter) {
                Mat33 &device_Rcurr = device_cast<Mat33>(Rcurr);
                float3 &device_tcurr = device_cast<float3>(tcurr);

                Eigen::Matrix<double, 6, 6, Eigen::RowMajor> A;
                Eigen::Matrix<double, 6, 1> b;
#if 0
        device::tranformMaps(vmap_curr, nmap_curr, device_Rcurr, device_tcurr, vmap_g_curr, nmap_g_curr);
        findCoresp(vmap_g_curr, nmap_g_curr, device_Rprev_inv, device_tprev, intr(level_index), vmap_g_prev, nmap_g_prev, distThres_, angleThres_, coresp);
        device::estimateTransform(vmap_g_prev, nmap_g_prev, vmap_g_curr, coresp, gbuf_, sumbuf_, A.data(), b.data());

        //cv::gpu::GpuMat ma(coresp.rows(), coresp.cols(), CV_32S, coresp.ptr(), coresp.step());
        //cv::Mat cpu;
        //ma.download(cpu);
        //cv::imshow(names[level_index] + string(" --- coresp white == -1"), cpu == -1);
#else
                estimateCombined(device_Rcurr, device_tcurr, vmap_curr,
                                 nmap_curr, device_Rprev_inv, device_tprev,
                                 intr(level_index), vmap_g_prev, nmap_g_prev,
                                 distThres_, angleThres_, gbuf_, sumbuf_,
                                 A.data(), b.data());
#endif
                // checking nullspace
                double det = A.determinant();

                if (fabs(det) < 1e-15 || pcl_isnan(det)) {
                    if (pcl_isnan(det))
                        cout << "qnan" << endl;

                    reset();
                    return (false);
                }
                // float maxc = A.maxCoeff();

                Eigen::Matrix<float, 6, 1> result =
                    A.llt().solve(b).cast<float>();
                // Eigen::Matrix<float, 6, 1> result = A.jacobiSvd(ComputeThinU
                // | ComputeThinV).solve(b);

                float alpha = result(0);
                float beta = result(1);
                float gamma = result(2);

                Eigen::Matrix3f Rinc =
                    (Eigen::Matrix3f)AngleAxisf(gamma, Vector3f::UnitZ()) *
                    AngleAxisf(beta, Vector3f::UnitY()) *
                    AngleAxisf(alpha, Vector3f::UnitX());
                Vector3f tinc = result.tail<3>();

                // compose
                tcurr = Rinc * tcurr + tinc;
                Rcurr = Rinc * Rcurr;
            }
        }
    }
    // save tranform
    rmats_.push_back(Rcurr);
    tvecs_.push_back(tcurr);

    ///////////////////////////////////////////////////////////////////////////////////////////
    // Integration check - We do not integrate volume if camera does not move.
    float rnorm = rodrigues2(Rcurr.inverse() * Rprev).norm();
    float tnorm = (tcurr - tprev).norm();
    const float alpha = 1.f;
    bool integrate =
        (rnorm + alpha * tnorm) / 2 >= integration_metric_threshold_;

    ///////////////////////////////////////////////////////////////////////////////////////////
    // Volume integration
    float3 device_volume_size =
        device_cast<const float3>(tsdf_volume_->getSize());

    Matrix3frm Rcurr_inv = Rcurr.inverse();
    Mat33 &device_Rcurr_inv = device_cast<Mat33>(Rcurr_inv);
    float3 &device_tcurr = device_cast<float3>(tcurr);
    if (integrate) {
        // ScopeTime time("tsdf");
        // integrateTsdfVolume(depth_raw, intr, device_volume_size,
        // device_Rcurr_inv, device_tcurr, tranc_dist, volume_);
        integrateTsdfVolume(depth_raw, intr, device_volume_size,
                            device_Rcurr_inv, device_tcurr,
                            tsdf_volume_->getTsdfTruncDist(),
                            tsdf_volume_->data(), depthRawScaled_);
    }

    ///////////////////////////////////////////////////////////////////////////////////////////
    // Ray casting
    Mat33 &device_Rcurr = device_cast<Mat33>(Rcurr);
    {
        // ScopeTime time("ray-cast-all");
        raycast(intr, device_Rcurr, device_tcurr,
                tsdf_volume_->getTsdfTruncDist(), device_volume_size,
                tsdf_volume_->data(), vmaps_g_prev_[0], nmaps_g_prev_[0]);
        for (int i = 1; i < LEVELS; ++i) {
            resizeVMap(vmaps_g_prev_[i - 1], vmaps_g_prev_[i]);
            resizeNMap(nmaps_g_prev_[i - 1], nmaps_g_prev_[i]);
        }
        pcl::device::sync();
    }

    ++global_time_;
    return (true);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Affine3f pcl::gpu::KinfuTracker::getCameraPose(int time) const {
    if (time > (int)rmats_.size() || time < 0)
        time = rmats_.size() - 1;

    Eigen::Affine3f aff;
    aff.linear() = rmats_[time];
    aff.translation() = tvecs_[time];
    return (aff);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

size_t pcl::gpu::KinfuTracker::getNumberOfPoses() const {
    return rmats_.size();
}

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

const TsdfVolume &pcl::gpu::KinfuTracker::volume() const {
    return *tsdf_volume_;
}

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

TsdfVolume &pcl::gpu::KinfuTracker::volume() { return *tsdf_volume_; }

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

const ColorVolume &pcl::gpu::KinfuTracker::colorVolume() const {
    return *color_volume_;
}

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

ColorVolume &pcl::gpu::KinfuTracker::colorVolume() { return *color_volume_; }

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void pcl::gpu::KinfuTracker::getImage(View &view) const {
    // Eigen::Vector3f light_source_pose = tsdf_volume_->getSize() * (-3.f);
    Eigen::Vector3f light_source_pose = tvecs_[tvecs_.size() - 1];

    device::LightSource light;
    light.number = 1;
    light.pos[0] = device_cast<const float3>(light_source_pose);

    view.create(rows_, cols_);
    generateImage(vmaps_g_prev_[0], nmaps_g_prev_[0], light, view);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void pcl::gpu::KinfuTracker::getLastFrameCloud(
    DeviceArray2D<PointType> &cloud) const {
    cloud.create(rows_, cols_);
    DeviceArray2D<float4> &c = (DeviceArray2D<float4> &)cloud;
    device::convert(vmaps_g_prev_[0], c);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void pcl::gpu::KinfuTracker::getLastFrameNormals(
    DeviceArray2D<NormalType> &normals) const {
    normals.create(rows_, cols_);
    DeviceArray2D<float8> &n = (DeviceArray2D<float8> &)normals;
    device::convert(nmaps_g_prev_[0], n);
}

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

void pcl::gpu::KinfuTracker::initColorIntegration(int max_weight) {
    color_volume_ =
        pcl::gpu::ColorVolume::Ptr(new ColorVolume(*tsdf_volume_, max_weight));
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool pcl::gpu::KinfuTracker::operator()(const DepthMap &depth,
                                        const View &colors) {
    bool res = (*this)(depth);

    if (res && color_volume_) {
        const float3 device_volume_size =
            device_cast<const float3>(tsdf_volume_->getSize());
        device::Intr intr(fx_, fy_, cx_, cy_);

        Matrix3frm R_inv = rmats_.back().inverse();
        Vector3f t = tvecs_.back();

        Mat33 &device_Rcurr_inv = device_cast<Mat33>(R_inv);
        float3 &device_tcurr = device_cast<float3>(t);

        device::updateColorVolume(
            intr, tsdf_volume_->getTsdfTruncDist(), device_Rcurr_inv,
            device_tcurr, vmaps_g_prev_[0], colors, device_volume_size,
            color_volume_->data(), color_volume_->getMaxWeight());
    }

    return res;
}

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

namespace pcl {
namespace gpu {
PCL_EXPORTS void paint3DView(const KinfuTracker::View &rgb24,
                             KinfuTracker::View &view,
                             float colors_weight = 0.5f) {
    device::paint3DView(rgb24, view, colors_weight);
}

PCL_EXPORTS void mergePointNormal(const DeviceArray<PointXYZ> &cloud,
                                  const DeviceArray<Normal> &normals,
                                  DeviceArray<PointNormal> &output) {
    const size_t size = min(cloud.size(), normals.size());
    output.create(size);

    const DeviceArray<float4> &c = (const DeviceArray<float4> &)cloud;
    const DeviceArray<float8> &n = (const DeviceArray<float8> &)normals;
    const DeviceArray<float12> &o = (const DeviceArray<float12> &)output;
    device::mergePointNormal(c, n, o);
}

Eigen::Vector3f rodrigues2(const Eigen::Matrix3f &matrix) {
    Eigen::JacobiSVD<Eigen::Matrix3f> svd(matrix, Eigen::ComputeFullV |
                                                      Eigen::ComputeFullU);
    Eigen::Matrix3f R = svd.matrixU() * svd.matrixV().transpose();

    double rx = R(2, 1) - R(1, 2);
    double ry = R(0, 2) - R(2, 0);
    double rz = R(1, 0) - R(0, 1);

    double s = sqrt((rx * rx + ry * ry + rz * rz) * 0.25);
    double c = (R.trace() - 1) * 0.5;
    c = c > 1. ? 1. : c < -1. ? -1. : c;

    double theta = acos(c);

    if (s < 1e-5) {
        double t;

        if (c > 0)
            rx = ry = rz = 0;
        else {
            t = (R(0, 0) + 1) * 0.5;
            rx = sqrt(std::max(t, 0.0));
            t = (R(1, 1) + 1) * 0.5;
            ry = sqrt(std::max(t, 0.0)) * (R(0, 1) < 0 ? -1.0 : 1.0);
            t = (R(2, 2) + 1) * 0.5;
            rz = sqrt(std::max(t, 0.0)) * (R(0, 2) < 0 ? -1.0 : 1.0);

            if (fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) &&
                (R(1, 2) > 0) != (ry * rz > 0))
                rz = -rz;
            theta /= sqrt(rx * rx + ry * ry + rz * rz);
            rx *= theta;
            ry *= theta;
            rz *= theta;
        }
    } else {
        double vth = 1 / (2 * s);
        vth *= theta;
        rx *= vth;
        ry *= vth;
        rz *= vth;
    }
    return Eigen::Vector3d(rx, ry, rz).cast<float>();
}
} // namespace gpu
} // namespace pcl


#include <iostream>
#include <random>

#include <boost/lexical_cast.hpp>

#include "succinct/mapper.hpp"
#include "index_types.hpp"
#include "util.hpp"
#include "dec_time_prediction.hpp"

namespace ds2i {

    double measure_decoding_time(size_t sum_of_values, size_t n,
                                 std::vector<uint8_t> const& buf)
    {
        static const size_t runs = 256;
        std::vector<uint32_t> out_buf(mixed_block::block_size);

        // dry run to ignore one-time initializations (static variables, ...)
        mixed_block::decode(buf.data(), out_buf.data(),
                            sum_of_values, n);

        size_t spacing = 1 << 10;
        thread_local std::vector<uint8_t> readbuf(runs * spacing);
        thread_local std::vector<uint8_t const*> positions(runs);
        for (size_t run = 0; run < runs; ++run) {
            // try random alignments
            // XXX switch to c++ gens
            uint8_t* position = readbuf.data() + run * spacing + (rand() % 64);
            std::copy(buf.begin(), buf.end(), position);
            positions[run] = position;
        }

        double tick = get_time_usecs();
        for (auto position: positions) {
            mixed_block::decode(position, out_buf.data(), sum_of_values, n);
            do_not_optimize_away(out_buf[0]);
        }

        return (get_time_usecs() - tick) / runs * 1000;
    }

    void profile_block(std::vector<uint32_t> const& values,
                       uint32_t sum_of_values)
    {
        using namespace time_prediction;
        std::vector<uint8_t> buf;
        uint32_t n = values.size();
        feature_vector fv;
        values_statistics(values, fv);

        for (uint8_t t = 0; t < mixed_block::block_types; ++t) {
            mixed_block::block_type type = (mixed_block::block_type)t;
            for (mixed_block::compr_param_type param = 0;
                 param < mixed_block::compr_params(type); ++param) {
                buf.clear();
                if (!mixed_block::compression_stats(type, param, values.data(),
                                                    sum_of_values, n, buf, fv)) {
                    continue;
                }

                double time = measure_decoding_time(sum_of_values, n, buf);

                stats_line()
                    ("type", (int)t)
                    ("time", time)
                    (fv)
                    ;
            }
        }
    }

    template <typename IndexType>
    void profile_decoding(const char* index_filename,
                          double p)
    {
        std::default_random_engine rng(1729);
        std::uniform_real_distribution<double> dist01(0.0, 1.0);

        IndexType index;
        logger() << "Loading index from " << index_filename << std::endl;
        boost::iostreams::mapped_file_source m(index_filename);
        succinct::mapper::map(index, m);

        std::vector<uint32_t> values;

        for (size_t l = 0; l < index.size(); ++l) {
            if (l % 1000000 == 0) {
                logger() << l << " lists processed" << std::endl;
            }

            auto blocks = index[l].get_blocks();
            for (auto const& block: blocks) {
                // only measure full blocks
                if (block.size == mixed_block::block_size && dist01(rng) < p) {
                    block.decode_doc_gaps(values);
                    profile_block(values, block.doc_gaps_universe);
                    block.decode_freqs(values);
                    profile_block(values, uint32_t(-1));
                }
            }
        }

        logger() << index.size() << " lists processed" << std::endl;
    }
}

int main(int /* argc */, const char** argv)
{
    using namespace ds2i;

    std::string type = argv[1];
    const char* index_filename = argv[2];
    double p = boost::lexical_cast<double>(argv[3]);

    if (false) {
#define LOOP_BODY(R, DATA, T)                           \
        } else if (type == BOOST_PP_STRINGIZE(T)) {     \
            profile_decoding<BOOST_PP_CAT(T, _index)>   \
                (index_filename, p);                    \
            /**/

        BOOST_PP_SEQ_FOR_EACH(LOOP_BODY, _, DS2I_BLOCK_INDEX_TYPES);
#undef LOOP_BODY
    } else {
        logger() << "ERROR: Unknown type " << type << std::endl;
    }

}