/*
 * Copyright (C) 2005-2019 Centre National d'Etudes Spatiales (CNES)
 *
 * This file is part of Orfeo Toolbox
 *
 *     https://www.orfeo-toolbox.org/
 *
 * 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 "itkNumericTraits.h"

#include "otbSailModel.h"
#include "otb_boost_expint_header.h"
#include <boost/shared_ptr.hpp>
#include "otbMath.h"

// TODO check EPSILON matlab
#define EPSILON 0.0000000000000000000000001

namespace otb
{

/** Constructor */
SailModel::SailModel()
  : m_LAI(2), m_Angl(50), m_PSoil(1), m_Skyl(70), m_HSpot(0.2), m_TTS(30), m_TTO(0), m_PSI(0), m_FCoverView(0.0), m_UseSoilFile(false), m_SoilIndex(0)
{
  this->ProcessObject::SetNumberOfRequiredInputs(2);
  this->ProcessObject::SetNumberOfRequiredOutputs(4);

  SpectralResponseType::Pointer vRefl = static_cast<SpectralResponseType*>(this->MakeOutput(0).GetPointer());
  this->itk::ProcessObject::SetNthOutput(0, vRefl.GetPointer());

  SpectralResponseType::Pointer hRefl = static_cast<SpectralResponseType*>(this->MakeOutput(1).GetPointer());
  this->itk::ProcessObject::SetNthOutput(1, hRefl.GetPointer());

  SpectralResponseType::Pointer vAbs = static_cast<SpectralResponseType*>(this->MakeOutput(2).GetPointer());
  this->itk::ProcessObject::SetNthOutput(2, vAbs.GetPointer());

  SpectralResponseType::Pointer hAbs = static_cast<SpectralResponseType*>(this->MakeOutput(3).GetPointer());
  this->itk::ProcessObject::SetNthOutput(3, hAbs.GetPointer());
}

/** Destructor */
SailModel::~SailModel()
{
}

/** Set/Get input reflectance */
void SailModel::SetReflectance(const SpectralResponseType* object)
{
  this->itk::ProcessObject::SetNthInput(0, const_cast<SpectralResponseType*>(object));
}

SailModel::SpectralResponseType* SailModel::GetReflectance()
{
  if (this->GetNumberOfInputs() != 2)
  {
    // exit
    return nullptr;
  }
  return static_cast<SpectralResponseType*>(this->itk::ProcessObject::GetInput(0));
}

/** Set/Get input transmittance */
void SailModel::SetTransmittance(const SpectralResponseType* object)
{
  this->itk::ProcessObject::SetNthInput(1, const_cast<SpectralResponseType*>(object));
}

SailModel::SpectralResponseType* SailModel::GetTransmittance()
{
  if (this->GetNumberOfInputs() != 2)
  {
    // exit
    return nullptr;
  }
  return static_cast<SpectralResponseType*>(this->itk::ProcessObject::GetInput(1));
}

/** Make output */
SailModel::DataObjectPointer SailModel::MakeOutput(DataObjectPointerArraySizeType)
{
  return static_cast<itk::DataObject*>(SpectralResponseType::New().GetPointer());
}

/** Get output viewing reflectance */
SailModel::SpectralResponseType* SailModel::GetViewingReflectance()
{
  if (this->GetNumberOfOutputs() < 4)
  {
    // exit
    return nullptr;
  }
  return static_cast<SpectralResponseType*>(this->itk::ProcessObject::GetOutput(0));
}

/** Get output hemispherical reflectance */
SailModel::SpectralResponseType* SailModel::GetHemisphericalReflectance()
{
  if (this->GetNumberOfOutputs() < 4)
  {
    // exit
    return nullptr;
  }
  return static_cast<SpectralResponseType*>(this->itk::ProcessObject::GetOutput(1));
}

/** Get output viewing absorptance */
SailModel::SpectralResponseType* SailModel::GetViewingAbsorptance()
{
  if (this->GetNumberOfOutputs() < 4)
  {
    // exit
    return nullptr;
  }
  return static_cast<SpectralResponseType*>(this->itk::ProcessObject::GetOutput(2));
}

/** Get output hemispherical absorptance */
SailModel::SpectralResponseType* SailModel::GetHemisphericalAbsorptance()
{
  if (this->GetNumberOfOutputs() < 4)
  {
    // exit
    return nullptr;
  }
  return static_cast<SpectralResponseType*>(this->itk::ProcessObject::GetOutput(3));
}

/** Set Parameters */
void SailModel::SetInput(const ParametersType& params)
{

  if (params.Size() != 8)
    itkExceptionMacro(<< "Must have 8 parameters in that order : LAI, Angl, PSoil, Skyl, HSpot, TTS, TTO, PSI");
  this->SetParameters(params);
  m_LAI   = params[0];
  m_Angl  = params[1];
  m_PSoil = params[2];
  m_Skyl  = params[3];
  m_HSpot = params[4];
  m_TTS   = params[5];
  m_TTO   = params[6];
  m_PSI   = params[7];
}

/** Get Parameters */
const SailModel::ParametersType SailModel::GetInput()
{
  ParametersType parameters = this->GetParameters();
  if (parameters.Size() != 8)
  {
    parameters[0] = m_LAI;
    parameters[1] = m_Angl;
    parameters[2] = m_PSoil;
    parameters[3] = m_Skyl;
    parameters[4] = m_HSpot;
    parameters[5] = m_TTS;
    parameters[6] = m_TTO;
    parameters[7] = m_PSI;
    this->SetParameters(parameters);
  }
  return this->GetParameters();
}

/** Generate data */
void SailModel::GenerateData()
{

  SpectralResponseType::Pointer inRefl   = this->GetReflectance();
  SpectralResponseType::Pointer inTrans  = this->GetTransmittance();
  SpectralResponseType::Pointer outVRefl = this->GetViewingReflectance();
  SpectralResponseType::Pointer outHRefl = this->GetHemisphericalReflectance();
  SpectralResponseType::Pointer outVAbs  = this->GetViewingAbsorptance();
  SpectralResponseType::Pointer outHAbs  = this->GetHemisphericalAbsorptance();

  // LEAF ANGLE DISTRIBUTION
  double     rd = CONST_PI / 180;
  VectorType lidf;
  this->Calc_LIDF(m_Angl, lidf);

  double cts, cto, ctscto, tants, tanto, cospsi, dso;
  cts    = std::cos(rd * m_TTS);
  cto    = std::cos(rd * m_TTO);
  ctscto = cts * cto;
  tants  = std::tan(rd * m_TTS);
  tanto  = std::tan(rd * m_TTO);
  cospsi = std::cos(rd * m_PSI);
  dso    = std::sqrt(tants * tants + tanto * tanto - 2. * tants * tanto * cospsi);

  // angular distance, compensation of shadow length
  // Calculate geometric factors associated with extinction and scattering
  // Initialise sums
  double     ks  = 0;
  double     ko  = 0;
  double     bf  = 0;
  double     sob = 0;
  double     sof = 0;
  double     ttl, ctl, ksli, koli, sobli, sofli, bfli;
  double     chi_s, chi_o, frho, ftau;
  VectorType result(4);

  // Weighted sums over LIDF
  for (unsigned int i = 0; i < lidf.size(); ++i)
  {
    ttl = 2.5 + 5 * i; // leaf inclination discrete values
    ctl = std::cos(rd * ttl);
    // SAIL volume scattering phase function gives interception and portions to be
    // multiplied by rho and tau

    this->Volscatt(m_TTS, m_TTO, m_PSI, ttl, result);
    chi_s = result[0];
    chi_o = result[1];
    frho  = result[2];
    ftau  = result[3];

    //********************************************************************************
    //*                   SUITS SYSTEM COEFFICIENTS
    //*
    //*       ks  : Extinction coefficient for direct solar flux
    //*       ko  : Extinction coefficient for direct observed flux
    //*       att : Attenuation coefficient for diffuse flux
    //*       sigb : Backscattering coefficient of the diffuse downward flux
    //*       sigf : Forwardscattering coefficient of the diffuse upward flux
    //*       sf  : Scattering coefficient of the direct solar flux for downward diffuse flux
    //*       sb  : Scattering coefficient of the direct solar flux for upward diffuse flux
    //*       vf   : Scattering coefficient of upward diffuse flux in the observed direction
    //*       vb   : Scattering coefficient of downward diffuse flux in the observed direction
    //*       w   : Bidirectional scattering coefficient
    //********************************************************************************

    // Extinction coefficients
    ksli = chi_s / cts;
    koli = chi_o / cto;

    // Area scattering coefficient fractions
    sobli = frho * CONST_PI / ctscto;
    sofli = ftau * CONST_PI / ctscto;
    bfli  = ctl * ctl;
    ks    = ks + ksli * lidf[i];
    ko    = ko + koli * lidf[i];
    bf    = bf + bfli * lidf[i];
    sob   = sob + sobli * lidf[i];
    sof   = sof + sofli * lidf[i];
  }

  // Geometric factors to be used later with rho and tau
  double sdb, sdf, dob, dof, ddb, ddf;
  sdb = 0.5 * (ks + bf);
  sdf = 0.5 * (ks - bf);
  dob = 0.5 * (ko + bf);
  dof = 0.5 * (ko - bf);
  ddb = 0.5 * (1. + bf);
  ddf = 0.5 * (1. - bf);

  double lambda, Es, Ed, Rsoil1, Rsoil2, rsoil0, rho, tau, PARdiro, PARdifo;
  double sigb, sigf, att, m2, m, sb, sf, vb, vf, w;
  double tss, too, tsstoo, rdd, tdd, rsd, tsd, rdo, tdo, rsos, rsod;
  double rddt, rsdt, rdot, rsodt, rsost, rsot, dn;
  double e1, e2, rinf, rinf2, re, denom, J1ks, J2ks, J1ko, J2ko;
  double Ps, Qs, Pv, Qv, z, g1, g2, Tv1, Tv2, T1, T2, T3;
  double alf, sumint, fhot, x1, y1, f1, fint, x2, y2, f2;
  double resh, resv, absh, absv;

  int nbdata = sizeof(DataSpecP5B) / sizeof(DataSpec);
  for (int i = 0; i < nbdata; ++i)
  {
    lambda = DataSpecP5B[i].lambda;
    Es     = DataSpecP5B[i].directLight;       // 8
    Ed     = DataSpecP5B[i].diffuseLight;      // 9
    Rsoil1 = DataSpecP5B[i].drySoil;           // 10
    Rsoil2 = DataSpecP5B[i].wetSoil;           // 11
    rho    = inRefl->GetResponse()[i].second;  // rho = LRT[1][i];
    tau    = inTrans->GetResponse()[i].second; // tau = LRT[2][i];

    // direct/diffuse light
    // Es = direct
    // Ed = diffuse
    PARdiro = (1 - m_Skyl / 100.) * Es;
    PARdifo = (m_Skyl / 100.) * Ed;

    // Soil Reflectance Properties
    // rsoil1 = dry soil
    // rsoil2 = wet soil
    if (!m_UseSoilFile)
    {
      rsoil0 = m_PSoil * Rsoil1 + (1 - m_PSoil) * Rsoil2;
    }
    else
    {
      rsoil0 = m_SoilDataBase->GetReflectance(m_SoilIndex, lambda) * m_PSoil;
    }

    // Here rho and tau come in
    sigb = ddb * rho + ddf * tau;
    sigf = ddf * rho + ddb * tau;
    att  = 1. - sigf;
    m2   = (att + sigb) * (att - sigb);
    if (m2 <= 0)
      m2 = 0;
    m    = std::sqrt(m2);

    sb = sdb * rho + sdf * tau;
    sf = sdf * rho + sdb * tau;
    vb = dob * rho + dof * tau;
    vf = dof * rho + dob * tau;
    w  = sob * rho + sof * tau;

    // Here the LAI comes in
    // Outputs for the case LAI = 0
    if (m_LAI < 0)
    {
      // tss = 1;
      too    = 1;
      tsstoo = 1;
      rdd    = 0;
      tdd    = 1;
      rsd    = 0;
      tsd    = 0;
      rdo    = 0;
      tdo    = 0;
      // rso = 0;
      rsos = 0;
      rsod = 0;

      rddt  = rsoil0;
      rsdt  = rsoil0;
      rdot  = rsoil0;
      rsodt = 0;
      rsost = rsoil0;
      // rsot = rsoil0;
    }

    // Other cases (LAI > 0)
    e1    = exp(-m * m_LAI);
    e2    = e1 * e1;
    rinf  = (att - m) / sigb;
    rinf2 = rinf * rinf;
    re    = rinf * e1;
    denom = 1. - rinf2 * e2;

    J1ks = Jfunc1(ks, m, m_LAI);
    J2ks = Jfunc2(ks, m, m_LAI);
    J1ko = Jfunc1(ko, m, m_LAI);
    J2ko = Jfunc2(ko, m, m_LAI);

    Ps = (sf + sb * rinf) * J1ks;
    Qs = (sf * rinf + sb) * J2ks;
    Pv = (vf + vb * rinf) * J1ko;
    Qv = (vf * rinf + vb) * J2ko;

    rdd = rinf * (1. - e2) / denom;
    tdd = (1. - rinf2) * e1 / denom;
    tsd = (Ps - re * Qs) / denom;
    rsd = (Qs - re * Ps) / denom;
    tdo = (Pv - re * Qv) / denom;
    rdo = (Qv - re * Pv) / denom;

    tss = exp(-ks * m_LAI);
    too = exp(-ko * m_LAI);
    z   = Jfunc3(ks, ko, m_LAI);
    g1  = (z - J1ks * too) / (ko + m);
    g2  = (z - J1ko * tss) / (ks + m);

    Tv1 = (vf * rinf + vb) * g1;
    Tv2 = (vf + vb * rinf) * g2;
    T1  = Tv1 * (sf + sb * rinf);
    T2  = Tv2 * (sf * rinf + sb);
    T3  = (rdo * Qs + tdo * Ps) * rinf;

    // Multiple scattering contribution to bidirectional canopy reflectance
    rsod = (T1 + T2 - T3) / (1. - rinf2);

    // Treatment of the hotspot-effect
    alf = 1e6;
    // Apply correction 2/(K+k) suggested by F.-M. Bron
    if (m_HSpot > 0)
      alf = (dso / m_HSpot) * 2. / (ks + ko);
    if (alf > 200)
      alf = 200;
    if (alf == 0)
    {
      // The pure hotspot - no shadow
      tsstoo = tss;
      sumint = (1 - tss) / (ks * m_LAI);
    }
    else
    {
      // Outside the hotspot
      fhot = m_LAI * std::sqrt(ko * ks);
      // Integrate by exponential Simpson method in 20 steps
      // the steps are arranged according to equal partitioning
      // of the slope of the joint probability function
      x1     = 0;
      y1     = 0;
      f1     = 1;
      fint   = (1. - exp(-alf)) * 0.05;
      sumint = 0;

      for (unsigned int j = 1; j <= 20; ++j)
      {
        if (j < 20)
          x2 = -std::log(1. - j * fint) / alf;
        else
          x2   = 1;
        y2     = -(ko + ks) * m_LAI * x2 + fhot * (1. - exp(-alf * x2)) / alf;
        f2     = exp(y2);
        sumint = sumint + (f2 - f1) * (x2 - x1) / (y2 - y1);
        x1     = x2;
        y1     = y2;
        f1     = f2;
      }
      tsstoo = f1;
    }

    // Bidirectional reflectance
    // Single scattering contribution
    rsos = w * m_LAI * sumint;
    // Total canopy contribution
    // rso=rsos+rsod;
    // Interaction with the soil
    dn = 1. - rsoil0 * rdd;

    rddt = rdd + tdd * rsoil0 * tdd / dn;
    rsdt = rsd + (tsd + tss) * rsoil0 * tdd / dn;
    rdot = rdo + tdd * rsoil0 * (tdo + too) / dn;

    rsodt = rsod + ((tss + tsd) * tdo + (tsd + tss * rsoil0 * rdd) * too) * rsoil0 / dn;
    rsost = rsos + tsstoo * rsoil0;
    rsot  = rsost + rsodt;

    resh = (rddt * PARdifo + rsdt * PARdiro) / (PARdiro + PARdifo);
    resv = (rdot * PARdifo + rsot * PARdiro) / (PARdiro + PARdifo);

    absh = (1 - rddt - (1 - rsoil0) * (tdd + (tdd * rdd * rsoil0) / dn));
    absv = (1 - rsdt - (1 - rsoil0) * (tss + (tss * rsoil0 * rdd + tsd) / dn));

    SpectralResponseType::PairType response;
    response.first  = lambda / 1000.0;
    response.second = resh;
    outHRefl->GetResponse().push_back(response);
    response.second = resv;
    outVRefl->GetResponse().push_back(response);
    response.second = absh;
    outHAbs->GetResponse().push_back(response);
    response.second = absv;
    outVAbs->GetResponse().push_back(response);
  }
  m_FCoverView = 1 - too;
}

void SailModel::Calc_LIDF(const double a, VectorType& lidf) const
{
  int        ala = a;
  VectorType freq;
  Campbell(ala, freq);
  lidf = freq;
}

void SailModel::Campbell(const double ala, VectorType& freq) const
{
  unsigned int n      = 18;
  double       excent = exp(-1.6184e-5 * std::pow(ala, 3) + 2.1145e-3 * ala * ala - 1.2390e-1 * ala + 3.2491);
  double       sum    = 0;
  unsigned int tx2, tx1;
  double       tl1, tl2, x1, x2, v, alpha, alpha2, x12, x22, alpx1, alpx2, dum, almx1, almx2;
  VectorType   temp;

  for (unsigned int i = 0; i < n; ++i)
  {
    tx2 = 5 * i;
    tx1 = 5 * (i + 1);
    tl1 = tx1 * CONST_PI / 180;
    tl2 = tx2 * CONST_PI / 180;

    x1 = excent / sqrt(1. + excent * excent * std::tan(tl1) * std::tan(tl1));
    x2 = excent / sqrt(1. + excent * excent * std::tan(tl2) * std::tan(tl2));
    if (excent == 1)
    {
      v = std::abs(cos(tl1) - cos(tl2));
      temp.push_back(v);
      sum = sum + v;
    }
    else
    {
      alpha  = excent / std::sqrt(std::abs(1. - excent * excent));
      alpha2 = alpha * alpha;
      x12    = x1 * x1;
      x22    = x2 * x2;
      if (excent > 1)
      {
        alpx1 = std::sqrt(alpha2 + x12);
        alpx2 = std::sqrt(alpha2 + x22);
        dum   = x1 * alpx1 + alpha2 * log(x1 + alpx1);
        v     = std::abs(dum - (x2 * alpx2 + alpha2 * log(x2 + alpx2)));
        temp.push_back(v);
        sum = sum + v;
      }
      else
      {
        almx1 = sqrt(alpha2 - x12);
        almx2 = sqrt(alpha2 - x22);
        dum   = x1 * almx1 + alpha2 * asin(x1 / alpha);
        v     = std::abs(dum - (x2 * almx2 + alpha2 * asin(x2 / alpha)));
        temp.push_back(v);
        sum = sum + v;
      }
    }
  }

  for (unsigned int i = 0; i < n; ++i)
  {
    freq.push_back(temp[i] / sum);
  }
}

void SailModel::Volscatt(const double tts, const double tto, const double psi, const double ttl, VectorType& result) const
{

  double rd     = CONST_PI / 180;
  double costs  = std::cos(rd * tts);
  double costo  = std::cos(rd * tto);
  double sints  = std::sin(rd * tts);
  double sinto  = std::sin(rd * tto);
  double cospsi = std::cos(rd * psi);
  double psir   = rd * psi;
  double costl  = std::cos(rd * ttl);
  double sintl  = std::sin(rd * ttl);
  double cs     = costl * costs;
  double co     = costl * costo;
  double ss     = sintl * sints;
  double so     = sintl * sinto;

  // ..............................................................................
  //     betas -bts- and betao -bto- computation
  //     Transition angles (beta) for solar (betas) and view (betao) directions
  //     if thetav+thetal>pi/2, bottom side of the leaves is observed for leaf azimut
  //     interval betao+phi<leaf azimut<2pi-betao+phi.
  //     if thetav+thetal<pi/2, top side of the leaves is always observed, betao=pi
  //     same consideration for solar direction to compute betas
  // ..............................................................................
  double cosbts, cosbto, bts, ds, chi_s, bto, doo, chi_o;
  double btran1, btran2, bt1, bt2, bt3, t1, t2, denom, frho, ftau;

  cosbts = 5;
  if (std::abs(ss) > 1e-6)
    cosbts = -cs / ss;

  cosbto = 5;
  if (std::abs(so) > 1e-6)
    cosbto = -co / so;

  if (std::abs(cosbts) < 1)
  {
    bts = std::acos(cosbts);
    ds  = ss;
  }
  else
  {
    bts = CONST_PI;
    ds  = cs;
  }

  chi_s = 2. / CONST_PI * ((bts - CONST_PI * 0.5) * cs + std::sin(bts) * ss);

  if (std::abs(cosbto) < 1)
  {
    bto = std::acos(cosbto);
    doo = so;
  }
  else if (tto < 90)
  {
    bto = CONST_PI;
    doo = co;
  }
  else
  {
    bto = 0;
    doo = -co;
  }
  chi_o = 2. / CONST_PI * ((bto - CONST_PI * 0.5) * co + std::sin(bto) * so);

  // ..............................................................................
  //   Computation of auxiliary azimut angles bt1, bt2, bt3 used
  //   for the computation of the bidirectional scattering coefficient w
  // .............................................................................

  btran1 = std::abs(bts - bto);
  btran2 = CONST_PI - std::abs(bts + bto - CONST_PI);

  if (psir <= btran1)
  {
    bt1 = psir;
    bt2 = btran1;
    bt3 = btran2;
  }
  else
  {
    bt1 = btran1;
    if (psir <= btran2)
    {
      bt2 = psir;
      bt3 = btran2;
    }
    else
    {
      bt2 = btran2;
      bt3 = psir;
    }
  }

  t1 = 2. * cs * co + ss * so * cospsi;
  t2 = 0;
  if (bt2 > 0)
    t2  = sin(bt2) * (2. * ds * doo + ss * so * cos(bt1) * cos(bt3));
  denom = 2. * CONST_PI * CONST_PI;
  frho  = ((CONST_PI - bt2) * t1 + t2) / denom;
  ftau  = (-bt2 * t1 + t2) / denom;

  if (frho < 0)
    frho = 0;
  if (ftau < 0)
    ftau = 0;

  result[0] = chi_s;
  result[1] = chi_o;
  result[2] = frho;
  result[3] = ftau;
}

double SailModel::Jfunc1(const double k, const double l, const double t) const
{
  // J1 function with avoidance of singularity problem
  double v;
  double del = (k - l) * t;
  if (std::abs(del) > 1e-3)
  {
    v = (exp(-l * t) - exp(-k * t)) / (k - l);
    return v;
  }
  else
  {
    v = 0.5 * t * (exp(-k * t) + exp(-l * t)) * (1. - del * del / 12.);
    return v;
  }
}

double SailModel::Jfunc2(const double k, const double l, const double t) const
{
  double v;
  v = (1. - exp(-(k + l) * t)) / (k + l);
  return v;
}

double SailModel::Jfunc3(const double k, const double l, const double t) const
{
  double v;
  v = (1. - exp(-(k + l) * t)) / (k + l);
  return v;
}

void SailModel::PrintSelf(std::ostream& os, itk::Indent indent) const
{
  Superclass::PrintSelf(os, indent);
}

void SailModel::UseExternalSoilDB(std::shared_ptr<SoilDataBase> SoilDB, size_t SoilIndex)
{
  m_UseSoilFile  = true;
  m_SoilIndex    = SoilIndex;
  m_SoilDataBase = SoilDB;
}
} // end namespace otb


///////////////////////////////////////////////////////////////////////////////
// filter out cosmics events
// -------------------------
// 2013-03-01: first day of the sequester - require an event to have:
//
// 1. a reconstructed track with |d0| < 150mm
//
///////////////////////////////////////////////////////////////////////////////

// C++ includes
#include <iostream>
#include <stdexcept>
#include <string>

#include "TH1.h"

// Framework includes
#include "art/Framework/Core/EDFilter.h"
#include "art/Framework/Principal/Event.h"
#include "art/Framework/Core/ModuleMacros.h"
#include "art/Framework/Principal/Handle.h"
#include "art_root_io/TFileService.h"
//#include <boost/shared_ptr.hpp>
#include "fhiclcpp/ParameterSet.h"
#include "CalorimeterGeom/inc/DiskCalorimeter.hh"
#include "GeometryService/inc/GeomHandle.hh"
#include "GeometryService/inc/GeometryService.hh"
#include "RecoDataProducts/inc/CaloHitCollection.hh"
#include "MCDataProducts/inc/GenParticleCollection.hh"
#include "RecoDataProducts/inc/StrawHitCollection.hh"
#include "TrackerGeom/inc/Tracker.hh"

#include "BTrk/TrkBase/HelixParams.hh"
#include "RecoDataProducts/inc/KalRepPtrCollection.hh"

using namespace std;

namespace mu2e {

  class CosmicFilter : public art::EDFilter {
  public:

    struct Hist_t {
      TH1F*  fNTracks;			// number of reconstructed tracks
      TH1F*  fNGoodTracks;		// N(tracks passing all cuts)
      TH1F*  fTrackD0;			// impact parameter
      TH1F*  fTrackZ0[2];		// Z0 in the point of closest approach
      TH1F*  fP; 			// track momentum
    } _hist;
					// module parameters
    std::string  fTrkPatRecModuleLabel;
    int          fDiagLevel;
    double       fMaxD0;
    double       fMaxZ0;
					// otehr variables
    int          _nTracks;
					// Pointers to histograms & ntuples

  public:

    explicit CosmicFilter(fhicl::ParameterSet const& pset);
    virtual ~CosmicFilter() {}

    virtual void beginJob();
    virtual bool filter  (art::Event& event);

  };

    CosmicFilter::CosmicFilter(fhicl::ParameterSet const& pset):
    EDFilter{pset},
    fTrkPatRecModuleLabel(pset.get<std::string>   ("trkPatRecModuleLabel")),
    fDiagLevel           (pset.get<int>           ("diagLevel"           )),
    fMaxD0               (pset.get<double>        ("maxD0"               )), // in mm
    fMaxZ0               (pset.get<double>        ("maxZ0"               ))  // in mm
    {}

//-----------------------------------------------------------------------------
// Get access to the TFile service and book histograms
//-----------------------------------------------------------------------------
  void CosmicFilter::beginJob(){

    art::ServiceHandle<art::TFileService> tfs;

    _hist.fTrackD0     = tfs->make<TH1F>("trk_d0"  ,"Track D0"       ,500, -500., 500.);
    _hist.fNTracks     = tfs->make<TH1F>("ntrk"    ,"N(tracks)"      ,100,    0., 100.);
    _hist.fNGoodTracks = tfs->make<TH1F>("ngtrk"   ,"N(good tracks)" ,100,    0., 100.);
    _hist.fTrackZ0[0]  = tfs->make<TH1F>("trk_z0_0","Track Z0[0]"    ,500,-2500.,2500.);
    _hist.fTrackZ0[1]  = tfs->make<TH1F>("trk_z0_1","Track Z0[1]"    ,500,-2500.,2500.);
    _hist.fP           = tfs->make<TH1F>("p"       ,"Track Mom (S=0)",200,    0., 400.);
  }

//-----------------------------------------------------------------------------
  bool CosmicFilter::filter(art::Event& anEvent) {

    const KalRep*   trk;
    double          d0, z0;
    int             n_good_tracks;

    bool rc = false;

    art::Handle<KalRepPtrCollection> krepsHandle;
    anEvent.getByLabel(fTrkPatRecModuleLabel,"DownstreameMinus", krepsHandle);
    const KalRepPtrCollection*  list_of_kreps(0);

    _nTracks = 0;
    if (krepsHandle.isValid()) {
      list_of_kreps = krepsHandle.product();
      _nTracks      = list_of_kreps->size();
    }

    n_good_tracks = 0;
    for (int i=0; i<_nTracks; i++) {
      trk = list_of_kreps->at(i).get();

      d0 = trk->helix(0).d0();
      z0 = trk->helix(0).z0();

      _hist.fTrackD0->Fill(d0);
      _hist.fTrackZ0[0]->Fill(z0);
      if (fabs(d0) < fMaxD0) _hist.fTrackZ0[1]->Fill(z0);

      if ((fabs(d0) < fMaxD0) && (fabs(z0) < fMaxZ0)) {
	n_good_tracks += 1;
      }

      CLHEP::Hep3Vector mom = trk->momentum(0); // at S=0
      _hist.fP->Fill(mom.mag());
    }

    _hist.fNTracks->Fill(_nTracks);
    _hist.fNGoodTracks->Fill(n_good_tracks);

    rc = (n_good_tracks > 0);

    return rc;
  }
}

using mu2e::CosmicFilter;
DEFINE_ART_MODULE(CosmicFilter);


/*! @file stats.cc
 *  @brief Statistical measures calculation.
 *  @author Markovtsev Vadim <v.markovtsev@samsung.com>
 *  @version 1.0
 *
 *  @section Notes
 *  This code partially conforms to Google C++ Style Guide.
 *
 *  @section Copyright
 *  Copyright © 2013 Samsung R&D Institute Russia
 *
 *  @section License
 *  Licensed to the Apache Software Foundation (ASF) under one
 *  or more contributor license agreements.  See the NOTICE file
 *  distributed with this work for additional information
 *  regarding copyright ownership.  The ASF licenses this file
 *  to you 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 "src/transforms/stats.h"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wold-style-cast"
#include <boost/regex.hpp>
#pragma GCC diagnostic pop
#include <cmath>
#include <simd/instruction_set.h>

namespace sound_feature_extraction {
namespace transforms {

std::set<StatsType> Parse(const std::string& value,
                          identity<std::set<StatsType>>) {
  static const std::unordered_map<std::string, StatsType> map {
    { internal::kStatsTypeAverageStr, kStatsTypeAverage },
    { internal::kStatsTypeStdDeviationStr, kStatsTypeStdDeviation },
    { internal::kStatsTypeSkewnessStr, kStatsTypeSkewness },
    { internal::kStatsTypeKurtosisStr, kStatsTypeKurtosis }
  };

  static const boost::regex all_regex("^\\s*(\\w+\\s*(\\s|$))+");
  boost::smatch match;
  if (!boost::regex_match(value, match, all_regex)) {
    throw InvalidParameterValueException();
  }

  if (value == internal::kStatsTypeAllStr) {
    return { kStatsTypeAverage, kStatsTypeStdDeviation, kStatsTypeSkewness,
             kStatsTypeKurtosis };
  }
  std::set<StatsType> ret;
  std::transform(boost::sregex_token_iterator(value.begin(), value.end(),
                                            boost::regex("\\s*(\\w+)\\s*"),
                                            1),
                 boost::sregex_token_iterator(),
                 std::inserter(ret, ret.begin()),
                 [](const std::string& subval) {
    auto stit = map.find(subval);
    if (stit == map.end()) {
      throw InvalidParameterValueException();
    }
    return stit->second;
  });

  return ret;
}

const std::unordered_map<int, Stats::CalculateFunc> Stats::kStatsFuncs {
  { kStatsTypeAverage, Stats::CalculateAverage },
  { kStatsTypeStdDeviation, Stats::CalculateStdDeviation },
  { kStatsTypeSkewness, Stats::CalculateSkewness },
  { kStatsTypeKurtosis, Stats::CalculateKurtosis }
};

Stats::Stats()
    : types_(kDefaultStatsTypes()),
      interval_(kDefaultInterval),
      overlap_(kDefaultOverlap) {
}

ALWAYS_VALID_TP(Stats, types)

bool Stats::validate_interval(const int& value) noexcept {
  return value >= 2;
}

bool Stats::validate_overlap(const int& value) noexcept {
  return value >= 0;
}

void Stats::Initialize() const {
  if (interval_ > 0 && overlap_ >= interval_) {
    throw InvalidParameterValueException("overlap", std::to_string(overlap_),
                                         HostName());
  }
  if (input_format_->Size() < static_cast<size_t>(interval_)) {
    throw UnableToCalculateStatsException(input_format_->Size(), interval_);
  }
}

size_t Stats::OnInputFormatChanged(size_t buffersCount) {
  if (interval_ != 0) {
    int step = interval_ - overlap_;
    auto ratio = (input_format_->Size() - interval_) / step + 1;
    if (((input_format_->Size() - interval_) % step) == 0) {
      output_format_->SetSize(ratio * types_.size());
    } else {
      output_format_->SetSize((ratio + 1) * types_.size());
    }
  } else {
    output_format_->SetSize(types_.size());
  }
  return buffersCount;
}

void Stats::Do(const float* in, float* out) const noexcept {
  float rawMoments[kStatsTypeCount];
  if (interval_ == 0) {
    CalculateRawMoments(true, in, 0, input_format_->Size(), rawMoments);
    Calculate(rawMoments, out);
  } else {
    size_t i;
    size_t step = interval_ - overlap_;
    for (i = 0; i < input_format_->Size() - interval_ + 1; i += step) {
      CalculateRawMoments(true, in, i, interval_, rawMoments);
      Calculate(rawMoments, out + i / step * types_.size());
    }
    if ((input_format_->Size() - interval_) % step != 0) {
      int index = input_format_->Size() - interval_;
      CalculateRawMoments(true, in, index, interval_, rawMoments);
      Calculate(rawMoments, out + i / step * types_.size());
    }
  }
}

void Stats::Calculate(const float* rawMoments, float* out) const noexcept {
  for (auto stat : types_) {
    int sind = 0;
    int istat = stat;
    while (istat >>= 1) {
      sind++;
    }
    out[sind] = kStatsFuncs.find(stat)->second(rawMoments);
  }
}

void Stats::CalculateRawMoments(bool simd, const float* in, int startIndex,
                                int length, float* rawMoments) noexcept {
  float avg1 = 0, avg2 = 0, avg3 = 0, avg4 = 0;
  auto end_index = startIndex + length;
  if (simd) {
#ifdef __AVX__
    __m256 avg1vec = _mm256_setzero_ps();
    __m256 avg2vec = _mm256_setzero_ps();
    __m256 avg3vec = _mm256_setzero_ps();
    __m256 avg4vec = _mm256_setzero_ps();
    for (int i = startIndex; i < end_index - 7; i+=8) {
      __m256 val = _mm256_loadu_ps(in + i);
      avg1vec = _mm256_add_ps(avg1vec, val);
      __m256 val2 = _mm256_mul_ps(val, val);
      avg2vec = _mm256_add_ps(avg2vec, val2);
      val = _mm256_mul_ps(val2, val);
      avg3vec = _mm256_add_ps(avg3vec, val);
      val2 = _mm256_mul_ps(val2, val2);
      avg4vec = _mm256_add_ps(avg4vec, val2);
    }
    avg1vec = _mm256_hadd_ps(avg1vec, avg1vec);
    avg1vec = _mm256_hadd_ps(avg1vec, avg1vec);
    avg1 += _mm256_get_ps(avg1vec, 0);
    avg1 += _mm256_get_ps(avg1vec, 4);
    avg2vec = _mm256_hadd_ps(avg2vec, avg2vec);
    avg2vec = _mm256_hadd_ps(avg2vec, avg2vec);
    avg2 += _mm256_get_ps(avg2vec, 0);
    avg2 += _mm256_get_ps(avg2vec, 4);
    avg3vec = _mm256_hadd_ps(avg3vec, avg3vec);
    avg3vec = _mm256_hadd_ps(avg3vec, avg3vec);
    avg3 += _mm256_get_ps(avg3vec, 0);
    avg3 += _mm256_get_ps(avg3vec, 4);
    avg4vec = _mm256_hadd_ps(avg4vec, avg4vec);
    avg4vec = _mm256_hadd_ps(avg4vec, avg4vec);
    avg4 += _mm256_get_ps(avg4vec, 0);
    avg4 += _mm256_get_ps(avg4vec, 4);
    for (int i = ((end_index) & ~0x7); i < end_index; i++) {
      float v = in[i];
      avg1 += v;
      float v2 = v * v;
      avg2 += v2;
      v *= v2;
      avg3 += v;
      v2 *= v2;
      avg4 += v2;
    }
  } else {
#elif defined(__ARM_NEON__)
    float32x4_t avg1vec = vdupq_n_f32(0);
    float32x4_t avg2vec = vdupq_n_f32(0);
    float32x4_t avg3vec = vdupq_n_f32(0);
    float32x4_t avg4vec = vdupq_n_f32(0);
    for (int i = startIndex; i < end_index - 3; i+=4) {
      float32x4_t val = vld1q_f32(in + i);
      avg1vec = vaddq_f32(avg1vec, val);
      float32x4_t val2 = vmulq_f32(val, val);
      avg2vec = vaddq_f32(avg2vec, val2);
      val = vmulq_f32(val2, val);
      avg3vec = vaddq_f32(avg3vec, val);
      val2 = vmulq_f32(val2, val2);
      avg4vec = vaddq_f32(avg4vec, val2);
    }
    avg1 += vgetq_lane_f32(avg1vec, 0);
    avg1 += vgetq_lane_f32(avg1vec, 1);
    avg1 += vgetq_lane_f32(avg1vec, 2);
    avg1 += vgetq_lane_f32(avg1vec, 3);
    avg2 += vgetq_lane_f32(avg2vec, 0);
    avg2 += vgetq_lane_f32(avg2vec, 1);
    avg2 += vgetq_lane_f32(avg2vec, 2);
    avg2 += vgetq_lane_f32(avg2vec, 3);
    avg3 += vgetq_lane_f32(avg3vec, 0);
    avg3 += vgetq_lane_f32(avg3vec, 1);
    avg3 += vgetq_lane_f32(avg3vec, 2);
    avg3 += vgetq_lane_f32(avg3vec, 3);
    avg4 += vgetq_lane_f32(avg4vec, 0);
    avg4 += vgetq_lane_f32(avg4vec, 1);
    avg4 += vgetq_lane_f32(avg4vec, 2);
    avg4 += vgetq_lane_f32(avg4vec, 3);
    for (int i = ((end_index) & ~0x3); i < end_index; i++) {
      float v = in[i];
      avg1 += v;
      float v2 = v * v;
      avg2 += v2;
      v *= v2;
      avg3 += v;
      v2 *= v2;
      avg4 += v2;
    }
  } else {
#else
  } {
#endif
    for (int i = startIndex; i < end_index; i++) {
      float v = in[i];
      avg1 += v;
      float v2 = v * v;
      avg2 += v2;
      v *= v2;
      avg3 += v;
      v2 *= v2;
      avg4 += v2;
    }
  }
  avg1 /= length;
  avg2 /= length;
  avg3 /= length;
  avg4 /= length;
  rawMoments[0] = avg1;
  rawMoments[1] = avg2;
  rawMoments[2] = avg3;
  rawMoments[3] = avg4;
}

float Stats::CalculateAverage(const float* rawMoments) noexcept {
  return rawMoments[0];
}

float Stats::CalculateStdDeviation(const float* rawMoments) noexcept {
  auto value = rawMoments[1] - rawMoments[0] * rawMoments[0];
  if (value < 0) {
    return 0;
  }
  value = sqrtf(value);
  return value;
}

float Stats::CalculateSkewness(const float* rawMoments) noexcept {
  float avg1 = rawMoments[0];
  float avg2 = rawMoments[1];
  float avg3 = rawMoments[2];
  double u2 = avg2 - avg1 * avg1;
  if (u2 <= 0) {
    return 0;
  }
  double u3 = avg3 - 3 * avg2 * avg1 + 2 * avg1 * avg1 * avg1;
  auto value = u3 / (sqrt(u2) * u2);
  return value;
}

float Stats::CalculateKurtosis(const float* rawMoments) noexcept {
  float avg1 = rawMoments[0];
  float avg2 = rawMoments[1];
  float avg3 = rawMoments[2];
  float avg4 = rawMoments[3];
  double u2 = avg2 - avg1 * avg1;
  if (u2 == 0) {
    return -2.f;
  }
  double u4 = avg4 - 4 * avg3 * avg1 + 6 * avg2 * avg1 * avg1
      - 3 * avg1 * avg1 * avg1 * avg1;
  auto value = u4 / (u2 * u2) - 3;
  return value;
}

RTP(Stats, types)
RTP(Stats, interval)
RTP(Stats, overlap)
REGISTER_TRANSFORM(Stats);

}  // namespace transforms
}  // namespace sound_feature_extraction


//!
//! Contains the interface for the thermal forward model.
//!
//! \file fwdmodel/thermal.hpp
//! \author Lachlan McCalman
//! \date 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include "fwdmodel/fwd.hpp"
#include <boost/multi_array.hpp>

namespace obsidian
{
  namespace fwd
  {
    boost::multi_array<double, 3> fillAndPad(const Eigen::VectorXd& values, const ThermalSpec& spec);

    boost::multi_array<double, 3> eastWest(const boost::multi_array<double, 3>& pad);

    boost::multi_array<double, 3> cells(const boost::multi_array<double, 3>& pad, uint axis);

    boost::multi_array<double, 3> isocells(const boost::multi_array<double, 3>& pad);

    boost::multi_array<double, 3> temp(const boost::multi_array<double, 3>& eastwest, const boost::multi_array<double, 3>& northsouth,
                                       const boost::multi_array<double, 3>& updown, const boost::multi_array<double, 3>& sCells,
                                       const Eigen::MatrixXd& tempZ0, const Eigen::MatrixXd& zLowBound, bool zLowBoundIsHeatFlow,
                                       double xSize, double ySize, double zSize);

  } // namespace fwd
} // namespace obsidian


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_AVX_SIMD_FUNCTION_REVERSE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX_SIMD_FUNCTION_REVERSE_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/combine.hpp>
#include <boost/simd/function/slice.hpp>
#include <boost/simd/detail/dispatch/meta/as_floating.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( reverse_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::type64_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( const A0 & a0 ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<A0>;
      auto const b0 = bitwise_cast<f_t>(a0);
      return bitwise_cast<A0>(f_t(_mm256_permute_pd( _mm256_permute2f128_pd(b0,b0,1), 5 )));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( reverse_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::type32_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( const A0 & a0 ) const BOOST_NOEXCEPT
    {
      using f_t = bd::as_floating_t<A0>;
      auto const b0 = bitwise_cast<f_t>(a0);
      auto const tmp = _mm256_permute_ps(b0, _MM_SHUFFLE(0,1,2,3) );
      return bitwise_cast<A0>(f_t(_mm256_permute2f128_ps(tmp,tmp,1)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( reverse_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::fundamental_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( const A0 & a0 ) const BOOST_NOEXCEPT
    {
      return combine( reverse(slice_high(a0)), reverse(slice_low(a0)) );
    }
  };
} } }

#endif


#include <iostream>
#include <opencv2/opencv.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <pcl/common/time.h>
#include <unistd.h>
#include <math.h>

#include "Mapping.h"
#include "Map_plane.h"
#include "Frame.h"
#include "tictoc_profiler/profiler.hpp"
#include "pop_up_wall/popup_plane.h"

#include <ros/ros.h>
#include <ros/package.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/Image.h>
#include <message_filters/sync_policies/exact_time.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>

#include <Eigen/Dense>
#include <Eigen/Geometry>

// for recording data analysis
#include <fstream>
#include <iostream>
#include <string>
#include <sstream>

using namespace std;
using namespace Eigen;
using namespace cv;

const double PI=3.14159265;
const int recordEveryNth=10;
int counter=-1;
int marker_int=0;

template<typename Derived>
inline bool is_finite(const Eigen::MatrixBase<Derived>& x)
{
	return ((x.array() == x.array())).all();
}

geometry_msgs::Pose posenode_to_geomsgs(const isam::Pose3d &pose)
{
	geometry_msgs::Pose pose_msg;
	Eigen::Matrix4d wTo = pose.wTo();

	pose_msg.position.x=wTo(0,3);
	pose_msg.position.y=wTo(1,3);
	pose_msg.position.z=wTo(2,3);

	Matrix3d pose_rot=wTo.block(0,0,3,3);
	Eigen::Quaterniond pose_quat(pose_rot);
	pose_msg.orientation.x = pose_quat.x();
	pose_msg.orientation.y = pose_quat.y();
	pose_msg.orientation.z = pose_quat.z();
	pose_msg.orientation.w = pose_quat.w();
	return pose_msg;
}

class plane_slam
{
public:
	plane_slam(ros::NodeHandle &nn);
	~plane_slam();

	cv::Mat _color;
	ros::Time _timestamp;
	std_msgs::Header img_header;
	string world_frame_id,sensor_frame_id;

	Eigen::Matrix3f Kalib;
	Eigen::Matrix4d InitToWolrd;  //odometry initial frame
	int width;
	int height;
	ros::NodeHandle n;

	shared_ptr<Mapper_mono> mapper;

	// Plane pop up object
	popup_plane *pop_up_obj;

	ros::Publisher pub_rect_raw_img,pub_rect_raw_sync_img, pub_cnn_img, pub_crf_img, pup_polygon_img, pup_good_polygon_img, pub_cnn_blend,pub_cnn_label, pub_crf_blend;
	ros::Publisher pub_pose_sync,pub_pose_odom_sync,pub_modified_odom_pose;
	ros::Publisher pub_slam_pose,pub_slam_bound_polygon,pub_slam_bound_polygon_hist,pub_curr_bound_polygon;
	ros::Publisher pub_curr_cloud,pub_all_cloud;

	// purely plane slam
	void img_callback_cnn_crf(const sensor_msgs::Image::ConstPtr& msg_image,const sensor_msgs::Image::ConstPtr& label_image);

	// for the current frame, draw the bounding polygon for each plane, each bounding polygon is 3*N
	void pub_curr_bounding_plane(const vector<MatrixXf>& all_3d_bound_polygons_world);
	int max_slam_bounding_plane_marker_num;

	// draw all the landmark planes in SLAM, directly use the plane's latest bounding vertex (updated by projection in after each slam update)
	void pub_all_plane_landmark_poly(const vector<Map_plane*> &all_plane_landmarks);

	// draw all the previouly planes in each frames.
	void pub_all_hist_frame_planes_poly(const std::vector< tracking_frame* >& all_frames);

protected:
  // camera  calibration parameters, default is ueye camera
	double calib_fx=371.975264;
	double calib_fy=315.632343;
	double calib_cx=372.163582;
	double calib_cy=250.592551;

	bool final_reproject_label_img=false;
	bool final_reproject_depth_img=false;
	bool save_edge_labelmap=false;
	bool use_loop_close=false;
	int last_frame_id = -1; // finally show all the reprojected point cloud
	cv_bridge::CvImageConstPtr cv_ptr_rgb_img,cv_ptr_label;

	bool if_record_pose=false;
	ofstream costsLogfile;

	std::string package_path;
};

plane_slam::plane_slam(ros::NodeHandle &nn)
{
	n=nn;
// 	_timestamp=0;

	n.param ("/planar_test/package_path", package_path, package_path); // no '/' at last

	n.param ("/planar_test/final_reproject_label_img", final_reproject_label_img, final_reproject_label_img);
	n.param ("/planar_test/final_reproject_depth_img", final_reproject_depth_img, final_reproject_depth_img);
	n.param ("/planar_test/save_edge_labelmap", save_edge_labelmap, save_edge_labelmap);
	n.param ("/planar_test/use_loop_close", use_loop_close, use_loop_close);
	n.param ("/planar_test/last_frame_id", last_frame_id, last_frame_id);
	n.param ("/planar_test/if_record_pose", if_record_pose, if_record_pose);

	n.param ("/planar_test/calib_fx", calib_fx, calib_fx);
	n.param ("/planar_test/calib_fy", calib_fy, calib_fy);
	n.param ("/planar_test/calib_cx", calib_cx, calib_cx);
	n.param ("/planar_test/calib_cy", calib_cy, calib_cy);

	Kalib << calib_fx, 0, calib_fy,
		0, calib_cx, calib_cy,
		0,   0,   1;

	world_frame_id="/world";
	sensor_frame_id="/camera_rgb_optical_frame";

	pop_up_obj = new popup_plane();
	pop_up_obj->set_calibration(Kalib);

	pop_up_obj->Init_python();  //whether using python funtion

	pop_up_obj->walllength_threshold = 3.0;

// 	pop_up_obj->simple_polygon_mode = true;
// 	pop_up_obj->plane_cam_dist_thre = 50;
// 	pop_up_obj->ceiling_threshold = 2.8;

        InitToWolrd<<1, 0, 0, 0,
		     0, 0, 1, 0,
		     0,-1, 0, 1,
		     0, 0, 0, 1;

	mapper = make_shared<Mapper_mono>();
	mapper->calib=Kalib;
	mapper->inv_calib=Kalib.inverse();
	mapper->initialize_orb_matching();
	mapper->pop_up_obj=pop_up_obj;

	pub_rect_raw_img = n.advertise<sensor_msgs::Image>("/rect_color_raw", 10);
	pub_rect_raw_sync_img = n.advertise<sensor_msgs::Image>("/rect_color_raw_sync", 10);
	pub_cnn_img = n.advertise<sensor_msgs::Image>("/cnn_pred", 10);
	pub_cnn_label = n.advertise<sensor_msgs::Image>("/cnn_label", 10);
	pub_crf_img = n.advertise<sensor_msgs::Image>("/crf_opti", 10);
	pub_cnn_blend = n.advertise<sensor_msgs::Image>("/cnn_pred_blend", 10);
	pub_crf_blend = n.advertise<sensor_msgs::Image>("/crf_opti_blend", 10);
	pup_polygon_img = n.advertise<sensor_msgs::Image>("/pop_polygon", 10);
	pup_good_polygon_img = n.advertise<sensor_msgs::Image>("/pop_good_polygon", 10);

	pub_slam_bound_polygon = n.advertise<visualization_msgs::MarkerArray>( "/slam_plane_3d_polygons", 10 );
	pub_slam_bound_polygon_hist = n.advertise<visualization_msgs::MarkerArray>( "/slam_plane_3d_polygons_hist", 10 );
	pub_curr_bound_polygon = n.advertise<visualization_msgs::MarkerArray>( "/curr_plane_3d_polygons", 10 );

	pub_slam_pose = n.advertise<geometry_msgs::PoseArray>( "/slam_pose", 10 );
	pub_pose_sync=n.advertise<geometry_msgs::PoseStamped>( "/lsd_slam/pose_sync", 10 );
	pub_pose_odom_sync=n.advertise<nav_msgs::Odometry>( "/lsd_slam/pose_odom_sync", 10 );
	pub_modified_odom_pose=n.advertise<nav_msgs::Odometry>( "/lsd_slam/corrput_odom_pose", 10 );

	pub_curr_cloud = n.advertise<pclRGB> ("pop_cloud", 50);
	pub_all_cloud = n.advertise<pclRGB> ("pop_cloud_all", 50);

	max_slam_bounding_plane_marker_num=0;

	if (if_record_pose){
	    string cost_record_name=package_path+"/temp/slam_pose_array.txt";
	    costsLogfile.open(cost_record_name.c_str());
	}
}

plane_slam::~plane_slam()
{
	if (if_record_pose) costsLogfile.close();
	delete pop_up_obj;
}

void plane_slam::pub_all_plane_landmark_poly(const vector<Map_plane*> &all_plane_landmarks)
{
	visualization_msgs::MarkerArray plane_markers;
	visualization_msgs::Marker marker;
	marker.header=img_header;
	marker.header.frame_id="/world";
	marker.id = 0; //0
	marker.type = visualization_msgs::Marker::LINE_STRIP;
	marker.action = visualization_msgs::Marker::ADD;
	marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; marker.color.a = 1.0;
	marker.scale.x = 0.05;

	for (int c = 0; c < all_plane_landmarks.size(); c++){
	    if (all_plane_landmarks[c]->frame_seq_id<=counter-15)
	      if (all_plane_landmarks[c]->being_tracked_times<10)  // for older frames, only visualize the plane that has been tracked over enough times.
		continue;
	    if (all_plane_landmarks[c]->frame_seq_id<=counter-8)
	      if (all_plane_landmarks[c]->being_tracked_times<5)
		continue;
	      if (all_plane_landmarks[c]->deteted_by_merge)  // if it is a dead node, being merged node, skip this
		  continue;
  // 	   cout<<"show plane frame id "<<all_plane_landmarks[c].frame_seq_id<<"  tracked time  "<<all_plane_landmarks[c].being_tracked_times<<endl;
	      marker.id++;
	      marker.points.resize(all_plane_landmarks[c]->plane_bound_close_3D_polys.cols());
	      for (int i=0;i<all_plane_landmarks[c]->plane_bound_close_3D_polys.cols();i++)
	      {
		marker.points[i].x=all_plane_landmarks[c]->plane_bound_close_3D_polys(0,i);
		marker.points[i].y=all_plane_landmarks[c]->plane_bound_close_3D_polys(1,i);
		marker.points[i].z=all_plane_landmarks[c]->plane_bound_close_3D_polys(2,i);
	      }
	      plane_markers.markers.push_back(marker);
	}
	for (int i=0;i<plane_markers.markers.size();i++) {
	    plane_markers.markers[i].color.r= (double)i/plane_markers.markers.size();
	    plane_markers.markers[i].color.g= 1-(double)i/plane_markers.markers.size();
	}
	pub_slam_bound_polygon.publish(plane_markers);
}

void plane_slam::pub_all_hist_frame_planes_poly(const std::vector< tracking_frame* >& all_frames)
{
	visualization_msgs::MarkerArray plane_markers;
	visualization_msgs::Marker marker;
	marker.header=img_header;
	marker.header.frame_id="/world";
	marker.id = 0; //0
	marker.type = visualization_msgs::Marker::LINE_STRIP;
	marker.action = visualization_msgs::Marker::ADD;
    //     marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 1.0; marker.color.a = 1;  // paper color
	marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; marker.color.a = 1;
	marker.scale.x = 0.05;

	for (int frame_ind=0;frame_ind<all_frames.size();frame_ind=frame_ind+2){  // every two frames
	    for (int plane_ind=0;plane_ind<all_frames[frame_ind]->all_3d_bound_polygons_good_world.size();plane_ind++){
		if (all_frames[frame_ind]->good_plane_indices(plane_ind)==0)  // don't show ground
		    continue;
		int tracked_times=all_frames[frame_ind]->observed_planes[plane_ind]->being_tracked_times;
		int frame_sequ_id=all_frames[frame_ind]->frame_seq_id;
		if ( frame_sequ_id<=counter-15 )  // for older frames, only visualize the plane that has been tracked over enough times.
		  if (tracked_times<10)
		    continue;
		if ( frame_sequ_id<=counter-8 )
		  if (tracked_times<5)
		    continue;
		marker.id++;
		marker.points.resize(all_frames[frame_ind]->all_3d_bound_polygons_good_world[plane_ind].cols());
		for (int i=0;i<all_frames[frame_ind]->all_3d_bound_polygons_good_world[plane_ind].cols();i++){
		  marker.points[i].x=all_frames[frame_ind]->all_3d_bound_polygons_good_world[plane_ind](0,i);
		  marker.points[i].y=all_frames[frame_ind]->all_3d_bound_polygons_good_world[plane_ind](1,i);
		  marker.points[i].z=all_frames[frame_ind]->all_3d_bound_polygons_good_world[plane_ind](2,i);
		}
		plane_markers.markers.push_back(marker);
	    }
	}
	for (int i=0;i<plane_markers.markers.size();i++){  // paper color not using this
	    plane_markers.markers[i].color.r= (double)i/plane_markers.markers.size();
	    plane_markers.markers[i].color.g= 1-(double)i/plane_markers.markers.size();
	}

	pub_slam_bound_polygon_hist.publish(plane_markers);
}

void plane_slam::pub_curr_bounding_plane(const vector<MatrixXf>& all_3d_bound_polygons_world)
{
	visualization_msgs::MarkerArray plane_markers;
	visualization_msgs::Marker marker;
	marker.header=img_header;
	marker.header.frame_id="/world";
	marker.id = 0; //0
	marker.type = visualization_msgs::Marker::LINE_STRIP;
	marker.action = visualization_msgs::Marker::ADD;
	marker.color.r = 0.0; marker.color.g = 0.0; marker.color.b = 1.0; marker.color.a = 1.0;
	marker.scale.x = 0.05;

	for (int c = 0; c < all_3d_bound_polygons_world.size(); c++){
	      marker.id++;
	      marker.points.resize(all_3d_bound_polygons_world[c].cols());
	      for (int i=0;i<all_3d_bound_polygons_world[c].cols();i++)
	      {
		marker.points[i].x=all_3d_bound_polygons_world[c](0,i);
		marker.points[i].y=all_3d_bound_polygons_world[c](1,i);
		marker.points[i].z=all_3d_bound_polygons_world[c](2,i);
	      }
	      plane_markers.markers.push_back(marker);
	}
	if (max_slam_bounding_plane_marker_num<marker.id)  // if last frame has more planes, some planes will still show if we don't push the void landmarks.
	    max_slam_bounding_plane_marker_num=marker.id;
	else{
	  while (plane_markers.markers.size()!=max_slam_bounding_plane_marker_num){
	    marker.id++;
	    visualization_msgs::Marker marker_new;
	    marker_new.scale.x=0.05;
	    marker_new.color.a=0;
	    marker_new.header.frame_id="/world";
	    marker_new.id=marker.id;
	    plane_markers.markers.push_back(marker_new);
	  }
	}
	pub_curr_bound_polygon.publish(plane_markers);
}

void plane_slam::img_callback_cnn_crf(const sensor_msgs::Image::ConstPtr& color_msg_img,const sensor_msgs::Image::ConstPtr& label_msg_img)
{
	counter++;
	cout<<"------------------------------------------------------------"<<endl;
	cout<<"processing kf "<<counter<<"   "<<color_msg_img->header.stamp<<endl;
	ca::Profiler::tictoc("process one frame");
        img_header=color_msg_img->header;
	width=color_msg_img->width;
	height=color_msg_img->height;

	isam::Pose3d modified_slam_odom_pose;   // lastest slam plus latest modifieded odometry
	isam::Pose3d modified_odometry;
	if (counter==0){
	    Eigen::Matrix4d curr_transToWolrd;   // a camera space point multiplied by this, goes to world frame.
	    curr_transToWolrd.setIdentity();
	    curr_transToWolrd<<-0.4563,  0.2444, -0.8556,-1.2432,  // tum fr3 no texture far initial pose by ground truth
				0.8898,  0.1249, -0.4388,1.9219,
				-0.0004, -0.9616, -0.2745,1.0547;
				 0,   0,   0,   1;
	    modified_odometry=isam::Pose3d(curr_transToWolrd);
	    modified_slam_odom_pose=modified_odometry;
	}else if (counter>0){
	    if (counter==1)
		modified_odometry=isam::Pose3d();   // zero odometry
	    else{
		//NOTE a simple constant motion model is used. can be replaced by more accurate odometry
	        modified_odometry=mapper->all_frames.back()->pose_vertex->value().ominus(mapper->all_frames[mapper->all_frames.size()-2]->pose_vertex->value());   // const motion assumption
	    }
	    modified_slam_odom_pose=mapper->all_frames.back()->pose_vertex->value().oplus(modified_odometry);
	}

	cv::Mat bgr_raw_img, label_img;
	cv_ptr_rgb_img = cv_bridge::toCvShare(color_msg_img,sensor_msgs::image_encodings::TYPE_8UC3);	//BGR format
	cv_ptr_rgb_img->image.copyTo(bgr_raw_img);
	ROS_ASSERT(bgr_raw_img.channels()==3);  // color image

	cv::Mat gray_img;
	cv::cvtColor(bgr_raw_img, gray_img, CV_BGR2GRAY);

	cv_ptr_label = cv_bridge::toCvShare(label_msg_img,label_msg_img->encoding);
	cv_ptr_label->image.copyTo(label_img);

	cv_bridge::CvImage out_image;
	out_image.header=img_header;

	if (label_msg_img->encoding==sensor_msgs::image_encodings::TYPE_32FC1){  // if it is cnn unary image,    crf image type is CV_8UC1
	    cv::resize(label_img,label_img,bgr_raw_img.size(),0,0, CV_INTER_LINEAR);  // resize to normal size
	    threshold(label_img,label_img,0.5,255,cv::THRESH_BINARY);  // threshold to be 0,255
	    label_img.convertTo(label_img,CV_8UC1);
	    out_image.image=label_img;
	    out_image.encoding=sensor_msgs::image_encodings::TYPE_8UC1;
	    pub_cnn_label.publish(out_image);
	}

	Mat blend_img;
	blend_segmentation(bgr_raw_img,label_img,blend_img);
	out_image.image=blend_img;
	out_image.encoding=sensor_msgs::image_encodings::TYPE_8UC3;
	pub_crf_blend.publish(out_image.toImageMsg());

	pop_up_obj->walllength_threshold = 3.0;
	pop_up_obj->simple_polygon_mode = false;
	pop_up_obj->ceiling_threshold = 5.0;
	pop_up_obj->line_lbd_ptr->use_LSD = false;

	// Plane pop up polygons
	ca::Profiler::tictoc("get edges");
	pop_up_obj->get_ground_edges(bgr_raw_img,label_img);
	ca::Profiler::tictoc("get edges");
	Matrix4d pop_up_pose=modified_slam_odom_pose.wTo();

	MatrixX4f utilized_ground_seg2d_lines  = pop_up_obj->ground_seg2d_lines_actual;     // whether include connected segments

	ca::Profiler::tictoc("get equation");
	pop_up_obj->get_plane_equation(utilized_ground_seg2d_lines,pop_up_pose.cast<float>());	// using latest plane slam pose plus odometry
	ca::Profiler::tictoc("get equation");

	if (!is_finite<MatrixX4f>(pop_up_obj->all_planes_sensor))  //check nan
	    ROS_ERROR_STREAM("detect nan elements in plane parameters");

	// publish good ground 2d polygons
	cv::Mat raw_img_cp=bgr_raw_img.clone();
	for (int i=0;i<utilized_ground_seg2d_lines.rows();i++)      // plot the lines
	{
	    if (check_element_in_vector(i,pop_up_obj->good_plane_indices))  // good lines in red, bad lines in blue
		line(raw_img_cp, Point(utilized_ground_seg2d_lines(i,0),utilized_ground_seg2d_lines(i,1)), \
	             Point(utilized_ground_seg2d_lines(i,2),utilized_ground_seg2d_lines(i,3)), Scalar(0,0,255),4 , 8, 0);
	    else
		line(raw_img_cp, Point(utilized_ground_seg2d_lines(i,0),utilized_ground_seg2d_lines(i,1)), \
	             Point(utilized_ground_seg2d_lines(i,2),utilized_ground_seg2d_lines(i,3)), Scalar(255,0,0),4 , 8, 0);
	}
	out_image.image=raw_img_cp;
	out_image.encoding=sensor_msgs::image_encodings::TYPE_8UC3;
	pup_good_polygon_img.publish(out_image.toImageMsg());

	// Plane pop up point cloud  this is only for visulization
	pop_up_obj->generate_cloud(bgr_raw_img, pop_up_pose.cast<float>(),true,true,true,save_edge_labelmap);
// 	if (counter % 10 == 0){
	    pop_up_obj->pcl_cloud_world->header.frame_id="/world";
	    pop_up_obj->pcl_cloud_world->header.seq=counter;
	    pop_up_obj->pcl_cloud_world->header.stamp=img_header.stamp.toNSec() / 1000ull;
	    pub_curr_cloud.publish(*(pop_up_obj->pcl_cloud_world));
// 	}

	cout<<"Detect plane number "<<pop_up_obj->good_plane_number<<endl;
	if (pop_up_obj->good_plane_number>0){  // use the good planes to do slam, not including manually connected edges
	      vector<Map_plane> new_planes(pop_up_obj->good_plane_number);
	      tracking_frame*  newframe( new tracking_frame);
	      newframe->good_plane_dist_cam.resize(pop_up_obj->good_plane_number);
	      vector<MatrixXf> all_3d_bound_polygons_good_world(pop_up_obj->good_plane_number);  //3d
	      for (int i=0;i< pop_up_obj->good_plane_number;i++)
	      {
		  int good_plane_ind=pop_up_obj->good_plane_indices[i]; // plane index in all the planes, including ground and manually connected plane
		  new_planes[i].temp_value= isam::Plane3d(pop_up_obj->all_planes_sensor.row(good_plane_ind).cast<double>());
		  new_planes[i].t_stamp=img_header.stamp;
		  new_planes[i].frame_seq_id=counter;
		  new_planes[i].plane_cloud=pop_up_obj->partplane_clouds_open_all[i];   // plane's point cloud
		  new_planes[i].plane_bound_close_2D_polys=pop_up_obj->all_closed_2d_bound_polygons[good_plane_ind];
		  new_planes[i].plane_bound_close_3D_polys=pop_up_obj->all_3d_bound_polygons_world[good_plane_ind];
		  new_planes[i].ground_seg2d_lines=utilized_ground_seg2d_lines;  // all the planes havs the same ground2d segmentation image
		  new_planes[i].frame_plane_indice=good_plane_ind;
		  new_planes[i].good_plane_indice=i;
		  new_planes[i].plane_dist_cam=pop_up_obj->all_plane_dist_to_cam[good_plane_ind];
		  new_planes[i].observer_frames.push_back(newframe);
		  new_planes[i].being_tracked_times = 1;
		  all_3d_bound_polygons_good_world[i]=pop_up_obj->all_3d_bound_polygons_world[good_plane_ind];
		  newframe->good_plane_dist_cam[i]=pop_up_obj->all_plane_dist_to_cam[good_plane_ind];
	      }
	      newframe->t_stamp=img_header.stamp;
	      newframe->frame_seq_id=counter;
	      newframe->frame_img=bgr_raw_img;
	      newframe->ground_seg2d_lines=new_planes[0].ground_seg2d_lines;
	      newframe->all_3d_bound_polygons_good_world=all_3d_bound_polygons_good_world;
	      newframe->all_3d_bound_polygons_world_raw=all_3d_bound_polygons_good_world;
	      newframe->partplane_clouds=pop_up_obj->partplane_clouds_open_all;
	      newframe->good_plane_indices=pop_up_obj->good_plane_indices;
	      newframe->temp_pose=modified_odometry;

	      pub_curr_bounding_plane(all_3d_bound_polygons_good_world);  // current 3d bounding box.
	      if (counter==last_frame_id)
		 mapper->last_frame_process=true;

	      ca::Profiler::tictoc("slam process");
	      mapper->processFrame(new_planes, newframe);
	      ca::Profiler::tictoc("slam process");
	      mapper->update_plane_measurement();
	}

	isam::Pose3d pose_node;
	geometry_msgs::PoseArray all_poses;
	for (int i = 0; i < mapper->all_frames.size(); i++){
	    pose_node = mapper->all_frames[i]->pose_vertex->value();
	    all_poses.poses.push_back(posenode_to_geomsgs(pose_node));
	}
	all_poses.header=img_header;
	all_poses.header.frame_id=world_frame_id;
	pub_slam_pose.publish(all_poses);

	if (counter==last_frame_id){
	      if (if_record_pose){
		  costsLogfile << "# timestamp tx ty tz qx qy qz qw"<<"\n";
		  for (int i=0;i<all_poses.poses.size();i++)
		  {
		    costsLogfile<<mapper->all_frames[i]->t_stamp<<" "<<all_poses.poses[i].position.x<<" "<<all_poses.poses[i].position.y<<" "<<all_poses.poses[i].position.z<<" ";
		    costsLogfile<<all_poses.poses[i].orientation.x<<" "<<all_poses.poses[i].orientation.y<<" "<<all_poses.poses[i].orientation.z<<" "<<all_poses.poses[i].orientation.w<<endl;
		  }
	      }
	}

// 	cout<<"mapper->all_landmarks   "<<mapper->all_landmarks.size()<<endl;

        pub_all_plane_landmark_poly(mapper->all_landmarks);
	pub_all_hist_frame_planes_poly(mapper->all_frames);

	ca::Profiler::tictoc("process one frame");

	if (counter==last_frame_id){
	    ROS_ERROR_STREAM("publish all cloud");
	    int cloud_seq_id=0;
	    for (int frame_ind=0;frame_ind<mapper->all_frames.size();frame_ind++) {
		Matrix4d latest_pose = mapper->all_frames[frame_ind]->pose_vertex->value().wTo();  // reproject to get plane measurements
		for (int plane_ind=0;plane_ind<mapper->all_frames[frame_ind]->partplane_clouds.size();plane_ind++){
		    int tracked_times=mapper->all_frames[frame_ind]->observed_planes[plane_ind]->being_tracked_times;
		    int frame_sequ_id=mapper->all_frames[frame_ind]->frame_seq_id;
    // 		    cout<<tracked_times<<endl;
		    if (!final_reproject_label_img){
		      if ( frame_sequ_id<=counter-10 ){
			  if (frame_ind %3 !=0)  // show every N frame's depth
			    continue;}
		      else{
			if (frame_ind %2 !=0)  // show every N frame's depth
			    continue;
		      }
		    }

		    if ( frame_sequ_id<=counter-15 )  // for older frames, only visualize the plane that has been tracked over enough times.
		      if (tracked_times<10)
			continue;
		    if ( frame_sequ_id<=counter-8 )
		      if (tracked_times<5)
			continue;
    		    if ( frame_sequ_id<=counter-4 )
    		      if (tracked_times<2)
    			continue;
		    pclRGB::Ptr plane_cloud=mapper->all_frames[frame_ind]->partplane_clouds[plane_ind];
		    for (int i=0;i<plane_cloud->points.size();i++){
			Vector3d raw_pt(plane_cloud->points[i].x,plane_cloud->points[i].y,plane_cloud->points[i].z);
			isam::Plane3d curr_plane=mapper->all_frames[frame_ind]->observed_planes[plane_ind]->plane_vertex->value();

			Vector3f updated_pt=curr_plane.project_to_plane(raw_pt).cast<float>();

    // 			double cam_pt_dist=(updated_pt.head(2)-latest_pose.col(3).head(2)).norm();
    // 			if (cam_pt_dist<5)  // if point too far from camera, don't show
    // 			    continue;

			plane_cloud->points[i].x=updated_pt(0);
			plane_cloud->points[i].y=updated_pt(1);
			plane_cloud->points[i].z=updated_pt(2);
		    }
		    plane_cloud->header.frame_id="/world";
		    plane_cloud->header.seq=cloud_seq_id;
		    pop_up_obj->pcl_cloud_world->header.stamp=(ros::Time::now().toNSec() / 1000ull)+cloud_seq_id*1000;
		    cloud_seq_id++;
		    pub_all_cloud.publish(*plane_cloud);
		    usleep(25000);  //sleep for 25ms
		}
	    }
	    ROS_ERROR_STREAM("finish pub cloud num "<<cloud_seq_id);
	    return;
	}

}

int main(int argc, char** argv)
{
	ca::Profiler::enable();

	ros::init(argc, argv, "pop_slam");
	ros::NodeHandle nh;
	ros::NodeHandle n;
	plane_slam planar(n);

	// whole plane slam without using pose odometry
	static message_filters::Subscriber<sensor_msgs::Image>  rgb_img_sub(nh, "/rect_color_raw" , 10);  //  /rect_color_raw   /mono/image_rect_color  rect_color_raw_sync
	static message_filters::Subscriber<sensor_msgs::Image>  crf_img_sub(nh , "/cnn_label", 10);  // crf_opti  // cnn_pred
	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image,sensor_msgs::Image> MySyncPolicy;
	static message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), rgb_img_sub, crf_img_sub);
	sync.registerCallback(boost::bind(&plane_slam::img_callback_cnn_crf,&planar, _1, _2));

	ros::spin();
	ca::Profiler::print_aggregated(std::cout);

	return 0;
}


// make sure we get a new main function here
#define BOOST_TEST_MAIN
#include "fixtures.h"
#include <boost/test/unit_test.hpp>

#include "gtfs/dataset.hpp"
#include "gtfs/read_csv.hpp"
#include "id/stop.hpp"

#include "geometric/bounding_box.hpp"
#include "geometric/coordinate.hpp"
#include "search/coordinate_to_stop.hpp"

#include <iostream>

using namespace nepomuk;

BOOST_AUTO_TEST_CASE(lookup_lines_from_stops)
{
    // This needs to be replaced by a dedicated fixture (see
    // https://github.com/mapbox/nepomuk/issues/37)
    gtfs::CSVDiscSource source(TRANSIT_THREE_LINES_EXAMPLE_FIXTURE);
    auto dataset = gtfs::readCSV(source);

    auto make_coordinate_lookup = [&]() {
        std::vector<std::pair<geometric::WGS84Coordinate, StopID>> data;
        std::for_each(dataset.stops.begin(), dataset.stops.end(), [&](auto const &element) {
            if (!element.location_type || *element.location_type == gtfs::LocationType::STOP)
                data.push_back(std::make_pair(element.location, element.id));
        });
        return search::CoordinateToStop(data);
    };

    auto coordinate_lookup = make_coordinate_lookup();

    geometric::WGS84Coordinate coordinate(
        geometric::makeLatLonFromDouble<geometric::FixedLongitude>(0.00501),
        geometric::makeLatLonFromDouble<geometric::FixedLatitude>(0.00511));

    auto closest_stop = coordinate_lookup.nearest(coordinate);
    BOOST_CHECK_EQUAL(closest_stop.first, StopID{0});

    geometric::WGS84Coordinate coordinate2(
        geometric::makeLatLonFromDouble<geometric::FixedLongitude>(0.004),
        geometric::makeLatLonFromDouble<geometric::FixedLatitude>(0.005));

    auto distance = geometric::distance(closest_stop.second, coordinate);

    auto const coords_in_circle = coordinate_lookup.all(coordinate, distance * 1.1);
    BOOST_CHECK(coords_in_circle.size() == 1 && coords_in_circle.front().first == StopID{0});

    auto const count_nearest = coordinate_lookup.nearest(coordinate, 1);
    BOOST_CHECK_EQUAL(count_nearest.front().first, StopID{0});

    geometric::WGS84BoundingBox bbox(coordinate2, coordinate);

    auto all = coordinate_lookup.all(bbox);
    BOOST_CHECK(all.size() == 1);
    BOOST_CHECK_EQUAL(all.front().first, StopID{0});
}


/*******************************************************************************
 *
 *  Filename    : MakeRooFit.cc
 *  Description : Making RooFit objects to be passed over to Higgs Combine
 *  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
 *
*******************************************************************************/
#include "TstarAnalysis/CompareDataMC/interface/SampleRooFitMgr.hh"
#include "TstarAnalysis/CompareDataMC/interface/FileNames.hh"
#include <string>
#include <vector>
#include <iostream>
#include <boost/exception/diagnostic_information.hpp>
#include <boost/program_options.hpp>

using namespace std;
namespace opt = boost::program_options;

//------------------------------------------------------------------------------
//   Static data members
//------------------------------------------------------------------------------
static SampleRooFitMgr* data;
static SampleRooFitMgr* mc;
static vector<SampleRooFitMgr*> signal_list;

//------------------------------------------------------------------------------
//   Helper functions
//------------------------------------------------------------------------------
void InitDataAndSignal();
void InitMC();

// in src/RooFit_SimFit.cc
extern void MakeSimFit(SampleRooFitMgr*, vector<SampleRooFitMgr*>&);

// in src/RooFit_SideBand.cc
extern void MakeCheckPlot(SampleRooFitMgr*);
extern void MakeSideBand(SampleRooFitMgr*,SampleRooFitMgr*);

// in src/RooFit_MCTemplate.cc
extern void MakeTemplate(SampleRooFitMgr*,SampleRooFitMgr*,vector<SampleRooFitMgr*>& );

// in src/RooFit_Bias.cc
extern void MakeBias(SampleRooFitMgr*,SampleRooFitMgr*,vector<SampleRooFitMgr*>&);

//------------------------------------------------------------------------------
//   Main control flow
//------------------------------------------------------------------------------

int main(int argc, char* argv[]) {

   opt::options_description desc("MakeRooFit options");
   opt::variables_map       vm;

   desc.add_options()
      ("help","produce help message and exit")
      ("channel", opt::value<string>(), "Channel to run" )
      ("method" , opt::value<string>(), "Which fitting method to run" )
      ("fitfunc", opt::value<string>(), "Which fitting function to use")
   ;

   try{
      opt::store(opt::parse_command_line(argc, argv, desc), vm);
      opt::notify(vm);
   } catch( boost::exception& e ){
      cerr << "Error parsing command!" << endl;
      cerr << boost::diagnostic_information(e) << endl;
      cerr << desc << endl;
      return 1;
   }

   if(vm.count("help")) {
      cerr << desc << endl;
      return 0;
   }

   if( vm.count("channel") ){
      SetChannelType( vm["channel"].as<string>() );
      SampleRooFitMgr::LoadJsonFile( GetJsonFile() );
   } else {
      cerr << "Settings for option [channel] not found!" << endl;
      cerr << desc << endl;
      return 1;
   }

   SampleRooFitMgr::x().setRange("FitRange",SampleRooFitMgr::MinFitMass(),SampleRooFitMgr::MaxMass());

   if( !vm.count("method") ){
      cerr << "options [method] not found!" << endl;
      cerr << desc << endl;
      return 1;
   } else {
      SetMethod( vm["method"].as<string>() );
   }

   if( !vm.count("fitfunc") ) {
      cerr << "option [fitfunc] not found!" << endl;
      cerr << desc << endl;
      return 1;
   } else {
      SetFitFunc( vm["fitfunc"].as<string>() );
   }

   if( GetMethod() == "SimFit"  ){
      cout  << "Running SimFit Method!" << endl;
      InitDataAndSignal();
      MakeSimFit(data,signal_list);
   } else if( GetMethod() == "Template" ){
      cout << "Running MC template method!" << endl;
      InitDataAndSignal();
      InitMC();
      MakeTemplate(data,mc,signal_list);
   } else if( GetMethod().find("Bias") != string::npos ){
      cout << "Running Bias check!" << endl;
      InitDataAndSignal();
      InitMC();
      MakeBias(data,mc,signal_list);
   } else if( GetMethod() == "SideBand"  ){
      cout << "Running side band method!" << endl;
      InitDataAndSignal();
      MakeCheckPlot(data);
      for( auto& sample : signal_list ){
         MakeSideBand(data,sample);
      }
   } else {
      cerr << "Unrecognized method!" << endl;
      return 1;
   }

   return 0;
}

//------------------------------------------------------------------------------
//   Helper function implementations
//------------------------------------------------------------------------------
void InitDataAndSignal()
{
   data = new SampleRooFitMgr("Data");
   data->MakeReduceDataSet("FitRange",RooFit::CutRange("FitRange"));

   signal_list.push_back( new SampleRooFitMgr("tstar_M700"));
   signal_list.push_back( new SampleRooFitMgr("tstar_M800"));
   signal_list.push_back( new SampleRooFitMgr("tstar_M900"));
   signal_list.push_back( new SampleRooFitMgr("tstar_M1000"));
   signal_list.push_back( new SampleRooFitMgr("tstar_M1100"));
   signal_list.push_back( new SampleRooFitMgr("tstar_M1200"));
   // signal_list.push_back( new SampleRooFitMgr("tstar_M1300"));
   signal_list.push_back( new SampleRooFitMgr("tstar_M1400"));
   signal_list.push_back( new SampleRooFitMgr("tstar_M1500"));
   signal_list.push_back( new SampleRooFitMgr("tstar_M1600"));
}

void InitMC()
{
   mc = new SampleRooFitMgr("Background");
}


/*********************************************************************
* Rice University Software Distribution License
*
* Copyright (c) 2010, Rice University
* All Rights Reserved.
*
* For a full description see the file named LICENSE.
*
*********************************************************************/

/* Author: Ioan Sucan */

#include "OpenDEWorld.inc"
#include "OMPLEnvironment.inc"
#include "OMPLSetup.inc"

#include "displayOpenDE.h"
#include "omplapp/config.h"

#include <drawstuff/drawstuff.h>

#include <boost/thread.hpp>

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif

static DisplayOpenDESpaces                         DISP;
static std::vector< std::pair<double, double> > POINTS;
static bool                                     drawTree = true;

static void start()
{
    dAllocateODEDataForThread(dAllocateMaskAll);
    static float xyz[3] = { 3.8548f,9.0843f,7.5900f} ;
    static float hpr[3] = { -145.5f,-22.5f,0.25f } ;

    dsSetViewpoint (xyz,hpr);
}

static void command (int cmd)
{
    if ((char)cmd == 't')
        drawTree = !drawTree;
}

static void simLoop (int /*pause*/)
{
    DISP.displaySpaces();

    if (drawTree)
    {
        glPointSize(2.0);
        glColor3f(1.0f, 0.0f, 0.0f);
        glBegin(GL_POINTS);
        for (unsigned int i = 0 ; i < POINTS.size() ; ++i)
            glVertex3d(POINTS[i].first, POINTS[i].second, 0.05);
        glEnd();
    }

    static ompl::time::duration d = ompl::time::seconds(0.001);
    boost::this_thread::sleep(d);
}

static void playPath(oc::OpenDESimpleSetup *ss)
{
    while (1)
    {
        ss->playSolutionPath(0.005);
        static ompl::time::duration d = ompl::time::seconds(1);
        boost::this_thread::sleep(d);
    }
}

int main(int argc, char **argv)
{
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start = &start;
    fn.step = &simLoop;
    fn.command = &command;
    fn.stop = 0;

    char *textures = (char*)alloca(strlen(OMPLAPP_RESOURCE_DIR) + 10);
    strcpy(textures, OMPLAPP_RESOURCE_DIR);
    strcat(textures, "/textures");
    fn.path_to_textures = textures;

    dInitODE2(0);

    bodies = 0;
    joints = 0;
    boxes = 0;
    spheres = 0;
    resetSimulation();
    DISP.addSpace(space, 0.9, 0.9, 0.5);
    DISP.setGeomColor(avoid_box_geom, 0.9, 0.0, 0.0);
    DISP.setGeomColor(movable_box_geom[0], 0.1, 0.8, 0.8);
    DISP.setGeomColor(movable_box_geom[1], 0.1, 0.8, 0.8);
    DISP.setGeomColor(movable_box_geom[2], 0.1, 0.8, 0.8);
    DISP.setGeomColor(movable_box_geom[3], 0.1, 0.8, 0.8);
    DISP.setGeomColor(goal_geom, 0.0, 0.9, 0.1);

    oc::OpenDEEnvironmentPtr ce(new CarEnvironment());
    ob::StateSpacePtr sm(new CarStateSpace(ce));

    oc::ControlSpacePtr cm(new CarControlSpace(sm));

    oc::OpenDESimpleSetup ss(cm);
    ss.setGoal(ob::GoalPtr(new CarGoal(ss.getSpaceInformation(), GOAL_X, GOAL_Y)));
    ob::RealVectorBounds vb(3);
    vb.low[0] = -50;
    vb.low[1] = -50;
    vb.low[2] = -5;
    vb.high[0] = 50;
    vb.high[1] = 50;
    vb.high[2] = 10;
    ss.setVolumeBounds(vb);

    ss.setup();
    ss.print();
    boost::thread *th = NULL;

    std::cout << "Planning for at most 60 seconds ..." << std::endl;

    if (ss.solve(60))
    {
        std::cout << "Solved!" << std::endl;
        ob::ScopedState<oc::OpenDEStateSpace> last(ss.getSpaceInformation());
        last = ss.getSolutionPath().getStates().back();
        std::cout << "Reached: " << last->getBodyPosition(0)[0] << " " << last->getBodyPosition(0)[1] << std::endl;

        POINTS.clear();
        ob::PlannerData pd(ss.getSpaceInformation());
        ss.getPlannerData(pd);
        for (unsigned int i = 0 ; i < pd.numVertices() ; ++i)
        {
            const double *pos = pd.getVertex(i).getState()->as<oc::OpenDEStateSpace::StateType>()->getBodyPosition(0);
            POINTS.push_back(std::make_pair(pos[0], pos[1]));
        }

        th = new boost::thread(boost::bind(&playPath, &ss));
    }

    // run simulation
    dsSimulationLoop(argc, argv, 640, 480, &fn);
    if (th)
    {
        th->interrupt();
        th->join();
    }

    dSpaceDestroy (space);
    dWorldDestroy (world);
    dCloseODE();

    return 0;
}


#pragma once

#include <ros/ros.h>

#include <Eigen/Dense>
#include <iostream>
#include <memory>
#include <vector>
#include <limits>
#include <ompl/datastructures/BinaryHeap.h>

namespace optimized_motion_planner {

class RRTXNode {
public:
	RRTXNode() = default;
	RRTXNode(const Eigen::Vector3d& state, double time_from_start = 0);
	~RRTXNode();

	// for nearest neighbor tree
	std::shared_ptr<RRTXNode> parent;
	std::vector<std::shared_ptr<RRTXNode>> children;

	// for extend part
	std::vector<std::pair<std::shared_ptr<RRTXNode>, bool>> nbh;

	// for neighbors, original in is N0-(v), out is N0+(v), running in is Nr-(v), out is Nr+(v)
	std::vector<std::shared_ptr<RRTXNode>> n0_in; // -
	std::vector<std::shared_ptr<RRTXNode>> n0_out; // +
	std::vector<std::shared_ptr<RRTXNode>> nr_in; // -
	std::vector<std::shared_ptr<RRTXNode>> nr_out; // +

	struct node_compare{
		bool operator()(const std::shared_ptr<RRTXNode>& a, const std::shared_ptr<RRTXNode>& b){
			double a_min_cost = std::min(a->get_lmc(), a->get_g_cost());
			double b_min_cost = std::min(b->get_lmc(), b->get_g_cost());

			if (a_min_cost == b_min_cost) return a->get_g_cost() > b->get_g_cost();
			return a_min_cost > b_min_cost;
		}
	};

	// for check if the node is in the priority queue
	ompl::BinaryHeap<std::shared_ptr<RRTXNode>, node_compare>::Element *handle;

	void set_lmc(double lmc, bool if_inf = false) {
		if (if_inf) {
			this->lmc_ = std::numeric_limits<double>::infinity();
		} else {
			this->lmc_ = lmc;
		}
	};

	double get_lmc() const {
		return this->lmc_;
	};

	void set_g_cost(double g_cost, bool if_inf = false) {
		if (if_inf) {
			this->g_cost_ = std::numeric_limits<double>::infinity();
		} else {
			this->g_cost_ = g_cost;
		}
	};

	double get_g_cost() const {
		return this->g_cost_;
	};

	void set_time(double time_from_start) {
		this->time_ = time_from_start;
	};

	double get_time() const {
		return this->time_;
	};

	void set_state_by_vector(const Eigen::Vector3d& new_state) {
		this->state_ = new_state;
	};

	void set_state_by_value(double x, double y, double z) {
		this->state_(0) = x;
		this->state_(1) = y;
		this->state_(2) = z;
	};

	Eigen::Vector3d get_state() const {
		return this->state_;
	};

	void set_unique_id(int unique_id) {
		this->unique_id_ = unique_id;
	}

	int get_unique_id() {
		return this->unique_id_;
	}

	void add_to_neighbor_edge_list(double edge);
	void update_neighbor_edge_list(int neighbor_index, double edge);
	double get_neighbor_edge(int neighbor_index);
private:
	// state of quadrotor
	Eigen::Vector3d state_;

	// cost-to-goal
	double lmc_;
	double g_cost_;

	// time from start
	double time_{0};

	int unique_id_;

	std::vector<double> neighbor_edge_list_;
};

}


//          Copyright Oliver Kowalke 2014.
// 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)

#include <cstdlib>
#include <exception>
#include <functional>
#include <iostream>
#include <memory>
#include <sstream>

#include <boost/context/execution_context.hpp>

/*
 * grammar:
 *   P ---> E '\0'
 *   E ---> T {('+'|'-') T}
 *   T ---> S {('*'|'/') S}
 *   S ---> digit | '(' E ')'
 */
class Parser{
   char next;
   std::istream& is;
   std::function<void(char)> cb;

   char pull(){
        return std::char_traits<char>::to_char_type(is.get());
   }

   void scan(){
       do{
           next=pull();
       }
       while(isspace(next));
   }

public:
   Parser(std::istream& is_,std::function<void(char)> cb_) :
      next(), is(is_), cb(cb_)
    {}

   void run() {
      scan();
      E();
   }

private:
   void E(){
      T();
      while (next=='+'||next=='-'){
         cb(next);
         scan();
         T();
      }
   }

   void T(){
      S();
      while (next=='*'||next=='/'){
         cb(next);
         scan();
         S();
      }
   }

   void S(){
      if (isdigit(next)){
         cb(next);
         scan();
      }
      else if(next=='('){
         cb(next);
         scan();
         E();
         if (next==')'){
             cb(next);
             scan();
         }else{
             throw std::runtime_error("parsing failed");
         }
      }
      else{
         throw std::runtime_error("parsing failed");
      }
   }
};

int main() {
    try {
        std::istringstream is("1+1");
        bool done=false;
        std::exception_ptr except;

        // create handle to main execution context
        auto sink(boost::context::execution_context::current());
        // execute parser in new execution context
        boost::context::execution_context source(
                [&sink,&is,&done,&except](void*){
                // create parser with callback function
                Parser p(is,
                         [&sink](char ch){
                                // resume main execution context
                                sink(&ch);
                        });
                    try {
                        // start recursive parsing
                        p.run();
                    } catch (...) {
                        // store other exceptions in exception-pointer
                        except = std::current_exception();
                    }
                    // set termination flag
                    done=true;
                    // resume main execution context
                    sink();
                });

        // user-code pulls parsed data from parser
        // invert control flow
        void* vp = source();
        if (except) {
            std::rethrow_exception(except);
        }
        while( ! done) {
            printf("Parsed: %c\n",* static_cast<char*>(vp));
            vp = source();
            if (except) {
                std::rethrow_exception(except);
            }
        }
        std::cout << "main: done" << std::endl;
        return EXIT_SUCCESS;
    } catch ( std::exception const& e) {
        std::cerr << "exception: " << e.what() << std::endl;
    }
    return EXIT_FAILURE;
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "opengl-precomp.h"  // Precompiled header

#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_matrices.h>  // for extract*()
#include <mrpt/opengl/CGeneralizedCylinder.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/stl_serialization.h>
#include <Eigen/Dense>
#include "opengl_internals.h"

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::opengl;
using namespace mrpt::poses;

using namespace std;

IMPLEMENTS_SERIALIZABLE(
	CGeneralizedCylinder, CRenderizableDisplayList, mrpt::opengl)

void CGeneralizedCylinder::TQuadrilateral::calculateNormal()
{
	double ax = points[1].x - points[0].x;
	double ay = points[1].y - points[0].y;
	double az = points[1].z - points[0].z;
	double bx = points[2].x - points[0].x;
	double by = points[2].y - points[0].y;
	double bz = points[2].z - points[0].z;
	normal[0] = az * by - ay * bz;
	normal[1] = ax * bz - az * bx;
	normal[2] = ay * bx - ax * by;
	double s = 0;
	for (double i : normal) s += i * i;
	s = sqrt(s);
	for (double& i : normal) i /= s;
}

#if MRPT_HAS_OPENGL_GLUT
class FQuadrilateralRenderer
{
   private:
	const mrpt::img::TColor& color;

   public:
	void operator()(const CGeneralizedCylinder::TQuadrilateral& t) const
	{
		glNormal3d(t.normal[0], t.normal[1], t.normal[2]);
		for (const auto& point : t.points)
			glVertex3d(point.x, point.y, point.z);
	}
	FQuadrilateralRenderer(const mrpt::img::TColor& c) : color(c) {}
	~FQuadrilateralRenderer() = default;
};
#endif

void CGeneralizedCylinder::getMeshIterators(
	const vector<TQuadrilateral>& m,
	vector<TQuadrilateral>::const_iterator& begin,
	vector<TQuadrilateral>::const_iterator& end) const
{
	if (fullyVisible)
	{
		begin = m.begin();
		end = m.end();
	}
	else
	{
		ASSERT_(getNumberOfSections() > 0);
		size_t qps =
			m.size() / getNumberOfSections();  // quadrilaterals per section
		begin = m.begin() + qps * firstSection;
		end = m.begin() + qps * lastSection;
	}
}

void CGeneralizedCylinder::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
	if (!meshUpToDate) updateMesh();
	checkOpenGLError();
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

	glBegin(GL_QUADS);
	glColor4ub(m_color.R, m_color.G, m_color.B, m_color.A);
	vector<TQuadrilateral>::const_iterator begin, end;
	getMeshIterators(mesh, begin, end);
	for_each(begin, end, FQuadrilateralRenderer(m_color));
	glEnd();
	if (m_color.A != 1.0) glDisable(GL_BLEND);

#endif
}

inline void createMesh(
	const CMatrixDynamic<TPoint3D_data>& pointsMesh, size_t R, size_t C,
	vector<CGeneralizedCylinder::TQuadrilateral>& mesh)
{
	mesh.reserve(R * C);
	for (size_t i = 0; i < R; i++)
		for (size_t j = 0; j < C; j++)
			mesh.emplace_back(
				pointsMesh(i, j), pointsMesh(i, j + 1),
				pointsMesh(i + 1, j + 1), pointsMesh(i + 1, j));
}

bool CGeneralizedCylinder::traceRay(const CPose3D& o, double& dist) const
{
	if (!meshUpToDate || !polysUpToDate) updatePolys();
	return math::traceRay(polys, (o - this->m_pose).asTPose(), dist);
}

void CGeneralizedCylinder::updateMesh() const
{
	CRenderizableDisplayList::notifyChange();

	size_t A = axis.size();
	vector<TPoint3D> genX = generatrix;
	if (closed && genX.size() > 2) genX.push_back(genX[0]);
	size_t G = genX.size();
	mesh.clear();
	if (A > 1 && G > 1)
	{
		pointsMesh = CMatrixDynamic<TPoint3D_data>(A, G);
		for (size_t i = 0; i < A; i++)
			for (size_t j = 0; j < G; j++)
				pointsMesh(i, j) = axis[i].composePoint(genX[j]);
		createMesh(pointsMesh, A - 1, G - 1, mesh);
	}
	meshUpToDate = true;
	polysUpToDate = false;
}

uint8_t CGeneralizedCylinder::serializeGetVersion() const { return 1; }
void CGeneralizedCylinder::serializeTo(mrpt::serialization::CArchive& out) const
{
	writeToStreamRender(out);
	out << axis << generatrix;  // In version 0, axis was a vector<TPoint3D>. In
	// version 1, it is a vector<CPose3D>.
}

void CGeneralizedCylinder::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		{
			readFromStreamRender(in);
			vector<TPoint3D> a;
			in >> a >> generatrix;
			generatePoses(a, axis);
			meshUpToDate = false;
			polysUpToDate = false;
			break;
		}
		case 1:
			readFromStreamRender(in);
			// version 0
			in >> axis >> generatrix;
			meshUpToDate = false;
			polysUpToDate = false;
			break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
	CRenderizableDisplayList::notifyChange();
}

void generatePolygon(
	CPolyhedron::Ptr& poly, const vector<TPoint3D>& profile,
	const CPose3D& pose)
{
	math::TPolygon3D p(profile.size());
	for (size_t i = 0; i < profile.size(); i++)
		pose.composePoint(
			profile[i].x, profile[i].y, profile[i].z, p[i].x, p[i].y, p[i].z);
	vector<math::TPolygon3D> convexPolys;
	if (!math::splitInConvexComponents(p, convexPolys))
		convexPolys.push_back(p);
	poly = std::make_shared<CPolyhedron>(convexPolys);
}

void CGeneralizedCylinder::getOrigin(CPolyhedron::Ptr& poly) const
{
	if (!meshUpToDate) updateMesh();
	if (axis.size() < 2 || generatrix.size() < 3)
		throw std::logic_error("Not enough points.");
	size_t i = fullyVisible ? 0 : firstSection;
	generatePolygon(poly, generatrix, axis[i]);
	poly->setPose(this->m_pose);
	poly->setColor(getColor());
}

void CGeneralizedCylinder::getEnd(CPolyhedron::Ptr& poly) const
{
	if (!meshUpToDate) updateMesh();
	if (axis.size() < 2 || generatrix.size() < 3)
		throw std::logic_error("Not enough points.");
	size_t i = (fullyVisible ? axis.size() : lastSection) - 1;
	generatePolygon(poly, generatrix, axis[i]);
	poly->setPose(this->m_pose);
	poly->setColor(getColor());
}

void CGeneralizedCylinder::generateSetOfPolygons(
	std::vector<TPolygon3D>& res) const
{
	if (!meshUpToDate || !polysUpToDate) updatePolys();
	size_t N = polys.size();
	res.resize(N);
	for (size_t i = 0; i < N; i++) res[i] = polys[i].poly;
}

void CGeneralizedCylinder::getClosedSection(
	size_t index1, size_t index2, mrpt::opengl::CPolyhedron::Ptr& poly) const
{
	if (index1 > index2) swap(index1, index2);
	if (index2 >= axis.size() - 1) throw std::logic_error("Out of range");
	if (!meshUpToDate) updateMesh();
	auto ROIpoints = CMatrixDynamic<TPoint3D_data>(
		pointsMesh.asEigen().block(index1, 0, index2 + 1, pointsMesh.cols()));

	// At this point, ROIpoints contains a matrix of TPoints in which the number
	// of rows equals (index2-index1)+2 and there is a column
	// for each vertex in the generatrix.
	if (!closed)
	{
		CVectorDynamic<TPoint3D_data> vec(ROIpoints.rows());
		vec.asEigen() = ROIpoints.col(0);
		ROIpoints.appendCol(vec);
	}
	vector<TPoint3D> vertices;
	ROIpoints.asVector(vertices);
	size_t nr = ROIpoints.rows() - 1;
	size_t nc = ROIpoints.cols() - 1;
	vector<vector<uint32_t>> faces;
	faces.reserve(nr * nc + 2);
	vector<uint32_t> tmp(4);
	for (size_t i = 0; i < nr; i++)
		for (size_t j = 0; j < nc; j++)
		{
			size_t base = (nc + 1) * i + j;
			tmp[0] = base;
			tmp[1] = base + 1;
			tmp[2] = base + nc + 2;
			tmp[3] = base + nc + 1;
			faces.push_back(tmp);
		}
	tmp.resize(nr + 1);
	for (size_t i = 0; i < nr + 1; i++) tmp[i] = i * (nc + 1);
	faces.push_back(tmp);
	for (size_t i = 0; i < nr + 1; i++) tmp[i] = i * (nc + 2) - 1;
	poly = CPolyhedron::Create(vertices, faces);
}

void CGeneralizedCylinder::removeVisibleSectionAtStart()
{
	CRenderizableDisplayList::notifyChange();
	if (fullyVisible)
	{
		if (!getNumberOfSections()) throw std::logic_error("No more sections");
		fullyVisible = false;
		firstSection = 1;
		lastSection = getNumberOfSections();
	}
	else if (firstSection >= lastSection)
		throw std::logic_error("No more sections");
	else
		firstSection++;
}
void CGeneralizedCylinder::removeVisibleSectionAtEnd()
{
	CRenderizableDisplayList::notifyChange();
	if (fullyVisible)
	{
		if (!getNumberOfSections()) throw std::logic_error("No more sections");
		fullyVisible = false;
		firstSection = 0;
		lastSection = getNumberOfSections() - 1;
	}
	else if (firstSection >= lastSection)
		throw std::logic_error("No more sections");
	else
		lastSection--;
}

void CGeneralizedCylinder::updatePolys() const
{
	CRenderizableDisplayList::notifyChange();

	if (!meshUpToDate) updateMesh();
	size_t N = mesh.size();
	polys.resize(N);
	TPolygon3D tmp(4);
	for (size_t i = 0; i < N; i++)
	{
		for (size_t j = 0; j < 4; j++) tmp[j] = mesh[i].points[j];
		polys[i] = tmp;
	}
	polysUpToDate = true;
}

void CGeneralizedCylinder::generatePoses(
	const vector<TPoint3D>& pIn, std::vector<mrpt::poses::CPose3D>& pOut)
{
	size_t N = pIn.size();
	if (N == 0)
	{
		pOut.resize(0);
		return;
	}
	vector<double> yaws;
	yaws.reserve(N);
	vector<TPoint3D>::const_iterator it1 = pIn.begin(), it2;
	for (;;)
		if ((it2 = it1 + 1) == pIn.end())
			break;
		else
		{
			yaws.push_back(atan2(it2->y - it1->y, it2->x - it1->x));
			it1 = it2;
		}
	yaws.push_back(*yaws.rbegin());
	pOut.resize(N);
	for (size_t i = 0; i < N; i++)
	{
		const TPoint3D& p = pIn[i];
		pOut[i] = CPose3D(p.x, p.y, p.z, yaws[i], 0, 0);
	}
}

bool CGeneralizedCylinder::getFirstSectionPose(CPose3D& p)
{
	if (axis.size() <= 0) return false;
	p = axis[0];
	return true;
}

bool CGeneralizedCylinder::getLastSectionPose(CPose3D& p)
{
	if (axis.size() <= 0) return false;
	p = *axis.rbegin();
	return true;
}

bool CGeneralizedCylinder::getFirstVisibleSectionPose(CPose3D& p)
{
	if (fullyVisible) return getFirstSectionPose(p);
	if (getVisibleSections() <= 0) return false;
	p = axis[firstSection];
	return true;
}

bool CGeneralizedCylinder::getLastVisibleSectionPose(CPose3D& p)
{
	if (fullyVisible) return getLastSectionPose(p);
	if (getVisibleSections() <= 0) return false;
	p = axis[lastSection];
	return true;
}

void CGeneralizedCylinder::getBoundingBox(
	mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
{
	bb_min = TPoint3D(0, 0, 0);
	bb_max = TPoint3D(0, 0, 0);

	// Convert to coordinates of my parent:
	m_pose.composePoint(bb_min, bb_min);
	m_pose.composePoint(bb_max, bb_max);
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;
using arma::inplace_trans;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenMat : public Expected {
    public:
      ExpectedInPlaceGenMat() {
        cout << "Compute ExpectedInPlaceGenMat(): " << endl;

          vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
            InputClass::GenMat
          });

          for (vector<pair<string, void*>> input : inputs) {
            _fileSuffix = "";

            int n = 0;
            for (pair<string, void*> value : input) {
              switch (n) {
                case 0:
                  _fileSuffix += value.first;
                  _genMat = *static_cast<Mat<double>*>(value.second);
                  break;
              }
              ++n;
            }

            cout << "Using input: " << _fileSuffix << endl;

            _copyOfGenMat = _genMat;

            expectedArmaInplace_trans();

            _genMat = _copyOfGenMat;
            expectedMatOnes();

            _genMat = _copyOfGenMat;
            expectedMatZeros();

            _genMat = _copyOfGenMat;
            expectedMatInPlaceIncrement();

            _genMat = _copyOfGenMat;
            expectedMatInPlaceDecrement();

            _genMat = _copyOfGenMat;
            expectedMatEye();
          }

          cout << "done." << endl;
        }

    protected:
      Mat<double> _genMat;
      Mat<double> _copyOfGenMat;

      void expectedArmaInplace_trans() {
        cout << "- Compute expectedArmaInplace_trans() ... ";

        inplace_trans(_genMat);
        save<double>("Arma.inplace_trans", _genMat);

        cout << "done." << endl;
      }

      void expectedMatOnes() {
        cout << "- Compute expectedMatOnes() ... ";

        _genMat.ones();
        save<double>("Mat.ones", _genMat);

        cout << "done." << endl;
      }

      void expectedMatZeros() {
        cout << "- Compute expectedMatZeros() ... ";

        _genMat.zeros();
        save<double>("Mat.zeros", _genMat);

        cout << "done." << endl;
      }

      void expectedMatInPlaceIncrement() {
        cout << "- Compute expectedMatInPlaceIncrement() ... ";

        _genMat++;
        save<double>("Mat.inPlaceIncrement", _genMat);

        cout << "done." << endl;
      }

      void expectedMatInPlaceDecrement() {
        cout << "- Compute expectedMatInPlaceDecrement() ... ";

        _genMat--;
        save<double>("Mat.inPlaceDecrement", _genMat);

        cout << "done." << endl;
      }

      void expectedMatEye() {
        cout << "- Compute expectedMatEye() ... ";

        _genMat.eye();
        save<double>("Mat.eye", _genMat);

        cout << "done." << endl;
      }
  };
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_AGNOSTIC_UNIFORM_POINT_DISTRIBUTION_DISCRETE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_AGNOSTIC_UNIFORM_POINT_DISTRIBUTION_DISCRETE_HPP

#include <random>
#include <iterator>

#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/transform.hpp>

#include <boost/geometry/extensions/random/strategies/uniform_point_distribution.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace uniform_point_distribution {

template
<
    typename Point,
    typename DomainGeometry
>
struct single_point_distribution
{
    single_point_distribution(DomainGeometry const& g) {}
    template<typename Strategy>
    bool equals(DomainGeometry const& l_domain,
                DomainGeometry const& r_domain,
                Strategy const& r_strategy) const
    {
        return boost::geometry::equals(l_domain.domain(), r_domain.domain());
    }
    template<typename Gen>
    Point apply(Gen& g, DomainGeometry const& d)
    {
        Point r;
        boost::geometry::transform(d, r);
        return r;
    }
    void reset(DomainGeometry const&) {};
};

template
<
    typename Point,
    typename DomainGeometry
>
struct multi_point_distribution
{
    multi_point_distribution(DomainGeometry const& p) {}
    bool equals(DomainGeometry const& l_domain,
                DomainGeometry const& r_domain,
                multi_point_distribution const& r_strategy) const
    {
        if(boost::size(l_domain) != boost::size(r_domain)) return false;
        for (std::size_t i = 0; i < boost::size(l_domain); ++i)
            if( !boost::geometry::equals( *(std::begin(l_domain) + i),
                                          *(std::begin(r_domain) + i)) )
            {
                return false;
            }
        return true;
    }
    template<typename Gen>
    Point apply(Gen& g, DomainGeometry const& d)
    {
        Point r;
        if(boost::size(d) == 0) return r;
        std::uniform_int_distribution<std::size_t>
            int_dist(0, boost::size(d) - 1);
        boost::geometry::transform(*(std::begin(d) + int_dist(g)), r);
        return r;
    }
    void reset(DomainGeometry const&) {}
};

namespace services {

template
<
    typename Point,
    typename DomainGeometry,
    int Dim,
    typename CsTag
>
struct default_strategy
<
    Point,
    DomainGeometry,
    pointlike_tag,
    single_tag,
    Dim,
    CsTag
> : public single_point_distribution<Point, DomainGeometry> {
    typedef single_point_distribution<Point, DomainGeometry> base;
    using base::base;
};

template
<
    typename Point,
    typename DomainGeometry,
    int Dim,
    typename CsTag
>
struct default_strategy
<
    Point,
    DomainGeometry,
    pointlike_tag,
    multi_tag,
    Dim,
    CsTag
> : public multi_point_distribution<Point, DomainGeometry> {
    typedef multi_point_distribution<Point, DomainGeometry> base;
    using base::base;
};

} // namespace services

}} // namespace strategy::uniform_point_distribution

}} // namespace boost::geometry

#endif //  BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_AGNOSTIC_UNIFORM_POINT_DISTRIBUTION_DISCRETE_HPP


// -*- C++ -*-
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
//                                   Jiao Lin
//                      California Institute of Technology
//                      (C) 2005-2010 All Rights Reserved
//
// {LicenseText}
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

#include <boost/python.hpp>
#include "mccomponents/math/rootfinding.h"

namespace wrap_mccomponents {

  void wrap_rootsfinders()
  {
    using namespace boost::python;
    using namespace mccomponents::math;

    class_<RootFinder, boost::noncopyable>
      ("RootFinder", no_init)
      ;

    class_<Algorithms::Bracketing::Ridder::ZRidd, bases<RootFinder> >
      ("ZRidd",
       init<double>() )
      ;

    class_<RootsFinder, boost::noncopyable>
      ("RootsFinder", no_init)
      ;

    class_<FindRootsEvenly, bases<RootsFinder> >
      ("FindRootsEvenly",
       init<const RootFinder &, size_t> ()
       [with_custodian_and_ward<1,2>()]
       )
      ;
  }

}

// version
// $Id$

// End of file