// ======================================================================
/*!
 * \file NFmiAngle.cpp
 * \brief Määrittelyt luokkien NFmiAngle, NFmiWindDirection, NFmiLatitude
 *        ja NFmiLongitude metodeille.
 */
// ======================================================================
/*!
 * \class NFmiAngle
 *
 * Määrittely yleiselle kulmasuureelle. Luokan keskeinen ominaisuus on, että
 * luokan metodit huolehtivat automaattisesti, että kulma pysyy sallituissa
 * rajoissa jokaisen operaation jälkeen.
 */
// ======================================================================

#include "NFmiAngle.h"
#include <boost/functional/hash.hpp>
#include <macgyver/Exception.h>
#include <iostream>

using namespace std;

// ======================================================================
/*!
 * Konstruktori luokalle NFmiAngle asettaa sisäiset arvot käyttäen SetValue
 * metodia, jotta mahdollinen kulmamuunnos tulee suoritetuksi, ja kulman
 * rajat tarkistetuiksi.
 *
 * \param theValue Undocumented
 * \param theUnit Undocumented
 */
// ======================================================================

NFmiAngle::NFmiAngle(double theValue, FmiAngleUnit theUnit)
    : itsValue(0)  // dummy to prevent advanced warnings
{
  try
  {
    SetValue(theValue, theUnit);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Muutosmetodi yleiselle kulmalle. Asetettu arvo muutetaan asteiksi,
 * jos annettu yksikkö on radiaani. Asteille suoritetaan ainoastaan
 * negatiivisuus tarkistus, minkä voi olettaa bugiksi. Jostain syystä
 * korrekti koodi on vaan kommentoitu pois. Ei pitäisi luottaa
 * käyttäjään näin paljon.
 *
 * \bug Ei varmista kunnolla, että kulma on sallituissa rajoissa.
 *
 * \param theValue Undocumented
 * \param theUnit Undocumented
 */
// ======================================================================

void NFmiAngle::SetValue(double theValue, FmiAngleUnit theUnit)
{
  try
  {
    if (theUnit == kRad)
    {
      itsValue = 180 / kPii * theValue;
      if (itsValue != 0 && fmod(itsValue, 360) == 0)
        itsValue = 360;
      else
        itsValue = fmod(itsValue, 360);
    }
    else
      itsValue = theValue;  // fmod(theValue, 360);

    if (itsValue < 0)
      itsValue = itsValue + 360;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Suurempi-kuin vertailuoperaattori kulmille. Tämä on periaatteessa
 * väärin, standardi komposiitit vaativat nimenomaan < ja == operaattorien
 * määrittelyn. Eli, nyt kulmia ei saa esim. laitettua listaan.
 *
 * \param theAngle Undocumented
 * \return Undocumented
 */
// ======================================================================

bool NFmiAngle::operator>(const NFmiAngle& theAngle) const
{
  try
  {
    return (itsValue > theAngle.itsValue);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Lisäysoperaattori.
 *
 * \bug Toteutus vaikuttaa tyhmältä. Miksei käytetä SetValue metodia,
 * vaan luodaan turha temppi?
 *
 * \param theAngle Undocumented
 * \return Undocumented
 */
// ======================================================================

NFmiAngle& NFmiAngle::operator+=(const NFmiAngle& theAngle)
{
  try
  {
    itsValue = NFmiAngle(itsValue + theAngle.itsValue).itsValue;
    return *this;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Vähennysoperaattori.
 *
 * \bug Toteutus vaikuttaa tyhmältä. Miksei käytetä SetValue metodia,
 * vaan luodaan turha temppi?
 *
 * \param theAngle Undocumented
 * \return Undocumented
 */
// ======================================================================

NFmiAngle& NFmiAngle::operator-=(const NFmiAngle& theAngle)
{
  try
  {
    itsValue = NFmiAngle(itsValue - theAngle.itsValue).itsValue;
    return *this;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Konstruktori luokalle NFmiWindDirection asettaa sisäiset arvot käyttäen
 * NFmiAngle luokan SetValue metodia, jotta mahdollinen kulmamuunnos tulee
 * suoritetuksi, ja kulman rajat tarkistetuiksi.
 *
 * \param theValue Undocumented
 * \param theUnit Undocumented
 */
// ======================================================================

NFmiWindDirection::NFmiWindDirection(double theValue, FmiAngleUnit theUnit)
    : NFmiAngle(theValue, theUnit)
{
}

// ======================================================================
/*!
 * Spesiaalikonstruktori luokalle NFmiWindDirection asettaa sisäiset arvot
 * perustuen tuulen U- ja V-komponentteihin. Jos kumpi tahansa komponentti
 * on kFloatMissing, asetetaan myös kulma puuttuvaksi.
 *
 * \param theU Undocumented
 * \param theV Undocumented
 */
// ======================================================================

NFmiWindDirection::NFmiWindDirection(double theU, double theV) : NFmiAngle()
{
  try
  {
    if (theU == kFloatMissing || theV == kFloatMissing)
    {
      itsValue = kFloatMissing;
      return;
    }

    if (theU == 0 && theV == 0)
    {
      SetValue(0, kDeg);
    }
    else
    {
      SetValue(atan2(theU, theV) + kPii, kRad);
    }
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Vähennysoperaattori.
 *
 * \bug Siis, jos suunta on operaation jälkeen > 180, esim. 200, niin
 * 360-200 = 160, uusi suunta. Tämähän on peilausta eikä moduloa.
 * Mikä hiton logiikka tässä on? Onkos näissä luokissa kaikki vialla?
 * Itse toteutuskin on omituinen temppiluokkineen. Koko perintä
 * on hieman omituista, kaikki virtuaalimetodit puuttuvat.
 *
 * \param theAngle Undocumented
 * \return Undocumented
 */
// ======================================================================

NFmiWindDirection& NFmiWindDirection::operator-=(const NFmiWindDirection& theAngle)
{
  try
  {
    itsValue = NFmiAngle(NFmiAngle::operator-=(theAngle)).Value();
    if (itsValue > 180)
      itsValue = 360 - itsValue;

    return *this;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Konstruktori luokalle NFmiLatitude asettaa sisäiset arvot käyttäen
 * NFmiLatitude luokan SetValue metodia, jotta mahdollinen kulmamuunnos tulee
 * suoritetuksi, ja kulman rajat tarkistetuiksi.
 *
 * \param theAngle Undocumented
 * \param theUnit Undocumented
 */
// ======================================================================

NFmiLatitude::NFmiLatitude(double theAngle, FmiAngleUnit theUnit)
{
  try
  {
    SetValue(theAngle, theUnit);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Asetusmetodi latitudille. Metodi varmistaa, että annettu arvo
 * rajoitetaan välille -90 <= latitudi <= 90.
 *
 * \bug Annettu kulmayksikkö jätetään täysin huomioimatta, selvästikin
 * oletetaan, että yksikkö on asteina. Lisäksi oletettujen
 * pyöristysvirheiden korjailu on aina riskaabelia. Lisäksi SetValue
 * kutsuu joskus itseään uudelleen, mikä on vähän ihmeellistä, kun
 * arvo kerran tunnetaan.
 *
 * \param theAngle Undocumented
 * \param theUnit Undocumented
 */
// ======================================================================

void NFmiLatitude::SetValue(double theAngle, FmiAngleUnit theUnit)
{
  try
  {
    if (fabs(theAngle - 90) < 0.0000001)
      itsValue = 90;
    else if (fabs(theAngle + 90) < 0.0000001)
      itsValue = -90;
    else if (theAngle > 90)
      SetValue(90, theUnit);
    else if (theAngle < -90)
      SetValue(-90, theUnit);
    else
      itsValue = theAngle;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Vähennysoperaattori.
 *
 * \bug Ja taas luodaan turha temppi SetValue metodin kutsumisen sijaan.
 *
 * \param theAngle Undocumented
 * \return Undocumented
 */
// ======================================================================

NFmiLatitude& NFmiLatitude::operator-=(const NFmiLatitude& theAngle)
{
  try
  {
    itsValue = NFmiLatitude(itsValue - theAngle.itsValue).itsValue;
    return *this;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Lisäysoperaattori
 *
 * \bug Ja taas luodaan turha temppi SetValue metodin kutsumisen sijaan.
 *
 * \param theAngle Undocumented
 * \return Undocumented
 */
// ======================================================================

NFmiLatitude& NFmiLatitude::operator+=(const NFmiLatitude& theAngle)
{
  try
  {
    itsValue = NFmiLatitude(itsValue + theAngle.itsValue).itsValue;
    return *this;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Vähennysoperaattori annetulla kulmalla.
 *
 * \bug Ja taas luodaan turha temppi SetValue metodin kutsumisen sijaan.
 *
 * \param theAngle Undocumented
 * \return Undocumented
 */
// ======================================================================

NFmiLatitude& NFmiLatitude::operator-=(const double& theAngle)
{
  try
  {
    itsValue = NFmiLatitude(itsValue - theAngle).itsValue;
    return *this;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Lisäysoperaattori
 *
 * \bug Ja taas luodaan turha temppi SetValue metodin kutsumisen sijaan.
 *
 * \param theAngle Undocumented
 * \return Undocumented
 */
// ======================================================================

NFmiLatitude& NFmiLatitude::operator+=(const double& theAngle)
{
  try
  {
    itsValue = NFmiLatitude(itsValue + theAngle).itsValue;
    return *this;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Konstruktori luokalle NFmiLongitude asettaa sisäiset arvot käyttäen
 * NFmiLongitude luokan SetValue metodia, jotta mahdollinen kulmamuunnos tulee
 * suoritetuksi, ja kulman rajat tarkistetuiksi.
 *
 * \param theAngle Undocumented
 * \param theUnit Undocumented
 */
// ======================================================================

NFmiLongitude::NFmiLongitude(double theAngle, bool usePacificView, FmiAngleUnit theUnit)
    : fPacificView(usePacificView)
{
  try
  {
    SetValue(theAngle, theUnit);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Asetusmetodi longitudille. Metodi varmistaa, että annettu arvo
 * rajoitetaan välille -180 <= latitudi <= 180.
 *
 * \bug Annettu kulmayksikkö jätetään täysin huomioimatta, selvästikin
 * oletetaan, että yksikkö on asteina. Lisäksi käytetään rekursiota
 * rajaehtojen toteuttamiseksi, kun pitäisi käyttää oikeata mod
 * käskyä. Muuten käsky voi kestää tarpeettoman kauan. Mitä jos
 * joku antaa arvon 100000? Monta iteraatiota menee...?
 *
 * \param theAngle Undocumented
 * \param theUnit Undocumented
 */
// ======================================================================

void NFmiLongitude::SetValue(double theAngle, FmiAngleUnit theUnit)
{
  try
  {
    if (fPacificView)
    {
      if (::fabs(theAngle - 360.) < 0.0000001)
        itsValue = 360.;
      else if (::fabs(theAngle + 360.) < 0.0000001)
        itsValue = 0.;
      else if (theAngle > 360)
        SetValue(theAngle - 360, theUnit);
      else if (theAngle < 0)
        SetValue(theAngle + 360, theUnit);
      else
        itsValue = theAngle;
    }
    else
    {
      if (::fabs(theAngle - 180.) < 0.0000001)
        itsValue = 180.;
      else if (::fabs(theAngle + 180.) < 0.0000001)
        itsValue = -180.;
      else if (theAngle > 180)
        SetValue(theAngle - 360, theUnit);
      else if (theAngle < -180)
        SetValue(theAngle + 360, theUnit);
      else
        itsValue = theAngle;
    }
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Vähennysoperaattori.
 *
 * \bug Ja taas luodaan turha temppi SetValue metodin kutsumisen sijaan.
 *
 * \param theAngle Undocumented
 * \return Undocumented
 */
// ======================================================================

NFmiLongitude& NFmiLongitude::operator-=(const NFmiLongitude& theAngle)
{
  try
  {
    itsValue = NFmiLongitude(itsValue - theAngle.itsValue, fPacificView).itsValue;
    return *this;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Lisäysoperaattori
 *
 * \bug Ja taas luodaan turha temppi SetValue metodin kutsumisen sijaan.
 *
 * \param theAngle Undocumented
 * \return Undocumented
 */
// ======================================================================

NFmiLongitude& NFmiLongitude::operator+=(const NFmiLongitude& theAngle)
{
  try
  {
    itsValue = NFmiLongitude(itsValue + theAngle.itsValue, fPacificView).itsValue;
    return *this;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Vähennysoperaattori annetulla kulmalla.
 *
 * \bug Ja taas luodaan turha temppi SetValue metodin kutsumisen sijaan.
 *
 * \param theAngle Undocumented
 * \return Undocumented
 */
// ======================================================================

NFmiLongitude& NFmiLongitude::operator-=(const double& theAngle)
{
  try
  {
    itsValue = NFmiLongitude(itsValue - theAngle, fPacificView).itsValue;
    return *this;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * Lisäysoperaattori annetulla kulmalla.
 *
 * \bug Ja taas luodaan turha temppi SetValue metodin kutsumisen sijaan.
 *
 * \param theAngle Undocumented
 * \return Undocumented
 */
// ======================================================================

NFmiLongitude& NFmiLongitude::operator+=(const double& theAngle)
{
  try
  {
    itsValue = NFmiLongitude(itsValue + theAngle, fPacificView).itsValue;
    return *this;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================
/*!
 * \brief Hash value
 */
// ======================================================================

std::size_t NFmiAngle::HashValue() const
{
  try
  {
    return boost::hash_value(itsValue);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================


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

#include <boost/simd/toolbox/bitwise/functions/clz.hpp>
#include <boost/simd/include/functions/scalar/ffs.hpp>
#include <boost/simd/include/functions/scalar/bitwise_cast.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/assert.hpp>

#ifdef BOOST_MSVC
#include <intrin.h>
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::clz_, tag::cpu_, (A0)
                            , (scalar_< type64_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
      {
        result_type t1 = bitwise_cast<result_type>(a0);
        BOOST_ASSERT_MSG( t1, "clz not defined for 0" );

      #ifdef __GNUC__
        return __builtin_clzll(t1);
      #else
        return boost::simd::ffs(reversebits(t1))-1;
      #endif
      }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::clz_, tag::cpu_
                            , (A0)
                            , (scalar_< type32_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type t1 = bitwise_cast<result_type>(a0);
      BOOST_ASSERT_MSG( t1, "clz not defined for 0" );

    #ifdef __GNUC__
      return __builtin_clz(t1);
    #else
      return boost::simd::ffs(reversebits(t1))-1;
     #endif
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::clz_, tag::cpu_
                            , (A0)
                            , (scalar_< type16_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type t1 = bitwise_cast<result_type>(a0);
      return boost::simd::clz(uint32_t(t1))-16;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::clz_, tag::cpu_ , (A0)
                            , (scalar_< type8_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type t1 = bitwise_cast<result_type>(a0);
      return boost::simd::clz(uint32_t(t1))-24;
    }
  };
} } }

#endif


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

#include <boost/simd/bitwise/functions/bitwise_select.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::bitwise_select_, boost::simd::tag::altivec_
             , (A0)(A1)
             , ((simd_< arithmetic_<A0>, boost::simd::tag::altivec_ >))
               ((simd_< arithmetic_<A1>, boost::simd::tag::altivec_ >))
               ((simd_< arithmetic_<A1>, boost::simd::tag::altivec_ >))
             )
  {
    typedef A1 result_type;
    inline result_type operator()(const A0& a0, const A1& a1, const A1&a2) const
    {
      typedef typename dispatch::meta::as_integer<A0, unsigned>::type itype;
      return vec_sel(a2(), a1(), bitwise_cast<itype>(a0)());
    }
  };

} } }

#endif

#endif


#include <boost/graph/distributed/shuffled_distribution.hpp>


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NETKET_RPROP_HPP
#define NETKET_RPROP_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <iostream>
#include "abstract_stepper.hpp"

namespace netket {

class Rprop {
  // decay constant
  double etap_;
  double etam_;

  int npar_;

  Eigen::VectorXd oldgrad_;
  Eigen::VectorXd delta_;

  double deltamin_;
  double deltamax_;
  double delta0_;

 public:
  Rprop(double etam, double etap, double delta0, double deltamin,
        double deltamax)
      : etap_(etap),
        etam_(etam),
        deltamin_(deltamin),
        deltamax_(deltamax),
        delta0_(delta0) {
    npar_ = -1;
  }

  void SetNpar(int npar) {
    npar_ = npar;

    oldgrad_.resize(npar_);
    oldgrad_ = Eigen::VectorXd::Ones(npar_);
    delta_.resize(npar_);
    delta_ = delta0_ * Eigen::VectorXd::Ones(npar_);
  }

  void Update(const Eigen::VectorXd &grad, Eigen::VectorXd &pars) {
    assert(npar_ > 0);
    double normgrad = 1. / std::sqrt(grad.norm());

    for (int i = 0; i < npar_; i++) {
      if (grad(i) * oldgrad_(i) > 0) {
        delta_(i) = std::min(deltamax_, delta_(i) * etap_);
        oldgrad_(i) = grad(i);
      } else if (grad(i) * oldgrad_(i) < 0) {
        delta_(i) = std::max(deltamin_, delta_(i) * etam_);
        oldgrad_(i) = 0;
      } else {
        oldgrad_(i) = grad(i);
      }
      if (grad(i) > 0) {
        pars(i) -= std::min(delta_(i), normgrad);
      } else if (grad(i) < 0) {
        pars(i) += std::min(delta_(i), normgrad);
      }
    }
    std::cerr << delta_.mean() << std::endl;
  }

  void Reset() {}
};

}  // namespace netket

#endif


/****
   Copyright 2005-2007, Moshe Looks

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
****/

#include "hash_util.h"
#include "math_util.h"
#include <iostream>
#include <boost/bind.hpp>

using namespace __gnu_cxx;
using namespace std;
using namespace boost;

template<typename T>
void foo(T n) {
  cout << bind(operator+=,ref(n),_1)(42) << endl;
}

int main() {
  random_access_hash_map<int,int> ma;
  for (int i=0;i<1000;++i)
    ma.insert(make_pair(i,i));
  for (int i=1000;i<2000;++i)
    ma.replace(ma.find(i-1000),make_pair(i,-i));
  for (int i=0;i<1000;++i)
    assert(ma.find(i)==ma.end());
  for (int i=1000;i<2000;++i) {
    assert(ma.find(i)!=ma.end());
    assert(ma.find(i)->second==-i);
  }

  foo(100);
}


#include "storm/generator/PrismNextStateGenerator.h"

#include <boost/container/flat_map.hpp>
#include <boost/any.hpp>

#include "storm/models/sparse/StateLabeling.h"

#include "storm/storage/expressions/SimpleValuation.h"
#include "storm/storage/sparse/PrismChoiceOrigins.h"

#include "storm/builder/jit/Distribution.h"

#include "storm/solver/SmtSolver.h"

#include "storm/utility/constants.h"
#include "storm/utility/macros.h"
#include "storm/exceptions/InvalidArgumentException.h"
#include "storm/exceptions/WrongFormatException.h"
#include "storm/exceptions/UnexpectedException.h"

namespace storm {
    namespace generator {

        template<typename ValueType, typename StateType>
        PrismNextStateGenerator<ValueType, StateType>::PrismNextStateGenerator(storm::prism::Program const& program, NextStateGeneratorOptions const& options) : PrismNextStateGenerator<ValueType, StateType>(program.substituteConstantsFormulas(), options, false) {
            // Intentionally left empty.
        }

        template<typename ValueType, typename StateType>
        PrismNextStateGenerator<ValueType, StateType>::PrismNextStateGenerator(storm::prism::Program const& program, NextStateGeneratorOptions const& options, bool) : NextStateGenerator<ValueType, StateType>(program.getManager(), options), program(program), rewardModels(), hasStateActionRewards(false) {
            STORM_LOG_TRACE("Creating next-state generator for PRISM program: " << program);
            STORM_LOG_THROW(!this->program.specifiesSystemComposition(), storm::exceptions::WrongFormatException, "The explicit next-state generator currently does not support custom system compositions.");

            // Only after checking validity of the program, we initialize the variable information.
            this->checkValid();
            this->variableInformation = VariableInformation(program, options.isAddOutOfBoundsStateSet());

            // Create a proper evalator.
            this->evaluator = std::make_unique<storm::expressions::ExpressionEvaluator<ValueType>>(program.getManager());

            if (this->options.isBuildAllRewardModelsSet()) {
                for (auto const& rewardModel : this->program.getRewardModels()) {
                    rewardModels.push_back(rewardModel);
                }
            } else {
                // Extract the reward models from the program based on the names we were given.
                for (auto const& rewardModelName : this->options.getRewardModelNames()) {
                    if (this->program.hasRewardModel(rewardModelName)) {
                        rewardModels.push_back(this->program.getRewardModel(rewardModelName));
                    } else {
                        STORM_LOG_THROW(rewardModelName.empty(), storm::exceptions::InvalidArgumentException, "Cannot build unknown reward model '" << rewardModelName << "'.");
                        STORM_LOG_THROW(this->program.getNumberOfRewardModels() == 1, storm::exceptions::InvalidArgumentException, "Reference to standard reward model is ambiguous.");
                    }
                }

                // If no reward model was yet added, but there was one that was given in the options, we try to build the
                // standard reward model.
                if (rewardModels.empty() && !this->options.getRewardModelNames().empty()) {
                    rewardModels.push_back(this->program.getRewardModel(0));
                }
            }

            // Determine whether any reward model has state action rewards.
            for (auto const& rewardModel : rewardModels) {
                hasStateActionRewards |= rewardModel.get().hasStateActionRewards();
            }

            // If there are terminal states we need to handle, we now need to translate all labels to expressions.
            if (this->options.hasTerminalStates()) {
                for (auto const& expressionOrLabelAndBool : this->options.getTerminalStates()) {
                    if (expressionOrLabelAndBool.first.isExpression()) {
                        this->terminalStates.push_back(std::make_pair(expressionOrLabelAndBool.first.getExpression(), expressionOrLabelAndBool.second));
                    } else {
                        if (program.hasLabel(expressionOrLabelAndBool.first.getLabel())) {
                            this->terminalStates.push_back(std::make_pair(this->program.getLabelExpression(expressionOrLabelAndBool.first.getLabel()), expressionOrLabelAndBool.second));
                        } else {
                            // If the label is not present in the program and is not a special one, we raise an error.
                            STORM_LOG_THROW(expressionOrLabelAndBool.first.getLabel() == "init" || expressionOrLabelAndBool.first.getLabel() == "deadlock", storm::exceptions::InvalidArgumentException, "Terminal states refer to illegal label '" << expressionOrLabelAndBool.first.getLabel() << "'.");
                        }
                    }
                }
            }
        }

        template<typename ValueType, typename StateType>
        bool PrismNextStateGenerator<ValueType, StateType>::canHandle(storm::prism::Program const& program) {
            // We can handle all valid prism programs (except for PTAs)
            return program.getModelType() != storm::prism::Program::ModelType::PTA;
        }

        template<typename ValueType, typename StateType>
        void PrismNextStateGenerator<ValueType, StateType>::checkValid() const {
            // If the program still contains undefined constants and we are not in a parametric setting, assemble an appropriate error message.
#ifdef STORM_HAVE_CARL
            if (!std::is_same<ValueType, storm::RationalFunction>::value && program.hasUndefinedConstants()) {
#else
            if (program.hasUndefinedConstants()) {
#endif
                std::vector<std::reference_wrapper<storm::prism::Constant const>> undefinedConstants = program.getUndefinedConstants();
                std::stringstream stream;
                bool printComma = false;
                for (auto const& constant : undefinedConstants) {
                    if (printComma) {
                        stream << ", ";
                    } else {
                        printComma = true;
                    }
                    stream << constant.get().getName() << " (" << constant.get().getType() << ")";
                }
                stream << ".";
                STORM_LOG_THROW(false, storm::exceptions::InvalidArgumentException, "Program still contains these undefined constants: " + stream.str());
            }

#ifdef STORM_HAVE_CARL
            else if (std::is_same<ValueType, storm::RationalFunction>::value && !program.undefinedConstantsAreGraphPreserving()) {
                auto undef = program.getUndefinedConstantsAsString();
                STORM_LOG_THROW(false, storm::exceptions::InvalidArgumentException, "The program contains undefined constants that appear in some places other than update probabilities and reward value expressions, which is not admitted. Undefined constants are: " << undef);
            }
#endif
        }

        template<typename ValueType, typename StateType>
        ModelType PrismNextStateGenerator<ValueType, StateType>::getModelType() const {
            switch (program.getModelType()) {
                case storm::prism::Program::ModelType::DTMC: return ModelType::DTMC;
                case storm::prism::Program::ModelType::CTMC: return ModelType::CTMC;
                case storm::prism::Program::ModelType::MDP: return ModelType::MDP;
                case storm::prism::Program::ModelType::MA: return ModelType::MA;
                case storm::prism::Program::ModelType::POMDP: return ModelType::POMDP;
                default:
                    STORM_LOG_THROW(false, storm::exceptions::WrongFormatException, "Invalid model type.");
            }
        }

        template<typename ValueType, typename StateType>
        bool PrismNextStateGenerator<ValueType, StateType>::isDeterministicModel() const {
            return program.isDeterministicModel();
        }

        template<typename ValueType, typename StateType>
        bool PrismNextStateGenerator<ValueType, StateType>::isDiscreteTimeModel() const {
            return program.isDiscreteTimeModel();
        }

        template<typename ValueType,typename StateType>
        bool PrismNextStateGenerator<ValueType, StateType>::isPartiallyObservable() const {
            return program.isPartiallyObservable();
        }

        template<typename ValueType, typename StateType>
        std::vector<StateType> PrismNextStateGenerator<ValueType, StateType>::getInitialStates(StateToIdCallback const& stateToIdCallback) {
            std::vector<StateType> initialStateIndices;

            // If all states are initial, we can simplify the enumeration substantially.
            if (program.hasInitialConstruct() && program.getInitialConstruct().getInitialStatesExpression().isTrue()) {
                CompressedState initialState(this->variableInformation.getTotalBitOffset(true));

                std::vector<int_fast64_t> currentIntegerValues;
                currentIntegerValues.reserve(this->variableInformation.integerVariables.size());
                for (auto const& variable : this->variableInformation.integerVariables) {
                    STORM_LOG_THROW(variable.lowerBound <= variable.upperBound, storm::exceptions::InvalidArgumentException, "Expecting variable with non-empty set of possible values.");
                    currentIntegerValues.emplace_back(0);
                    initialState.setFromInt(variable.bitOffset, variable.bitWidth, 0);
                }

                initialStateIndices.emplace_back(stateToIdCallback(initialState));

                bool done = false;
                while (!done) {
                    bool changedBooleanVariable = false;
                    for (auto const& booleanVariable : this->variableInformation.booleanVariables) {
                        if (initialState.get(booleanVariable.bitOffset)) {
                            initialState.set(booleanVariable.bitOffset);
                            changedBooleanVariable = true;
                            break;
                        } else {
                            initialState.set(booleanVariable.bitOffset, false);
                        }
                    }

                    bool changedIntegerVariable = false;
                    if (changedBooleanVariable) {
                        initialStateIndices.emplace_back(stateToIdCallback(initialState));
                    } else {
                        for (uint64_t integerVariableIndex = 0; integerVariableIndex < this->variableInformation.integerVariables.size(); ++integerVariableIndex) {
                            auto const& integerVariable = this->variableInformation.integerVariables[integerVariableIndex];
                            if (currentIntegerValues[integerVariableIndex] < integerVariable.upperBound - integerVariable.lowerBound) {
                                ++currentIntegerValues[integerVariableIndex];
                                changedIntegerVariable = true;
                            } else {
                                currentIntegerValues[integerVariableIndex] = integerVariable.lowerBound;
                            }
                            initialState.setFromInt(integerVariable.bitOffset, integerVariable.bitWidth, currentIntegerValues[integerVariableIndex]);

                            if (changedIntegerVariable) {
                                break;
                            }
                        }
                    }

                    if (changedIntegerVariable) {
                        initialStateIndices.emplace_back(stateToIdCallback(initialState));
                    }

                    done = !changedBooleanVariable && !changedIntegerVariable;
                }

                STORM_LOG_DEBUG("Enumerated " << initialStateIndices.size() << " initial states using brute force enumeration.");
            } else {
                // Prepare an SMT solver to enumerate all initial states.
                storm::utility::solver::SmtSolverFactory factory;
                std::unique_ptr<storm::solver::SmtSolver> solver = factory.create(program.getManager());

                std::vector<storm::expressions::Expression> rangeExpressions = program.getAllRangeExpressions();
                for (auto const& expression : rangeExpressions) {
                    solver->add(expression);
                }
                solver->add(program.getInitialStatesExpression());

                // Proceed ss long as the solver can still enumerate initial states.
                while (solver->check() == storm::solver::SmtSolver::CheckResult::Sat) {
                    // Create fresh state.
                    CompressedState initialState(this->variableInformation.getTotalBitOffset(true));

                    // Read variable assignment from the solution of the solver. Also, create an expression we can use to
                    // prevent the variable assignment from being enumerated again.
                    storm::expressions::Expression blockingExpression;
                    std::shared_ptr<storm::solver::SmtSolver::ModelReference> model = solver->getModel();
                    for (auto const& booleanVariable : this->variableInformation.booleanVariables) {
                        bool variableValue = model->getBooleanValue(booleanVariable.variable);
                        storm::expressions::Expression localBlockingExpression = variableValue ? !booleanVariable.variable : booleanVariable.variable;
                        blockingExpression = blockingExpression.isInitialized() ? blockingExpression || localBlockingExpression : localBlockingExpression;
                        initialState.set(booleanVariable.bitOffset, variableValue);
                    }
                    for (auto const& integerVariable : this->variableInformation.integerVariables) {
                        int_fast64_t variableValue = model->getIntegerValue(integerVariable.variable);
                        storm::expressions::Expression localBlockingExpression = integerVariable.variable != model->getManager().integer(variableValue);
                        blockingExpression = blockingExpression.isInitialized() ? blockingExpression || localBlockingExpression : localBlockingExpression;
                        initialState.setFromInt(integerVariable.bitOffset, integerVariable.bitWidth, static_cast<uint_fast64_t>(variableValue - integerVariable.lowerBound));
                    }

                    // Register initial state and return it.
                    StateType id = stateToIdCallback(initialState);
                    initialStateIndices.push_back(id);

                    // Block the current initial state to search for the next one.
                    if (!blockingExpression.isInitialized()) {
                        break;
                    }
                    solver->add(blockingExpression);
                }

                STORM_LOG_DEBUG("Enumerated " << initialStateIndices.size() << " initial states using SMT solving.");
            }

            return initialStateIndices;
        }

        template<typename ValueType, typename StateType>
        StateBehavior<ValueType, StateType> PrismNextStateGenerator<ValueType, StateType>::expand(StateToIdCallback const& stateToIdCallback) {
            // Prepare the result, in case we return early.
            StateBehavior<ValueType, StateType> result;

            // First, construct the state rewards, as we may return early if there are no choices later and we already
            // need the state rewards then.
            for (auto const& rewardModel : rewardModels) {
                ValueType stateRewardValue = storm::utility::zero<ValueType>();
                if (rewardModel.get().hasStateRewards()) {
                    for (auto const& stateReward : rewardModel.get().getStateRewards()) {
                        if (this->evaluator->asBool(stateReward.getStatePredicateExpression())) {
                            stateRewardValue += ValueType(this->evaluator->asRational(stateReward.getRewardValueExpression()));
                        }
                    }
                }
                result.addStateReward(stateRewardValue);
            }

            // If a terminal expression was set and we must not expand this state, return now.
            if (!this->terminalStates.empty()) {
                for (auto const& expressionBool : this->terminalStates) {
                    if (this->evaluator->asBool(expressionBool.first) == expressionBool.second) {
                        return result;
                    }
                }
            }

            // Get all choices for the state.
            result.setExpanded();

            std::vector<Choice<ValueType>> allChoices;
            if (this->getOptions().isApplyMaximalProgressAssumptionSet()) {
                // First explore only edges without a rate
                allChoices = getUnlabeledChoices(*this->state, stateToIdCallback, CommandFilter::Probabilistic);
                addLabeledChoices(allChoices, *this->state, stateToIdCallback, CommandFilter::Probabilistic);
                if (allChoices.empty()) {
                    // Expand the Markovian edges if there are no probabilistic ones.
                    allChoices = getUnlabeledChoices(*this->state, stateToIdCallback, CommandFilter::Markovian);
                    addLabeledChoices(allChoices, *this->state, stateToIdCallback, CommandFilter::Markovian);
                }
            } else {
                allChoices = getUnlabeledChoices(*this->state, stateToIdCallback);
                addLabeledChoices(allChoices, *this->state, stateToIdCallback);
            }

            std::size_t totalNumberOfChoices = allChoices.size();

            // If there is not a single choice, we return immediately, because the state has no behavior (other than
            // the state reward).
            if (totalNumberOfChoices == 0) {
                return result;
            }

            // If the model is a deterministic model, we need to fuse the choices into one.
            if (this->isDeterministicModel() && totalNumberOfChoices > 1) {
                Choice<ValueType> globalChoice;

                if (this->options.isAddOverlappingGuardLabelSet()) {
                    this->overlappingGuardStates->push_back(stateToIdCallback(*this->state));
                }

                // For CTMCs, we need to keep track of the total exit rate to scale the action rewards later. For DTMCs
                // this is equal to the number of choices, which is why we initialize it like this here.
                ValueType totalExitRate = this->isDiscreteTimeModel() ? static_cast<ValueType>(totalNumberOfChoices) : storm::utility::zero<ValueType>();

                // Iterate over all choices and combine the probabilities/rates into one choice.
                for (auto const& choice : allChoices) {
                    for (auto const& stateProbabilityPair : choice) {
                        if (this->isDiscreteTimeModel()) {
                            globalChoice.addProbability(stateProbabilityPair.first, stateProbabilityPair.second / totalNumberOfChoices);
                        } else {
                            globalChoice.addProbability(stateProbabilityPair.first, stateProbabilityPair.second);
                        }
                    }

                    if (hasStateActionRewards && !this->isDiscreteTimeModel()) {
                        totalExitRate += choice.getTotalMass();
                    }

                    if (this->options.isBuildChoiceLabelsSet() && choice.hasLabels()) {
                        globalChoice.addLabels(choice.getLabels());
                    }

                    if (this->options.isBuildChoiceOriginsSet() && choice.hasOriginData()) {
                        globalChoice.addOriginData(choice.getOriginData());
                    }
                }

                // Now construct the state-action reward for all selected reward models.
                for (auto const& rewardModel : rewardModels) {
                    ValueType stateActionRewardValue = storm::utility::zero<ValueType>();
                    if (rewardModel.get().hasStateActionRewards()) {
                        for (auto const& stateActionReward : rewardModel.get().getStateActionRewards()) {
                            for (auto const& choice : allChoices) {
                                if (stateActionReward.getActionIndex() == choice.getActionIndex() && this->evaluator->asBool(stateActionReward.getStatePredicateExpression())) {
                                    stateActionRewardValue += ValueType(this->evaluator->asRational(stateActionReward.getRewardValueExpression())) * choice.getTotalMass();
                                }
                            }

                        }
                    }
                    if (hasStateActionRewards) {
                        globalChoice.addReward(stateActionRewardValue / totalExitRate);
                    }
                }

                // Move the newly fused choice in place.
                allChoices.clear();
                allChoices.push_back(std::move(globalChoice));
            }

            // Move all remaining choices in place.
            for (auto& choice : allChoices) {
                result.addChoice(std::move(choice));
            }

            this->postprocess(result);

            return result;
        }

        template<typename ValueType, typename StateType>
        CompressedState PrismNextStateGenerator<ValueType, StateType>::applyUpdate(CompressedState const& state, storm::prism::Update const& update) {
            CompressedState newState(state);

            // NOTE: the following process assumes that the assignments of the update are ordered in such a way that the
            // assignments to boolean variables precede the assignments to all integer variables and that within the
            // types, the assignments to variables are ordered (in ascending order) by the expression variables.
            // This is guaranteed for PRISM models, by sorting the assignments as soon as an update is created.

            auto assignmentIt = update.getAssignments().begin();
            auto assignmentIte = update.getAssignments().end();

            // Iterate over all boolean assignments and carry them out.
            auto boolIt = this->variableInformation.booleanVariables.begin();
            for (; assignmentIt != assignmentIte && assignmentIt->getExpression().hasBooleanType(); ++assignmentIt) {
                while (assignmentIt->getVariable() != boolIt->variable) {
                    ++boolIt;
                }
                newState.set(boolIt->bitOffset, this->evaluator->asBool(assignmentIt->getExpression()));
            }

            // Iterate over all integer assignments and carry them out.
            auto integerIt = this->variableInformation.integerVariables.begin();
            for (; assignmentIt != assignmentIte && assignmentIt->getExpression().hasIntegerType(); ++assignmentIt) {
                while (assignmentIt->getVariable() != integerIt->variable) {
                    ++integerIt;
                }
                int_fast64_t assignedValue = this->evaluator->asInt(assignmentIt->getExpression());
                if (this->options.isAddOutOfBoundsStateSet()) {
                    if (assignedValue < integerIt->lowerBound || assignedValue > integerIt->upperBound) {
                        return this->outOfBoundsState;
                    }
                } else if (integerIt->forceOutOfBoundsCheck || this->options.isExplorationChecksSet()) {
                    STORM_LOG_THROW(assignedValue >= integerIt->lowerBound, storm::exceptions::WrongFormatException, "The update " << update << " leads to an out-of-bounds value (" << assignedValue << ") for the variable '" << assignmentIt->getVariableName() << "'.");
                    STORM_LOG_THROW(assignedValue <= integerIt->upperBound, storm::exceptions::WrongFormatException, "The update " << update << " leads to an out-of-bounds value (" << assignedValue << ") for the variable '" << assignmentIt->getVariableName() << "'.");
                }
                newState.setFromInt(integerIt->bitOffset, integerIt->bitWidth, assignedValue - integerIt->lowerBound);
                STORM_LOG_ASSERT(static_cast<int_fast64_t>(newState.getAsInt(integerIt->bitOffset, integerIt->bitWidth)) + integerIt->lowerBound == assignedValue, "Writing to the bit vector bucket failed (read " << newState.getAsInt(integerIt->bitOffset, integerIt->bitWidth) << " but wrote " << assignedValue << ").");
            }

            // Check that we processed all assignments.
            STORM_LOG_ASSERT(assignmentIt == assignmentIte, "Not all assignments were consumed.");

            return newState;
        }

        struct ActiveCommandData {
            ActiveCommandData(storm::prism::Module const* modulePtr, std::set<uint_fast64_t> const* commandIndicesPtr, typename std::set<uint_fast64_t>::const_iterator currentCommandIndexIt) : modulePtr(modulePtr), commandIndicesPtr(commandIndicesPtr), currentCommandIndexIt(currentCommandIndexIt) {
                // Intentionally left empty
            }
            storm::prism::Module const* modulePtr;
            std::set<uint_fast64_t> const* commandIndicesPtr;
            typename std::set<uint_fast64_t>::const_iterator currentCommandIndexIt;
        };

        template<typename ValueType, typename StateType>
        boost::optional<std::vector<std::vector<std::reference_wrapper<storm::prism::Command const>>>> PrismNextStateGenerator<ValueType, StateType>::getActiveCommandsByActionIndex(uint_fast64_t const& actionIndex, CommandFilter const& commandFilter) {

            // First check whether there is at least one enabled command at each module
            // This avoids evaluating unnecessarily many guards.
            // If we find one module without an enabled command, we return boost::none.
            // At the same time, we store pointers to the relevant modules, the relevant command sets and the first enabled command within each set.

            // Iterate over all modules.
            std::vector<ActiveCommandData> activeCommands;
            for (uint_fast64_t i = 0; i < program.getNumberOfModules(); ++i) {
                storm::prism::Module const& module = program.getModule(i);

                // If the module has no command labeled with the given action, we can skip this module.
                if (!module.hasActionIndex(actionIndex)) {
                    continue;
                }

                std::set<uint_fast64_t> const& commandIndices = module.getCommandIndicesByActionIndex(actionIndex);

                // If the module contains the action, but there is no command in the module that is labeled with
                // this action, we don't have any feasible command combinations.
                if (commandIndices.empty()) {
                    return boost::none;
                }

                // Look up commands by their indices and check if the guard evaluates to true in the given state.
                bool hasOneEnabledCommand = false;
                for (auto commandIndexIt = commandIndices.begin(), commandIndexIte = commandIndices.end(); commandIndexIt != commandIndexIte; ++commandIndexIt) {
                    storm::prism::Command const& command = module.getCommand(*commandIndexIt);
                    if (commandFilter != CommandFilter::All) {
                        STORM_LOG_ASSERT(commandFilter == CommandFilter::Markovian || commandFilter == CommandFilter::Probabilistic, "Unexpected command filter.");
                        if ((commandFilter == CommandFilter::Markovian) != command.isMarkovian()) {
                            continue;
                        }
                    }
                    if (this->evaluator->asBool(command.getGuardExpression())) {
                        // Found the first enabled command for this module.
                        hasOneEnabledCommand = true;
                        activeCommands.emplace_back(&module, &commandIndices, commandIndexIt);
                        break;
                    }
                }

                if (!hasOneEnabledCommand) {
                    return boost::none;
                }
            }

            // If we reach this point, there has to be at least one active command for each relevant module.
            std::vector<std::vector<std::reference_wrapper<storm::prism::Command const>>> result;

            // Iterate over all command sets.
            for (auto const& activeCommand : activeCommands) {
                std::vector<std::reference_wrapper<storm::prism::Command const>> commands;

                auto commandIndexIt = activeCommand.currentCommandIndexIt;
                // The command at the current position is already known to be enabled
                commands.push_back(activeCommand.modulePtr->getCommand(*commandIndexIt));

                // Look up commands by their indices and add them if the guard evaluates to true in the given state.
                auto commandIndexIte = activeCommand.commandIndicesPtr->end();
                for (++commandIndexIt; commandIndexIt != commandIndexIte; ++commandIndexIt) {
                    storm::prism::Command const& command = activeCommand.modulePtr->getCommand(*commandIndexIt);
                    if (commandFilter != CommandFilter::All) {
                        STORM_LOG_ASSERT(commandFilter == CommandFilter::Markovian || commandFilter == CommandFilter::Probabilistic, "Unexpected command filter.");
                        if ((commandFilter == CommandFilter::Markovian) != command.isMarkovian()) {
                            continue;
                        }
                    }
                    if (this->evaluator->asBool(command.getGuardExpression())) {
                        commands.push_back(command);
                    }
                }

                result.push_back(std::move(commands));
            }

            STORM_LOG_ASSERT(!result.empty(), "Expected non-empty list.");
            return result;
        }

        template<typename ValueType, typename StateType>
        std::vector<Choice<ValueType>> PrismNextStateGenerator<ValueType, StateType>::getUnlabeledChoices(CompressedState const& state, StateToIdCallback stateToIdCallback, CommandFilter const& commandFilter) {
            std::vector<Choice<ValueType>> result;

            // Iterate over all modules.
            for (uint_fast64_t i = 0; i < program.getNumberOfModules(); ++i) {
                storm::prism::Module const& module = program.getModule(i);

                // Iterate over all commands.
                for (uint_fast64_t j = 0; j < module.getNumberOfCommands(); ++j) {
                    storm::prism::Command const& command = module.getCommand(j);

                    // Only consider unlabeled commands.
                    if (command.isLabeled()) continue;

                    if (commandFilter != CommandFilter::All) {
                        STORM_LOG_ASSERT(commandFilter == CommandFilter::Markovian || commandFilter == CommandFilter::Probabilistic, "Unexpected command filter.");
                        if ((commandFilter == CommandFilter::Markovian) != command.isMarkovian()) {
                            continue;
                        }
                    }

                    // Skip the command, if it is not enabled.
                    if (!this->evaluator->asBool(command.getGuardExpression())) {
                        continue;
                    }

                    result.push_back(Choice<ValueType>(command.getActionIndex(), command.isMarkovian()));
                    Choice<ValueType>& choice = result.back();

                    // Remember the choice origin only if we were asked to.
                    if (this->options.isBuildChoiceOriginsSet()) {
                        CommandSet commandIndex { command.getGlobalIndex() };
                        choice.addOriginData(boost::any(std::move(commandIndex)));
                    }

                    // Iterate over all updates of the current command.
                    ValueType probabilitySum = storm::utility::zero<ValueType>();
                    for (uint_fast64_t k = 0; k < command.getNumberOfUpdates(); ++k) {
                        storm::prism::Update const& update = command.getUpdate(k);

                        ValueType probability = this->evaluator->asRational(update.getLikelihoodExpression());
                        if (probability != storm::utility::zero<ValueType>()) {
                            // Obtain target state index and add it to the list of known states. If it has not yet been
                            // seen, we also add it to the set of states that have yet to be explored.
                            StateType stateIndex = stateToIdCallback(applyUpdate(state, update));

                            // Update the choice by adding the probability/target state to it.
                            choice.addProbability(stateIndex, probability);
                            if (this->options.isExplorationChecksSet()) {
                                probabilitySum += probability;
                            }
                        }
                    }

                    // Create the state-action reward for the newly created choice.
                    for (auto const& rewardModel : rewardModels) {
                        ValueType stateActionRewardValue = storm::utility::zero<ValueType>();
                        if (rewardModel.get().hasStateActionRewards()) {
                            for (auto const& stateActionReward : rewardModel.get().getStateActionRewards()) {
                                if (stateActionReward.getActionIndex() == choice.getActionIndex() && this->evaluator->asBool(stateActionReward.getStatePredicateExpression())) {
                                    stateActionRewardValue += ValueType(this->evaluator->asRational(stateActionReward.getRewardValueExpression()));
                                }
                            }
                        }
                        choice.addReward(stateActionRewardValue);
                    }

                    if (this->options.isExplorationChecksSet()) {
                        // Check that the resulting distribution is in fact a distribution.
                        STORM_LOG_THROW(!program.isDiscreteTimeModel() || this->comparator.isOne(probabilitySum), storm::exceptions::WrongFormatException, "Probabilities do not sum to one for command '" << command << "' (actually sum to " << probabilitySum << ").");
                    }
                }
            }

            return result;
        }

        template<typename ValueType, typename StateType>
        void PrismNextStateGenerator<ValueType, StateType>::generateSynchronizedDistribution(storm::storage::BitVector const& state, ValueType const& probability, uint64_t position, std::vector<std::vector<std::reference_wrapper<storm::prism::Command const>>::const_iterator> const& iteratorList, storm::builder::jit::Distribution<StateType, ValueType>& distribution, StateToIdCallback stateToIdCallback) {

            if (storm::utility::isZero<ValueType>(probability)) {
                return;
            }

            if (position >= iteratorList.size()) {
                StateType id = stateToIdCallback(state);
                distribution.add(id, probability);
            } else {
                storm::prism::Command const& command = *iteratorList[position];
                for (uint_fast64_t j = 0; j < command.getNumberOfUpdates(); ++j) {
                    storm::prism::Update const& update = command.getUpdate(j);
                    generateSynchronizedDistribution(applyUpdate(state, update), probability * this->evaluator->asRational(update.getLikelihoodExpression()), position + 1, iteratorList, distribution, stateToIdCallback);
                }
            }
        }

        template<typename ValueType, typename StateType>
        void PrismNextStateGenerator<ValueType, StateType>::addLabeledChoices(std::vector<Choice<ValueType>>& choices, CompressedState const& state, StateToIdCallback stateToIdCallback, CommandFilter const& commandFilter) {

            for (uint_fast64_t actionIndex : program.getSynchronizingActionIndices()) {
                boost::optional<std::vector<std::vector<std::reference_wrapper<storm::prism::Command const>>>> optionalActiveCommandLists = getActiveCommandsByActionIndex(actionIndex, commandFilter);

                // Only process this action label, if there is at least one feasible solution.
                if (optionalActiveCommandLists) {
                    std::vector<std::vector<std::reference_wrapper<storm::prism::Command const>>> const& activeCommandList = optionalActiveCommandLists.get();
                    std::vector<std::vector<std::reference_wrapper<storm::prism::Command const>>::const_iterator> iteratorList(activeCommandList.size());

                    // Initialize the list of iterators.
                    for (size_t i = 0; i < activeCommandList.size(); ++i) {
                        iteratorList[i] = activeCommandList[i].cbegin();
                    }

                    storm::builder::jit::Distribution<StateType, ValueType> distribution;

                    // As long as there is one feasible combination of commands, keep on expanding it.
                    bool done = false;
                    while (!done) {
                        distribution.clear();
                        generateSynchronizedDistribution(state, storm::utility::one<ValueType>(), 0, iteratorList, distribution, stateToIdCallback);
                        distribution.compress();

                        // At this point, we applied all commands of the current command combination and newTargetStates
                        // contains all target states and their respective probabilities. That means we are now ready to
                        // add the choice to the list of transitions.
                        choices.push_back(Choice<ValueType>(actionIndex));

                        // Now create the actual distribution.
                        Choice<ValueType>& choice = choices.back();

                        // Remember the choice label and origins only if we were asked to.
                        if (this->options.isBuildChoiceLabelsSet()) {
                            choice.addLabel(program.getActionName(actionIndex));
                        }
                        if (this->options.isBuildChoiceOriginsSet()) {
                            CommandSet commandIndices;
                            for (uint_fast64_t i = 0; i < iteratorList.size(); ++i) {
                                commandIndices.insert(iteratorList[i]->get().getGlobalIndex());
                            }
                            choice.addOriginData(boost::any(std::move(commandIndices)));
                        }

                        // Add the probabilities/rates to the newly created choice.
                        ValueType probabilitySum = storm::utility::zero<ValueType>();
                        choice.reserve(std::distance(distribution.begin(), distribution.end()));
                        for (auto const& stateProbability : distribution) {
                            choice.addProbability(stateProbability.getState(), stateProbability.getValue());
                            if (this->options.isExplorationChecksSet()) {
                                probabilitySum += stateProbability.getValue();
                            }
                        }

                        if (this->options.isExplorationChecksSet()) {
                            // Check that the resulting distribution is in fact a distribution.
                            STORM_LOG_THROW(!program.isDiscreteTimeModel() || !this->comparator.isConstant(probabilitySum) || this->comparator.isOne(probabilitySum), storm::exceptions::WrongFormatException, "Sum of update probabilities do not some to one for some command (actually sum to " << probabilitySum << ").");
                        }

                        // Create the state-action reward for the newly created choice.
                        for (auto const& rewardModel : rewardModels) {
                            ValueType stateActionRewardValue = storm::utility::zero<ValueType>();
                            if (rewardModel.get().hasStateActionRewards()) {
                                for (auto const& stateActionReward : rewardModel.get().getStateActionRewards()) {
                                    if (stateActionReward.getActionIndex() == choice.getActionIndex() && this->evaluator->asBool(stateActionReward.getStatePredicateExpression())) {
                                        stateActionRewardValue += ValueType(this->evaluator->asRational(stateActionReward.getRewardValueExpression()));
                                    }
                                }
                            }
                            choice.addReward(stateActionRewardValue);
                        }

                        // Now, check whether there is one more command combination to consider.
                        bool movedIterator = false;
                        for (int_fast64_t j = iteratorList.size() - 1; !movedIterator && j >= 0; --j) {
                            ++iteratorList[j];
                            if (iteratorList[j] != activeCommandList[j].end()) {
                                movedIterator = true;
                            } else {
                                // Reset the iterator to the beginning of the list.
                                iteratorList[j] = activeCommandList[j].begin();
                            }
                        }

                        done = !movedIterator;
                    }
                }
            }
        }

        template<typename ValueType, typename StateType>
        storm::models::sparse::StateLabeling PrismNextStateGenerator<ValueType, StateType>::label(storm::storage::sparse::StateStorage<StateType> const& stateStorage, std::vector<StateType> const& initialStateIndices, std::vector<StateType> const& deadlockStateIndices) {
            // Gather a vector of labels and their expressions.
            std::vector<std::pair<std::string, storm::expressions::Expression>> labels;
            if (this->options.isBuildAllLabelsSet()) {
                for (auto const& label : program.getLabels()) {
                    labels.push_back(std::make_pair(label.getName(), label.getStatePredicateExpression()));
                }
            } else {
                for (auto const& labelName : this->options.getLabelNames()) {
                    if (program.hasLabel(labelName)) {
                        labels.push_back(std::make_pair(labelName, program.getLabelExpression(labelName)));
                    } else {
                        STORM_LOG_THROW(labelName == "init" || labelName == "deadlock", storm::exceptions::InvalidArgumentException, "Cannot build labeling for unknown label '" << labelName << "'.");
                    }
                }
            }

            return NextStateGenerator<ValueType, StateType>::label(stateStorage, initialStateIndices, deadlockStateIndices, labels);
        }

        template<typename ValueType, typename StateType>
        storm::storage::BitVector PrismNextStateGenerator<ValueType, StateType>::evaluateObservationLabels(CompressedState const& state) const {
            // TODO consider to avoid reloading by computing these bitvectors in an earlier build stage
            unpackStateIntoEvaluator(state, this->variableInformation, *this->evaluator);

            storm::storage::BitVector result(program.getNumberOfObservationLabels() * 64);
            for (uint64_t i = 0; i < program.getNumberOfObservationLabels(); ++i) {
                result.setFromInt(64*i,64,this->evaluator->asInt(program.getObservationLabels()[i].getStatePredicateExpression()));
            }
            return result;
        }

        template<typename ValueType, typename StateType>
        std::size_t PrismNextStateGenerator<ValueType, StateType>::getNumberOfRewardModels() const {
            return rewardModels.size();
        }

        template<typename ValueType, typename StateType>
        storm::builder::RewardModelInformation PrismNextStateGenerator<ValueType, StateType>::getRewardModelInformation(uint64_t const& index) const {
            storm::prism::RewardModel const& rewardModel = rewardModels[index].get();
            return storm::builder::RewardModelInformation(rewardModel.getName(), rewardModel.hasStateRewards(), rewardModel.hasStateActionRewards(), rewardModel.hasTransitionRewards());
        }

        template<typename ValueType, typename StateType>
        std::shared_ptr<storm::storage::sparse::ChoiceOrigins> PrismNextStateGenerator<ValueType, StateType>::generateChoiceOrigins(std::vector<boost::any>& dataForChoiceOrigins) const {
            if (!this->getOptions().isBuildChoiceOriginsSet()) {
                return nullptr;
            }

            std::vector<uint_fast64_t> identifiers;
            identifiers.reserve(dataForChoiceOrigins.size());

            std::map<CommandSet, uint_fast64_t> commandSetToIdentifierMap;
            // The empty commandset (i.e., the choices without origin) always has to get identifier getIdentifierForChoicesWithNoOrigin() -- which is assumed to be 0
            STORM_LOG_ASSERT(storm::storage::sparse::ChoiceOrigins::getIdentifierForChoicesWithNoOrigin() == 0, "The no origin identifier is assumed to be zero");
            commandSetToIdentifierMap.insert(std::make_pair(CommandSet(), 0));
            uint_fast64_t currentIdentifier = 1;
            for (boost::any& originData : dataForChoiceOrigins) {
                STORM_LOG_ASSERT(originData.empty() || boost::any_cast<CommandSet>(&originData) != nullptr, "Origin data has unexpected type: " << originData.type().name() << ".");

                CommandSet currentCommandSet = originData.empty() ? CommandSet() : boost::any_cast<CommandSet>(std::move(originData));
                auto insertionRes = commandSetToIdentifierMap.insert(std::make_pair(std::move(currentCommandSet), currentIdentifier));
                identifiers.push_back(insertionRes.first->second);
                if (insertionRes.second) {
                    ++currentIdentifier;
                }
            }

            std::vector<CommandSet> identifierToCommandSetMapping(currentIdentifier);
            for (auto const& setIdPair : commandSetToIdentifierMap) {
                identifierToCommandSetMapping[setIdPair.second] = setIdPair.first;
            }

            return std::make_shared<storm::storage::sparse::PrismChoiceOrigins>(std::make_shared<storm::prism::Program>(program), std::move(identifiers), std::move(identifierToCommandSetMapping));
        }

        template class PrismNextStateGenerator<double>;

#ifdef STORM_HAVE_CARL
        template class PrismNextStateGenerator<storm::RationalNumber>;
        template class PrismNextStateGenerator<storm::RationalFunction>;
#endif
    }
}


/*
** laser.hh
** Login : <mandor@ithaqua>
** Started on  Wed Jun  3 00:39:20 2009 mandor
** $Id$
**
** Copyright (C) 2009 mandor
** This program is free software; you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation; either version 2 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program; if not, write to the Free Software
** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/

#ifndef   	LASER_HH_
# define   	LASER_HH_
#include "map.hpp"
#include "posture.hpp"
#include <boost/shared_ptr.hpp>

namespace fastsim
{
  class Laser
  {
  public:
    Laser(float angle, float range, float gap_dist = 0.0f, float gap_angle = 0.0f) :
      _angle(angle), _range(range),
      _gap_dist(gap_dist), _gap_angle(gap_angle),
      _x(0), _y(0),
      _x_pixel(0), _y_pixel(0),
      _dist(-1) {}
    float update(const Posture& pos,
		 const boost::shared_ptr<Map>& map);
    float get_dist() const { return _dist; }
    float get_angle() const { return _angle; }
    float get_range() const { return _range; }
    float get_gap_dist() const { return _gap_dist; }
    float get_gap_angle() const { return _gap_angle; }
    // intersection point in world coordinates
    float get_x() const { return _x; }
    float get_y() const { return _y; }
    // intersection point in pixel coordinates
    int get_x_pixel() const { return _x_pixel; }
    int get_y_pixel() const { return _y_pixel; }
  protected:
    float _angle;
    float _range;
    float _gap_dist, _gap_angle;//polar coordinates of the laser (reference = robot)
    float _x, _y;
    int _x_pixel, _y_pixel;
    float _dist;
    //
    bool _try_pixel(const boost::shared_ptr<Map>& m, int x, int y);
    bool _line_inter(const boost::shared_ptr<Map>& m,
		     int y1, int x1, // src
		     int y2, int x2, // dest
		     int& x_res, int& y_res //res
		     );

  };
}

#endif	    /* !LASER_HH_ */


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/ap.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/core/make.hpp>
#include <boost/hana/core/tag_of.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

namespace wrap_detail {
    template <typename Datatype, typename X>
    struct wrapper {
        X unwrap;
        using hana_tag = Datatype;
    };

    template <typename Datatype>
    struct wrap_impl {
        template <typename X>
        constexpr auto operator()(X x) const {
            return wrapper<Datatype, X>{x};
        }
    };
}

template <typename Datatype>
constexpr wrap_detail::wrap_impl<Datatype> wrap{};

BOOST_HANA_CONSTEXPR_LAMBDA auto unwrap = [](auto x) {
    return x.unwrap;
};

//////////////////////////////////////////////////////////////////////////////
// core
//////////////////////////////////////////////////////////////////////////////
template <typename ...>
struct not_implemented;

//////////////////////////////////////////////////////////////////////////////
// Functor
//////////////////////////////////////////////////////////////////////////////
template <typename X, typename F, typename Enable = void>
not_implemented<X, F> fmap_impl{};

auto fmap = [](auto x, auto f) {
    return fmap_impl<
        hana::tag_of_t<decltype(x)>,
        hana::tag_of_t<decltype(f)>
    >(x, f);
};

//////////////////////////////////////////////////////////////////////////////
// Applicative
//////////////////////////////////////////////////////////////////////////////
template <typename F, typename X, typename Enable = void>
not_implemented<F, X> ap_impl{};

auto ap = [](auto f, auto x) {
    return ap_impl<
        hana::tag_of_t<decltype(f)>,
        hana::tag_of_t<decltype(x)>
    >(f, x);
};

template <typename A, typename Enable = void>
not_implemented<A> lift_impl{};

template <template <typename> class A>
auto lift = [](auto x) {
    return lift_impl<A<hana::tag_of_t<decltype(x)>>>(x);
};

//////////////////////////////////////////////////////////////////////////////
// Function
//////////////////////////////////////////////////////////////////////////////
template <typename F, typename X, typename Enable = void>
not_implemented<F, X> apply_impl{};

auto apply = [](auto f, auto x) {
    return apply_impl<
        hana::tag_of_t<decltype(f)>,
        hana::tag_of_t<decltype(x)>
    >(f, x);
};

template <typename Domain, typename Codomain>
struct Function;

template <typename Domain, typename Codomain>
auto function = [](auto f) {
    return wrap<Function<Domain, Codomain>>(f);
};

template <typename X, typename Y>
auto apply_impl<Function<X, Y>, X> = [](auto f, auto x) {
    return unwrap(f)(x);
};

//////////////////////////////////////////////////////////////////////////////
// List
//////////////////////////////////////////////////////////////////////////////
template <typename T>
struct List;

template <typename T>
auto list = [](auto ...xs) {
    return wrap<List<T>>(
        [=](auto f) { return f(xs...); }
    );
};

template <typename X, typename Y>
auto fmap_impl<List<X>, Function<X, Y>> = [](auto xs, auto f) {
    return unwrap(xs)([=](auto ...xs) {
        return list<Y>(apply(f, xs)...);
    });
};

template <typename X>
auto lift_impl<List<X>> = [](auto x) {
    return list<X>(x);
};

template <typename X, typename Y>
auto ap_impl<List<Function<X, Y>>, List<X>> = [](auto fs, auto xs) {
    auto hana_fs = unwrap(fs)([](auto ...fs) {
        return hana::make_tuple(hana::partial(apply, fs)...);
    });
    auto hana_xs = unwrap(xs)(hana::make_tuple);
    auto hana_result = hana::ap(hana_fs, hana_xs);

    return hana::unpack(hana_result, list<Y>);
};

//////////////////////////////////////////////////////////////////////////////
// Any
//////////////////////////////////////////////////////////////////////////////
struct Any;

auto any = [](auto x) {
    return wrap<Any>(x);
};

int main() {
    auto f = function<int, int>([](auto x) { return x + 1; });
    auto xs = list<int>(1, 2, 3, 4);
    fmap(xs, f);

    lift<List>(2);
    ap(list<Function<int, int>>(f, f), list<int>(1, 2));

    auto g = function<Any, int>([](auto /*x*/) {
        // We can't do anything with an Any, so there's not much choice here.
        return 1;
    });
    fmap(list<Any>(any(1), any('2'), any("345")), g);
}


/* Copyright (c) 2021, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking
 * platform" software is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

#include <opencv2/xfeatures2d.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <interest_point.h>           // from the isaac repo
#include <interest_point/matching.h>  // from the astrobee repo

#include <boost/filesystem.hpp>

#include <iostream>
#include <fstream>

// SIFT is doing so much better than SURF for haz cam images.
DEFINE_string(refiner_feature_detector, "SIFT", "The feature detector to use. SIFT or SURF.");
DEFINE_int32(sift_nFeatures, 10000, "Number of SIFT features");
DEFINE_int32(sift_nOctaveLayers, 3, "Number of SIFT octave layers");
DEFINE_double(sift_contrastThreshold, 0.02,
              "SIFT contrast threshold");  // decrease for more ip
DEFINE_double(sift_edgeThreshold, 10, "SIFT edge threshold");
DEFINE_double(sift_sigma, 1.6, "SIFT sigma");

namespace dense_map {

void detectFeatures(const cv::Mat& image, bool verbose,
                    // Outputs
                    cv::Mat* descriptors, Eigen::Matrix2Xd* keypoints) {
  bool histogram_equalization = false;

  // If using histogram equalization, need an extra image to store it
  cv::Mat* image_ptr = const_cast<cv::Mat*>(&image);
  cv::Mat hist_image;
  if (histogram_equalization) {
    cv::equalizeHist(image, hist_image);
    image_ptr = &hist_image;
  }

  std::vector<cv::KeyPoint> storage;

  if (FLAGS_refiner_feature_detector == "SIFT") {
    cv::Ptr<cv::xfeatures2d::SIFT> sift =
      cv::xfeatures2d::SIFT::create(FLAGS_sift_nFeatures, FLAGS_sift_nOctaveLayers, FLAGS_sift_contrastThreshold,
                                    FLAGS_sift_edgeThreshold, FLAGS_sift_sigma);
    sift->detect(image, storage);
    sift->compute(image, storage, *descriptors);

  } else if (FLAGS_refiner_feature_detector == "SURF") {
    interest_point::FeatureDetector detector("SURF");
    detector.Detect(*image_ptr, &storage, descriptors);

    // Undo the shift in the detector
    for (cv::KeyPoint& key : storage) {
      key.pt.x += image.cols / 2.0;
      key.pt.y += image.rows / 2.0;
    }

  } else {
    LOG(FATAL) << "Unknown feature detector: " << FLAGS_refiner_feature_detector;
  }

  if (verbose) std::cout << "Features detected " << storage.size() << std::endl;

  // Copy to data structures expected by subsequent code
  keypoints->resize(2, storage.size());
  Eigen::Vector2d output;
  for (size_t j = 0; j < storage.size(); j++) {
    keypoints->col(j) = Eigen::Vector2d(storage[j].pt.x, storage[j].pt.y);
  }
}

// This really likes haz cam first and nav cam second
void matchFeatures(std::mutex* match_mutex, int left_image_index, int right_image_index,
                   cv::Mat const& left_descriptors, cv::Mat const& right_descriptors,
                   Eigen::Matrix2Xd const& left_keypoints, Eigen::Matrix2Xd const& right_keypoints, bool verbose,
                   // output
                   MATCH_PAIR* matches) {
  std::vector<cv::DMatch> cv_matches;
  interest_point::FindMatches(left_descriptors, right_descriptors, &cv_matches);

  std::vector<cv::Point2f> left_vec;
  std::vector<cv::Point2f> right_vec;
  for (size_t j = 0; j < cv_matches.size(); j++) {
    int left_ip_index = cv_matches.at(j).queryIdx;
    int right_ip_index = cv_matches.at(j).trainIdx;

    // Get the keypoints from the good matches
    left_vec.push_back(cv::Point2f(left_keypoints.col(left_ip_index)[0], left_keypoints.col(left_ip_index)[1]));
    right_vec.push_back(cv::Point2f(right_keypoints.col(right_ip_index)[0], right_keypoints.col(right_ip_index)[1]));
  }

  if (left_vec.empty()) return;

  // These may need some tweaking but work reasonably well.
  double ransacReprojThreshold = 20.0;
  cv::Mat inlier_mask;
  int maxIters = 10000;
  double confidence = 0.8;

  // cv::Mat H = cv::findHomography(left_vec, right_vec, cv::RANSAC,
  // ransacReprojThreshold,
  //                               inlier_mask, maxIters, confidence);
  cv::Mat H =
    cv::estimateAffine2D(left_vec, right_vec, inlier_mask, cv::RANSAC, ransacReprojThreshold, maxIters, confidence);

  std::vector<InterestPoint> left_ip, right_ip;
  for (size_t j = 0; j < cv_matches.size(); j++) {
    int left_ip_index = cv_matches.at(j).queryIdx;
    int right_ip_index = cv_matches.at(j).trainIdx;

    if (inlier_mask.at<uchar>(j, 0) == 0) continue;

    cv::Mat left_desc = left_descriptors.row(left_ip_index);
    cv::Mat right_desc = right_descriptors.row(right_ip_index);

    InterestPoint left;
    left.setFromCvKeypoint(left_keypoints.col(left_ip_index), left_desc);

    InterestPoint right;
    right.setFromCvKeypoint(right_keypoints.col(right_ip_index), right_desc);

    left_ip.push_back(left);
    right_ip.push_back(right);
  }

  if (verbose) std::cout << "Number of matches: " << left_ip.size() << std::endl;

  // Update the shared variable using a lock
  match_mutex->lock();
  *matches = std::make_pair(left_ip, right_ip);
  match_mutex->unlock();
}

void writeIpRecord(std::ofstream& f, InterestPoint const& p) {
  f.write(reinterpret_cast<const char*>(&(p.x)), sizeof(p.x));
  f.write(reinterpret_cast<const char*>(&(p.y)), sizeof(p.y));
  f.write(reinterpret_cast<const char*>(&(p.ix)), sizeof(p.ix));
  f.write(reinterpret_cast<const char*>(&(p.iy)), sizeof(p.iy));
  f.write(reinterpret_cast<const char*>(&(p.orientation)), sizeof(p.orientation));
  f.write(reinterpret_cast<const char*>(&(p.scale)), sizeof(p.scale));
  f.write(reinterpret_cast<const char*>(&(p.interest)), sizeof(p.interest));
  f.write(reinterpret_cast<const char*>(&(p.polarity)), sizeof(p.polarity));
  f.write(reinterpret_cast<const char*>(&(p.octave)), sizeof(p.octave));
  f.write(reinterpret_cast<const char*>(&(p.scale_lvl)), sizeof(p.scale_lvl));
  uint64_t size = p.size();
  f.write(reinterpret_cast<const char*>((&size)), sizeof(uint64));
  for (size_t i = 0; i < p.descriptor.size(); ++i)
    f.write(reinterpret_cast<const char*>(&(p.descriptor[i])), sizeof(p.descriptor[i]));
}

// Write matches to disk
void writeMatchFile(std::string match_file, std::vector<InterestPoint> const& ip1,
                    std::vector<InterestPoint> const& ip2) {
  std::ofstream f;
  f.open(match_file.c_str(), std::ios::binary | std::ios::out);
  std::vector<InterestPoint>::const_iterator iter1 = ip1.begin();
  std::vector<InterestPoint>::const_iterator iter2 = ip2.begin();
  uint64 size1 = ip1.size();
  uint64 size2 = ip2.size();
  f.write(reinterpret_cast<const char*>(&size1), sizeof(uint64));
  f.write(reinterpret_cast<const char*>(&size2), sizeof(uint64));
  for (; iter1 != ip1.end(); ++iter1) writeIpRecord(f, *iter1);
  for (; iter2 != ip2.end(); ++iter2) writeIpRecord(f, *iter2);
  f.close();
}

void saveImagesAndMatches(std::string const& left_prefix, std::string const& right_prefix,
                          std::pair<int, int> const& index_pair, MATCH_PAIR const& match_pair,
                          std::vector<cv::Mat> const& images) {
  // Add 10000 to have them be listed nicely
  std::ostringstream oss_left;
  oss_left << left_prefix << "_image" << index_pair.first + 10000 << ".jpg";
  std::string left_image_file = oss_left.str();
  std::cout << "Writing: " << left_image_file << std::endl;
  cv::imwrite(left_image_file, images[index_pair.first]);

  std::ostringstream oss_right;
  oss_right << right_prefix << "_image" << index_pair.second + 10000 << ".jpg";
  std::string right_image_file = oss_right.str();
  std::cout << "Writing: " << right_image_file << std::endl;
  cv::imwrite(right_image_file, images[index_pair.second]);

  std::string left_stem = boost::filesystem::path(left_image_file).stem().string();
  std::string right_stem = boost::filesystem::path(right_image_file).stem().string();

  std::string match_file = left_stem + "__" + right_stem + ".match";
  std::cout << "Writing: " << left_image_file << ' ' << right_image_file << ' ' << match_file << std::endl;
  writeMatchFile(match_file, match_pair.first, match_pair.second);
}

}  // end namespace dense_map