#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/dict.hpp>
#include <boost/python/list.hpp>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/math/mean_and_variance.h>
#include <xfel/metrology/legacy_scale/parameters.h>
#include <xfel/metrology/legacy_scale/bandpass_gaussian.h>
#include <xfel/metrology/legacy_scale/vector_collection.h>
#include <xfel/metrology/legacy_scale/a_g_conversion.h>
#include <xfel/metrology/legacy_scale/quadrants.h>

#include <vector>
#include <map>

using namespace boost::python;
namespace xfel{
namespace boost_python { namespace {

  void
  xfel_scale_init_module() {
    using namespace boost::python;

    typedef return_value_policy<return_by_value> rbv;
    class_<xfel_legacy::parameter::parameter_array>(
      "parameter_array",no_init)
      .add_property("x",
       make_getter(&xfel_legacy::parameter::parameter_array::parameters, rbv()))
      .add_property("gradients",
       make_getter(&xfel_legacy::parameter::parameter_array::gradients,rbv()))
      .add_property("curvatures",
       make_getter(&xfel_legacy::parameter::parameter_array::curvatures,rbv()))
    ;
    class_<xfel_legacy::parameter::organizer_base>(
      "organizer_base",init<>())
      .def("register",&xfel_legacy::parameter::organizer_base::register_array,
        (arg("tag"),arg("ndata"),arg("kdim")=1,arg("data")))
      .def("register_local",&xfel_legacy::parameter::organizer_base::register_local_array,
        (arg("tag"),arg("ndata"),arg("kdim")=1,arg("data")))
      .def("as_x_array",&xfel_legacy::parameter::organizer_base::as_x_array)
      .def("get_gradient_array",
           (xfel_legacy::farray(xfel_legacy::parameter::organizer_base::*)()const)
           &xfel_legacy::parameter::organizer_base::get_gradient_array)
      .def("set_gradient_array",&xfel_legacy::parameter::organizer_base::set_gradient_array)
      .def("get_curvature_array",&xfel_legacy::parameter::organizer_base::get_curvature_array)
      .def("from_x_array",&xfel_legacy::parameter::organizer_base::from_x_array)
      .def("initialize_gradients_curvatures",
         &xfel_legacy::parameter::organizer_base::initialize_gradients_curvatures)
      .def("rezero_gradients_curvatures",
         &xfel_legacy::parameter::organizer_base::rezero_gradients_curvatures)
    ;
    class_<xfel_legacy::algorithm::mark5_iteration,
           bases<xfel_legacy::parameter::organizer_base> >(
      "mark5_iteration",init<>())
      .def("set_refined_origins_to_c",
         &xfel_legacy::algorithm::mark5_iteration::set_refined_origins_to_c)
      .def("compute_target",&xfel_legacy::algorithm::mark5_iteration::compute_target,
        (arg_("tox"),arg_("toy"),
         arg_("spotcx"),arg_("spotcy"),
         arg_("spotfx"),arg_("spotfy"),
         arg_("master_tiles"), arg_("frames"),
         arg_("part_distance")))
      .def("compute_functional_only",
        &xfel_legacy::algorithm::mark5_iteration::compute_functional_only,
        (arg_("tox"),arg_("toy"),
         arg_("spotcx"),arg_("spotcy"),
         arg_("spotfx"),arg_("spotfy"),
         arg_("master_tiles"), arg_("frames"),
         arg_("part_distance")))
      .add_property("model_calcx",
         make_getter(&xfel_legacy::algorithm::mark5_iteration::model_calcx, rbv()))
      .add_property("model_calcy",
         make_getter(&xfel_legacy::algorithm::mark5_iteration::model_calcy, rbv()))
      .def("set_vector_collection",
         &xfel_legacy::algorithm::mark5_iteration::set_vector_collection)
      .def("uncorrected_detector_to_laboratory_frame",
        &xfel_legacy::algorithm::mark5_iteration::uncorrected_detector_to_laboratory_frame,
        (arg_("tox"),arg_("toy"),
         arg_("spotfx"),arg_("spotfy"),
         arg_("master_tiles")
        ))
    ;

    class_<xfel_legacy::parameter::vector_array >(
      "vector_array",init<>())
      .add_property("gradients",
       make_getter(&xfel_legacy::parameter::vector_array::gradients,rbv()))
    ;

    class_<xfel_legacy::parameter::vector_collection >(
      "vector_collection",init<>())
      .def(init<xfel_legacy::iarray,xfel_legacy::marray>())
      .def("collect_vector_information",
        &xfel_legacy::parameter::vector_collection::collect_vector_information)
      .def("register",
        &xfel_legacy::parameter::vector_collection::register_tag,(arg_("tag")))
    ;

    class_<xfel_legacy::parameter::streak_parameters >(
      "streak_parameters",no_init)
      .add_property("rotax",make_getter(
         &xfel_legacy::parameter::streak_parameters::rotax, rbv()))
      .add_property("position",make_getter(
         &xfel_legacy::parameter::streak_parameters::position, rbv()))
      .add_property("rotax_excursion_rad",make_getter(
         &xfel_legacy::parameter::streak_parameters::rotax_excursion_rad, rbv()))
     ;

    class_<xfel_legacy::parameter::bandpass_gaussian>("bandpass_gaussian",
      init<rstbx::bandpass::parameters_bp3 const&>(arg("parameters")))
      .def("set_active_areas", &xfel_legacy::parameter::bandpass_gaussian::set_active_areas)
      .def("set_sensor_model", &xfel_legacy::parameter::bandpass_gaussian::set_sensor_model,(
         arg("thickness_mm"), arg("mu_rho"), arg("signal_penetration")))
      .def("picture_fast_slow_force",
         &xfel_legacy::parameter::bandpass_gaussian::picture_fast_slow_force)
      .def("gaussian_fast_slow",
         &xfel_legacy::parameter::bandpass_gaussian::gaussian_fast_slow)
      .add_property("hi_E_limit",make_getter(
         &xfel_legacy::parameter::bandpass_gaussian::hi_E_limit, rbv()))
      .add_property("lo_E_limit",make_getter(
         &xfel_legacy::parameter::bandpass_gaussian::lo_E_limit, rbv()))
      .add_property("mean_position",make_getter(
         &xfel_legacy::parameter::bandpass_gaussian::mean_position, rbv()))
      .add_property("calc_radial_length",make_getter(
         &xfel_legacy::parameter::bandpass_gaussian::calc_radial_length, rbv()))
      .add_property("part_distance",make_getter(
         &xfel_legacy::parameter::bandpass_gaussian::part_distance, rbv()))
      .add_property("observed_flag",make_getter(
         &xfel_legacy::parameter::bandpass_gaussian::observed_flag, rbv()))
      .def("set_subpixel", &xfel_legacy::parameter::bandpass_gaussian::set_subpixel)
      .def("set_mosaicity", &xfel_legacy::parameter::bandpass_gaussian::set_mosaicity)
      .def("set_domain_size", &xfel_legacy::parameter::bandpass_gaussian::set_domain_size)
      .def("set_bandpass", &xfel_legacy::parameter::bandpass_gaussian::set_bandpass)
      .def("set_orientation", &xfel_legacy::parameter::bandpass_gaussian::set_orientation)
      .def("set_detector_origin",
         &xfel_legacy::parameter::bandpass_gaussian::set_detector_origin)
      .def("set_distance", &xfel_legacy::parameter::bandpass_gaussian::set_distance)
      .def("set_vector_output_pointers",
         &xfel_legacy::parameter::bandpass_gaussian::set_vector_output_pointers,
         (arg_("vector_collection"),arg_("frame_id")))
      .def("measure_bandpass_and_mosaic_parameters", &
      xfel_legacy::parameter::bandpass_gaussian::measure_bandpass_and_mosaic_parameters
         ,(arg("radial"), arg("azimut"), arg("domain_sz_inv_ang"),
           arg("lab_frame_obs")
          ))
      .add_property("wavelength_fit_ang",make_getter(
         &xfel_legacy::parameter::bandpass_gaussian::wavelength_fit_ang, rbv()))
      .add_property("mosaicity_fit_rad",make_getter(
         &xfel_legacy::parameter::bandpass_gaussian::mosaicity_fit_rad, rbv()))
      .add_property("wavelength_fit_ang_sigma",make_getter(
         &xfel_legacy::parameter::bandpass_gaussian::wavelength_fit_ang_sigma, rbv()))
      .add_property("mosaicity_fit_rad_sigma",make_getter(
         &xfel_legacy::parameter::bandpass_gaussian::mosaicity_fit_rad_sigma, rbv()))
      .def("simple_forward_calculation_spot_position",
           &xfel_legacy::parameter::bandpass_gaussian::simple_forward_calculation_spot_position,
           (arg_("wavelength"), arg_("observation_no")))
      .def("simple_part_excursion_part_rotxy",
           &xfel_legacy::parameter::bandpass_gaussian::simple_part_excursion_part_rotxy,
           (arg_("wavelength"), arg_("observation_no"),
            arg_("dA_drotxy")))
    ;

    //class_<cctbx::agconvert::AG>("AGconvert", init<>())
    //  .def("forward", &cctbx::agconvert::AG::forward)
    //  .def("validate_and_setG", &cctbx::agconvert::AG::validate_and_setG)
    //  .def("back_as_orientation", &cctbx::agconvert::AG::back_as_orientation)
    //  .def("back", &cctbx::agconvert::AG::back)
    //  .add_property("G",make_getter(&cctbx::agconvert::AG::G, rbv()))
    //;
    def("best_fit_limit",xfel_legacy::parameter::best_fit_limit);
    def("quadrant_self_correlation",xfel_legacy::qsc);
  }

}
}} // namespace xfel::boost_python::<anonymous>

BOOST_PYTHON_MODULE(xfel_legacy_scale_ext)
{
  xfel::boost_python::xfel_scale_init_module();

}


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2016.
//
// This software is released under a three-clause BSD license:
//  * 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 any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// 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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: Andreas Bertsch $
// $Authors: Andreas Bertsch $
// --------------------------------------------------------------------------

#include <OpenMS/COMPARISON/SPECTRA/SpectrumAlignmentScore.h>
#include <OpenMS/COMPARISON/SPECTRA/SpectrumAlignment.h>
#include <OpenMS/DATASTRUCTURES/ListUtils.h>
#include <cmath>

#include <boost/math/special_functions/erf.hpp>

using namespace std;

namespace OpenMS
{
  SpectrumAlignmentScore::SpectrumAlignmentScore() :
    PeakSpectrumCompareFunctor()
  {
    setName(SpectrumAlignmentScore::getProductName());
    defaults_.setValue("tolerance", 0.3, "Defines the absolute (in Da) or relative (in ppm) tolerance");
    defaults_.setValue("is_relative_tolerance", "false", "if true, the tolerance value is interpreted as ppm");
    defaults_.setValidStrings("is_relative_tolerance", ListUtils::create<String>("true,false"));
    defaults_.setValue("use_linear_factor", "false", "if true, the intensities are weighted with the relative m/z difference");
    defaults_.setValidStrings("use_linear_factor", ListUtils::create<String>("true,false"));
    defaults_.setValue("use_gaussian_factor", "false", "if true, the intensities are weighted with the relative m/z difference using a gaussian");
    defaults_.setValidStrings("use_gaussian_factor", ListUtils::create<String>("true,false"));
    defaultsToParam_();
  }

  SpectrumAlignmentScore::SpectrumAlignmentScore(const SpectrumAlignmentScore & source) :
    PeakSpectrumCompareFunctor(source)
  {
  }

  SpectrumAlignmentScore::~SpectrumAlignmentScore()
  {
  }

  SpectrumAlignmentScore & SpectrumAlignmentScore::operator=(const SpectrumAlignmentScore & source)
  {
    if (this != &source)
    {
      PeakSpectrumCompareFunctor::operator=(source);
    }
    return *this;
  }

  double SpectrumAlignmentScore::operator()(const PeakSpectrum & spec) const
  {
    return operator()(spec, spec);
  }

  double SpectrumAlignmentScore::operator()(const PeakSpectrum & s1, const PeakSpectrum & s2) const
  {
    const double tolerance = (double)param_.getValue("tolerance");
    bool is_relative_tolerance = param_.getValue("is_relative_tolerance").toBool();
    bool use_linear_factor = param_.getValue("use_linear_factor").toBool();
    bool use_gaussian_factor = param_.getValue("use_gaussian_factor").toBool();

    if (use_linear_factor && use_gaussian_factor)
    {
      cerr << "Warning: SpectrumAlignmentScore, use either 'use_linear_factor' or 'use_gaussian_factor'!" << endl;
    }

    SpectrumAlignment aligner;
    Param p;
    p.setValue("tolerance", tolerance);
    p.setValue("is_relative_tolerance", (String)param_.getValue("is_relative_tolerance"));
    aligner.setParameters(p);

    vector<pair<Size, Size> > alignment;
    aligner.getSpectrumAlignment(alignment, s1, s2);

    double score(0), sum(0), sum1(0), sum2(0);
    for (PeakSpectrum::ConstIterator it1 = s1.begin(); it1 != s1.end(); ++it1)
    {
      sum1 += it1->getIntensity() * it1->getIntensity();
    }

    for (PeakSpectrum::ConstIterator it1 = s2.begin(); it1 != s2.end(); ++it1)
    {
      sum2 += it1->getIntensity() * it1->getIntensity();
    }

    for (vector<pair<Size, Size> >::const_iterator it = alignment.begin(); it != alignment.end(); ++it)
    {
      //double factor(0.0);
      //factor = (epsilon - fabs(s1[it->first].getPosition()[0] - s2[it->second].getPosition()[0])) / epsilon;
      double mz_tolerance(tolerance);

      if (is_relative_tolerance)
      {
        mz_tolerance = mz_tolerance * s1[it->first].getPosition()[0] / 1e6;
      }

      double mz_difference(fabs(s1[it->first].getPosition()[0] - s2[it->second].getPosition()[0]));
      double factor = 1.0;

      if (use_linear_factor || use_gaussian_factor)
      {
        factor = getFactor_(mz_tolerance, mz_difference, use_gaussian_factor);
      }
      sum += sqrt(s1[it->first].getIntensity() * s2[it->second].getIntensity() * factor);
    }

    score = sum / (sqrt(sum1 * sum2));

    return score;
  }

  double SpectrumAlignmentScore::getFactor_(double mz_tolerance, double mz_difference, bool is_gaussian) const
  {
    double factor(0.0);

    if (is_gaussian)
    {
      static const double denominator = mz_tolerance * 3.0 * sqrt(2.0);
      factor = boost::math::erfc(mz_difference / denominator);
      //cerr << "Factor: " << factor << " " << mz_tolerance << " " << mz_difference << endl;
    }
    else
    {
      factor = (mz_tolerance - mz_difference) / mz_tolerance;
    }
    return factor;
  }

}


#pragma once

#include <cstdint>
#include <cassert>

#include <ros/message_traits.h>
#include <ros/serialization.h>

#include <std_msgs/Float64.h>

#include <sel_map_mesh/Defs.hpp>
#include <Eigen/Dense>

// Minimally define custom types to reduce copies for the message passing
namespace sel_map
{
namespace msg_adaptor
{

  // Wrap a pointer to vertex normals
  struct VertexNormalsArray
  {
    typedef Eigen::Ref<const sel_map::mesh::PointArray_t, Eigen::Aligned16>* _vertex_normals_ptr_type;
    _vertex_normals_ptr_type vertex_normals_ptr;
  };

} // namespace msg_adaptor
} // namespace sel_map

// Make it work with ros (direct wraps are not publishable!)
namespace ros
{
namespace serialization
{

  template<>
  struct Serializer<sel_map::msg_adaptor::VertexNormalsArray>
  {
    template<typename Stream>
    inline static void write(Stream& stream, const sel_map::msg_adaptor::VertexNormalsArray& m)
    {
      // For cleanliness
      typedef std_msgs::Float64::_data_type float64_t;

      // Prepend the OUTER vector length
      uint32_t num_vertices = m.vertex_normals_ptr->rows();
      stream.next(num_vertices);
      // Eigen Map it to store
      const uint32_t point_len = 3*(uint32_t)sizeof(float64_t);
      auto res = Eigen::Map<Eigen::Array<float64_t, Eigen::Dynamic, 3, Eigen::RowMajor>,
                            Eigen::Unaligned>((float64_t*)stream.advance(num_vertices*point_len), num_vertices, 3);
      res = *m.vertex_normals_ptr;
    }

    template<typename Stream>
    inline static void read(Stream& stream, sel_map::msg_adaptor::VertexNormalsArray& m)
    {
      // Not planning on considering this, so crash out
      assert(false);
    }

    inline static uint32_t serializedLength(const sel_map::msg_adaptor::VertexNormalsArray& m)
    {
      // For cleanliness
      typedef std_msgs::Float64::_data_type float64_t;

      // Match the serialized length as found above
      return (uint32_t)sizeof(uint32_t) + m.vertex_normals_ptr->rows() * 3 * (uint32_t)sizeof(float64_t);
    }
  };

} // namespace serialization
} // namespace ros


#include "tsdf_volume.h"

#include <iostream>
#include <fstream>
#include <cmath>
#include <Eigen/Geometry>
#include <opencv2/highgui/highgui.hpp>

TSDFVolume::TSDFVolume(const Vec3i &dimensions, const Vec3f &size, const Mat3f &K, const float truncationDistance, size_t frameNumber) :
    m_dim(dimensions),
    m_gridSize(m_dim[0] * m_dim[1] * m_dim[2]),
    m_size(size),
    m_voxelSize(Vec3f(m_size.cwiseQuotient(m_dim.cast<float>()))),
    m_tsdf(0),
    m_weights(0),
    m_colorR(0),
    m_colorG(0),
    m_colorB(0),
    m_weightsColor(0),
    m_delta(truncationDistance),
    m_deltaInv(1.0f / truncationDistance),
    m_K(K),
    m_frameNumber(frameNumber)
{
    init();
}

TSDFVolume::~TSDFVolume()
{
    release();
}

bool TSDFVolume::init()
{
    try
    {
        // allocate and initialize memory
        m_tsdf = new float[m_gridSize];
        std::fill_n(m_tsdf, m_gridSize, -1.0f);

        m_weights = new float[m_gridSize];
        std::fill_n(m_weights, m_gridSize, 0.0f);

        unsigned char defaultColor = 127; //0;
        m_colorR = new unsigned char[m_gridSize];
        std::fill_n(m_colorR, m_gridSize, defaultColor);
        m_colorG = new unsigned char[m_gridSize];
        std::fill_n(m_colorG, m_gridSize, defaultColor);
        m_colorB = new unsigned char[m_gridSize];
        std::fill_n(m_colorB, m_gridSize, defaultColor);

        m_weightsColor = new float[m_gridSize];
        std::fill_n(m_weightsColor, m_gridSize, 0.0f);
    }
    catch (...)
    {
        release();
        return false;
    }
    return true;
}

void TSDFVolume::release()
{
    delete[] m_tsdf;
    m_tsdf = 0;
    delete[] m_weights;
    m_weights = 0;
    delete[] m_colorR;
    m_colorR = 0;
    delete[] m_colorG;
    m_colorG = 0;
    delete[] m_colorB;
    m_colorB = 0;
    delete[] m_weightsColor;
    m_weightsColor = 0;
}

float TSDFVolume::truncate(float sdf) const
{
    float tsdf = sdf;
    if (tsdf > m_delta)
        tsdf = m_delta;
    else if (tsdf < -m_delta)
        tsdf = -m_delta;
    return tsdf * m_deltaInv;   // normalize tsdf value to interval [-1.0,...,1.0]
}

void TSDFVolume::integrate(const Mat4f &pose, const cv::Mat &color, const cv::Mat &depth, const cv::Mat &normals)
{
    Mat3f R = pose.topLeftCorner(3,3);
    Vec3f t = pose.topRightCorner(3,1);

    const float* ptrDepth = (float*)depth.data;
    unsigned char* ptrColor = 0;
    if (!color.empty())
        ptrColor = (unsigned char*)color.data;
    int h = depth.rows;
    int w = depth.cols;
    float fx = m_K(0, 0);
    float fy = m_K(1, 1);
    float cx = m_K(0, 2);
    float cy = m_K(1, 2);

    // project each voxel into output image
    for (size_t z = 0; z < m_dim[2]; ++z)
    {
        for (size_t y = 0; y < m_dim[1]; ++y)
        {
            for (size_t x = 0; x < m_dim[0]; ++x)
            {
                size_t off = z*m_dim[0]*m_dim[1] + y*m_dim[0] + x;

                // transform voxel into view
                Vec3i vx(x, y, z);
                Vec3f pt = voxelToWorld(vx);
                Vec3f ptTf = R * pt + t;

                // check if voxel is behind the camera
                float distVoxel = ptTf[2];
                if (distVoxel <= 0.0f)
                    continue;

                // project point into camera and round to nearest integer to avoid smearing out of object boundaries to the background
                float zInv = 1.0f / ptTf[2];
                float uf = (fx * ptTf[0] * zInv) + cx;
                float vf = (fy * ptTf[1] * zInv) + cy;
                int u = static_cast<int>(uf + 0.5f);
                int v = static_cast<int>(vf + 0.5f);

                if (u >= 0 && u < w && v >= 0 && v < h)
                {
                    size_t idx = v*w + u;
                    size_t idx3 = idx * 3;
                    float depthIn = ptrDepth[idx];
                    if (depthIn == 0.0f || std::isnan(depthIn))
                    {
                        continue;
                    }

                    // compute tsdf value
                    float sdfVal = ptTf[2] - depthIn;

                    // truncate tsdf value
                    float tsdfVal = truncate(sdfVal);

                    // compute weight
                    float wTsdf = 0.0f;
                    if (sdfVal <= 0)//-m_delta)
                    {
                        wTsdf = 1.0f;
                    }
                    else if (sdfVal <= m_delta)
                    {
                        // constant weighting function
                        // wTsdf = 1.0f;
                        // linear weighting function
                        //  wTsdf = (tsdfVal + 1.0f) / 2.0f;
                        wTsdf = 1.0f - tsdfVal;
                    }

                    float wTsdfNew = 0.0f;
                    if (wTsdf > 0.0f)
                    {
                        // average weight
                        float wTsdfOld = m_weights[off];
                        float tsdfOld = m_tsdf[off];
                        // average tsdf value
                        wTsdfNew = wTsdfOld + wTsdf;
                        float tsdfNew = (tsdfOld * wTsdfOld + tsdfVal * wTsdf) / wTsdfNew;
                        m_tsdf[off] = tsdfNew;

                        m_weights[off] = wTsdfNew;
                    }

#if 1
                    // separate weighting for colors
                    float wColorOld = m_weightsColor[off];
                    if (sdfVal <= m_delta || wColorOld == 0.0f)
                    {
                        float wColor = wTsdf;
                        float wColorNew;
                        Vec3b c(ptrColor[idx3], ptrColor[idx3+1], ptrColor[idx3+2]);
                        unsigned char cR = c[2];
                        unsigned char cG = c[1];
                        unsigned char cB = c[0];
                        if (wColorOld == 0.0f)
                        {
                            wColorNew = wColor;
                        }
                        else
                        {
                            // compute color averaging weights
                            wColorNew = wColorOld + wColor;
                            float w0 = wColorOld / wColorNew;
                            float w1 = 1.0f - w0;
                            // compute average color
                            cR = (unsigned char)(m_colorR[off] * w0 + cR * w1);
                            cG = (unsigned char)(m_colorG[off] * w0 + cG * w1);
                            cB = (unsigned char)(m_colorB[off] * w0 + cB * w1);
                        }
                        m_weightsColor[off] = wColorNew;

                        // voxel color
                        m_colorR[off] = cR;
                        m_colorG[off] = cG;
                        m_colorB[off] = cB;
                    }
#endif
                }
            }
        }
    }
}

float TSDFVolume::interpolate3(float x, float y, float z) const
{
    Vec3f pt(x, y, z);
    Vec3f voxel = (pt + m_size.cast<float>()*0.5f).cwiseQuotient(m_voxelSize);
    return interpolate3voxel(voxel[0], voxel[1], voxel[2]);
}

float TSDFVolume::interpolate3voxel(float x, float y, float z) const
{
    Vec3f voxel(x, y, z);
    if (voxel[0] < 0.0f || voxel[0] > m_dim[0] - 1 ||
            voxel[1] < 0.0f || voxel[1] > m_dim[1] - 1 ||
            voxel[2] < 0.0f || voxel[2] > m_dim[2] - 1)
        return -1.0f;

    // tri-linear interpolation
    const int x0 = static_cast<int>(voxel[0]);
    const int y0 = static_cast<int>(voxel[1]);
    const int z0 = static_cast<int>(voxel[2]);
    const int x1 = x0 + 1;
    const int y1 = y0 + 1;
    const int z1 = z0 + 1;

    if (x1 >= m_dim[0] || y1 >= m_dim[1] || z1 >= m_dim[2])
        return m_tsdf[z0 * m_dim[0] * m_dim[1] + y0 * m_dim[0] + x0];

    const float xd = voxel[0] - x0;
    const float yd = voxel[1] - y0;
    const float zd = voxel[2] - z0;

    const float sdf000 = m_tsdf[z0 * m_dim[0] * m_dim[1] + y0 * m_dim[0] + x0];
    const float sdf010 = m_tsdf[z0 * m_dim[0] * m_dim[1] + y1 * m_dim[0] + x0];
    const float sdf001 = m_tsdf[z1 * m_dim[0] * m_dim[1] + y0 * m_dim[0] + x0];
    const float sdf011 = m_tsdf[z1 * m_dim[0] * m_dim[1] + y1 * m_dim[0] + x0];
    const float sdf100 = m_tsdf[z0 * m_dim[0] * m_dim[1] + y0 * m_dim[0] + x1];
    const float sdf110 = m_tsdf[z0 * m_dim[0] * m_dim[1] + y1 * m_dim[0] + x1];
    const float sdf101 = m_tsdf[z1 * m_dim[0] * m_dim[1] + y0 * m_dim[0] + x1];
    const float sdf111 = m_tsdf[z1 * m_dim[0] * m_dim[1] + y1 * m_dim[0] + x1];

    const float c00 = sdf000 * (1.0f - xd) + sdf100 * xd;
    const float c10 = sdf010 * (1.0f - xd) + sdf110 * xd;
    const float c01 = sdf001 * (1.0f - xd) + sdf101 * xd;
    const float c11 = sdf011 * (1.0f - xd) + sdf111 * xd;

    const float c0 = c00 * (1.0f - yd) + c10 * yd;
    const float c1 = c01 * (1.0f - yd) + c11 * yd;

    return c0 * (1.0f - zd) + c1 * zd;
}

bool TSDFVolume::load(const std::string &filename)
{
    std::ifstream inFile(filename.c_str(), std::ios::binary);
    if (!inFile.is_open())
        return false;

    release();

    inFile.read((char*)m_dim.data(), sizeof(int) * 3);
    m_gridSize = m_dim[0] * m_dim[1] * m_dim[2];
    inFile.read((char*)m_size.data(), sizeof(double) * 3);
    inFile.read((char*)&m_delta, sizeof(float));
    m_deltaInv = 1.0f / m_delta;

    bool ok = init();

    inFile.read((char*)m_tsdf, sizeof(float) * m_gridSize);
    inFile.read((char*)m_weights, sizeof(float) * m_gridSize);

    inFile.read((char*)m_colorR, sizeof(unsigned char) * m_gridSize);
    inFile.read((char*)m_colorG, sizeof(unsigned char) * m_gridSize);
    inFile.read((char*)m_colorB, sizeof(unsigned char) * m_gridSize);
    inFile.read((char*)m_weightsColor, sizeof(float) * m_gridSize);

    inFile.close();

    return ok;
}

bool TSDFVolume::save(const std::string &filename)
{
    std::ofstream outFile(filename.c_str(), std::ios::binary);
    if (!outFile.is_open())
        return false;

    outFile.write((const char*)m_dim.data(), sizeof(int) * 3);
    outFile.write((const char*)m_size.data(), sizeof(double) * 3);
    outFile.write((const char*)&m_delta, sizeof(float));
    outFile.write((const char*)m_tsdf, sizeof(float) * m_gridSize);
    outFile.write((const char*)m_weights, sizeof(float) * m_gridSize);

    outFile.write((const char*)m_colorR, sizeof(unsigned char) * m_gridSize);
    outFile.write((const char*)m_colorG, sizeof(unsigned char) * m_gridSize);
    outFile.write((const char*)m_colorB, sizeof(unsigned char) * m_gridSize);
    outFile.write((const char*)m_weightsColor, sizeof(float) * m_gridSize);

    outFile.close();
}

Vec3f TSDFVolume::surfaceNormal(int i, int j, int k)
{
    Vec3f n = Vec3f::Zero();
    if (i < 0 || j < 0 || k < 0 ||
            i >= m_dim[0]-1 || j >= m_dim[1]-1 || k >= m_dim[2]-1)
        return n;

    size_t idx0 = k * m_dim[0] * m_dim[1] + j * m_dim[0] + i;
    size_t idx1 = k * m_dim[0] * m_dim[1] + j * m_dim[0] + (i+1);
    size_t idx2 = k * m_dim[0] * m_dim[1] + (j+1) * m_dim[0] + i;
    size_t idx3 = (k+1) * m_dim[0] * m_dim[1] + j * m_dim[0] + i;

    float sdf0 = m_tsdf[idx0];
    n[0] = m_tsdf[idx1] - sdf0;
    n[1] = m_tsdf[idx2] - sdf0;
    n[2] = m_tsdf[idx3] - sdf0;
    if (n.norm() != 0.0f)
        n.normalize();
    return n;
}

void TSDFVolume::bbox(Vec3 &min, Vec3 &max) const
{
    min = Vec3(-m_size[0]*0.5, -m_size[1]*0.5, -m_size[2]*0.5);
    max = Vec3(m_size[0]*0.5, m_size[1]*0.5, m_size[2]*0.5);
}

void TSDFVolume::setSize(const Vec3f &size)
{
    m_size = size;
    m_voxelSize = m_size.cwiseQuotient(m_dim.cast<float>());
}

Vec3f TSDFVolume::size() const
{
    return m_size;
}

Vec3i TSDFVolume::dimensions() const
{
    return m_dim;
}

Vec3f TSDFVolume::voxelToWorld(const Vec3i &voxel) const
{
    Vec3f pt = voxel.cast<float>().cwiseProduct(m_voxelSize) - m_size*0.5f;
    return pt;
}

Vec3f TSDFVolume::voxelToWorld(int i, int j, int k) const
{
    return voxelToWorld(Vec3i(i, j, k));
}

Vec3f TSDFVolume::voxelToWorld(const Vec3f &voxel) const
{
    Vec3f pt = voxel.cwiseProduct(m_voxelSize) - m_size*0.5f;
    return pt;
}

Vec3i TSDFVolume::worldToVoxel(const Vec3f &pt) const
{
    Vec3f voxelSizeInv(1.0 / m_voxelSize[0], 1.0 / m_voxelSize[1], 1.0 / m_voxelSize[2]);
    Vec3f voxelF = (pt + 0.5f * m_size).cwiseProduct(voxelSizeInv);
    Vec3i voxelIdx = voxelF.cast<int>();
    return voxelIdx;
}

Vec3f TSDFVolume::worldToVoxelF(const Vec3f &pt) const
{
    Vec3f voxelSizeInv(1.0 / m_voxelSize[0], 1.0 / m_voxelSize[1], 1.0 / m_voxelSize[2]);
    Vec3f voxelF = (pt + 0.5f * m_size).cwiseProduct(voxelSizeInv);
    return voxelF;
}


//   GAMBIT: Global and Modular BSM Inference Tool
//   *********************************************
///  \file
///
///  Declarations of B->Kstar mumu theory error
///  class.
///
///  *********************************************
///
///  Authors (add name and date if you modify):
///
///  \author Marcin Chrzaszcz
///          (mchrzasz@cern.ch)
///  \date 2016 August
///
///  \author Pat Scott
///          (p.scott@imperial.ac.uk)
///  \date 2017 Mar
///
///  *********************************************

#ifndef __Kstarmumu_theory_err_hpp__
#define __Kstarmumu_theory_err_hpp__

#include "gambit/Utils/util_types.hpp"
#include <boost/numeric/ublas/matrix.hpp>

namespace Gambit
{

  namespace FlavBit
  {

    /// Holder for theory errors for B->K* mumu observables
    class Kstarmumu_theory_err
    {

      private:

        int size_obs;
        std::vector<str> names_obs;
        std::vector< std::vector< double > >covariance;
        std::map<str,int> map_kstarmumu;

      public:

        /// Constructor
        Kstarmumu_theory_err();
        /// Return theory error covariance matrix for selected observables
        boost::numeric::ublas::matrix<double> get_th_cov(std::vector<str> observables);

    };

  }

}

#endif //#defined __Kstarmumu_theory_err_hpp__