// Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef UUID_5265FC7CA1C011DE9EBDFFA956D89593
#define UUID_5265FC7CA1C011DE9EBDFFA956D89593

#include <boost/qvm/assert.hpp>
#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/enable_if.hpp>
#include <boost/qvm/inline.hpp>
#include <boost/qvm/mat_traits.hpp>

namespace boost {
namespace qvm {
////////////////////////////////////////////////

namespace qvm_detail {
template <int Col, class OriginalMatrix> class col_ {
  col_(col_ const &);
  col_ &operator=(col_ const &);
  ~col_();

public:
  template <class T> BOOST_QVM_INLINE_TRIVIAL col_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <int Col, class OriginalMatrix>
struct vec_traits<qvm_detail::col_<Col, OriginalMatrix>> {
  typedef qvm_detail::col_<Col, OriginalMatrix> this_vector;
  typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
  static int const dim = mat_traits<OriginalMatrix>::rows;
  BOOST_QVM_STATIC_ASSERT(Col >= 0);
  BOOST_QVM_STATIC_ASSERT(Col < mat_traits<OriginalMatrix>::cols);

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_vector const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return mat_traits<OriginalMatrix>::template read_element<I, Col>(
        reinterpret_cast<OriginalMatrix const &>(x));
  }

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type &write_element(this_vector &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return mat_traits<OriginalMatrix>::template write_element<I, Col>(
        reinterpret_cast<OriginalMatrix &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int i, this_vector const &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < dim);
    return mat_traits<OriginalMatrix>::read_element_idx(
        i, Col, reinterpret_cast<OriginalMatrix const &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type &
  write_element_idx(int i, this_vector &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < dim);
    return mat_traits<OriginalMatrix>::write_element_idx(
        i, Col, reinterpret_cast<OriginalMatrix &>(x));
  }
};

template <int Col, class OriginalMatrix, int D>
struct deduce_vec<qvm_detail::col_<Col, OriginalMatrix>, D> {
  typedef vec<typename mat_traits<OriginalMatrix>::scalar_type, D> type;
};

template <int Col, class OriginalMatrix, int D>
struct deduce_vec2<qvm_detail::col_<Col, OriginalMatrix>,
                   qvm_detail::col_<Col, OriginalMatrix>, D> {
  typedef vec<typename mat_traits<OriginalMatrix>::scalar_type, D> type;
};

template <int Col, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::col_<Col, A> const &>::type
    BOOST_QVM_INLINE_TRIVIAL
    col(A const &a) {
  return reinterpret_cast<typename qvm_detail::col_<Col, A> const &>(a);
}

template <int Col, class A>
typename boost::enable_if_c<is_mat<A>::value, qvm_detail::col_<Col, A> &>::type
    BOOST_QVM_INLINE_TRIVIAL
    col(A &a) {
  return reinterpret_cast<typename qvm_detail::col_<Col, A> &>(a);
}

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

namespace qvm_detail {
template <int Row, class OriginalMatrix> class row_ {
  row_(row_ const &);
  row_ &operator=(row_ const &);
  ~row_();

public:
  template <class T> BOOST_QVM_INLINE_TRIVIAL row_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <int Row, class OriginalMatrix>
struct vec_traits<qvm_detail::row_<Row, OriginalMatrix>> {
  typedef qvm_detail::row_<Row, OriginalMatrix> this_vector;
  typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
  static int const dim = mat_traits<OriginalMatrix>::cols;
  BOOST_QVM_STATIC_ASSERT(Row >= 0);
  BOOST_QVM_STATIC_ASSERT(Row < mat_traits<OriginalMatrix>::rows);

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_vector const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return mat_traits<OriginalMatrix>::template read_element<Row, I>(
        reinterpret_cast<OriginalMatrix const &>(x));
  }

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type &write_element(this_vector &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return mat_traits<OriginalMatrix>::template write_element<Row, I>(
        reinterpret_cast<OriginalMatrix &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int i, this_vector const &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < dim);
    return mat_traits<OriginalMatrix>::read_element_idx(
        Row, i, reinterpret_cast<OriginalMatrix const &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type &
  write_element_idx(int i, this_vector &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < dim);
    return mat_traits<OriginalMatrix>::write_element_idx(
        Row, i, reinterpret_cast<OriginalMatrix &>(x));
  }
};

template <int Row, class OriginalMatrix, int D>
struct deduce_vec<qvm_detail::row_<Row, OriginalMatrix>, D> {
  typedef vec<typename mat_traits<OriginalMatrix>::scalar_type, D> type;
};

template <int Row, class OriginalMatrix, int D>
struct deduce_vec2<qvm_detail::row_<Row, OriginalMatrix>,
                   qvm_detail::row_<Row, OriginalMatrix>, D> {
  typedef vec<typename mat_traits<OriginalMatrix>::scalar_type, D> type;
};

template <int Row, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::row_<Row, A> const &>::type
    BOOST_QVM_INLINE_TRIVIAL
    row(A const &a) {
  return reinterpret_cast<typename qvm_detail::row_<Row, A> const &>(a);
}

template <int Row, class A>
typename boost::enable_if_c<is_mat<A>::value, qvm_detail::row_<Row, A> &>::type
    BOOST_QVM_INLINE_TRIVIAL
    row(A &a) {
  return reinterpret_cast<typename qvm_detail::row_<Row, A> &>(a);
}

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

namespace qvm_detail {
template <class OriginalMatrix> class diag_ {
  diag_(diag_ const &);
  diag_ &operator=(diag_ const &);
  ~diag_();

public:
  template <class T> BOOST_QVM_INLINE_TRIVIAL diag_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};

template <int X, int Y, bool Which> struct diag_bool_dispatch;

template <int X, int Y> struct diag_bool_dispatch<X, Y, true> {
  static int const value = X;
};

template <int X, int Y> struct diag_bool_dispatch<X, Y, false> {
  static int const value = Y;
};
} // namespace qvm_detail

template <class OriginalMatrix>
struct vec_traits<qvm_detail::diag_<OriginalMatrix>> {
  typedef qvm_detail::diag_<OriginalMatrix> this_vector;
  typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
  static int const dim = qvm_detail::diag_bool_dispatch<
      mat_traits<OriginalMatrix>::rows, mat_traits<OriginalMatrix>::cols,
      mat_traits<OriginalMatrix>::rows <=
          mat_traits<OriginalMatrix>::cols>::value;

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_vector const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return mat_traits<OriginalMatrix>::template read_element<I, I>(
        reinterpret_cast<OriginalMatrix const &>(x));
  }

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type &write_element(this_vector &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return mat_traits<OriginalMatrix>::template write_element<I, I>(
        reinterpret_cast<OriginalMatrix &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int i, this_vector const &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < dim);
    return mat_traits<OriginalMatrix>::read_element_idx(
        i, i, reinterpret_cast<OriginalMatrix const &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type &
  write_element_idx(int i, this_vector &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < dim);
    return mat_traits<OriginalMatrix>::write_element_idx(
        i, i, reinterpret_cast<OriginalMatrix &>(x));
  }
};

template <class OriginalMatrix, int D>
struct deduce_vec<qvm_detail::diag_<OriginalMatrix>, D> {
  typedef vec<typename mat_traits<OriginalMatrix>::scalar_type, D> type;
};

template <class OriginalMatrix, int D>
struct deduce_vec2<qvm_detail::diag_<OriginalMatrix>,
                   qvm_detail::diag_<OriginalMatrix>, D> {
  typedef vec<typename mat_traits<OriginalMatrix>::scalar_type, D> type;
};

template <class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::diag_<A> const &>::type
    BOOST_QVM_INLINE_TRIVIAL
    diag(A const &a) {
  return reinterpret_cast<typename qvm_detail::diag_<A> const &>(a);
}

template <class A>
typename boost::enable_if_c<is_mat<A>::value, qvm_detail::diag_<A> &>::type
    BOOST_QVM_INLINE_TRIVIAL
    diag(A &a) {
  return reinterpret_cast<typename qvm_detail::diag_<A> &>(a);
}

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

namespace qvm_detail {
template <class OriginalMatrix> class translation_ {
  translation_(translation_ const &);
  ~translation_();

public:
  translation_ &operator=(translation_ const &x) {
    assign(*this, x);
    return *this;
  }

  template <class T>
  BOOST_QVM_INLINE_TRIVIAL translation_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <class OriginalMatrix>
struct vec_traits<qvm_detail::translation_<OriginalMatrix>> {
  typedef qvm_detail::translation_<OriginalMatrix> this_vector;
  typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
  static int const dim = mat_traits<OriginalMatrix>::rows - 1;
  BOOST_QVM_STATIC_ASSERT(mat_traits<OriginalMatrix>::rows ==
                          mat_traits<OriginalMatrix>::cols);
  BOOST_QVM_STATIC_ASSERT(mat_traits<OriginalMatrix>::rows >= 3);

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_vector const &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return mat_traits<OriginalMatrix>::template read_element<I, dim>(
        reinterpret_cast<OriginalMatrix const &>(x));
  }

  template <int I>
  static BOOST_QVM_INLINE_CRITICAL scalar_type &write_element(this_vector &x) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return mat_traits<OriginalMatrix>::template write_element<I, dim>(
        reinterpret_cast<OriginalMatrix &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int i, this_vector const &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < dim);
    return mat_traits<OriginalMatrix>::read_element_idx(
        i, dim, reinterpret_cast<OriginalMatrix const &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type &
  write_element_idx(int i, this_vector &x) {
    BOOST_QVM_ASSERT(i >= 0);
    BOOST_QVM_ASSERT(i < dim);
    return mat_traits<OriginalMatrix>::write_element_idx(
        i, dim, reinterpret_cast<OriginalMatrix &>(x));
  }
};

template <class OriginalMatrix, int D>
struct deduce_vec<qvm_detail::translation_<OriginalMatrix>, D> {
  typedef vec<typename mat_traits<OriginalMatrix>::scalar_type, D> type;
};

template <class OriginalMatrix, int D>
struct deduce_vec2<qvm_detail::translation_<OriginalMatrix>,
                   qvm_detail::translation_<OriginalMatrix>, D> {
  typedef vec<typename mat_traits<OriginalMatrix>::scalar_type, D> type;
};

template <class A>
typename boost::enable_if_c<is_mat<A>::value &&
                                mat_traits<A>::rows == mat_traits<A>::cols &&
                                mat_traits<A>::rows >= 3,
                            qvm_detail::translation_<A> const &>::type
    BOOST_QVM_INLINE_TRIVIAL
    translation(A const &a) {
  return reinterpret_cast<typename qvm_detail::translation_<A> const &>(a);
}

template <class A>
typename boost::enable_if_c<is_mat<A>::value &&
                                mat_traits<A>::rows == mat_traits<A>::cols &&
                                mat_traits<A>::rows >= 3,
                            qvm_detail::translation_<A> &>::type
    BOOST_QVM_INLINE_TRIVIAL
    translation(A &a) {
  return reinterpret_cast<typename qvm_detail::translation_<A> &>(a);
}

////////////////////////////////////////////////
} // namespace qvm
} // namespace boost

#endif


/*
  Author(s):      Matthew Celnik (msc37)
  Project:        sweepc (population balance solver)
  Sourceforge:    http://sourceforge.net/projects/mopssuite

  Copyright (C) 2008 Matthew S Celnik.

  Merged with the SubParticle class by Robert Patterson, October 2012

  File purpose:
    Implementation of the Particle class declared in the
    swp_particle.h header file.

  Licence:
    This file is part of "sweepc".

    sweepc is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License
    as published by the Free Software Foundation; either version 2
    of the License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

  Contact:
    Dr Markus Kraft
    Dept of Chemical Engineering
    University of Cambridge
    New Museums Site
    Pembroke Street
    Cambridge
    CB2 3RA
    UK

    Email:       mk306@cam.ac.uk
    Website:     http://como.cheng.cam.ac.uk
*/

#include "swp_particle.h"

#include "swp_particle_model.h"
#include "swp_particle_image.h"
#include "string_functions.h"

#include <boost/random/uniform_01.hpp>
#include <boost/random/lognormal_distribution.hpp>

#include <cmath>
#include <stdexcept>
#include <cassert>

using namespace Sweep;
using namespace std;

// CONSTRUCTORS AND DESTRUCTORS.

// Default constructor (protected).
Particle::Particle(void)
: m_Position(0.0)
, m_PositionTime(0.0)
, m_StatWeight(1.0)
, m_primary(NULL)
, m_CoagCount(0)
, m_FragCount(0)
, m_createt(0.0)
, mLPDAtime(0.0)
{
}

// Initialising constructor.
Particle::Particle(double time, const Sweep::ParticleModel &model)
: m_Position(0.0)
, m_PositionTime(0.0)
, m_StatWeight(1.0)
, m_CoagCount(0)
, m_FragCount(0)
, m_createt(0.0)
, mLPDAtime(0.0)
{
    m_primary = new AggModels::Primary(time, model);
}

/*!
 * @param[in]	time	Create time
 * @param[in]	weight	Statistical weight of new particle
 * @param[in]	model	Particle model that interprets contents of particle
 */
Particle::Particle(double time, double weight, const Sweep::ParticleModel &model)
: m_Position(0.0)
, m_PositionTime(0.0)
, m_StatWeight(weight)
, m_CoagCount(0)
, m_FragCount(0)
, m_createt(0.0)
, mLPDAtime(0.0)
{
    m_primary = new AggModels::Primary(time, model);
}

// Initialising constructor (from Primary particle).
Particle::Particle(Sweep::AggModels::Primary &pri)
: m_Position(0.0)
, m_PositionTime(0.0)
, m_StatWeight(1.0)
, m_primary(&pri)
, m_CoagCount(0)
, m_FragCount(0)
{
    m_createt = pri.CreateTime();
    mLPDAtime = pri.CreateTime();
}

// Copy constructor.
Particle::Particle(const Sweep::Particle &copy)
: m_primary(NULL)
{
    // Use assignment operator.
    *this = copy;
}

/*!
 * @brief Read the particle from the binary stream.
 *
 * This method must read the data in the same way in which it is written in Serialize.
 *
 * @param[in,out]	 in		             Input binary stream
 * @param[in]        model	             Particle model defining interpretation of particle data
 * @param[in,out]    duplicates          Addresses of PAHs for use when reading primary particles
 *
 * @exception		 invalid_argument    Stream not ready
 */
Particle::Particle(std::istream &in, const Sweep::ParticleModel &model, void *duplicates)
{
    if(in.good()) {
        m_primary = ModelFactory::ReadPrimary(in, model, duplicates);

        in.read(reinterpret_cast<char*>(&m_Position), sizeof(m_Position));
        in.read(reinterpret_cast<char*>(&m_PositionTime), sizeof(m_PositionTime));
        in.read(reinterpret_cast<char*>(&m_StatWeight), sizeof(m_StatWeight));
        in.read(reinterpret_cast<char*>(&m_CoagCount), sizeof(m_CoagCount));
        in.read(reinterpret_cast<char*>(&m_FragCount), sizeof(m_FragCount));
        in.read(reinterpret_cast<char*>(&m_createt), sizeof(m_createt));
        in.read(reinterpret_cast<char*>(&mLPDAtime), sizeof(mLPDAtime));
    }
    else {
        throw std::invalid_argument("Input stream not ready \
        (Sweep::Particle stream reading constructor");
    }
}

// Default destructor.
Particle::~Particle()
{
    // deleting a null pointer is not a problem - it just does nothing
    delete m_primary;
}

/*!
 * @param[in]		xml			XML node specifying the particle
 * @param[in]		model		Particle model that defines the interpretation of the particle data
 *
 * @return		Pointer to new particle constructed on the heap (caller must delete).
 *
 * @exception	runtime_error	Unrecognised component
 * @exception	runtime_error	Unrecognised tracker
 * @exception	runtime_error	Non-positive statistical weight
 */
Particle* Particle::createFromXMLNode(const CamXML::Element& xml, const Sweep::ParticleModel& model)
{
    // Read initial particle composition.
    vector<CamXML::Element*> subitems;
    xml.GetChildren("component", subitems);
    fvector components(model.ComponentCount(), 0);

    for (vector<CamXML::Element*>::iterator j=subitems.begin(); j!=subitems.end(); ++j) {
        // Get component ID.
        string str = (*j)->GetAttributeValue("id");
        int id = model.ComponentIndex(str);

        if (id >= 0) {
            // Get component value (XML uses dx to match format for inception).
            str = (*j)->GetAttributeValue("dx");
            components[id] = Strings::cdble(str);
        } else {
            // Unknown component in mechanism.
            throw std::runtime_error(str + ": Component not found in mechanism \
                                       (Sweep, Particle::createFromXMLNode).");
        }
    }

    // Read initial tracker variable values.
    xml.GetChildren("track", subitems);
    fvector trackers(model.TrackerCount(), 0);

    for (vector<CamXML::Element*>::iterator j=subitems.begin(); j!=subitems.end(); j++) {
        // Get tracker ID.
        string str = (*j)->GetAttributeValue("id");
        int id = model.GetTrackerIndex(str);

        if (id >= 0) {
            // Get tracker value (XML uses dx to match format for inception).
            str = (*j)->GetAttributeValue("dx");
            trackers[id] = Strings::cdble(str);
        } else {
            // Unknown tracker variable in mechanism.
            throw std::runtime_error(str + ": Tracker variable not found in mechanism. \
                                       (Sweep, Particle::createFromXMLNode).");
        }
    }

    // Pointer to new particle
    // \TODO wrap in an auto_ptr for exception safety
    Particle* pNew = model.CreateParticle(0.0);

    // Read any statistical weight
    const CamXML::Element* const pWeightNode = xml.GetFirstChild("weight");
    if(pWeightNode != NULL) {
    	std::string str = pWeightNode->Data();
    	const double wt = Strings::cdble(str);
    	if(wt <= 0) {
    		throw std::runtime_error("Particle statistical weight must be >0, not " + str +
    				                 "(Sweep, Particle::createFromXMLNode).");
    	}
    	else
            pNew->setStatisticalWeight(wt);
    }

    // Read SurfVol specific parameters
    if (model.AggModel() == Sweep::AggModels::SurfVol_ID) {
        const CamXML::Element* const pSurfNode = xml.GetFirstChild("surf");
        if (pSurfNode != NULL) {
            std::string str = pSurfNode->Data();
            const double surf = Strings::cdble(str);
            if(surf <= 0) {
                throw std::runtime_error("Particle surface area must be >0, not " + str +
                                         "(Sweep, Particle::createFromXMLNode).");
            }
            else
                pNew->m_primary->SetSurfaceArea(surf);
        }
    }

    // Initialise the new particle.
    pNew->Primary()->SetComposition(components);
    pNew->Primary()->SetValues(trackers);
    pNew->UpdateCache();

    return pNew;
}

// Version of the above XML reader that will construct (bintree) particles given
// (mean,std) number of primaries per particle and (mean,std) number of component
// units per particle (assuming normal distributions).
// To do: add sintering if required, switch to lognormal distribution when suitable parameters are known.
/*!
* @param[in]		xml			XML node specifying the particle
* @param[in]		model		Particle model that defines the interpretation of the particle data
* @param[in]        rng         Random number generator
*
* @return		Pointer to new particle constructed on the heap (caller must delete).
*
* @exception	runtime_error	Unrecognised component
* @exception	runtime_error	Unrecognised tracker
* @exception	runtime_error	Non-positive statistical weight
*/
Particle* Particle::createFromXMLNodeDetailed(const CamXML::Element& xml, const Sweep::ParticleModel& model, rng_type &rng)
{
	// Read initial particle composition.
	vector<CamXML::Element*> subitems, subitems_pri;
	xml.GetChildren("component", subitems);
	xml.GetChildren("nprimaries", subitems_pri);
	fvector components(model.ComponentCount(), 0);
	fvector components_var(model.ComponentCount(), 0);
	fvector temp_components(model.ComponentCount(), 0);

	for (vector<CamXML::Element*>::iterator j = subitems.begin(); j != subitems.end(); ++j) {
		// Get component ID.
		string str = (*j)->GetAttributeValue("id");
		int id = model.ComponentIndex(str);

		if (id >= 0) {
			// Get component value (XML uses dx to match format for inception).
			str = (*j)->GetAttributeValue("dx");
			components[id] = Strings::cdble(str);
			// Get component std
			std::string str2;
			str2 = (*j)->GetAttributeValue("dx_var");
			if (str2 != "")
			{
				components_var[id] = Strings::cdble(str2);
				if (components_var[id] <= 0)
					throw std::runtime_error("Component variance must be positive. \
											 	(Sweep, Particle::createFromXMLNode).");
			}
			else
				components_var[id] = 0.0;

			/*cout << "\nIn particle fn\n";
			cout << "==============\n";
			cout << "ncomponent_ave = " << components[0] << "\n";*/

			// Convert to distribution parameters
			double ncomp_sqd = components[0] * components[0];
			double mu_ncomp = std::log(ncomp_sqd / sqrt(components_var[0] + ncomp_sqd));
			double sigma_sqd_ncomp = std::log(1.0 + (components_var[0] / ncomp_sqd));

			// Choose amount of component for this particle
			double ncomp_i = boost::random::lognormal_distribution<double>(mu_ncomp, sigma_sqd_ncomp)(rng);

			// Choosing a huge number of components is not memory sustainable, set some maximum
			double ncomp_max = 100000000000.0;
			if (ncomp_i > ncomp_max)
			{
				std::cout << "ncomp_i > ncomp_max, setting ncomp_i = " << ncomp_max << "\n";
				ncomp_i = ncomp_max;
			}

			components[id] = ncomp_i;

			// Scale any other components, assuming they must exist in stoichiometric amounts
			//double sf_i = ncomp_i / components[0];
			//for (size_t iter = 0; iter < components.size(); iter++)
			//	components[iter] *= sf_i;

			//cout << "ncomponent = " << components[0] << "\n";
		}
		else {
			// Unknown component in mechanism.
			throw std::runtime_error(str + ": Component not found in mechanism \
										     (Sweep, Particle::createFromXMLNode).");
		}
	}

	// Read initial tracker variable values.
	xml.GetChildren("track", subitems);
	fvector trackers(model.TrackerCount(), 0);

	for (vector<CamXML::Element*>::iterator j = subitems.begin(); j != subitems.end(); j++) {
		// Get tracker ID.
		string str = (*j)->GetAttributeValue("id");
		int id = model.GetTrackerIndex(str);

		if (id >= 0) {
			// Get tracker value (XML uses dx to match format for inception).
			str = (*j)->GetAttributeValue("dx");
			trackers[id] = Strings::cdble(str);
		}
		else {
			// Unknown tracker variable in mechanism.
			throw std::runtime_error(str + ": Tracker variable not found in mechanism. \
										     (Sweep, Particle::createFromXMLNode).");
		}
	}

	//TODO wrap in an auto_ptr for exception safety
	Particle* pNew1 = model.CreateParticle(0.0);

	///////////////////////////////////////////////////////////////////////////////////////////
	// If bintree primary, read average number of primaries per particle and standard deviation
	if (model.AggModel() == Sweep::AggModels::BinTree_ID)
	{
		double npri, npri_var;
		string str1, str2;

		for (vector<CamXML::Element*>::iterator j = subitems_pri.begin(); j != subitems_pri.end(); ++j)
		{
			// Get mean number of primaries per particle
			str1 = (*j)->GetAttributeValue("npri");
			if (str1 != "")
			{
				npri = Strings::cdble(str1);
			}
			else
				npri = 1.0;

			// Get variance of number of primaries per particle
			str2 = (*j)->GetAttributeValue("npri_var");
			if (str2 != "")
			{
				npri_var = Strings::cdble(str2);
				if (npri_var <= 0)
					throw std::runtime_error("Particle primary count variance must be positive. \
											 	(Sweep, Particle::createFromXMLNode).");
			}
			else
				npri_var = 0.07;
		}

		// Convert to distribution parameters
		double npri_sqd = npri * npri;
		double mu_npri = std::log(npri_sqd / sqrt(npri_var + npri_sqd));
		double sigma_sqd_npri = std::log(1.0 + (npri_var / npri_sqd));

		// Choose number of primaries for this particle
		// to do - give this an upper bound, same with npri
		unsigned int npri_i = ceil(boost::random::lognormal_distribution<double>(mu_npri, sigma_sqd_npri)(rng));

		// Choosing a huge number of primaries is not memory sustainable, set some maximum
		unsigned int npri_max = 1000;
		if (npri_i > npri_max)
		{
			std::cout << "npri_i > npri_max, setting npri_i = " << npri_max << "\n";
			npri_i = npri_max;
		}
		else if (npri_i < 1)
		{
			std::cout << "npri_i < 1, setting npri_i = 1" << "\n";
			npri_i = 1;
		}

		//cout << "npri = " << npri_i << "\n";

		// Choose division of rutile amongst primaries
		boost::uniform_01<rng_type&, double> uniformGenerator(rng);
		fvector fracs(npri_i, 0);
		double fracs_total = 0.0;
		for (unsigned int j = 0; j < npri_i; j++)
		{
			fracs[j] = uniformGenerator();
			fracs_total += fracs[j];
		}
		for (unsigned int j = 0; j < npri_i; j++)
		{
			fracs[j] *= (1.0 / fracs_total);
			//cout << " * " << fracs[j] << "\n";
		}

		// Read any statistical weight
		const CamXML::Element* const pWeightNode = xml.GetFirstChild("weight");
		if (pWeightNode != NULL) {
			std::string str = pWeightNode->Data();
			const double wt = Strings::cdble(str);
			if (wt <= 0) {
				throw std::runtime_error("Particle statistical weight must be >0, not " + str +
					"(Sweep, Particle::createFromXMLNode).");
			}
			else {
				pNew1->setStatisticalWeight(wt);

				// Initialise the first new particle, this will be the base particle.
				for (size_t iter = 0; iter < temp_components.size(); iter++)
					temp_components[iter] = ceil(components[iter] * fracs[0]);

				//cout << "ncomp_i = " << temp_components[0] << "\n";

				pNew1->Primary()->SetComposition(temp_components);
				pNew1->Primary()->SetValues(trackers); // what is this?
				pNew1->UpdateCache();

				//TODO wrap in an auto_ptr for exception safety
				Particle* pNew2;

				// Initialise all other particles, coagulating with the base particle.
				for (unsigned int j = 1; j < npri_i; j++)
				{
					pNew2 = model.CreateParticle(0.0);
					pNew2->setStatisticalWeight(wt);

					// Ensure there is a minimum number of components in the particle
					double ncomp_min = 2.0;
					double adjust = 0.0;
					if (ceil(components[0] * fracs[j]) < ncomp_min)
						adjust = ncomp_min;
					for (size_t iter = 0; iter < temp_components.size(); iter++)
						temp_components[iter] = ceil(components[iter] * fracs[j] + (adjust * components[iter] / components[0]));
					pNew2->Primary()->SetComposition(temp_components);
					pNew2->Primary()->SetValues(trackers); // what is this?
					pNew2->UpdateCache();

					//cout << "ncomp_i = " << temp_components[0] << "\n";

					// Perform coagulation
					// To do: work out if the weight should be modified in the weighted case
					pNew1->Coagulate(*pNew2, rng);
					pNew1->UpdateCache();
				}

				//cout << "Done: dcol(Pi) = " << pNew1->CollDiameter() << "\n";
			}
		}
	}
	///////////////////////////////////////////////////////////////////////////////////////////
	else
	{
		//cout << "Not using bintree model\n";

		// Read any statistical weight
		const CamXML::Element* const pWeightNode = xml.GetFirstChild("weight");
		if (pWeightNode != NULL) {
			std::string str = pWeightNode->Data();
			const double wt = Strings::cdble(str);
			if (wt <= 0) {
				throw std::runtime_error("Particle statistical weight must be >0, not " + str +
					"(Sweep, Particle::createFromXMLNode).");
			}
			else
				pNew1->setStatisticalWeight(wt);
		}

		// Read SurfVol specific parameters
		if (model.AggModel() == Sweep::AggModels::SurfVol_ID) {
			const CamXML::Element* const pSurfNode = xml.GetFirstChild("surf");
			if (pSurfNode != NULL) {
				std::string str = pSurfNode->Data();
				const double surf = Strings::cdble(str);
				if (surf <= 0) {
					throw std::runtime_error("Particle surface area must be >0, not " + str +
						"(Sweep, Particle::createFromXMLNode).");
				}
				else
					pNew1->m_primary->SetSurfaceArea(surf);
			}
		}

		for (size_t iter = 0; iter < components.size(); iter++)
			components[iter] = ceil(components[iter]);

		//cout << "ncomp_i = " << components[0] << "\n";

		// Initialise the new particle.
		pNew1->Primary()->SetComposition(components);
		pNew1->Primary()->SetValues(trackers);
		pNew1->UpdateCache();

		//cout << "Done: dcol(Pi) = " << pNew1->CollDiameter() << "\n";
	}

	return pNew1;
}

// Compound assignment (coagulation).
// OPERATOR OVERLOADING.

// Assignment operator.
Particle &Particle::operator=(const Sweep::Particle &rhs)
{
    if (this != &rhs) {
        // Copy primary.
        if (rhs.m_primary != NULL) {
            // Does not matter if m_primary is null
            delete m_primary;
            m_primary = rhs.m_primary->Clone();
        } else {
            delete m_primary;
            m_primary = NULL;
        }

        // Copy remaining data
        m_Position = rhs.m_Position;
        m_PositionTime = rhs.m_PositionTime;
        m_StatWeight = rhs.m_StatWeight;
        m_CoagCount = rhs.m_CoagCount;
        m_FragCount = rhs.m_FragCount;
        m_createt = rhs.m_createt;
        mLPDAtime = rhs.mLPDAtime;
    }
    return *this;
}

// PRIMARY PARTICLE CHILD.

/*!
 * Access the child primary particle which contains the physical details
 * of the particle
 *
 *@return   Pointer to primary particle or Null if none present
 */
Sweep::AggModels::Primary *const Particle::Primary()
{
    return m_primary;
}

/*!
 * Access the child primary particle which contains the physical details
 * of the particle
 *
 *@return   Pointer to primary particle or Null if none present
 */
const Sweep::AggModels::Primary *const Particle::Primary() const
{
    return m_primary;
}

/*!
 * Pass through to primary particle
 */
double Particle::SphDiameter() const
{
    return m_primary->SphDiameter();
}

/*!
 * Pass through to primary particle
 */
double Particle::CollDiameter() const
{
    return m_primary->CollDiameter();
}

/*!
 * Pass through to primary particle
 */
double Particle::MobDiameter() const
{
    return m_primary->MobDiameter();
}

/*!
 * Pass through to primary particle
 */
double Particle::SurfaceArea() const
{
    return m_primary->SurfaceArea();
}

/*!
 * Pass through to primary particle
 */
double Particle::SphSurfaceArea() const
{
    return m_primary->SphSurfaceArea();
}

/*!
 * Pass through to primary particle
 */
double Particle::Volume() const
{
    return m_primary->Volume();
}

/*!
 * Pass through to primary particle
 */
double Particle::Mass(void) const
{
    return m_primary->Mass();
}

//! Pass through to primary particle.
int Particle::NumCarbon(void) const
{
    return m_primary->NumCarbon();
}

//! Pass through to primary particle.
int Particle::Frag(void) const
{
    return m_primary->Frag();
}

int Particle::NumRings() const
{
	return m_primary->NumRings();
}
/*!
 * Pass through to primary particle
 */
double Particle::GetSites(void) const
{
    return m_primary->GetSites();
}

/*!
 * Pass through to primary particle
 */
double Particle::GetSintRate(void) const
{
    return m_primary->GetSintRate();
}

/*!
 * Pass through to primary particle
 */
double Particle::GetCoverageFraction(void) const
{
    return m_primary->GetCoverageFraction();
}

/*!
 * It is not clear to me why the number of primary particles is
 * not used as the number of sub-units (riap 01.10.2012).
 *
 * @param[in]   Reciprocal of number of sub-units in the aggregate
 *
 * @return      Geometric mean of sub-unit diameter
 */
double Particle::avgeomdiam(double oneovernumsubpart) const
{
    return std::pow(m_primary->Property(Sweep::iDsph),oneovernumsubpart);
}

//! Phase composition term for phase transformation process
double Particle::GetPhaseTerm(void) const
{
	return m_primary->GetPhaseTerm();
}

/*!
 * Provide an interface that allows run time specification of particle properties
 * for use in process rate calculations.  It is currently used for some surface
 * reactions.  Where possible, the use of specific accessors should be preferred.
 *
 *@param[in]    id      Symbolic index of property required
 *
 *@return       Requested particle property
 *
 *@pre          A valid primary particle is present (the data is not stored in SubParticle)
 */
double Particle::Property(PropID id) const
{
    switch (id) {
        case iDsph:      // Equivalent sphere diameter.
            return SphDiameter();
        case iDcol:   // Collision diameter.
            return CollDiameter();
        case iDmob:   // Mobility diameter.
            return MobDiameter();
        case iS:      // Surface area.
            return SurfaceArea();
        case iV:      // Volume.
            return Volume();
        case iM:      // Mass.
            return Mass();

        //! Number of carbon atoms.
        case iNumCarbon:
            return NumCarbon();

        //! Fragmentation flag.
        case iFrag:
            return Frag();

        // Collision rate properties:
        case iD2:
            return CollDiameter() * CollDiameter();
        case iD_1:
            return 1.0 / CollDiameter();
        case iD_2:
            return 1.0 / CollDiameter() / CollDiameter();
        case iM_1_2:
            return 1.0 / std::sqrt(Mass());
        case iD2_M_1_2:
            return CollDiameter() * CollDiameter() / std::sqrt(Mass());
        case iASN:
            return GetSites();
        case iSintRate:
            return GetSintRate();
        case iCoverage:
            return GetCoverageFraction();
        case iFS:
            throw std::logic_error("Free surface no longer supported (Particle::Property)");
            return 0.0;
        case -1:
            // Special case property, used to select particles
            // uniformly.
            return 1.0;
		case iAn_2_3_comp:
			return GetPhaseTerm();
        default:
            throw std::logic_error("Unrecognised property requested (Particle::Property)");
            return 0.0;
    }
}

// PARTICLE COMPOSITION.

// Returns the composition vector.
const fvector &Particle::Composition() const
{
    return m_primary->Composition();
}

// Returns the ith component value.  Returns 0.0 if i is invalid.
double Particle::Composition(unsigned int i) const
{
    if (i < Composition().size()) {
        return Composition()[i];
    } else {
        return 0.0;
    }
}

// TRACKER VARIABLE VALUES.

/*!
 * Returns the tracker value vector.
 */
const fvector &Particle::Values() const
{
    return m_primary->Values();
}

// Returns the ith tracker variable value.  Returns 0.0 if i is invalid.
double Particle::Values(unsigned int i) const
{
    if (i < Values().size()) {
        return Values()[i];
    } else {
        return 0.0;
    }
}

/*!
 * Both position and the associated time must be updated together.  This is
 * because it makes no sense to specify a position without knowing when it
 * applies.
 *
 *@param[in]    x       New position of particle
 *@param[in]    t       Time at which new position is correct
 */
void Particle::setPositionAndTime(const double x, const double t) {
    m_Position = x;
    m_PositionTime = t;
}

/*!
 * Set last LPDA update time; function name is historical (and confusing).
 *
 *@param[in]    t   Time to which LPDA has just been performed
 */
void Particle::SetTime(double t)
{
    m_primary->SetTime(t);
    mLPDAtime = t;
}

/*!
 *  Recalculates the derived properties from the unique properties.
 *  This function moves down the tree of subparticles from the top to the bottom.
 */
void Particle::UpdateCache(void)
{
    // Get cache from primary particle.
    m_primary->UpdateCache();

    m_createt = m_primary->CreateTime();
    mLPDAtime    = m_primary->LastUpdateTime();
}

/*!
 *@return   Number of coagulation events since counter was reset
 */
unsigned int Particle::getCoagCount() const {
    return m_CoagCount;
}

/*!
 *@return   Number of coagulation events since counter was reset
 */
unsigned int Particle::getFragCount() const {
    return m_FragCount;
}

// PARTICLE ADJUSTMENT AND PROCESSES.

/*!
 * Apply the given composition and values changes n times
 *
 *@param[in]        n       Number of times to apply changes
 *
 *@return       Number of times changes actually applied
 */
unsigned int Particle::Adjust(const fvector &dcomp,
                              const fvector &dvalues,
                              rng_type &rng,
                              unsigned int n)
{
    unsigned int m = n;

    // This is a leaf-node sub-particle as it contains a
    // primary particle.  The adjustment is applied to
    // the primary.
    m = m_primary->Adjust(dcomp, dvalues, rng, n);

    // Where-ever the adjustment has been applied this sub-particle must
    // now update its cache.
    UpdateCache();

    return m;
}

/*!
 * Apply the given composition and values changes n times
 * while allowing for the special nature of IntParticleReaction.
 *
 *@param[in]        n       Number of times to apply changes
 *
 *@return       Number of times changes actually applied
 */
unsigned int Particle::AdjustIntPar(const fvector &dcomp,
                                    const fvector &dvalues,
                                    rng_type &rng,
                                    unsigned int n)
{
    unsigned int m = n;

    // This is a leaf-node sub-particle as it contains a
    // primary particle.  The adjustment is applied to
    // the primary.
    m = m_primary->AdjustIntPar(dcomp, dvalues, rng, n);

    // Where-ever the adjustment has been applied this sub-particle must
    // now update its cache.
    UpdateCache();

    return m;
}

/*!
 * Apply the given composition and values changes n times
 * for the phase transformation
 *
 *@param[in]        n       Number of times to apply changes
 *
 *@return       Number of times changes actually applied
 */
unsigned int Particle::AdjustPhase(const fvector &dcomp,
                              const fvector &dvalues,
                              rng_type &rng,
                              unsigned int n)
{

	unsigned int m = n;

    // This is a leaf-node sub-particle as it contains a
    // primary particle.  The adjustment is applied to
    // the primary.
    n = m_primary->AdjustPhase(dcomp, dvalues, rng, n);

	// Adjust phase may return n < m if the selected primary does not contain enough components.
	// The loop tries to apply the adjustment to other primaries (when using the bintree model).
	int loops = 0;
	while (n<m && loops < 20){  //max number of loops set to 20
		n += m_primary->AdjustPhase(dcomp, dvalues, rng, m-n);
		loops ++;
	}

    // Where-ever the adjustment has been applied this sub-particle must
    // now update its cache.
    UpdateCache();

    return m;
}

// Phase transformation
void Particle::Melt(rng_type &rng, Cell &sys)
{

	// This is a leaf-node sub-particle as it contains a
	// primary particle.  The adjustment is applied to
	// the primary.
	m_primary->Melt(rng, sys);

	// Where-ever the adjustment has been applied this sub-particle must
	// now update its cache.
	UpdateCache();

	return;
}

/*!
 * Combines this particle with another.
 *
 * \param[in]       rhs         Particle to add to current instance
 * \param[in,out]   rng         Random number generator
 * \return      Reference to the current instance after rhs has been added
 */
Particle &Particle::Coagulate(const Particle &rhs, rng_type &rng)
{
    m_primary->Coagulate(*rhs.m_primary, rng);
    UpdateCache();

    return *this;
}

/*!
 * Combines this particle with another.
 *
 * \param[in]       rhs         Particle to add to current instance
 * \param[in,out]   rng         Random number generator
 * \return      Reference to the current instance after rhs has been added
 */
Particle &Particle::Fragment(const Particle &rhs, rng_type &rng)
{
    m_primary->Fragment(*rhs.m_primary, rng);
    UpdateCache();

    return *this;
}

/*!
 * Sinter the particle over the specified amount of time
 *
 *@param[in]        dt      Length of sintering time
 *@param[in]        sys     Cell containing the particle and providing details of the environment
 *@param[in]        model   Sintering model to define sintering rate
 *@param[in,out]    rng     Random number generator object
 *@param[in]        wt      Statistical weight of particle
 */
void Particle::Sinter(double dt, Cell &sys,
                      const Processes::SinteringModel &model,
                      rng_type &rng,
                      double wt)
{
    m_primary->Sinter(dt, sys, model, rng, wt);
}

// Creates a clone of the particle.
Particle *const Particle::Clone() const
{
    return new Particle(*this);
}

/*!
 * Perform checks on the internal data structure.  This is mainly for
 * testing and checking purposes; it should not be called from performance
 * critical sections.
 *
 * It was not quite clear in the old SubParticle code (now merged into this class)
 * to what extent this method should check the integrity
 * of the data structure and to what extent it should verify the physical
 * meaning.  I think the two purposes may have become a little mixed.
 * (riap2 24 Jan 2011)
 *
 * @return	true iff internal structures pass tests
 */
bool Particle::IsValid() const {
	return (m_primary != NULL) && (m_primary->IsValid()) && (m_StatWeight > 0);
}

void Particle::writeParticlePOVRAY(std::ofstream &out) const
{
    // First construct the image
    Sweep::Imaging::ParticleImage image;
    image.Construct(*(this), *(Primary()->ParticleModel()));

    // Write it to file
    image.WritePOVRAY(out);

    // Now we can delete the image
}

/*!
 * @brief Writes the object to a binary stream
 *
 * @param[in,out]	 out		         Output binary stream
 * @param[in,out]    duplicates          Addresses of PAHs that have already been serialised
 * @exception		 invalid_argument    Output stream not ready
 *
 * @pre              IsValid             returns true indicating (amongst other things) that the primary particle is initialised
 */
void Particle::Serialize(std::ostream &out, void *duplicates) const
{
	assert(IsValid());

    if (out.good()) {
        ModelFactory::WritePrimary(*m_primary, out, duplicates);

        // Output the data members in this class
        out.write((char*)&m_Position, sizeof(m_Position));
        out.write((char*)&m_PositionTime, sizeof(m_PositionTime));
        out.write((char*)&m_StatWeight, sizeof(m_StatWeight));
        out.write((char*)&m_CoagCount, sizeof(m_CoagCount));
        out.write((char*)&m_FragCount, sizeof(m_FragCount));
        out.write(reinterpret_cast<const char*>(&m_createt), sizeof(m_createt));
        out.write(reinterpret_cast<const char*>(&mLPDAtime), sizeof(mLPDAtime));
    }
    else {
        throw std::invalid_argument("Output stream not ready \
                                    (Sweep, Particle::Serialize).");
    }
}

//! Returns the frame position and orientation, and primary coordinates
//! Used by particle tracking for videos
void Particle::getFrameCoords(std::vector<fvector> &coords) const
{
	m_primary->GetFrameCoords(coords);
}

//! Initialise primary particle tracking for videos
void Particle::setTracking()
{
	m_primary->setTracking();
}

//! Remove primary tracking
void Particle::removeTracking()
{
	m_primary->removeTracking();
}


/*///////////////////////////////////////////////////////////////////////////////////////
//
// THIS IS A MODIFICATION OF OPENCV SOURCE CODE
//
//*/

/*///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's 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.
//
//   * The name of the copyright holders may not 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 Intel Corporation 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.
//
//*/

/*
 This is a variation of
 "Stereo Processing by Semiglobal Matching and Mutual Information"
 by Heiko Hirschmuller.

 This version was adapted from the OpenCV implementation to use
 a matching cost volume instead of two stereo rectified images.

 */

#include <limits.h>
#include "semiGlobalMatching.h"
#include <iostream>
#include <d3d_base/exception.h>
#include <opencv2/core/internal.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <Eigen/Dense>

using std::cout;
using std::endl;
using Eigen::Matrix;

using namespace D3D;

SemiGlobalMatching::SemiGlobalMatching()
{
    outputDepthMapEnabled = true;
    outputLabelsEnabled =true;
    outputUniquenessRatiosEnabled = false;
    outputCostsEnabled = false;
    subPixelEnabled = true;
//    minDisparity = numberOfDisparities = 0;
//    SADWindowSize = 0;
    P1 = 5; P2 = 8;
//    disp12MaxDiff = 0;
//    preFilterCap = 0;
//    uniquenessRatio = 0;
//    speckleWindowSize = 0;
//    speckleRange = 0;
    fullDP = true;
}

SemiGlobalMatching::SemiGlobalMatching(int _P1, int _P2, bool _fullDP )
{
    outputDepthMapEnabled = true;
    outputLabelsEnabled =true;
    outputUniquenessRatiosEnabled = false;
    outputCostsEnabled = false;
    subPixelEnabled = true;
    P1 = _P1;
    P2 = _P2;
    fullDP = _fullDP;
}

/*
  This is an adaptation to the original open cv code to enable the semi global matching to mach on
  a cost volume acquired by plane sweep. Thus the comments might make no sense sometimes and also too much
  memory is allocated. But the implementation seems to be still fast.

 computes disparity for "roi" in img1 w.r.t. img2 and write it to disp1buf.
 that is, disp1buf(x, y)=d means that img1(x+roi.x, y+roi.y) ~ img2(x+roi.x-d, y+roi.y).
 minD <= d < maxD.
 disp2full is the reverse disparity map, that is:
 disp2full(x+roi.x,y+roi.y)=d means that img2(x+roi.x, y+roi.y) ~ img1(x+roi.x+d, y+roi.y)

 note that disp1buf will have the same size as the roi and
 disp2full will have the same size as img1 (or img2).
 On exit disp2buf is not the final disparity, it is an intermediate result that becomes
 final after all the tiles are processed.

 the disparity in disp1buf is written with sub-pixel accuracy
 (4 fractional bits, see CvStereoSGBM::DISP_SCALE),
 using quadratic interpolation, while the disparity in disp2buf
 is written as is, without interpolation.

 disp2cost also has the same size as img1 (or img2).
 It contains the minimum current cost, used to find the best disparity, corresponding to the minimal cost.
 */
void SemiGlobalMatching::matchFromPlaneSweep(const Grid<float>& costVolume, const Grid<Eigen::Vector4d>& planes, const CameraMatrix<double>& cam, float costThresh)
{
    Matrix<double, 3, 3> refKinvT = Matrix<double, 3, 3>::Zero();
    if (outputDepthMapEnabled)
    {
        result = DepthMap<float, double>(costVolume.getWidth(), costVolume.getHeight(), cam);
        refKinvT  = cam.getK().inverse().transpose();
    }
//    for(int z=0; z<costVolume.getDepth(); ++z)
//    {
//        cout<<costVolume(847,247,z)<<" - ";
//    }
//    cout<<endl;
    if (outputUniquenessRatiosEnabled)
    {
        uniquenessRatios = Grid<float>(costVolume.getWidth(), costVolume.getHeight(), 1 ,1);
    }

    if (outputCostsEnabled)
    {
        costs = Grid<float>(costVolume.getWidth(), costVolume.getHeight(), 1, 0);
    }

    cv::Mat disp1(costVolume.getHeight(), costVolume.getWidth(), CV_16S );

#if CV_SSE2
    static const uchar LSBTab[] =
    {
        0, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
        5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
        6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
        5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
        7, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
        5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
        6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
        5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0, 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0
    };

    volatile bool useSIMD = cv::checkHardwareSupport(CV_CPU_SSE2);
#endif

    const int ALIGN = 16;
    const int DISP_SCALE = SemiGlobalMatching::DISP_SCALE;
    const CostType MAX_COST = SHRT_MAX;

    int minD = 0, maxD = costVolume.getDepth();
    int k, width = costVolume.getWidth(), height = costVolume.getHeight();
    int minX1 = 0, maxX1 = width;
    int D = maxD - minD, width1 = costVolume.getWidth();
    int INVALID_DISP = minD - 1, INVALID_DISP_SCALED = INVALID_DISP*DISP_SCALE;
    int npasses = fullDP ? 2 : 1;

    if( minX1 >= maxX1 )
    {
        disp1 = cv::Scalar::all(INVALID_DISP_SCALED);
        return;
    }

    if (D % 16 != 0)
    {
        D3D_THROW_EXCEPTION("Number of labels must be a multiple of 16 because of the alignment.")
    }

    // NR - the number of directions. the loop on x below that computes Lr assumes that NR == 8.
    // if you change NR, please, modify the loop as well.
    int D2 = D+16, NRD2 = NR2*D2;

    // the number of L_r(.,.) and min_k L_r(.,.) lines in the buffer:
    // for 8-way dynamic programming we need the current row and
    // the previous row, i.e. 2 rows in total
    const int NLR = 2;
    const int LrBorder = NLR - 1;

    // for each possible stereo match (img1(x,y) <=> img2(x-d,y))
    // we keep pixel difference cost (C) and the summary cost over NR directions (S).
    // we also keep all the partial costs for the previous line L_r(x,d) and also min_k L_r(x, k)
    size_t costBufSize = width1*D;
    size_t CSBufSize = costBufSize*(fullDP ? height : 1);
    size_t minLrSize = (width1 + LrBorder*2)*NR2, LrSize = minLrSize*D2;
    int SH2 = 0;
    int hsumBufNRows = SH2*2 + 2;
    size_t totalBufSize = (LrSize + minLrSize)*NLR*sizeof(CostType) + // minLr[] and Lr[]
            costBufSize*(hsumBufNRows + 1)*sizeof(CostType) +  // hsumBuf, pixdiff
            CSBufSize*2*sizeof(CostType) + 1024;// C, S
    if (totalBufSize > (unsigned int)std::numeric_limits<int>::max())
    {
        //std::cout << "totalBufSize = " << totalBufSize << std::endl;
        D3D_THROW_EXCEPTION("Image too big for SGM");
    }
    if( !buffer.data || !buffer.isContinuous() ||
            buffer.cols*buffer.rows*buffer.elemSize() < totalBufSize )
        buffer.create(1, (int)totalBufSize, CV_8U);

    // summary cost over different (nDirs) directions
    CostType* Cbuf = (CostType*)cv::alignPtr(buffer.data, ALIGN);
    CostType* Sbuf = Cbuf + CSBufSize;
    CostType* hsumBuf = Sbuf + CSBufSize;
    CostType* pixDiff = hsumBuf + costBufSize*hsumBufNRows;

    // add P2 to every C(x,y). it saves a few operations in the inner loops
    for( k = 0; k < width1*D; k++ )
        Cbuf[k] = (CostType)P2;

    for( int pass = 1; pass <= npasses; pass++ )
    {
        int x1, y1, x2, y2, dx, dy;

        if( pass == 1 )
        {
            y1 = 0; y2 = height; dy = 1;
            x1 = 0; x2 = width1; dx = 1;
        }
        else
        {
            y1 = height-1; y2 = -1; dy = -1;
            x1 = width1-1; x2 = -1; dx = -1;
        }

        CostType *Lr[NLR]={0}, *minLr[NLR]={0};

        for( k = 0; k < NLR; k++ )
        {
            // shift Lr[k] and minLr[k] pointers, because we allocated them with the borders,
            // and will occasionally use negative indices with the arrays
            // we need to shift Lr[k] pointers by 1, to give the space for d=-1.
            // however, then the alignment will be imperfect, i.e. bad for SSE,
            // thus we shift the pointers by 8 (8*sizeof(short) == 16 - ideal alignment)
            Lr[k] = pixDiff + costBufSize + LrSize*k + NRD2*LrBorder + 8;
            memset( Lr[k] - LrBorder*NRD2 - 8, 0, LrSize*sizeof(CostType) );
            minLr[k] = pixDiff + costBufSize + LrSize*NLR + minLrSize*k + NR2*2;
            memset( minLr[k] - LrBorder*NR2, 0, minLrSize*sizeof(CostType) );
        }

        for( int y = y1; y != y2; y += dy )
        {
            int x, d;
            DispType* disp1ptr = disp1.ptr<DispType>(y);
            CostType* C = Cbuf + (!fullDP ? 0 : y*costBufSize);
            CostType* S = Sbuf + (!fullDP ? 0 : y*costBufSize);

            if( pass == 1 ) // compute C on the first pass, and reuse it on the second pass, if any.
            {

                // costs are copied in from given grid
                for (int x = 0; x < width1; x++)
                {
                    for (int d = 0; d < D; d++)
                    {
                        // we assume that the given costs are in the range [0, 1]
                        // so we multiply them by some constant to get a reasonable
                        // resolution in the short values
                        C[x*D + d] = std::min(costThresh,costVolume(x,y,d))*10000;
                    }
                }

                // also, clear the S buffer
                for( k = 0; k < width1*D; k++ )
                    S[k] = 0;
            }

            // clear the left and the right borders
            memset( Lr[0] - NRD2*LrBorder - 8, 0, NRD2*LrBorder*sizeof(CostType) );
            memset( Lr[0] + width1*NRD2 - 8, 0, NRD2*LrBorder*sizeof(CostType) );
            memset( minLr[0] - NR2*LrBorder, 0, NR2*LrBorder*sizeof(CostType) );
            memset( minLr[0] + width1*NR2, 0, NR2*LrBorder*sizeof(CostType) );

            /*
             [formula 13 in the paper]
             compute L_r(p, d) = C(p, d) +
             min(L_r(p-r, d),
             L_r(p-r, d-1) + P1,
             L_r(p-r, d+1) + P1,
             min_k L_r(p-r, k) + P2) - min_k L_r(p-r, k)
             where p = (x,y), r is one of the directions.
             we process all the directions at once:
             0: r=(-dx, 0)
             1: r=(-1, -dy)
             2: r=(0, -dy)
             3: r=(1, -dy)
             4: r=(-2, -dy)
             5: r=(-1, -dy*2)
             6: r=(1, -dy*2)
             7: r=(2, -dy)
             */
            for( x = x1; x != x2; x += dx )
            {
                int xm = x*NR2, xd = xm*D2;

                int delta0 = minLr[0][xm - dx*NR2] + P2, delta1 = minLr[1][xm - NR2 + 1] + P2;
                int delta2 = minLr[1][xm + 2] + P2, delta3 = minLr[1][xm + NR2 + 3] + P2;

                CostType* Lr_p0 = Lr[0] + xd - dx*NRD2;
                CostType* Lr_p1 = Lr[1] + xd - NRD2 + D2;
                CostType* Lr_p2 = Lr[1] + xd + D2*2;
                CostType* Lr_p3 = Lr[1] + xd + NRD2 + D2*3;

                Lr_p0[-1] = Lr_p0[D] = Lr_p1[-1] = Lr_p1[D] =
                        Lr_p2[-1] = Lr_p2[D] = Lr_p3[-1] = Lr_p3[D] = MAX_COST;

                CostType* Lr_p = Lr[0] + xd;
                const CostType* Cp = C + x*D;
                //                cout << Cp[0] << " " << Cp[1] << endl;
                CostType* Sp = S + x*D;

#if CV_SSE2
                if( useSIMD )
                {
                    __m128i _P1 = _mm_set1_epi16((short)P1);

                    __m128i _delta0 = _mm_set1_epi16((short)delta0);
                    __m128i _delta1 = _mm_set1_epi16((short)delta1);
                    __m128i _delta2 = _mm_set1_epi16((short)delta2);
                    __m128i _delta3 = _mm_set1_epi16((short)delta3);
                    __m128i _minL0 = _mm_set1_epi16((short)MAX_COST);

                    for( d = 0; d < D; d += 8 )
                    {
                        __m128i Cpd = _mm_load_si128((const __m128i*)(Cp + d));
                        __m128i L0, L1, L2, L3;

                        L0 = _mm_load_si128((const __m128i*)(Lr_p0 + d));
                        L1 = _mm_load_si128((const __m128i*)(Lr_p1 + d));
                        L2 = _mm_load_si128((const __m128i*)(Lr_p2 + d));
                        L3 = _mm_load_si128((const __m128i*)(Lr_p3 + d));

                        L0 = _mm_min_epi16(L0, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p0 + d - 1)), _P1));
                        L0 = _mm_min_epi16(L0, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p0 + d + 1)), _P1));

                        L1 = _mm_min_epi16(L1, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p1 + d - 1)), _P1));
                        L1 = _mm_min_epi16(L1, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p1 + d + 1)), _P1));

                        L2 = _mm_min_epi16(L2, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p2 + d - 1)), _P1));
                        L2 = _mm_min_epi16(L2, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p2 + d + 1)), _P1));

                        L3 = _mm_min_epi16(L3, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p3 + d - 1)), _P1));
                        L3 = _mm_min_epi16(L3, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p3 + d + 1)), _P1));

                        L0 = _mm_min_epi16(L0, _delta0);
                        L0 = _mm_adds_epi16(_mm_subs_epi16(L0, _delta0), Cpd);

                        L1 = _mm_min_epi16(L1, _delta1);
                        L1 = _mm_adds_epi16(_mm_subs_epi16(L1, _delta1), Cpd);

                        L2 = _mm_min_epi16(L2, _delta2);
                        L2 = _mm_adds_epi16(_mm_subs_epi16(L2, _delta2), Cpd);

                        L3 = _mm_min_epi16(L3, _delta3);
                        L3 = _mm_adds_epi16(_mm_subs_epi16(L3, _delta3), Cpd);

                        _mm_store_si128( (__m128i*)(Lr_p + d), L0);
                        _mm_store_si128( (__m128i*)(Lr_p + d + D2), L1);
                        _mm_store_si128( (__m128i*)(Lr_p + d + D2*2), L2);
                        _mm_store_si128( (__m128i*)(Lr_p + d + D2*3), L3);

                        __m128i t0 = _mm_min_epi16(_mm_unpacklo_epi16(L0, L2), _mm_unpackhi_epi16(L0, L2));
                        __m128i t1 = _mm_min_epi16(_mm_unpacklo_epi16(L1, L3), _mm_unpackhi_epi16(L1, L3));
                        t0 = _mm_min_epi16(_mm_unpacklo_epi16(t0, t1), _mm_unpackhi_epi16(t0, t1));
                        _minL0 = _mm_min_epi16(_minL0, t0);

                        __m128i Sval = _mm_load_si128((const __m128i*)(Sp + d));

                        L0 = _mm_adds_epi16(L0, L1);
                        L2 = _mm_adds_epi16(L2, L3);
                        Sval = _mm_adds_epi16(Sval, L0);
                        Sval = _mm_adds_epi16(Sval, L2);

                        _mm_store_si128((__m128i*)(Sp + d), Sval);
                    }

                    _minL0 = _mm_min_epi16(_minL0, _mm_srli_si128(_minL0, 8));
                    _mm_storel_epi64((__m128i*)&minLr[0][xm], _minL0);
                }
                else
#endif
                {
                    int minL0 = MAX_COST, minL1 = MAX_COST, minL2 = MAX_COST, minL3 = MAX_COST;

                    for( d = 0; d < D; d++ )
                    {
                        int Cpd = Cp[d], L0, L1, L2, L3;

                        L0 = Cpd + std::min((int)Lr_p0[d], std::min(Lr_p0[d-1] + P1, std::min(Lr_p0[d+1] + P1, delta0))) - delta0;
                        L1 = Cpd + std::min((int)Lr_p1[d], std::min(Lr_p1[d-1] + P1, std::min(Lr_p1[d+1] + P1, delta1))) - delta1;
                        L2 = Cpd + std::min((int)Lr_p2[d], std::min(Lr_p2[d-1] + P1, std::min(Lr_p2[d+1] + P1, delta2))) - delta2;
                        L3 = Cpd + std::min((int)Lr_p3[d], std::min(Lr_p3[d-1] + P1, std::min(Lr_p3[d+1] + P1, delta3))) - delta3;

                        Lr_p[d] = (CostType)L0;
                        minL0 = std::min(minL0, L0);

                        Lr_p[d + D2] = (CostType)L1;
                        minL1 = std::min(minL1, L1);

                        Lr_p[d + D2*2] = (CostType)L2;
                        minL2 = std::min(minL2, L2);

                        Lr_p[d + D2*3] = (CostType)L3;
                        minL3 = std::min(minL3, L3);

                        Sp[d] = cv::saturate_cast<CostType>(Sp[d] + L0 + L1 + L2 + L3);
                    }
                    minLr[0][xm] = (CostType)minL0;
                    minLr[0][xm+1] = (CostType)minL1;
                    minLr[0][xm+2] = (CostType)minL2;
                    minLr[0][xm+3] = (CostType)minL3;
                }
            }

            if( pass == npasses )
            {
                for( x = 0; x < width; x++ )
                {
                    disp1ptr[x] = (DispType)INVALID_DISP_SCALED;
                }

                for( x = width1 - 1; x >= 0; x-- )
                {
                    CostType* Sp = S + x*D;
                    int minS = MAX_COST, bestDisp = -1;

                    if( npasses == 1 )
                    {
                        int xm = x*NR2, xd = xm*D2;

                        int minL0 = MAX_COST;
                        int delta0 = minLr[0][xm + NR2] + P2;
                        CostType* Lr_p0 = Lr[0] + xd + NRD2;
                        Lr_p0[-1] = Lr_p0[D] = MAX_COST;
                        CostType* Lr_p = Lr[0] + xd;

                        const CostType* Cp = C + x*D;

#if CV_SSE2
                        if( useSIMD )
                        {
                            __m128i _P1 = _mm_set1_epi16((short)P1);
                            __m128i _delta0 = _mm_set1_epi16((short)delta0);

                            __m128i _minL0 = _mm_set1_epi16((short)minL0);
                            __m128i _minS = _mm_set1_epi16(MAX_COST), _bestDisp = _mm_set1_epi16(-1);
                            __m128i _d8 = _mm_setr_epi16(0, 1, 2, 3, 4, 5, 6, 7), _8 = _mm_set1_epi16(8);

                            for( d = 0; d < D; d += 8 )
                            {
                                __m128i Cpd = _mm_load_si128((const __m128i*)(Cp + d)), L0;

                                L0 = _mm_load_si128((const __m128i*)(Lr_p0 + d));
                                L0 = _mm_min_epi16(L0, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p0 + d - 1)), _P1));
                                L0 = _mm_min_epi16(L0, _mm_adds_epi16(_mm_loadu_si128((const __m128i*)(Lr_p0 + d + 1)), _P1));
                                L0 = _mm_min_epi16(L0, _delta0);
                                L0 = _mm_adds_epi16(_mm_subs_epi16(L0, _delta0), Cpd);

                                _mm_store_si128((__m128i*)(Lr_p + d), L0);
                                _minL0 = _mm_min_epi16(_minL0, L0);
                                L0 = _mm_adds_epi16(L0, *(__m128i*)(Sp + d));
                                _mm_store_si128((__m128i*)(Sp + d), L0);

                                __m128i mask = _mm_cmpgt_epi16(_minS, L0);
                                _minS = _mm_min_epi16(_minS, L0);
                                _bestDisp = _mm_xor_si128(_bestDisp, _mm_and_si128(_mm_xor_si128(_bestDisp,_d8), mask));
                                _d8 = _mm_adds_epi16(_d8, _8);
                            }

                            short CV_DECL_ALIGNED(16) bestDispBuf[8];
                            _mm_store_si128((__m128i*)bestDispBuf, _bestDisp);

                            _minL0 = _mm_min_epi16(_minL0, _mm_srli_si128(_minL0, 8));
                            _minL0 = _mm_min_epi16(_minL0, _mm_srli_si128(_minL0, 4));
                            _minL0 = _mm_min_epi16(_minL0, _mm_srli_si128(_minL0, 2));

                            __m128i qS = _mm_min_epi16(_minS, _mm_srli_si128(_minS, 8));
                            qS = _mm_min_epi16(qS, _mm_srli_si128(qS, 4));
                            qS = _mm_min_epi16(qS, _mm_srli_si128(qS, 2));

                            minLr[0][xm] = (CostType)_mm_cvtsi128_si32(_minL0);
                            minS = (CostType)_mm_cvtsi128_si32(qS);

                            qS = _mm_shuffle_epi32(_mm_unpacklo_epi16(qS, qS), 0);
                            qS = _mm_cmpeq_epi16(_minS, qS);
                            int idx = _mm_movemask_epi8(_mm_packs_epi16(qS, qS)) & 255;

                            bestDisp = bestDispBuf[LSBTab[idx]];
                        }
                        else
#endif
                        {
                            for( d = 0; d < D; d++ )
                            {
                                int L0 = Cp[d] + std::min((int)Lr_p0[d], std::min(Lr_p0[d-1] + P1, std::min(Lr_p0[d+1] + P1, delta0))) - delta0;

                                Lr_p[d] = (CostType)L0;
                                minL0 = std::min(minL0, L0);

                                int Sval = Sp[d] = cv::saturate_cast<CostType>(Sp[d] + L0);
                                if( Sval < minS )
                                {
                                    minS = Sval;
                                    bestDisp = d;
                                }
                            }
                            minLr[0][xm] = (CostType)minL0;
                        }
                    }
                    else
                    {
                        for( d = 0; d < D; d++ )
                        {
                            int Sval = Sp[d];
                            if( Sval < minS )
                            {
                                minS = Sval;
                                bestDisp = d;
                            }
                        }
                    }

                    if (outputUniquenessRatiosEnabled)
                    {
                        for (int d = 0; d < D; d++)
                        {
                            if (costVolume(x,y,d) == 0)
                                uniquenessRatios(x,y) = 0;
                            else if (costVolume(x,y,bestDisp)/costVolume(x,y,d) < uniquenessRatios(x,y))
                                uniquenessRatios(x,y)  = costVolume(x,y,bestDisp)/costVolume(x,y,d);
                        }
                    }

                    if (outputCostsEnabled)
                    {
                        costs(x,y) = costVolume(x,y,bestDisp);
                    }

                    if (outputDepthMapEnabled)
                    {
                        // compute depth from plane id
                        double d = planes(bestDisp,0)(3);

                        //std::cout << "D = " << D << "  bestDisp = " << bestDisp << "  subPixelEnabled = " << subPixelEnabled << std::endl;

                        if (subPixelEnabled && bestDisp > 0 && bestDisp < D-1)
                        {
                            const double denom = Sp[bestDisp+1] + Sp[bestDisp-1] - 2*Sp[bestDisp];
                           // std::cout << "denom = " << denom << std::endl;
                            double offset = 0.0;
                            if (denom > 1e-2)
                            {
                                offset = (Sp[bestDisp-1] - Sp[bestDisp])/denom - 0.5f;
                            }

                            if (offset < 0)
                            {
                                const int oPlaneIdx = bestDisp -1;

                                float oPlaneD = planes(oPlaneIdx,0)(3);

                                float dStep = (1.0f/oPlaneD - 1.0f/d)*offset;
                                //std::cout << "dStep = " << dStep << std::endl;
                                d = 1.0f/(1.0f/d - dStep);

                            }
                            if (offset > 0)
                            {
                                const int oPlaneIdx = bestDisp + 1;

                                float oPlaneD = planes(oPlaneIdx,0)(3);

                                float dStep = (1.0f/d - 1.0f/oPlaneD)*offset;
                                //std::cout << "dStep = " << dStep << std::endl;
                                d = 1.0f/(1.0f/d - dStep);
                            }
                        }
                        Matrix<double, 3, 1> n = planes(bestDisp, 0).head(3);
                        Matrix<double, 3, 1> pos;
                        pos(0) = x; pos(1) = y; pos(2) = 1;

                        double denom = pos.transpose()*refKinvT*n;
                        result(x,y) = -d/denom;
                    }
                }
            }

            // now shift the cyclic buffers
            std::swap( Lr[0], Lr[1] );
            std::swap( minLr[0], minLr[1] );
        }
    }
}

void SemiGlobalMatching::setSmoothness(int P1, int P2)
{
    if (!(P2 > P1 && P1 > 0))
    {
        D3D_THROW_EXCEPTION("P2 must be bigger than P1 and both positive.");
    }

    this->P1 = P1;
    this->P2 = P2;
}

DepthMap<float, double> SemiGlobalMatching::getDepthMap()
{
    return result;
}

Grid<int> SemiGlobalMatching::getLabels()
{
    return labels;
}

void SemiGlobalMatching::enableOutputCostsEnabled(bool enabled)
{
    this->outputCostsEnabled = enabled;
}

void SemiGlobalMatching::enableOutputDepthMap(bool enabled)
{
    this->outputDepthMapEnabled = enabled;
}

void SemiGlobalMatching::enableOutputUniquenessRatios(bool enabled)
{
    this->outputUniquenessRatiosEnabled = enabled;
}

Grid<float> SemiGlobalMatching::getCosts()
{
    return costs;
}

Grid<float> SemiGlobalMatching::getUniquenessRatios()
{
    return uniquenessRatios;
}

void SemiGlobalMatching::enableOutputLabels(bool enabled)
{
    this->outputLabelsEnabled = enabled;
}

void SemiGlobalMatching::matchFromPlaneSweepWtihImage(const Grid<float> &costVolume, const Grid<Eigen::Vector4d> &planes, const CameraMatrix<double> &cam, cv::Mat image, float P1F, float P2F, float k)
{
    Matrix<double, 3, 3> refKinvT = Matrix<double, 3, 3>::Zero();
    if (outputDepthMapEnabled)
    {
        result = DepthMap<float, double>(costVolume.getWidth(), costVolume.getHeight(), cam);
        refKinvT  = cam.getK().inverse().transpose();
    }

    regularizeCostVolumeWithImage(costVolume, image, P1F, P2F, k);

    if (outputDepthMapEnabled)
    {
        for (unsigned int y = 0; y < costVolume.getHeight(); y++)
            for (unsigned int x = 0; x < costVolume.getWidth(); x++)
            {
                Matrix<double, 3, 1> n = planes(labels(x,y), 0).head(3);
                double d = planes(labels(x,y),0)(3);

                if (subPixelEnabled && labels(x,y) > 0 && labels(x,y) < (int)costVolume.getHeight()-1)
                {

                    if (offsets(x,y) < 0)
                    {
                        const int oPlaneIdx = labels(x,y) -1;

                        float oPlaneD = planes(oPlaneIdx,0)(3);

                        float dStep = (1.0f/oPlaneD - 1.0f/d)*offsets(x,y);
                        //std::cout << "dStep = " << dStep << std::endl;
                        d = 1.0f/(1.0f/d - dStep);

                    }
                    if (offsets(x,y) > 0)
                    {
                        const int oPlaneIdx = labels(x,y) + 1;

                        float oPlaneD = planes(oPlaneIdx,0)(3);

                        float dStep = (1.0f/d - 1.0f/oPlaneD)*offsets(x,y);
                        //std::cout << "dStep = " << dStep << std::endl;
                        d = 1.0f/(1.0f/d - dStep);
                    }
                }

                Matrix<double, 3, 1> pos;
                pos(0) = x; pos(1) = y; pos(2) = 1;

                double denom = pos.transpose()*refKinvT*n;
                result(x,y) = -d/denom;
            }
    }

}

// Non optimized but easy to change version of semi global matching
void SemiGlobalMatching::regularizeCostVolumeWithImage(const Grid<float>& costVolume, cv::Mat image, float P1F, float P2F, float k)
{
    D3D::Grid<float> summedCosts = D3D::Grid<float>(costVolume.getWidth(), costVolume.getHeight(), costVolume.getDepth());
    const int numPaths = 8;

    D3D::Grid<float> pathCosts = D3D::Grid<float>(costVolume.getWidth(), costVolume.getHeight(), costVolume.getDepth());

    int r1, r2;
    int xS, yS;
    int xR, yR;
    int xE, yE;

    for (int i = 0; i < numPaths; i++)
    {
        switch (i)
        {
        case 0:
            r1 = -1;
            r2 = 1;
            xS = costVolume.getWidth()-1;
            yS = 0;
            xR = -1;
            yR = 1;
            xE = -1;
            yE = costVolume.getHeight();
            break;
        case 1:
            r1 = 1;
            r2 = -1;
            xS = 0;
            yS = costVolume.getHeight()-1;
            xR = 1;
            yR = -1;
            xE = costVolume.getWidth();
            yE = -1;
            break;
        case 2:
            r1 = -1;
            r2 = -1;
            xS = costVolume.getWidth()-1;
            yS = costVolume.getHeight()-1;
            xR = -1;
            yR = -1;
            xE = -1;
            yE = -1;
            break;
        case 3:
            r1 = 1;
            r2 = 1;
            xS = 0;
            yS = 0;
            xR = 1;
            yR = 1;
            xE = costVolume.getWidth();
            yE = costVolume.getHeight();
            break;
        case 4:
            r1 = 0;
            r2 = -1;
            xS = 0;
            yS = costVolume.getHeight() - 1;
            xR = 1;
            yR = -1;
            xE = costVolume.getWidth();
            yE = -1;
            break;
        case 5:
            r1 = 0;
            r2 = 1;
            xS = 0;
            yS = 0;
            xR = 1;
            yR = 1;
            xE = costVolume.getWidth();
            yE = costVolume.getHeight();
            break;
        case 6:
            r1 = -1;
            r2 = 0;
            xS = costVolume.getWidth() - 1;
            yS = 0;
            xR = -1;
            yR = 1;
            xE = -1;
            yE = costVolume.getHeight();
            break;
        case 7:
            r1 = 1;
            r2 = 0;
            xS = 0;
            yS = 0;
            xR = 1;
            yR = 1;
            xE = costVolume.getWidth();
            yE = costVolume.getHeight();
            break;
        }

        for (int y = yS; y != yE; y += yR)
        {
            std::cout << y << std::endl;
            for (int x = xS; x != xE; x += xR)
            {
                // std::cout << xE << std::endl;
                // std::cout << x << std::endl;

                const int x0 = x - r1;
                const int y0 = y - r2;

                if (x0 < 0 || y0 < 0 || x0 >= (int) costVolume.getWidth() || y0 >= (int) costVolume.getHeight())
                {
                    for (int d = 0; d < (int) costVolume.getDepth(); d++)
                        pathCosts(x,y,d) = costVolume(x,y,d);
                }
                else
                {
                    // compute P2' based on the image gradient
//                    const float ib0 = (float) image.at<unsigned char>(y0,3*x0)/255.0f;
//                    const float ib1 = (float) image.at<unsigned char>(y,3*x)/255.0f;
//                    const float ig0 = (float) image.at<unsigned char>(y0,3*x0+1)/255.0f;
//                    const float ig1 = (float) image.at<unsigned char>(y,3*x+1)/255.0f;
//                    const float ir0 = (float) image.at<unsigned char>(y0,3*x0+2)/255.0f;
//                    const float ir1 = (float) image.at<unsigned char>(y,3*x+2)/255.0f;

//                    const float grad = std::fabs(ib0 - ib1) + std::fabs(ig0 - ig1) + std::fabs(ir0 - ir1);
                    const float P2Fp = P2F; //std::max(P1F, k*P2F*3/grad);

                    float lastMin = pathCosts(x0,y0,0);
                    for (int d = 1; d < (int) costVolume.getDepth(); d++)
                    {
                        if (pathCosts(x0,y0,d) < lastMin)
                            lastMin = pathCosts(x0,y0,d);
                    }

                    const float jumpCost = lastMin + P2Fp;

                    for (int d = 0; d < (int) costVolume.getDepth(); d++)
                    {
                        float dispMinCost = std::min(jumpCost, pathCosts(x0,y0,d));
                        if (d-1 >= 0)
                        {
                            const float m1Cost = pathCosts(x0,y0,d-1) + P1F;
                            if (m1Cost < dispMinCost)
                                dispMinCost = m1Cost;
                        }
                        if (d+1 < (int) costVolume.getDepth())
                        {
                            const float p1Cost = pathCosts(x0,y0,d+1) + P1F;
                            if (p1Cost < dispMinCost)
                                dispMinCost = p1Cost;
                        }

                        pathCosts(x,y,d) = costVolume(x,y,d) + dispMinCost - lastMin;
                    }
                }
            }
        }

        for (int y = 0; y < (int) costVolume.getHeight(); y++)
            for (int x = 0; x < (int) costVolume.getWidth(); x++)
                for (int d = 0; d < (int) costVolume.getDepth(); d++)
                {
                    if (i == 0)
                    {
                        summedCosts(x,y,d) = pathCosts(x,y,d)/(float)numPaths;
                    }
                    else
                    {
                        summedCosts(x,y,d) += pathCosts(x,y,d)/(float)numPaths;
                    }
                }
    }

    pathCosts.resize(1,1,1);

    labels.resize(costVolume.getWidth(), costVolume.getHeight(), 1);

    if (subPixelEnabled)
    {
        offsets.resize(costVolume.getWidth(), costVolume.getHeight(), 1);
    }

    for (int y = 0; y < (int) costVolume.getHeight(); y++)
        for (int x = 0; x < (int) costVolume.getWidth(); x++)
        {
            int bestLabel = 0;
            float bestCost = summedCosts(x,y,0);

            for (int d = 1; d < (int) costVolume.getDepth(); d++)
            {
                if (summedCosts(x,y,d) < bestCost)
                {
                    bestLabel = d;
                    bestCost = summedCosts(x,y,d);
                }
            }

            if (subPixelEnabled)
            {
                // if subpixel is enabled we need to compute the label offsets
                offsets(x,y) = 0;

                if (bestLabel > 0 && bestLabel < (int)costVolume.getDepth()-1)
                {
                    const float denom = summedCosts(x,y,bestLabel+1) + summedCosts(x,y,bestLabel-1) - 2*summedCosts(x,y,bestLabel);
                    // std::cout << "denom = " << denom << std::endl;
                    if (denom > 1e-2)
                    {
                        offsets(x,y) = (summedCosts(x,y,bestLabel-1) - summedCosts(x,y,bestLabel))/denom - 0.5f;
                    }
                }

            }

            labels(x,y) = bestLabel;
        }
}