// Copyright  (C)  2009  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

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

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

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#ifndef KDL_ARTICULATEDBODYINERTIA_HPP
#define KDL_ARTICULATEDBODYINERTIA_HPP

#include "frames.hpp"

#include "rotationalinertia.hpp"
#include "rigidbodyinertia.hpp"

#include <Eigen/Core>

namespace KDL {

    /**
     *	\brief 6D Inertia of a articulated body
     *
     *	The inertia is defined in a certain reference point and a certain reference base.
     *	The reference point does not have to coincide with the origin of the reference frame.
     */
    class ArticulatedBodyInertia{
    public:

        /**
         * 	This constructor creates a zero articulated body inertia matrix,
         */
        ArticulatedBodyInertia(){
            *this=ArticulatedBodyInertia::Zero();
        }

        /**
         * 	This constructor creates a cartesian space articulated body inertia matrix,
         * 	the arguments is a rigid body inertia.
         */
        ArticulatedBodyInertia(const RigidBodyInertia& rbi);

        /**
         * 	This constructor creates a cartesian space inertia matrix,
         * 	the arguments are the mass, the vector from the reference point to cog and the rotational inertia in the cog.
         */
        explicit ArticulatedBodyInertia(double m, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero());

        /**
         * Creates an inertia with zero mass, and zero RotationalInertia
         */
        static inline ArticulatedBodyInertia Zero(){
            return ArticulatedBodyInertia(Eigen::Matrix3d::Zero(),Eigen::Matrix3d::Zero(),Eigen::Matrix3d::Zero());
        };

        ~ArticulatedBodyInertia(){};

        friend ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I);
        friend ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib);
        friend ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib);
        friend ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib);
        friend ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib);
        friend Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t);
        friend ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I);
        friend ArticulatedBodyInertia operator*(const Rotation& R,const ArticulatedBodyInertia& I);

        /**
         * Reference point change with v the vector from the old to
         * the new point expressed in the current reference frame
         */
        ArticulatedBodyInertia RefPoint(const Vector& p);

        ArticulatedBodyInertia(const Eigen::Matrix3d& M,const Eigen::Matrix3d& H,const Eigen::Matrix3d& I);

        Eigen::Matrix3d M;
        Eigen::Matrix3d H;
        Eigen::Matrix3d I;
    };

    /**
     * Scalar product: I_new = double * I_old
     */
    ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I);
    /**
     * addition I: I_new = I_old1 + I_old2, make sure that I_old1
     * and I_old2 are expressed in the same reference frame/point,
     * otherwise the result is worth nothing
     */
    ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib);
    ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib);
    ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib);
    ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib);

    /**
     * calculate spatial momentum: h = I*v
     * make sure that the twist v and the inertia are expressed in the same reference frame/point
     */
    Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t);

    /**
     * Coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b.
     */
    ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I);
    /**
     * Reference frame orientation change Ia = R_a_b*Ib with R_a_b
     * the rotation of b expressed in a
     */
    ArticulatedBodyInertia operator*(const Rotation& R,const ArticulatedBodyInertia& I);

}
#endif


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "Utils/Geometry/GeometryUtilities.h"
#include "Utils/Geometry/AtomCollection.h"
#include "Utils/Geometry/ElementInfo.h"
#include "Utils/Math/QuaternionFit.h"
#include <Eigen/Eigenvalues>
#include <limits>

namespace Scine {
namespace Utils {
namespace Geometry {

PositionCollection translatePositions(const PositionCollection& positions, const Eigen::Ref<Eigen::RowVector3d>& translation) {
  PositionCollection pc = positions;
  translatePositionsInPlace(pc, translation);
  return pc;
}

void translatePositionsInPlace(PositionCollection& positions, const Eigen::Ref<Eigen::RowVector3d>& translation) {
  positions.rowwise() += translation;
}

int getIndexOfClosestAtom(const PositionCollection& positions, const Position& targetPosition,
                          double squaredDistanceConsideredZero) {
  assert(positions.rows() != 0 && "Cannot determine closest atom if there are no atoms!");
  double minimalDistanceSquared = std::numeric_limits<double>::max();
  int closestAtom = 0;

  auto nAtoms = static_cast<int>(positions.rows());

  for (int i = 0; i < nAtoms; i++) {
    double currentSquaredDistance = (positions.row(i) - targetPosition).squaredNorm();
    if (currentSquaredDistance <= squaredDistanceConsideredZero)
      continue;
    if (currentSquaredDistance < minimalDistanceSquared) {
      minimalDistanceSquared = currentSquaredDistance;
      closestAtom = i;
    }
  }

  return closestAtom;
}

int getIndexOfAtomInStructure(const AtomCollection& structure, const Atom& atom, double squaredDistanceConsideredZero) {
  int index = 0;
  ElementType element = atom.getElementType();
  for (const auto& a : structure) {
    if (a.getElementType() == element) { // First check whether the element types match
      double squaredDistance = (a.getPosition() - atom.getPosition()).squaredNorm();
      if (squaredDistance <= squaredDistanceConsideredZero) { // Check whether the atoms have basically the same position
        return index;
      }
    }
    index++;
  }
  throw std::runtime_error("The given atom was not found in the given structure.");
}

Eigen::MatrixXd positionVectorToMatrix(const Eigen::VectorXd& v) {
  assert(v.size() % 3 == 0);

  using namespace Eigen;
  using RowMajorMatrix = Matrix<double, Dynamic, Dynamic, RowMajor>;

  auto numberParticles = v.size() / 3;
  Map<const RowMajorMatrix> mapMatrix(v.data(), numberParticles, 3);
  return mapMatrix;
}

Eigen::VectorXd positionMatrixToVector(const Eigen::MatrixXd& m) {
  assert(m.cols() == 3);

  using namespace Eigen;
  using RowMajorMatrix = Matrix<double, Dynamic, Dynamic, RowMajor>;

  RowMajorMatrix m2(m);
  Map<const VectorXd> mapVector(m2.data(), m2.size());

  return mapVector;
}

void alignPositions(const PositionCollection& reference, PositionCollection& positions) {
  QuaternionFit fit(reference, positions);
  positions = fit.getFittedData();
}

std::vector<double> getMasses(const ElementTypeCollection& elements) {
  std::vector<double> masses;
  masses.reserve(elements.size());
  std::transform(elements.begin(), elements.end(), std::back_inserter(masses), ElementInfo::mass);
  return masses;
}

Position getCenterOfMass(const PositionCollection& positions, const std::vector<double>& masses) {
  Position P;
  P.setZero();
  double totalMass = 0;
  for (int i = 0; i < positions.rows(); ++i) {
    P += masses[i] * positions.row(i);
    totalMass += masses[i];
  }
  P /= totalMass;
  return P;
}

Position getCenterOfMass(const AtomCollection& structure) {
  auto masses = getMasses(structure.getElements());
  return getCenterOfMass(structure.getPositions(), masses);
}

Position getAveragePosition(const PositionCollection& positions) {
  return positions.colwise().sum() / positions.rows();
}

Eigen::Matrix3d calculateInertiaTensor(const PositionCollection& positions, const std::vector<double>& masses,
                                       const Position& centerOfMass) {
  double Ixx = 0, Iyy = 0, Izz = 0, Ixy = 0, Ixz = 0, Iyz = 0;
  for (int i = 0; i < positions.rows(); ++i) {
    double m = masses[i];
    auto dp = positions.row(i) - centerOfMass;
    double x = dp.x();
    double y = dp.y();
    double z = dp.z();
    Ixx += m * (y * y + z * z);
    Iyy += m * (x * x + z * z);
    Izz += m * (x * x + y * y);
    Ixy -= m * x * y;
    Ixz -= m * x * z;
    Iyz -= m * y * z;
  }
  Eigen::Matrix3d In;
  In << Ixx, Ixy, Ixz, Ixy, Iyy, Iyz, Ixz, Iyz, Izz;
  return In;
}

PrincipalMomentsOfInertia calculatePrincipalMoments(const PositionCollection& positions,
                                                    const std::vector<double>& masses, const Position& centerOfMass) {
  auto In = calculateInertiaTensor(positions, masses, centerOfMass);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
  es.compute(In);

  PrincipalMomentsOfInertia pmi;
  pmi.eigenvalues = es.eigenvalues();
  pmi.eigenvectors = es.eigenvectors();
  return pmi;
}

Eigen::MatrixXd calculateTranslationAndRotationModes(const PositionCollection& positions, const ElementTypeCollection& elements) {
  auto nAtoms = positions.rows();
  auto masses = Utils::Geometry::getMasses(elements);
  auto centerOfMass = Utils::Geometry::getCenterOfMass(positions, masses);
  auto principalMoments = Utils::Geometry::calculatePrincipalMoments(positions, masses, centerOfMass);

  const auto& X = principalMoments.eigenvectors;

  Eigen::MatrixXd allRotoTranslationVectors(3 * nAtoms, 6);

  for (int i = 0; i < nAtoms; ++i) {
    // Translation
    allRotoTranslationVectors.block(3 * i, 0, 3, 3) = Eigen::MatrixXd::Identity(3, 3);

    // Rotation
    Eigen::Vector3d P = X.transpose() * (positions.row(i) - centerOfMass).transpose();
    Eigen::Matrix3d d;
    d.col(0) = P.y() * X.col(2) - P.z() * X.col(1);
    d.col(1) = P.z() * X.col(0) - P.x() * X.col(2);
    d.col(2) = P.x() * X.col(1) - P.y() * X.col(0);

    allRotoTranslationVectors.block(3 * i, 3, 3, 3) = d;
  }

  // look which roto-translational modes are valid
  std::vector<int> validRotoTranslationModes;
  for (int i = 0; i < 6; ++i) {
    // The norm will be zero for "invalid" roto-translation modes (single atoms, linear molecules).
    auto squaredNorm = allRotoTranslationVectors.col(i).squaredNorm();
    if (squaredNorm > 0.1) {
      validRotoTranslationModes.push_back(i);
    }
  }

  // return only the valid modes
  auto numberRotoTranslationModes = static_cast<int>(validRotoTranslationModes.size());
  Eigen::MatrixXd rotoTranslationVectors(3 * nAtoms, numberRotoTranslationModes);
  for (int i = 0; i < numberRotoTranslationModes; ++i) {
    rotoTranslationVectors.col(i) = allRotoTranslationVectors.col(validRotoTranslationModes[i]);
  }
  rotoTranslationVectors.colwise().normalize();

  return rotoTranslationVectors;
}

Eigen::MatrixXd calculateRotTransFreeTransformMatrix(const PositionCollection& positions,
                                                     const ElementTypeCollection& elements, bool massWeighted) {
  auto rotoTranslation = calculateTranslationAndRotationModes(positions, elements);
  // If mass-weighted Hessian shall be transformed the rotation and translation modes have to be adapted accordingly
  if (massWeighted) {
    int nAtoms = elements.size();
    auto masses = Geometry::getMasses(elements);
    for (int i = 0; i < nAtoms; ++i) {
      rotoTranslation.middleRows(3 * i, 3) *= std::sqrt(masses[i]);
    }
    rotoTranslation.colwise().normalize();
  }
  auto nDims = rotoTranslation.rows();
  auto numberRotoTranslationModes = rotoTranslation.cols();

  srand(42);
  Eigen::MatrixXd A = Eigen::MatrixXd::Random(nDims, nDims);

  A.leftCols(numberRotoTranslationModes) = rotoTranslation;

  // Orthogonalization
  auto n = nDims;
  auto m = nDims;
  Eigen::MatrixXd R_1(n, n);
  Eigen::MatrixXd Q_1(m, n);
  for (int i = 0; i < n; ++i) {
    R_1(i, i) = A.col(i).norm();
    Q_1.col(i) = A.col(i) / R_1(i, i);
    for (int j = i + 1; j < n; ++j) {
      R_1(i, j) = Q_1.col(i).transpose() * A.col(j);
      A.col(j) -= (Q_1.col(i) * R_1(i, j));
    }
  }

  A.colwise().normalize();

  return A.rightCols(nDims - numberRotoTranslationModes);
}

} /* namespace Geometry */
} /* namespace Utils */
} /* namespace Scine */


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <iostream>
#include <vw/Image.h>
#include <vw/FileIO.h>

#include <vw/Cartography/GeoReference.h>
#include <vw/Cartography/GeoTransform.h>
#include <vw/Image/ImageMath.h>
#include <vw/Image/Algorithms.h>
#include <vw/Math/Matrix.h>
#include <vw/Math/Vector.h>
#include <vw/Math/LinearAlgebra.h>
#include <vw/FileIO/DiskImageView.h>
#include <vw/tools/Common.h>

/*
Implements modified versions of finite difference opt.algorithms + fitting a plane to 9 points of a 3x3 window
*/

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <boost/filesystem/path.hpp>
namespace fs = boost::filesystem;

using namespace vw;
using namespace vw::math;
using namespace vw::cartography;

enum Algorithm { HORN, SA, FH, PLANEFIT };

struct Options {
  std::string input_file_name;
  std::string output_prefix;
  bool output_gradient;
  bool output_aspect;
  bool output_pretty; //probably more for debugging purposes than for anything else
  Algorithm algorithm;
  bool spherically_defined;
};

//basic utilities

Vector3 pixel_to_cart (Vector2 pos, double alt, GeoReference GR) {
  Vector2 loc_longlat2=GR.point_to_lonlat(GR.pixel_to_point(pos));
  Vector3 loc_longlat3(loc_longlat2(0),loc_longlat2(1),alt);
  Vector3 loc_cartesian=GR.datum().geodetic_to_cartesian(loc_longlat3);
  return loc_cartesian;
}

template <class ImageT>
Vector3 pixel_to_cart (Vector2 pos, DiskImageView<ImageT> img, GeoReference GR) {
  return pixel_to_cart(pos,img((int)pos[0],(int)pos[1]),GR);
}

double dist_from_2pi(double n) {
  if( fabs(n+2*M_PI) < fabs(n) ) return n+2*M_PI;
  if( fabs(n-2*M_PI) < fabs(n) ) return n-2*M_PI;
  return n;
}

//utilities...

Vector2 gradient_aspect_from_normals(Vector3 center_normal, Vector3 plane_normal) {
  //gradient angle is absval
  double dotprod=dot_prod(plane_normal,center_normal);
  double gradient_angle=acos(dotprod);
  Vector3 surface_normal_on_sphere_tangent_plane=normalize(plane_normal-dot_prod(plane_normal,center_normal)*center_normal);

  //get projection of (0,0,1) onto sphere tangent plane.
  Vector3 north(0,0,1);
  Vector3 north_projected=normalize(north-dot_prod(north,center_normal)*center_normal);

  //find the angle between those two
  double dotprod2=dot_prod(surface_normal_on_sphere_tangent_plane, north_projected);

  //figure out which angle it is.
  double aspect=acos(dotprod2);
  if(dotprod2>1) { dotprod2=1; aspect=0;}
  if(dotprod2<-1) { dotprod2=-1; aspect=0;}

  if( dot_prod((cross_prod(north_projected,surface_normal_on_sphere_tangent_plane)),center_normal) < 0)
    aspect=M_PI+aspect;
  else
    aspect=M_PI-aspect;

  if(aspect>=2*M_PI)
    aspect=aspect-2*M_PI;
  return Vector2(aspect,gradient_angle);
}

Vector2 gradient_aspect_from_dx_dy(double& dx, double& dy) {
  //total gradient:
  double gradient=norm_2(Vector2(dx,dy));
  double gradient_angle=atan(gradient);
  //aspect: dy/dx=tan(angle)
  double aspect=atan(dy/dx);
  if(dx<0) aspect=aspect+M_PI;
  aspect=2*M_PI-aspect+M_PI/2;
  if(aspect<0) aspect=2*M_PI+aspect;
  if(aspect>=2*M_PI) aspect=aspect-2*M_PI*(int)(aspect/2/M_PI);
  return Vector2(aspect,gradient_angle);
}

Vector2 gradient_aspect_from_dtheta_dphi(double rho, double theta, double phi, double& dtheta, double& dphi, Vector3 center) {
  double r_comp=1;
  double theta_comp=dtheta/rho;
  double phi_comp=1/(rho*sin(theta))*dphi;

  double n_x=r_comp*sin(theta)*cos(phi)-phi_comp*sin(phi)+theta_comp*cos(theta)*cos(phi);
  double n_y=r_comp*sin(theta)*sin(phi)+phi_comp*cos(phi)+theta_comp*cos(theta)*sin(phi);
  double n_z=r_comp*cos(theta)-theta_comp*sin(theta);

  Vector3 plane_normal=normalize(Vector3(n_x,n_y,n_z));
  center=normalize(center);
  return gradient_aspect_from_normals(center, plane_normal);
}

template <class ImageT>
Vector2 uneven_grid (const ::Options& opt, int x, int y, DiskImageView<ImageT> img, GeoReference GR) {

  Vector3 center=pixel_to_cart(Vector2(x,y),img,GR);
  Vector3 center_normal=normalize(center);
  Vector3 center_below=pixel_to_cart(Vector2(x,y),0,GR);

  Vector3 north=Vector3(0,0,1);
        Vector3 north_projected=normalize(north-dot_prod(north,center_normal)*center_normal);

  //define temporary axis
  Vector3 up_normal=normalize(north_projected);        //also theta hat
  Vector3 third=normalize(cross_prod(up_normal,center_normal));     //also phi hat

  //spherical
  Vector2 lonlat=GR.pixel_to_lonlat(Vector2(x,y));
  double lat=lonlat(1);
  double lon=lonlat(0);

  //rise= run*slope
  Matrix<double> rises;  //rise over vector distance
  Matrix<double> runs;  //components in either direction

  //different ways of weighting neighbors.
  if(opt.algorithm==HORN) { rises=Matrix<double>(12,1); runs=Matrix<double>(12,2); }
  if(opt.algorithm==SA)   { rises=Matrix<double>(8,1); runs=Matrix<double>(8,2); }
  if(opt.algorithm==FH)   { rises=Matrix<double>(4,1); runs=Matrix<double>(4,2); }

  int ct=0;
  for(int i=-1;i<=1;i++) {
    for(int j=-1;j<=1;j++) {
      if(i==0 && j==0) continue;
      int repeat=1;
      if(opt.algorithm==HORN) {
        if((i==0 && j!=0) || (i!=0 && j==0)) //for horn, weight direct neighbors twice
          repeat=2;
      }
      if(opt.algorithm==FH) {
        if(i!=0 && j!=0) //ignore diagonals
          continue;
      }

      for(int k=0;k<repeat;k++) {
        if(!opt.spherically_defined) {
          Vector3 neighbor=pixel_to_cart(Vector2(x+i,y+j),img,GR);
          Vector3 neighbor_below_rescale=normalize(neighbor)*(norm_2(center_below)/dot_prod(normalize(neighbor),center_normal));
          Vector3 v=neighbor_below_rescale-center_below;
          //or, project both onto tangent plane...
          rises(ct,0)=(img(x+i,y+j)-img(x,y))/norm_2(v);
          v=normalize(v);
          runs(ct,0)=dot_prod(v,up_normal);
          runs(ct,1)=dot_prod(v,third);
        } else {
          rises(ct,0)=(img(x+i,y+j)-img(x,y));
          Vector2 neighbor_lonlat=GR.pixel_to_lonlat(Vector2(x+i,y+j));
          runs(ct,0)=(neighbor_lonlat[1]-lonlat[1])*M_PI/180.0;
          runs(ct,1)=dist_from_2pi(neighbor_lonlat[0]-lonlat[0])*M_PI/180.0;
        }
        ct++;
      }
    }
  }
  //solve using least squares
  //gradient components=(VtV)-1Vt * rises
  Matrix<double> VtV(2,2);
  VtV=transpose(runs)*runs;
  Matrix<double> VtVinv(2,2);
  VtVinv(0,0)=VtV(1,1);
  VtVinv(1,1)=VtV(0,0);
  VtVinv(0,1)=-VtV(0,1);
  VtVinv(1,0)=-VtV(1,0);
  VtVinv=VtVinv/(VtV(0,0)*VtV(1,1)-VtV(0,1)*VtV(1,0));

  Matrix<double> ans=VtVinv*transpose(runs)*rises;

  if(opt.spherically_defined) {
    double rho=norm_2(center);
    double phi=lon/180.0*M_PI;
    double theta=(-lat+90.0)/180.0*M_PI;
    double dtheta=ans(0,0);
    double dphi=-ans(1,0);
    return gradient_aspect_from_dtheta_dphi(rho, theta, phi, dtheta, dphi, center);
  }
  //otherwise,
  return gradient_aspect_from_dx_dy(ans(0,1), ans(0,0));
}

template <class ImageT>
Vector2 interpolate_plane (int x, int y, DiskImageView<ImageT> img, GeoReference GR) {
  Matrix<double> A(9,4);
  int i=0;
  int j=0;
  int ct=0;
  Vector3 center_normal=pixel_to_cart(Vector2(x,y),img,GR);

  for(i=-1;i<=1;i++) {
    for(j=-1;j<=1;j++) {
      Vector3 tmp=pixel_to_cart(Vector2(x+i,y+j),img,GR);
      A(ct,0)=tmp(0);
      A(ct,1)=tmp(1);
      A(ct,2)=tmp(2);
      A(ct,3)=1;
      ct++;
    }
  }

  Matrix<double> U;
  Matrix<double> VT;
  Vector<double> s;

  svd(A, U, s, VT);
  Vector<double> plane_normal(3);
  plane_normal(0)=VT(3,0);
  plane_normal(1)=VT(3,1);
  plane_normal(2)=VT(3,2);
  //normalize sphere normal
  center_normal=normalize(center_normal);
  plane_normal=normalize(plane_normal);
  if(dot_prod(plane_normal,center_normal) <0) plane_normal=plane_normal*-1;
  return gradient_aspect_from_normals(center_normal, plane_normal);
}

template <class imageT>
void do_slopemap (const ::Options &opt) { //not sure what the arguments are

  GeoReference GR;
  read_georeference( GR, opt.input_file_name );

  DiskImageView<imageT> img(opt.input_file_name);

  int x;
  int y;

  ImageView<double> gradient_angle;
  ImageView<double> aspect;
  ImageView<PixelHSV<double> > pretty;

  if(opt.output_gradient)
    gradient_angle.set_size(img.cols(),img.rows());
  if(opt.output_aspect)
    aspect.set_size(img.cols(),img.rows());
  if(opt.output_pretty) pretty.set_size(img.cols(),img.rows());

  for(x=1;x<img.cols()-1;x++) {
    for(y=1;y<img.rows()-1;y++) {
      Vector2 res;
      //these are pretty similar...
      if(opt.algorithm==PLANEFIT) res=interpolate_plane(x,y,img,GR);
      else res=uneven_grid(opt, x,y,img,GR);

      if(opt.output_aspect)   aspect(x,y) = res(0);
      if(opt.output_gradient) gradient_angle(x,y) = res(1);
      if(opt.output_pretty)   pretty(x,y) = PixelHSV<double>(res(0),res(1),(res(1))+0.2*fabs(M_PI-res(0)));//(res(1)/M_PI*2)*fabs(M_PI-res(0)));
     }
  }
  ImageView<PixelRGB<uint8> > pretty2;

  if(opt.output_pretty) {
    select_channel(pretty,0)=normalize(select_channel(pretty,0),0,2*M_PI,0,1);
    select_channel(pretty,1)=normalize(select_channel(pretty,1),0,M_PI/2,0.1,1);
    select_channel(pretty,2)=normalize(select_channel(pretty,2),0.3,0.6);

    pretty2=pixel_cast_rescale<PixelRGB<uint8> >( copy(pretty) );
    pretty2=PixelRGB<uint8>(255,255,255)-pretty2;
  }
  //save everything to file
  if(opt.output_gradient) write_georeferenced_image( opt.output_prefix + "_gradient.tif" , gradient_angle, GR);
  if(opt.output_aspect)   write_georeferenced_image( opt.output_prefix + "_aspect.tif"   , aspect, GR);
  if(opt.output_pretty)   write_image( opt.output_prefix + "_pretty.tif"   , pretty2);
}

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

  ::Options opt;
  std::string algorithm_string;

  po::options_description desc("Description: Outputs gradient and/or aspect at each point of an input DEM with altitude values\n\nUsage: slopemap [options] <input file> \n\nOptions");
  desc.add_options()
    ("help,h", "Display this help messsage")
    ("input-file", po::value<std::string>(&opt.input_file_name), "Explicitly specify the input file")
    ("output-prefix,o", po::value<std::string>(&opt.output_prefix), "Specify the output prefix") //should add more description...
    ("no-aspect", "Do not output aspect")
    ("no-gradient", "Do not output gradient")
    ("pretty", "Output colored image.")
    ("opt.algorithm", po::value<std::string>(&algorithm_string)->default_value("horn"), "Choose an algorithm to calculate slope/aspect from [ horn, fh, sa, planefit ]. Horn: Horn's algorithm; FH: Fleming & Hoffer's (rook's case); SA: Sharpnack & Akin's (queen's case)")
    ("spherical", po::value<bool>(&opt.spherically_defined)->default_value(true), "Spherical/elliptical datum (recommended); otherwise, a flat grid");

  po::positional_options_description p;
  p.add("input-file", 1);

  po::variables_map vm;
  try {
    po::store( po::command_line_parser( argc, argv ).options(desc).positional(p).run(), vm );
    po::notify( vm );
  } catch (const po::error& e) {
    std::cout << "An error occured while parsing command line arguments.\n";
    std::cout << "\t" << e.what() << "\n\n";
    std::cout << desc << std::endl;
    return 1;
  }

  if( vm.count("help") ) {
    std::cout << desc << std::endl;
    return 1;
  }
  if( vm.count("input-file") != 1 ) {
    std::cout << "Error: Must specify exactly one input file!\n" << std::endl;
    std::cout << desc << std::endl;
    return 1;
  }

  if( opt.output_prefix == "" )
    opt.output_prefix=fs::path(opt.input_file_name).replace_extension().string();

  //checking strings

  boost::to_lower(algorithm_string);
  if( !(  algorithm_string == "horn" ||
    algorithm_string == "fh" ||
    algorithm_string == "sa" ||
    algorithm_string == "planefit" ||

    algorithm_string == "" ) ) { //it's okay if it isn't set?
    vw_out() << "Unknown opt.algorithm: " << algorithm_string << ". Options are : [ horn, fh, sa, planefit ]\n";
    exit(0);
  }
  else {
    if(algorithm_string=="horn")
      opt.algorithm=HORN;
    else if(algorithm_string=="fh")
      opt.algorithm=FH;
    else if(algorithm_string=="sa")
      opt.algorithm=SA;
    else if(algorithm_string=="planefit")
      opt.algorithm=PLANEFIT;
  }

  opt.output_aspect   = !(vm.count("no-aspect"));
  opt.output_gradient = !(vm.count("no-gradient"));
  opt.output_pretty   = vm.count("pretty");

  if(!opt.output_aspect && !opt.output_gradient && !opt.output_pretty) {
    vw_out() << "No output specified. Select at least one of [ gradient, output, pretty ].\n"
             << std::endl;
  }

  try {
    // Get the right pixel/channel type.
    ImageFormat fmt = tools::taste_image(opt.input_file_name);

    switch(fmt.pixel_format) {
    case VW_PIXEL_GRAY:
    case VW_PIXEL_GRAYA:
    case VW_PIXEL_RGB:
    case VW_PIXEL_RGBA:
      switch(fmt.channel_type) {
      case VW_CHANNEL_UINT8:  do_slopemap<PixelGray<uint8>   >(opt); break;
      case VW_CHANNEL_INT16:  do_slopemap<PixelGray<int16>   >(opt); break;
      case VW_CHANNEL_UINT16: do_slopemap<PixelGray<uint16>  >(opt); break;
      case VW_CHANNEL_FLOAT32:do_slopemap<PixelGray<float32> >(opt); break;
      case VW_CHANNEL_FLOAT64:do_slopemap<PixelGray<float64> >(opt); break;
      default:                do_slopemap<PixelGray<float32> >(opt); break;
      }
      break;
    default:
      std::cout << "Error: Unsupported pixel format.\n";
      exit(0);
    }
  } catch (const Exception& e) {
    std::cout << "Error: " << e.what() << std::endl;
  }
  return 0;

}


/**
 * Copyright (C) Omar Thor <omarthoro@gmail.com> - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 *
 * Written by Omar Thor <omarthoro@gmail.com>, 2017
 */

#ifndef SP_ALGO_NN_LAYER_LAYER_HPP
#define SP_ALGO_NN_LAYER_LAYER_HPP

#include <iosfwd>
#include <boost/assert.hpp>
#include "../config.hpp"
#include "../matrix.hpp"
#include "../types.hpp"
#include "params.hpp"
#include "sp/util/types.hpp"
#include "activation/op.hpp"
#include "detail/layers.hpp"

SP_ALGO_NN_NAMESPACE_BEGIN

/**
 * \file Basic layer abstract class
 */

/**
 * \brief Abstract layer of neural network
 *
 * All possible layer types extend this base type.
 *
 * \tparam Attached, if layer shares state with previous layer
 * \tparam DefaultOutputRange when false, must implement out_range
 * \tparam ConfiguresItself, when true, must implement
 *         '''configuration_impl(const size_t& batch_size, bool reset)'''
 *         and it must initialize weights and bias, if any
 */
template<
    typename InputVolume,
    typename OutputVolume,
    typename Derived,
    bool DefaultOutputRange = true,
    bool ConfiguresItself = false
>
struct layer {

    using derived_type = Derived;

    /**
     * \brief Input Dimensions
     */
    using input_dims = InputVolume;

    /**
     * \brief Output Dimensions
     */
    using output_dims = OutputVolume;

    constexpr static bool default_output_range = DefaultOutputRange;

    constexpr static bool configures_itself = ConfiguresItself;

    /**
     * Validate InputDims
     */
    static_assert(util::is_instantiation_of_v<InputVolume, volume_dims>, "InputDim template parameter must be an instance of input_dims");

    /**
     * Validate OutputDims
     */
    static_assert(util::is_instantiation_of_v<OutputVolume, volume_dims>, "InputDim template parameter must be an instance of input_dims");

    /**
     * Feed forward propagation
     *
     * \param input The input tensor
     */
    void forward_prop(tensor_4& input, tensor_4& output) {
        BOOST_ASSERT_MSG(
            detail::validate_dimensions<input_dims>(input),
            "Dimensions of input matches input dimension"
        );
        BOOST_ASSERT_MSG(
            detail::validate_dimensions<output_dims>(output),
            "Dimensions of output matches output dimensions"
        );
        derived().forward_prop_impl(input, output);
    }

    /**
     * Backward propagation
     * \param input The input tensor
     * \param output The output tensor
     */
    void backward_prop(     tensor_4& prev_out,
                            tensor_4& prev_delta,
                            tensor_4& curr_out,
                            tensor_4& curr_delta) {
        BOOST_ASSERT_MSG(
            detail::validate_dimensions<input_dims>(prev_out),
            "Dimensions prev_out matches input dimension"
        );
        BOOST_ASSERT_MSG(
            detail::validate_dimensions<input_dims>(prev_delta),
            "Dimensions prev_delta matches input dimension"
        );
        BOOST_ASSERT_MSG(
            detail::validate_dimensions<output_dims>(curr_out),
            "Dimensions of curr_out matches input dimensions"
        );
        BOOST_ASSERT_MSG(
            detail::validate_dimensions<output_dims>(curr_delta),
            "Dimensions of current_delta matches output dimensions"
        );
        derived().backward_prop_impl(prev_out, prev_delta, curr_out, curr_delta);
    }

    template<typename Optimizer>
    void update_weights(Optimizer& optimizer) {
        if constexpr (detail::has_weight_and_delta_v<derived_type>) {
            update_weights(optimizer, wdeltas, derived().dw, derived().w);
        }
        if constexpr (detail::has_bias_and_delta_v<derived_type>) {
            update_weights(optimizer, bdeltas, derived().db, derived().b);
        }
        clear_gradients();
    }

    /**
     * \brief Helper funciton which combines delta of the layer into a combined
     *        state which then applies the combined delta to the updating tensor
     * \tparam optimizer the Optimizing strategy
     * \tparam combined the tensor to stored the combined delta state of all samples
     * \tparam updating the sensor that is to be updated via the optimizer
     */
    template<typename Optimizer, typename CombiningTensor, typename FromTensor, typename ToTensor>
    void update_weights(Optimizer& optimizer, CombiningTensor& combined, FromTensor& delta, ToTensor& updating) {
        const size_t samples = delta.dimension(0);
        BOOST_ASSERT(samples >= 1);
        /* Sum*/
        combined = delta.chip(0, 0);
        for(size_t s = 1; s < samples; ++s) {
            /* add the other delta chips */
            combined += delta.chip(s, 0);
        }
        if(samples > 1) {
            const float_t reciprocal_batch_size = 1.0f / static_cast<float_t>(samples);
            combined = combined * reciprocal_batch_size;
        };
        optimizer.template update(combined, updating);
    }

    void clear_gradients() {
        if constexpr (detail::has_weight_and_delta_v<derived_type>) {
            derived().dw.setZero();
        }
        if constexpr (detail::has_bias_and_delta_v<derived_type>) {
            derived().db.setZero();
        }
    }

    /**
     * \brief Configures layer. Must be called before use. It is undefined
     *        behavior not to configure a layer before use.
     */
    void configure(const size_t& batch_size, bool reset = false) {
        if constexpr(configures_itself) {
            derived().configuration_impl(batch_size, reset);
        } else {
            default_configuration(batch_size, reset);
        }
    }

    /**
     * \brief Default configuration behavior
     * Can be called from a custom configure_impl in order to keep original default
     * configuration, or alternatively called
     */
    void default_configuration(const size_t& batch_size, bool reset = false) {
        if constexpr(detail::has_weight_and_delta_v<derived_type>) {
            detail::prepare_weights<typename derived_type::weights_dims>(derived().w);
            detail::prepare_and_zero_delta_weights<typename derived_type::weights_dims>(batch_size, derived().dw);
            if(reset) {
                detail::apply_weight_initializer(derived());
            }
        }
        if constexpr(detail::has_bias_and_delta_v<derived_type>) {
            detail::prepare_bias<typename derived_type::output_dims>(derived().b);
            detail::prepare_and_zero_bias_delta<typename derived_type::output_dims>(batch_size, derived().db);
            if(reset) {
                detail::apply_bias_initializer(derived());
            }
        }
        /* Always clear gradients */
        clear_gradients();
    }

    /**
     * Load the layer's weights from the input stream
     * @param is
     */
    void load(std::istream& is) {
        if constexpr(detail::has_weight_and_delta_v<derived_type>) {
            auto& w = derived().w;
            float_t tmp;
            for(size_t i = 0, len = w.size(); i < len; ++i) {
                is >> tmp;
                w.data()[i] = tmp;
            }
        }
        if constexpr(detail::has_bias_and_delta_v<derived_type>) {
            auto& b = derived().b;
            float_t tmp;
            for(size_t i = 0, len = b.size(); i < len; ++i) {
                is >> tmp;
                b.data()[i] = tmp;
            }
        }
    }

    /**
     * Save the layer's weights to the output stream
     * @param os
     */
    void save(std::ostream& os) {
        if constexpr(detail::has_weight_and_delta_v<derived_type>) {
            auto& w = derived().w;
            for(size_t i = 0, len = w.size(); i < len; ++i) {
                os << w.data()[i] << " ";
            }
        }
        if constexpr(detail::has_bias_and_delta_v<derived_type>) {
            auto& b = derived().b;
            for(size_t i = 0, len = b.size(); i < len; ++i) {
                os << b.data()[i] << " ";
            }
        }
    }

    /**
     * CRTP helper
     */
    const derived_type& derived() const {
        return *static_cast<const derived_type*>(this);
    }

    /**
     * CRTP helper
     */
    derived_type& derived() {
        return *static_cast<derived_type*>(this);
    }

    /**
     * \brief Returns the output range of a layer
     */
    valid_range
    range() {
        if constexpr(default_output_range) {
            return {-1.0f, 1.0f};
        } else {
            return derived().range_impl();
        }
    }

    /**
     * The target range of the layer for training purposes
     * @return
     */
    valid_range target_range() {
        return {0.0f, 1.0f};
    }

    /**
     * Whether or not a layer is trainable
     */
    bool is_trainable;

    /**
     * \brief Temporarily used in combine_grads
     */
    tensor_4 wdeltas;

    /**
     * \brief Temporarily used in combine_grads
     */
    tensor_1 bdeltas;

    /**
     * \brief Customization point for controlling weight initialization strategy of this layer
     */
    std::function<void(float_t*, float_t*, const size_t&, const size_t&)> weight_initializer;
    /**
     * \brief Customization point for controlling bias initializaiton strategy of this layer
     */
    std::function<void(float_t*, float_t*, const size_t&, const size_t&)> bias_initializer;
};

SP_ALGO_NN_NAMESPACE_END

#endif	/* SP_ALGO_NN_LAYER_LAYER_HPP */


#include <mex.h>
#include <Eigen/Core>
#include <chrono>
#include "iris/iris.h"

using namespace Eigen;

// mxGetPrSafe and eigenToMatlab lovingly ripped off from the Drake toolbox: https://github.com/RobotLocomotion/drake
double * mxGetPrSafe(const mxArray *pobj) {
  if (!mxIsDouble(pobj)) mexErrMsgIdAndTxt("Iris:mxGetPrSafe:WrongType", "mxGetPr can only be called on arguments which correspond to Matlab doubles");
  return mxGetPr(pobj);
}

mxArray* eigenToMatlab(const MatrixXd &m)
{
  // this avoids zero initialization that would occur using mxCreateDoubleMatrix with nonzero dimensions.
  // see https://classes.soe.ucsc.edu/ee264/Fall11/cmex.pdf, page 8
  mxArray* pm = mxCreateDoubleMatrix(0, 0, mxREAL);
  int rows = static_cast<int>(m.rows());
  int cols = static_cast<int>(m.cols());
  int numel = rows * cols;
  mxSetM(pm, rows);
  mxSetN(pm, cols);
  if (numel)
    mxSetData(pm, mxMalloc(sizeof(double) * numel));
  memcpy(mxGetPr(pm), m.data(), sizeof(double)* numel);
  return pm;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  // auto begin = std::chrono::high_resolution_clock::now();

  if (nrhs != 5 || nlhs > 6) {
    mexErrMsgTxt("usage: [A, b, C, d, p_history, e_history] = inflate_regionmex(obstacles, A_bounds, b_bounds, start, options)");
  }

  int narg = 0;

  const mxArray* obstacles = prhs[narg];
  if (!mxIsCell(obstacles)) {
    mexErrMsgTxt("obstacles should be a cell array of matrices");
  }
  narg++;

  Map<MatrixXd> A_bounds(mxGetPrSafe(prhs[narg]),
                         mxGetM(prhs[narg]),
                         mxGetN(prhs[narg]));
  narg++;
  Map<VectorXd> b_bounds(mxGetPrSafe(prhs[narg]),
                         mxGetNumberOfElements(prhs[narg]));
  narg++;

  Map<VectorXd> start(mxGetPrSafe(prhs[narg]),
                      mxGetNumberOfElements(prhs[narg]));
  narg++;

  // mexPrintf("made maps\n");

  const mxArray* options_ptr = prhs[narg];

  const int dim = start.size();
  if (A_bounds.cols() != dim) {
    mexPrintf("Problem dimension was inferred to be %d from length of [start], so [A_bounds] should have %d columns\n", start.size(), A_bounds.cols());
    mexErrMsgTxt("Dimension of problem does not match size of A_bounds");
  }
  if (A_bounds.rows() != b_bounds.size()) {
    mexErrMsgTxt("A_bounds and b_bounds should have the same number of rows");
  }

  iris::IRISProblem problem(dim);
  problem.setSeedPoint(start);

  // mexPrintf("set seed\n");

  const size_t n_obs = mxGetNumberOfElements(obstacles);
  for (size_t i=0; i < n_obs; i++) {
    const mxArray *obs_ptr = mxGetCell(obstacles, i);
    Map<MatrixXd> obs(mxGetPrSafe(obs_ptr),
                      mxGetM(obs_ptr),
                      mxGetN(obs_ptr));
    if (obs.rows() != dim) {
      mexErrMsgTxt("Each obstacle should be of size M x N where M is the dimension of [start].");
    }
    problem.addObstacle(obs);
  }

  // mexPrintf("added obstacles\n");

  iris::IRISOptions options;
  const mxArray *opt;
  opt = mxGetField(options_ptr, 0, "require_containment");
  if (opt) options.require_containment = static_cast<bool>(mxGetScalar(opt));

  opt = mxGetField(options_ptr, 0, "error_on_infeasible_start");
  if (opt) options.error_on_infeasible_start = static_cast<bool>(mxGetScalar(opt));

  opt = mxGetField(options_ptr, 0, "termination_threshold");
  if (opt) options.termination_threshold = mxGetScalar(opt);

  opt = mxGetField(options_ptr, 0, "iter_limit");
  if (opt) options.iter_limit = static_cast<int>(mxGetScalar(opt));

  // mexPrintf("got options\n");

  problem.setBounds(iris::Polyhedron(A_bounds, b_bounds));

  // auto end = std::chrono::high_resolution_clock::now();
  // auto elapsed = std::chrono::duration_cast<std::chrono::duration<float>>(end - begin);
  // std::cout << "pre-solve time: " << elapsed.count() << " s" << std::endl;

  iris::IRISRegion region;
  std::unique_ptr<iris::IRISDebugData> debug;
  // begin = std::chrono::high_resolution_clock::now();
  try {
    if (nlhs > 4) {
      debug.reset(new iris::IRISDebugData());
      region = iris::inflate_region(problem, options, debug.get());
    } else {
      region = iris::inflate_region(problem, options);
    }
  } catch (iris::InitialPointInfeasibleError &exception) {
    mexErrMsgIdAndTxt("IRIS:InfeasibleStart", "Initial point is infeasible");
  }
  // end = std::chrono::high_resolution_clock::now();
  // elapsed = std::chrono::duration_cast<std::chrono::duration<float>>(end - begin);
  // std::cout << "solve time: " << elapsed.count() << " s" << std::endl;

  // begin = std::chrono::high_resolution_clock::now();
  // mexPrintf("ran iris\n");

  narg = 0;
  if (nlhs > narg) plhs[narg] = eigenToMatlab(region.polyhedron.getA());
  narg++;

  if (nlhs > narg) plhs[narg] = eigenToMatlab(region.polyhedron.getB());
  narg++;

  if (nlhs > narg) plhs[narg] = eigenToMatlab(region.ellipsoid.getC());
  narg++;

  if (nlhs > narg) plhs[narg] = eigenToMatlab(region.ellipsoid.getD());
  narg++;

  if (nlhs > narg) {
    const size_t n_polys[1] = {debug->polyhedron_history.size()};
    plhs[narg] = mxCreateCellArray(1, n_polys);
    for (int i=0; i < debug->polyhedron_history.size(); i++) {
      const size_t dims[1] = {1};
      const char* fields[2] = {"A", "b"};
      mxArray* entry = mxCreateStructArray(1, dims, 2, fields);
      mxSetField(entry, 0, "A", eigenToMatlab(debug->polyhedron_history[i].getA()));
      mxSetField(entry, 0, "b", eigenToMatlab(debug->polyhedron_history[i].getB()));
      mxSetCell(plhs[narg], i, entry);
    }
  }
  narg++;

  if (nlhs > narg) {
    const size_t n_ellipsoids[1] = {debug->ellipsoid_history.size()};
    plhs[narg] = mxCreateCellArray(1, n_ellipsoids);
    for (int i=0; i < debug->ellipsoid_history.size(); i++) {
      const size_t dims[1] = {1};
      const char* fields[2] = {"C", "d"};
      mxArray* entry = mxCreateStructArray(1, dims, 2, fields);
      mxSetField(entry, 0, "C", eigenToMatlab(debug->ellipsoid_history[i].getC()));
      mxSetField(entry, 0, "d", eigenToMatlab(debug->ellipsoid_history[i].getD()));
      mxSetCell(plhs[narg], i, entry);
    }
  }
  narg++;

  // end = std::chrono::high_resolution_clock::now();
  // elapsed = std::chrono::duration_cast<std::chrono::duration<float>>(end - begin);
  // std::cout << "post-solve time: " << elapsed.count() << " s" << std::endl;
}


/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2014 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

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

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <mitsuba/render/scene.h>
#include <mitsuba/core/statistics.h>
#include <boost/math/distributions/normal.hpp>

MTS_NAMESPACE_BEGIN

/*!\plugin{adaptive}{Adaptive integrator}
 * \order{13}
 * \parameters{
 *     \parameter{maxError}{\Float}{Maximum relative error
 *         threshold\default{0.05}}
 *     \parameter{pValue}{\Float}{
 *         Required p-value to accept a sample \default{0.05}
 *     }
 *     \parameter{maxSampleFactor}{\Integer}{
 *         Maximum number of samples to be generated \emph{relative} to the
 *         number of configured pixel samples. The adaptive integrator
 *         will stop after this many samples, regardless of whether
 *         or not the error criterion was satisfied.
 *         A negative value will be interpreted as $\infty$.
 *         \default{32---for instance, when 64 pixel samples are configured in
 *         the \code{sampler}, this means that the adaptive integrator
 *         will give up after 32*64=2048 samples}
 *     }
 * }
 *
 * This ``meta-integrator'' repeatedly invokes a provided sub-integrator
 * until the computed radiance values satisfy a specified relative error bound
 * (5% by default) with a certain probability (95% by default). Internally,
 * it uses a Z-test to decide when to stop collecting samples. While repeatedly
 * applying a Z-test in this manner is not good practice in terms of
 * a rigorous statistical analysis, it provides a useful mathematically
 * motivated stopping criterion.
 *
 * \begin{xml}[caption={An example how to make the \pluginref{path} integrator adaptive}]
 * <integrator type="adaptive">
 *     <integrator type="path"/>
 * </integrator>
 * \end{xml}
 *
 * \remarks{
 *    \item The adaptive integrator needs a variance estimate to work
 *     correctly. Hence, the underlying sample generator should be set to a reasonably
 *     large number of pixel samples (e.g. 64 or higher) so that this estimate can be obtained.
 *    \item This plugin uses a relatively simplistic error heuristic that does not
 *    share information between pixels and only reasons about variance in image space.
 *    In the future, it will likely be replaced with something more robust.
 * }
 */
class AdaptiveIntegrator : public SamplingIntegrator {
public:
	AdaptiveIntegrator(const Properties &props) : SamplingIntegrator(props) {
		/* Maximum relative error threshold */
		m_maxError = props.getFloat("maxError", 0.05f);
		/* Maximum number of samples to take (relative to the number of pixel samples
		   that were configured in the sampler). The sample collection
		   will stop after this many samples even if the variance is still
		   too high. A negative value will be interpreted as infinity. */
		m_maxSampleFactor = props.getInteger("maxSampleFactor", 32);
		/* Required P-value to accept a sample. */
		m_pValue = props.getFloat("pValue", 0.05f);
		m_verbose = props.getBoolean("verbose", false);
	}

	AdaptiveIntegrator(Stream *stream, InstanceManager *manager)
	 : SamplingIntegrator(stream, manager) {
		m_subIntegrator = static_cast<SamplingIntegrator *>(manager->getInstance(stream));
		m_maxSampleFactor = stream->readInt();
		m_maxError = stream->readFloat();
		m_quantile = stream->readFloat();
		m_averageLuminance = stream->readFloat();
		m_pValue = stream->readFloat();
		m_verbose = false;
	}

	void addChild(const std::string &name, ConfigurableObject *child) {
		const Class *cClass = child->getClass();

		if (cClass->derivesFrom(MTS_CLASS(Integrator))) {
			if (!cClass->derivesFrom(MTS_CLASS(SamplingIntegrator)))
				Log(EError, "The sub-integrator must be derived from the class SamplingIntegrator");
			m_subIntegrator = static_cast<SamplingIntegrator *>(child);
		} else {
			Integrator::addChild(name, child);
		}
	}

	void configureSampler(const Scene *scene, Sampler *sampler) {
		SamplingIntegrator::configureSampler(scene, sampler);
		m_subIntegrator->configureSampler(scene, sampler);
	}

	bool preprocess(const Scene *scene, RenderQueue *queue, const RenderJob *job,
			int sceneResID, int sensorResID, int samplerResID) {
		if (!SamplingIntegrator::preprocess(scene, queue, job, sceneResID, sensorResID, samplerResID))
			return false;
		if (m_subIntegrator == NULL)
			Log(EError, "No sub-integrator was specified!");
		Sampler *sampler = static_cast<Sampler *>(Scheduler::getInstance()->getResource(samplerResID, 0));
		Sensor *sensor = static_cast<Sensor *>(Scheduler::getInstance()->getResource(sensorResID));
		if (sampler->getClass()->getName() != "IndependentSampler")
			Log(EError, "The error-controlling integrator should only be "
				"used in conjunction with the independent sampler");
		if (!m_subIntegrator->preprocess(scene, queue, job, sceneResID, sensorResID, samplerResID))
			return false;

		Vector2i filmSize = sensor->getFilm()->getSize();
		bool needsApertureSample = sensor->needsApertureSample();
		bool needsTimeSample = sensor->needsTimeSample();
		const int nSamples = 10000;
		Float luminance = 0;

		Point2 apertureSample(0.5f);
		Float timeSample = 0.5f;
		RadianceQueryRecord rRec(scene, sampler);

		/* Estimate the overall luminance on the image plane */
		for (int i=0; i<nSamples; ++i) {
			sampler->generate(Point2i(0));

			rRec.newQuery(RadianceQueryRecord::ERadiance, sensor->getMedium());
			rRec.extra = RadianceQueryRecord::EAdaptiveQuery;

			Point2 samplePos(rRec.nextSample2D());
			samplePos.x *= filmSize.x;
			samplePos.y *= filmSize.y;

			if (needsApertureSample)
				apertureSample = rRec.nextSample2D();
			if (needsTimeSample)
				timeSample = rRec.nextSample1D();

			RayDifferential eyeRay;
			Spectrum sampleValue = sensor->sampleRay(
				eyeRay, samplePos, apertureSample, timeSample);

			sampleValue *= m_subIntegrator->Li(eyeRay, rRec);
			luminance += sampleValue.getLuminance();
		}

		m_averageLuminance = luminance / nSamples;

		boost::math::normal dist(0, 1);
		m_quantile = (Float) boost::math::quantile(dist, 1-m_pValue/2);
		Log(EInfo, "Configuring for a %.1f%% confidence interval, quantile=%f, avg. luminance=%f",
			(1-m_pValue)*100, m_quantile, m_averageLuminance);
		return true;
	}

	void renderBlock(const Scene *scene, const Sensor *sensor,
			Sampler *sampler, ImageBlock *block, const bool &stop,
			const std::vector< TPoint2<uint8_t> > &points) const {
		typedef TSpectrum<Float, SPECTRUM_SAMPLES + 2> SpectrumAlphaWeight;

		bool needsApertureSample = sensor->needsApertureSample();
		bool needsTimeSample = sensor->needsTimeSample();

		if (sampler->getSampleCount() < 8)
			Log(EError, "Starting the adaptive integrator with less than 8 "
				"samples per pixel does not make much sense -- giving up.");

		RayDifferential eyeRay;
		RadianceQueryRecord rRec(scene, sampler);

		Float diffScaleFactor = 1.0f /
			std::sqrt((Float) sampler->getSampleCount());

		Point2 apertureSample(0.5f);
		Float timeSample = 0.5f;
		int borderSize = sensor->getFilm()->getReconstructionFilter()->getBorderSize();

		size_t sampleCount;
		block->clear();

		SpectrumAlphaWeight *target = (SpectrumAlphaWeight *) block->getBitmap()->getUInt8Data();
		SpectrumAlphaWeight *snapshot = (SpectrumAlphaWeight *) alloca(sizeof(SpectrumAlphaWeight)
			* (2*borderSize+1)*(2*borderSize+1));

		for (size_t i=0; i<points.size(); ++i) {
			Point2i offset = Point2i(points[i]) + Vector2i(block->getOffset());
			sampler->generate(offset);

			/* Before starting to place samples within the area of a single pixel, the
			   following takes a snapshot of all surrounding spectrum+weight+alpha
			   values. Those are then used later to ensure that adjacent pixels will
			   not be disproportionately biased by this pixel's contributions. */
			for (int y=0; y<2*borderSize+1; ++y) {
				SpectrumAlphaWeight *src = target + ((y+points[i].y)
					* block->getBitmap()->getWidth() + points[i].x);
				SpectrumAlphaWeight *dst = snapshot + y*(2*borderSize+1);
				memcpy(dst, src, sizeof(SpectrumAlphaWeight) * (2*borderSize+1));
			}

			Float mean = 0, meanSqr = 0.0f;
			sampleCount = 0;

			while (true) {
				if (stop)
					return;

				rRec.newQuery(RadianceQueryRecord::ESensorRay, sensor->getMedium());
				rRec.extra = RadianceQueryRecord::EAdaptiveQuery;

				Point2 samplePos(Point2(offset) + Vector2(rRec.nextSample2D()));
				if (needsApertureSample)
					apertureSample = rRec.nextSample2D();
				if (needsTimeSample)
					timeSample = rRec.nextSample1D();

				Spectrum sampleValue = sensor->sampleRayDifferential(
					eyeRay, samplePos, apertureSample, timeSample);
				eyeRay.scaleDifferential(diffScaleFactor);

				sampleValue *= m_subIntegrator->Li(eyeRay, rRec);

				Float sampleLuminance;
				if (block->put(samplePos, sampleValue, rRec.alpha)) {
					/* Check for problems with the sample */
					sampleLuminance = sampleValue.getLuminance();
				} else {
					sampleLuminance = 0.0f;
				}
				++sampleCount;
				sampler->advance();

				/* Numerically robust online variance estimation using an
				   algorithm proposed by Donald Knuth (TAOCP vol.2, 3rd ed., p.232) */
				const Float delta = sampleLuminance - mean;
				mean += delta / sampleCount;
				meanSqr += delta * (sampleLuminance - mean);

				if (m_maxSampleFactor >= 0 && sampleCount >= m_maxSampleFactor * sampler->getSampleCount()) {
					break;
				} else if (sampleCount >= sampler->getSampleCount()) {
					/* Variance of the primary estimator */
					const Float variance = meanSqr / (sampleCount-1);

					Float stdError = std::sqrt(variance/sampleCount);

					/* Half width of the confidence interval */
					Float ciWidth = stdError * m_quantile;

					/* Relative error heuristic */
					Float base = std::max(mean, m_averageLuminance * 0.01f);

					if (m_verbose && (sampleCount % 100) == 0)
						Log(EDebug, "%i samples, mean=%f, stddev=%f, std error=%f, ci width=%f, max allowed=%f", sampleCount, mean,
							std::sqrt(variance), stdError, ciWidth, base * m_maxError);

					if (ciWidth <= m_maxError * base)
						break;
				}
			}

			/* Ensure that a large amounts of samples in one pixel do not
			   bias neighboring pixels (due to the reconstruction filter) */
			Float factor = 1.0f / sampleCount;
			for (int y=0; y<2*borderSize+1; ++y) {
				SpectrumAlphaWeight *dst = target + ((y+points[i].y)
					* block->getBitmap()->getWidth() + points[i].x);
				SpectrumAlphaWeight *backup = snapshot + y*(2*borderSize+1);

				for (int x=0; x<2*borderSize+1; ++x)
					dst[x] = backup[x] * (1-factor) + dst[x] * factor;
			}
		}
	}

	Spectrum Li(const RayDifferential &ray, RadianceQueryRecord &rRec) const {
		return m_subIntegrator->Li(ray, rRec);
	}

	Spectrum E(const Scene *scene, const Intersection &its, const Medium *medium,
			Sampler *sampler, int nSamples, bool includeIndirect) const {
		return m_subIntegrator->E(scene, its, medium,
			sampler, nSamples, includeIndirect);
	}

	void serialize(Stream *stream, InstanceManager *manager) const {
		SamplingIntegrator::serialize(stream, manager);
		manager->serialize(stream, m_subIntegrator.get());

		stream->writeInt(m_maxSampleFactor);
		stream->writeFloat(m_maxError);
		stream->writeFloat(m_quantile);
		stream->writeFloat(m_averageLuminance);
		stream->writeFloat(m_pValue);
	}

	void bindUsedResources(ParallelProcess *proc) const {
		m_subIntegrator->bindUsedResources(proc);
	}

	void wakeup(ConfigurableObject *parent,
			std::map<std::string, SerializableObject *> &params) {
		m_subIntegrator->wakeup(this, params);
	}

	void cancel() {
		SamplingIntegrator::cancel();
		m_subIntegrator->cancel();
	}

	const Integrator *getSubIntegrator(int idx) const {
		if (idx != 0)
			return NULL;
		return m_subIntegrator.get();
	}

	std::string toString() const {
		std::ostringstream oss;
		oss << "AdaptiveIntegrator[" << endl
			<< "  maxSamples = " << m_maxSampleFactor << "," << endl
			<< "  maxError = " << m_maxError << "," << endl
			<< "  quantile = " << m_quantile << "," << endl
			<< "  pvalue = " << m_pValue << "," << endl
			<< "  subIntegrator = " << indent(m_subIntegrator->toString()) << endl
			<< "]";
		return oss.str();
	}

	MTS_DECLARE_CLASS()
private:
	ref<SamplingIntegrator> m_subIntegrator;
	Float m_maxError, m_quantile, m_pValue, m_averageLuminance;
	int m_maxSampleFactor;
	bool m_verbose;
};

MTS_IMPLEMENT_CLASS_S(AdaptiveIntegrator, false, SamplingIntegrator)
MTS_EXPORT_PLUGIN(AdaptiveIntegrator, "Adaptive integrator");
MTS_NAMESPACE_END


/**
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

// INCLUDES ===================================================================>

// Including Standard Libraries
#include <cstdlib>
#include <cassert>
#include <cstring>
#include <fstream>
#include <iostream>
#include <vector>
#include <string>
#include <algorithm>
#include <limits>
#include <cmath>

#include <boost/program_options.hpp>

#include <distributed_graphlab.hpp>

#include "image.hpp"

// Include the macro for the for each operation
#include <graphlab/macros_def.hpp>

// STRUCTS (Edge and Vertex data) =============================================>

/**
 * unused
 */
typedef char edge_data;

/**
 * The data associated with each variable in the pairwise markov
 * random field
 */
struct vertex_data {
  size_t sample;
  graphlab::unary_factor unary;
  void save(graphlab::oarchive &oarc) const {
    oarc << unary << sample;
  }

  void load(graphlab::iarchive &iarc) {
    iarc >> unary >> sample;
  }
}; // End of vertex data

typedef graphlab::distributed_graph<vertex_data, edge_data> graph_type;
typedef graphlab::distributed_types<graph_type> gl_types;

gl_types::distributed_glshared<graphlab::binary_factor> EDGE_FACTOR;

// GraphLab Update Function ===================================================>

/** Construct denoising ising model based on the image */
void construct_graph(image& img,
                     size_t num_rings,
                     double sigma,
                     gl_types::memory_graph& distributed_graph);

/**
 * The core gibbs update function.  This update satisfies
 * the graphlab update_function interface.
 */
void gibbs_update(gl_types::iscope& scope,
                  gl_types::icallback& scheduler);

// MAIN =======================================================================>
int main(int argc, char** argv) {
  std::cout << "This program creates and denoises a synthetic " << std::endl
            << "image using gibbs sampling inside " << std::endl
            << "the graphlab framework." << std::endl;

  // set the global logger
  global_logger().set_log_level(LOG_DEBUG);
  global_logger().set_log_to_console(true);

  bool makegraph = false;
  size_t colors = 5;
  size_t rows = 200;
  size_t cols = 200;
  double sigma = 1;
  double lambda = 2;
  std::string smoothing = "square";
  std::string orig_fn = "source_img.pgm";
  std::string noisy_fn = "noisy_img.pgm";
  std::string pred_fn = "pred_img.pgm";

  // Parse command line arguments --------------------------------------------->
  graphlab::command_line_options clopts("Loopy BP image denoising");
  clopts.use_distributed_options();
  clopts.attach_option("makegraph",
                       &makegraph, makegraph,
                       "Creates the disk distributed_graph");
  clopts.attach_option("colors",
                       &colors, colors,
                       "The number of colors in the noisy image");
  clopts.attach_option("rows",
                       &rows, rows,
                       "The number of rows in the noisy image");
  clopts.attach_option("cols",
                       &cols, cols,
                       "The number of columns in the noisy image");
  clopts.attach_option("sigma",
                       &sigma, sigma,
                       "Standard deviation of noise.");
  clopts.attach_option("lambda",
                       &lambda, lambda,
                       "Smoothness parameter (larger => smoother).");
  clopts.attach_option("smoothing",
                       &smoothing, smoothing,
                       "Options are {square, laplace}");
  clopts.attach_option("orig",
                       &orig_fn, orig_fn,
                       "Original image file name.");
  clopts.attach_option("noisy",
                       &noisy_fn, noisy_fn,
                       "Noisy image file name.");
  clopts.attach_option("pred",
                       &pred_fn, pred_fn,
                       "Predicted image file name.");

  clopts.set_scheduler_type("multiqueue_fifo");
  clopts.set_scope_type("edge");

  bool success = clopts.parse(argc, argv);
  if(!success) {
    return EXIT_FAILURE;
  }

  std::cout << "ncpus:          " << clopts.get_ncpus() << std::endl
            << "colors:         " << colors << std::endl
            << "rows:           " << rows << std::endl
            << "cols:           " << cols << std::endl
            << "sigma:          " << sigma << std::endl
            << "lambda:         " << lambda << std::endl
            << "smoothing:      " << smoothing << std::endl
            << "engine:         " << clopts.get_engine_type() << std::endl
            << "scope:          " << clopts.get_scope_type() << std::endl
            << "scheduler:      " << clopts.get_scheduler_type() << std::endl
            << "orig_fn:        " << orig_fn << std::endl
            << "noisy_fn:       " << noisy_fn << std::endl
            << "pred_fn:        " << pred_fn << std::endl;

  std::cout << "Creating a synthetic image. " << std::endl;
  image img(rows, cols);
  img.paint_sunset(colors);
  std::cout << "Saving image. " << std::endl;
  img.save(orig_fn.c_str());
  std::cout << "Corrupting Image. " << std::endl;
  img.corrupt(sigma);

  if (makegraph) {
    // Create synthetic images -------------------------------------------------->
    // Creating image for denoising
    std::cout << "Saving corrupted image. " << std::endl;
    img.save(noisy_fn.c_str());

    std::cout << "Constructing pairwise Markov Random Field. " << std::endl;
    gl_types::disk_graph dg("denoise", 64);
    gl_types::memory_graph g;

    construct_graph(img, colors, sigma, g);
    std::vector<graphlab::graph_partitioner::part_id_type> parts;
    graphlab::graph_partitioner::metis_partition(g, 64, parts);
    dg.create_from_graph(g, parts);
    dg.finalize();
    dg.make_memory_atoms();
    return 0;
  }

  graphlab::mpi_tools::init(argc, argv);

  graphlab::dc_init_param param;
  ASSERT_TRUE(graphlab::init_param_from_mpi(param));
  // create distributed control
  graphlab::distributed_control dc(param);
  // Create the distributed_graph --------------------------------------------------------->
  gl_types::distributed_core core(dc, "denoise.idx");
  // Set the engine options
  core.set_engine_options(clopts);
  core.build_engine();

  // Setup global shared variables -------------------------------------------->
  // Initialize the edge agreement factor
  std::cout << "Initializing shared edge agreement factor. " << std::endl;

  // dummy variables 0 and 1 and num_rings by num_rings
  graphlab::binary_factor edge_potential(0, colors, 0, colors);
  // Set the smoothing type
  if(smoothing == "square") {
    edge_potential.set_as_agreement(lambda);
  } else if (smoothing == "laplace") {
    edge_potential.set_as_laplace(lambda);
  } else {
    std::cout << "Invalid smoothing stype!" << std::endl;
    return EXIT_FAILURE;
  }
  std::cout << edge_potential << std::endl;

  EDGE_FACTOR.set(edge_potential);

  // Running the engine ------------------------------------------------------->
  core.sched_options().add_option("update_function",gibbs_update);

  std::cout << "Running the engine. " << std::endl;

  // Add the bp update to all vertices
  core.add_task_to_all(gibbs_update, 100.0);
  // Starte the engine
  double runtime = core.start();
  std::vector<vertex_data> all_vdata = core.graph().collect_vertices(0);
  if (dc.procid() == 0) {
    size_t update_count = core.last_update_count();
    std::cout << "Finished Running engine in " << runtime
              << " seconds." << std::endl
              << "Total updates: " << update_count << std::endl
              << "Efficiency: " << (double(update_count) / runtime)
              << " updates per second "
              << std::endl;

    // Saving the output -------------------------------------------------------->
    std::cout << "Rendering the cleaned image. " << std::endl;
    for(size_t v = 0; v < all_vdata.size(); ++v) {
      const vertex_data& vdata = all_vdata[v];
      img.pixel(v) = vdata.sample;
    }
    std::cout << "Saving cleaned image. " << std::endl;
    img.save(pred_fn.c_str());

    std::cout << "Done!" << std::endl;
  }
  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
} // End of main

// Implementations
// ============================================================>
void gibbs_update(gl_types::iscope& scope,
               gl_types::icallback& scheduler) {

  // Grab the state from the scope
  // ---------------------------------------------------------------->
  // Get the vertex data
  vertex_data& v_data = scope.vertex_data();

  // Get the in and out edges by reference
  gl_types::edge_list in_edges = scope.in_edge_ids();

  graphlab::unary_factor u = v_data.unary;
  graphlab::binary_factor efactor = EDGE_FACTOR.get_val();

  foreach(graphlab::edge_id_t ineid, in_edges) {
    size_t target_val = scope.const_neighbor_vertex_data(scope.source(ineid)).sample;
    for (size_t i = 0;i < u.arity(); ++i) {
      u.logP(i) += efactor.logP(target_val, i);
    }
  }
  u.normalize();
  v_data.sample = u.sample();
}

void construct_graph(image& img,
                     size_t num_rings,
                     double sigma,
                     gl_types::memory_graph& distributed_graph) {
  // Construct a single blob for the vertex data
  vertex_data vdata;
  vdata.unary.resize(num_rings);
  // Add all the vertices
  double sigmaSq = sigma*sigma;
  for(size_t i = 0; i < img.rows(); ++i) {
    for(size_t j = 0; j < img.cols(); ++j) {
      // Set the node potential
      double obs = img.pixel(i, j);
      for(size_t pred = 0; pred < num_rings; ++pred) {
        vdata.unary.logP(pred) =
          -(obs - pred)*(obs - pred) / (2.0 * sigmaSq);
      }
      vdata.unary.normalize();
      vdata.sample = graphlab::random::uniform<uint32_t>(0, num_rings - 1);
      // Store the actual data in the distributed_graph
      size_t vertid = distributed_graph.add_vertex(vdata);
      // Ensure that we are using a consistent numbering
      assert(vertid == img.vertid(i, j));
    } // end of for j in cols
  } // end of for i in rows

  // Add the edges

  for(size_t i = 0; i < img.rows(); ++i) {
    for(size_t j = 0; j < img.cols(); ++j) {
      size_t vertid = img.vertid(i,j);
      if(i-1 < img.rows()) {
        distributed_graph.add_edge(vertid, img.vertid(i-1, j), edge_data());
      }
      if(i+1 < img.rows()) {
        distributed_graph.add_edge(vertid, img.vertid(i+1, j), edge_data());
      }
      if(j-1 < img.cols()) {
        distributed_graph.add_edge(vertid, img.vertid(i, j-1), edge_data());
      } if(j+1 < img.cols()) {
        distributed_graph.add_edge(vertid, img.vertid(i, j+1), edge_data());
      }
    } // end of for j in cols
  } // end of for i in rows
  distributed_graph.compute_coloring();
  distributed_graph.finalize();
} // End of construct distributed_graph


// g2o - General Graph Optimization
// Copyright (C) 2012 R. Kümmerle
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <Eigen/Geometry>
#include <iostream>

#include "g2o/stuff/command_args.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_binary_edge.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/solvers/structure_only/structure_only_solver.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"

#include "EXTERNAL/ceres/autodiff.h"

#if defined G2O_HAVE_CHOLMOD
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#elif defined G2O_HAVE_CSPARSE
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#endif

using namespace g2o;
using namespace std;
using namespace Eigen;

/**
 * \brief camera vertex which stores the parameters for a pinhole camera
 *
 * The parameters of the camera are
 * - rx,ry,rz representing the rotation axis, whereas the angle is given by ||(rx,ry,rz)||
 * - tx,ty,tz the translation of the camera
 * - f the focal length of the camera
 * - k1, k2 two radial distortion parameters
 */
class VertexCameraBAL : public BaseVertex<9, Eigen::VectorXd>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexCameraBAL()
    {
    }

    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual void setToOriginImpl()
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    }

    virtual void oplusImpl(const double* update)
    {
      Eigen::VectorXd::ConstMapType v(update, VertexCameraBAL::Dimension);
      _estimate += v;
    }
};

/**
 * \brief 3D world feature
 *
 * A 3D point feature in the world
 */
class VertexPointBAL : public BaseVertex<3, Eigen::Vector3d>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexPointBAL()
    {
    }

    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual void setToOriginImpl()
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    }

    virtual void oplusImpl(const double* update)
    {
      Eigen::Vector3d::ConstMapType v(update);
      _estimate += v;
    }
};

/**
 * \brief edge representing the observation of a world feature by a camera
 *
 * see: http://grail.cs.washington.edu/projects/bal/
 * We use a pinhole camera model; the parameters we estimate for each camera
 * area rotation R, a translation t, a focal length f and two radial distortion
 * parameters k1 and k2. The formula for projecting a 3D point X into a camera
 * R,t,f,k1,k2 is:
 * P  =  R * X + t       (conversion from world to camera coordinates)
 * p  = -P / P.z         (perspective division)
 * p' =  f * r(p) * p    (conversion to pixel coordinates) where P.z is the third (z) coordinate of P.
 *
 * In the last equation, r(p) is a function that computes a scaling factor to undo the radial
 * distortion:
 * r(p) = 1.0 + k1 * ||p||^2 + k2 * ||p||^4.
 *
 * This gives a projection in pixels, where the origin of the image is the
 * center of the image, the positive x-axis points right, and the positive
 * y-axis points up (in addition, in the camera coordinate system, the positive
 * z-axis points backwards, so the camera is looking down the negative z-axis,
 * as in OpenGL).
 */
class EdgeObservationBAL : public BaseBinaryEdge<2, Vector2d, VertexCameraBAL, VertexPointBAL>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeObservationBAL()
    {
    }
    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }
    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    template<typename T>
    inline void cross(const T x[3], const T y[3], T result[3]) const
    {
      result[0] = x[1] * y[2] - x[2] * y[1];
      result[1] = x[2] * y[0] - x[0] * y[2];
      result[2] = x[0] * y[1] - x[1] * y[0];
    }

    template<typename T>
    inline T dot(const T x[3], const T y[3]) const { return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);}

    template<typename T>
    inline T squaredNorm(const T x[3]) const { return dot<T>(x, x);}

    /**
     * templatized function to compute the error as described in the comment above
     */
    template<typename T>
    bool operator()(const T* camera, const T* point, T* error) const
    {
      // Rodrigues' formula for the rotation
      T p[3];
      T theta = sqrt(squaredNorm(camera));
      if (theta > T(0)) {
        T v[3];
        v[0] = camera[0] / theta;
        v[1] = camera[1] / theta;
        v[2] = camera[2] / theta;
        T cth = cos(theta);
        T sth = sin(theta);

        T vXp[3];
        cross(v, point, vXp);
        T vDotp = dot(v, point);
        T oneMinusCth = T(1) - cth;

        for (int i = 0; i < 3; ++i)
          p[i] = point[i] * cth + vXp[i] * sth + v[i] * vDotp * oneMinusCth;
      } else {
        // taylor expansion for theta close to zero
        T aux[3];
        cross(camera, point, aux);
        for (int i = 0; i < 3; ++i)
          p[i] = point[i] + aux[i];
      }

      // translation of the camera
      p[0] += camera[3];
      p[1] += camera[4];
      p[2] += camera[5];

      // perspective division
      T projectedPoint[2];
      projectedPoint[0] = - p[0] / p[2];
      projectedPoint[1] = - p[1] / p[2];

      // conversion to pixel coordinates
      T radiusSqr = projectedPoint[0]*projectedPoint[0] + projectedPoint[1]*projectedPoint[1];
      T f         = T(camera[6]);
      T k1        = T(camera[7]);
      T k2        = T(camera[8]);
      T r_p       = T(1) + k1 * radiusSqr + k2 * radiusSqr * radiusSqr;
      T prediction[2];
      prediction[0] = f * r_p * projectedPoint[0];
      prediction[1] = f * r_p * projectedPoint[1];

      error[0] = prediction[0] - T(measurement()(0));
      error[1] = prediction[1] - T(measurement()(1));

      return true;
    }

    void computeError()
    {
      const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*>(vertex(0));
      const VertexPointBAL* point = static_cast<const VertexPointBAL*>(vertex(1));

      (*this)(cam->estimate().data(), point->estimate().data(), _error.data());
    }

    void linearizeOplus()
    {
      // use numeric Jacobians
      //BaseBinaryEdge<2, Vector2d, VertexCameraBAL, VertexPointBAL>::linearizeOplus();
      //return;

      const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*>(vertex(0));
      const VertexPointBAL* point = static_cast<const VertexPointBAL*>(vertex(1));
      typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;

      Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;
      Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;
      double *parameters[] = { const_cast<double*>(cam->estimate().data()), const_cast<double*>(point->estimate().data()) };
      double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };
      double value[Dimension];
      bool diffState = BalAutoDiff::Differentiate(*this, parameters, Dimension, value, jacobians);

      // copy over the Jacobians (convert row-major -> column-major)
      if (diffState) {
        _jacobianOplusXi = dError_dCamera;
        _jacobianOplusXj = dError_dPoint;
      } else {
        assert(0 && "Error while differentiating");
        _jacobianOplusXi.setZero();
        _jacobianOplusXi.setZero();
      }
    }
};

int main(int argc, char** argv)
{
  int maxIterations;
  bool verbose;
  bool usePCG;
  string outputFilename;
  string inputFilename;
  string statsFilename;
  CommandArgs arg;
  arg.param("i", maxIterations, 5, "perform n iterations");
  arg.param("o", outputFilename, "", "write points into a vrml file");
  arg.param("pcg", usePCG, false, "use PCG instead of the Cholesky");
  arg.param("v", verbose, false, "verbose output of the optimization process");
  arg.param("stats", statsFilename, "", "specify a file for the statistics");
  arg.paramLeftOver("graph-input", inputFilename, "", "file which will be processed");

  arg.parseArgs(argc, argv);

  typedef g2o::BlockSolver< g2o::BlockSolverTraits<9, 3> >  BalBlockSolver;
#ifdef G2O_HAVE_CHOLMOD
  string choleskySolverName = "CHOLMOD";
  typedef g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType> BalLinearSolver;
#elif defined G2O_HAVE_CSPARSE
  string choleskySolverName = "CSparse";
  typedef g2o::LinearSolverCSparse<BalBlockSolver::PoseMatrixType> BalLinearSolver;
#else
#error neither CSparse nor CHOLMOD are available
#endif
  typedef g2o::LinearSolverPCG<BalBlockSolver::PoseMatrixType> BalLinearSolverPCG;

  g2o::SparseOptimizer optimizer;
  std::unique_ptr<g2o::LinearSolver<BalBlockSolver::PoseMatrixType>> linearSolver;
  if (usePCG) {
    cout << "Using PCG" << endl;
    linearSolver = g2o::make_unique<BalLinearSolverPCG>();
  } else {
    cout << "Using Cholesky: " << choleskySolverName << endl;
    auto cholesky = g2o::make_unique<BalLinearSolver>();
    cholesky->setBlockOrdering(true);
    linearSolver = std::move(cholesky);
  }
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
    g2o::make_unique<BalBlockSolver>(std::move(linearSolver)));

  //solver->setUserLambdaInit(1);
  optimizer.setAlgorithm(solver);
  if (statsFilename.size() > 0){
    optimizer.setComputeBatchStatistics(true);
  }

  vector<VertexPointBAL*> points;
  vector<VertexCameraBAL*> cameras;

  // parse BAL dataset
  cout << "Loading BAL dataset " << inputFilename << endl;
  {
    ifstream ifs(inputFilename.c_str());
    int numCameras, numPoints, numObservations;
    ifs >> numCameras >> numPoints >> numObservations;

    cerr << PVAR(numCameras) << " " << PVAR(numPoints) << " " << PVAR(numObservations) << endl;

    int id = 0;
    cameras.reserve(numCameras);
    for (int i = 0; i < numCameras; ++i, ++id) {
      VertexCameraBAL* cam = new VertexCameraBAL;
      cam->setId(id);
      optimizer.addVertex(cam);
      cameras.push_back(cam);
    }

    points.reserve(numPoints);
    for (int i = 0; i < numPoints; ++i, ++id) {
      VertexPointBAL* p = new VertexPointBAL;
      p->setId(id);
      p->setMarginalized(true);
      bool addedVertex = optimizer.addVertex(p);
      if (! addedVertex) {
        cerr << "failing adding vertex" << endl;
      }
      points.push_back(p);
    }

    // read in the observation
    for (int i = 0; i < numObservations; ++i) {
      int camIndex, pointIndex;
      double obsX, obsY;
      ifs >> camIndex >> pointIndex >> obsX >> obsY;

      assert(camIndex >= 0 && (size_t)camIndex < cameras.size() && "Index out of bounds");
      VertexCameraBAL* cam = cameras[camIndex];
      assert(pointIndex >= 0 && (size_t)pointIndex < points.size() && "Index out of bounds");
      VertexPointBAL* point = points[pointIndex];

      EdgeObservationBAL* e = new EdgeObservationBAL;
      e->setVertex(0, cam);
      e->setVertex(1, point);
      e->setInformation(Eigen::Matrix2d::Identity());
      e->setMeasurement(Eigen::Vector2d(obsX, obsY));
      bool addedEdge = optimizer.addEdge(e);
      if (! addedEdge) {
        cerr << "error adding edge" << endl;
      }
    }

    // read in the camera params
    Eigen::VectorXd cameraParameter(9);
    for (int i = 0; i < numCameras; ++i) {
      for (int j = 0; j < 9; ++j)
        ifs >> cameraParameter(j);
      VertexCameraBAL* cam = cameras[i];
      cam->setEstimate(cameraParameter);
    }

    // read in the points
    Eigen::Vector3d p;
    for (int i = 0; i < numPoints; ++i) {
      ifs >> p(0) >> p(1) >> p(2);

      VertexPointBAL* point = points[i];
      point->setEstimate(p);
    }

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

  cout << "Initializing ... " << flush;
  optimizer.initializeOptimization();
  cout << "done." << endl;
  optimizer.setVerbose(verbose);
  cout << "Start to optimize" << endl;
  optimizer.optimize(maxIterations);

  if (statsFilename!=""){
    cerr << "writing stats to file \"" << statsFilename << "\" ... ";
    ofstream fout(statsFilename.c_str());
    const BatchStatisticsContainer& bsc = optimizer.batchStatistics();
    for (size_t i=0; i<bsc.size(); i++)
      fout << bsc[i] << endl;
    cerr << "done." << endl;
  }

  // dump the points
  if (outputFilename.size() > 0) {
    ofstream fout(outputFilename.c_str()); // loadable with meshlab
    fout
      << "#VRML V2.0 utf8\n"
      << "Shape {\n"
      << "  appearance Appearance {\n"
      << "    material Material {\n"
      << "      diffuseColor " << 1 << " " << 0 << " " << 0 << "\n"
      << "      ambientIntensity 0.2\n"
      << "      emissiveColor 0.0 0.0 0.0\n"
      << "      specularColor 0.0 0.0 0.0\n"
      << "      shininess 0.2\n"
      << "      transparency 0.0\n"
      << "    }\n"
      << "  }\n"
      << "  geometry PointSet {\n"
      << "    coord Coordinate {\n"
      << "      point [\n";
    for (vector<VertexPointBAL*>::const_iterator it = points.begin(); it != points.end(); ++it) {
      fout << (*it)->estimate().transpose() << endl;
    }
    fout << "    ]\n" << "  }\n" << "}\n" << "  }\n";
  }

  return 0;
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <string>
#include <unordered_map>
#include <utility>

#include "DataStructures/SliceVariables.hpp"
#include "DataStructures/Tags.hpp"
#include "DataStructures/Tensor/EagerMath/DotProduct.hpp"
#include "DataStructures/Tensor/EagerMath/Magnitude.hpp"
#include "DataStructures/Tensor/Slice.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/SizeOfElement.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Tags.hpp"
#include "Evolution/DiscontinuousGalerkin/NormalVectorTags.hpp"
#include "Evolution/Systems/NewtonianEuler/Tags.hpp"
#include "NumericalAlgorithms/LinearOperators/DefiniteIntegral.hpp"
#include "NumericalAlgorithms/LinearOperators/MeanValue.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"

/// \cond
template <size_t VolumeDim>
class ElementId;
/// \endcond

namespace NewtonianEuler {
namespace Limiters {
namespace Tci {

/// \ingroup LimitersGroup
/// \brief Implements the troubled-cell indicator from Krivodonova et al, 2004.
///
/// The KXRCF (these are the author initials) TCI is described in
/// \cite Krivodonova2004.
///
/// In summary, this TCI uses the size of discontinuities between neighboring DG
/// elements to determine the smoothness of the solution. This works because the
/// discontinuities converge rapidly for smooth solutions, therefore a large
/// discontinuity suggests a lack of smoothness and the need to apply a limiter.
///
/// The reference sets the constant we call `kxrcf_constant` to 1. This should
/// generally be a good threshold to use, though it might not be the optimal
/// value (in balancing robustness vs accuracy) for any particular problem.
///
/// This implementation
/// - does not support h- or p-refinement; this is checked by assertion.
/// - chooses not to check external boundaries, because this adds complexity.
///   However, by not checking external boundaries, the implementation may not
///   be robust for problems that feed in shocks through boundary conditions.
template <size_t VolumeDim, typename PackagedData>
bool kxrcf_indicator(
    const double kxrcf_constant, const Scalar<DataVector>& cons_mass_density,
    const tnsr::I<DataVector, VolumeDim>& cons_momentum_density,
    const Scalar<DataVector>& cons_energy_density, const Mesh<VolumeDim>& mesh,
    const Element<VolumeDim>& element,
    const std::array<double, VolumeDim>& element_size,
    const Scalar<DataVector>& det_logical_to_inertial_jacobian,
    const typename evolution::dg::Tags::NormalCovectorAndMagnitude<
        VolumeDim>::type& normals_and_magnitudes,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) noexcept {
  // Enforce restrictions on h-refinement, p-refinement
  if (UNLIKELY(alg::any_of(element.neighbors(),
                           [](const auto& direction_neighbors) noexcept {
                             return direction_neighbors.second.size() != 1;
                           }))) {
    ERROR("The Kxrcf TCI does not yet support h-refinement");
    // Removing this limitation will require adapting the surface integrals to
    // correctly acount for,
    // - multiple (smaller) neighbors contributing to the integral
    // - only a portion of a (larger) neighbor contributing to the integral
  }
  alg::for_each(neighbor_data, [&mesh](const auto& neighbor_and_data) noexcept {
    if (UNLIKELY(neighbor_and_data.second.mesh != mesh)) {
      ERROR("The Kxrcf TCI does not yet support p-refinement");
      // Removing this limitation will require generalizing the surface
      // integrals to make sure the meshes are consistent.
    }
  });
  // Check the mesh matches expectations:
  // - the extents must be uniform, because the TCI expects a unique "order" for
  //   the DG scheme. This shows up when computing the threshold parameter,
  //   h^(degree+1)/2
  // - the quadrature must be GL, because the current implementation doesn't
  //   handle extrapolation to the boundary, though this could be changed.
  // - the basis in principle could be changed, but until we need to change it
  //   we just check it's the expected Legendre for simplicity
  ASSERT(mesh == Mesh<VolumeDim>(mesh.extents(0), Spectral::Basis::Legendre,
                                 Spectral::Quadrature::GaussLobatto),
         "The Kxrcf TCI expects a uniform LGL mesh, but got mesh = " << mesh);

  bool inflow_boundaries_present = false;
  double inflow_area = 0.;
  double inflow_delta_density = 0.;
  double inflow_delta_energy = 0.;

  // Skip boundary integrations on external boundaries. This choice might be
  // problematic for evolutions (likely only simple test cases) that feed in
  // shocks through the boundary condition: the limiter might fail to activate
  // in the cell that touches the boundary.
  //
  // To properly compute the limiter at external boundaries we would need the
  // limiter to know about the boundary condition, which may be difficult to
  // do in a general way.
  for (const auto& [neighbor, data] : neighbor_data) {
    const auto& dir = neighbor.first;

    // Check consistency of neighbor_data with element and normals
    ASSERT(element.neighbors().contains(dir),
           "Received neighbor data from dir = "
               << dir << ", but element has no neighbor in this dir");
    ASSERT(normals_and_magnitudes.contains(dir),
           "Received neighbor data from dir = "
               << dir
               << ", but normals_and_magnitudes has no normal in this dir");
    ASSERT(normals_and_magnitudes.at(dir).has_value(),
           "The normals_and_magnitudes are not up-to-date in dir = " << dir);
    const auto& normal = get<evolution::dg::Tags::NormalCovector<VolumeDim>>(
        normals_and_magnitudes.at(dir).value());
    const auto& magnitude_of_normal =
        get<evolution::dg::Tags::MagnitudeOfNormal>(
            normals_and_magnitudes.at(dir).value());

    const size_t sliced_dim = dir.dimension();
    const size_t index_of_slice =
        (dir.side() == Side::Lower ? 0 : mesh.extents()[sliced_dim] - 1);
    const auto momentum_on_slice = data_on_slice(
        cons_momentum_density, mesh.extents(), sliced_dim, index_of_slice);
    const auto momentum_dot_normal = dot_product(momentum_on_slice, normal);

    // Skip boundaries with no significant inflow
    // Note: the cutoff value here is small but arbitrarily chosen.
    if (min(get(momentum_dot_normal)) > -1e-12) {
      continue;
    }
    inflow_boundaries_present = true;

    // This mask has value 1. for momentum_dot_normal < 0.
    //                     0. for momentum_dot_normal >= 0.
    const DataVector inflow_mask = 1. - step_function(get(momentum_dot_normal));
    // Mask is then weighted pointwise by the Jacobian determinant giving
    // surface integrals in inertial coordinates. This Jacobian determinant is
    // given by the product of the volume Jacobian determinant with the
    // magnitude of the unnormalized normal covectors.
    const DataVector weighted_inflow_mask =
        inflow_mask *
        get(data_on_slice(det_logical_to_inertial_jacobian, mesh.extents(),
                          sliced_dim, index_of_slice)) *
        get(magnitude_of_normal);

    inflow_area +=
        definite_integral(weighted_inflow_mask, mesh.slice_away(sliced_dim));

    // This is the step that is incompatible with h/p refinement. For use with
    // h/p refinement, would need to correctly obtain the neighbor solution on
    // the local grid points.
    const auto neighbor_vars_on_slice = data_on_slice(
        data.volume_data, mesh.extents(), sliced_dim, index_of_slice);

    const auto density_on_slice = data_on_slice(
        cons_mass_density, mesh.extents(), sliced_dim, index_of_slice);
    const auto& neighbor_density_on_slice =
        get<NewtonianEuler::Tags::MassDensityCons>(neighbor_vars_on_slice);
    inflow_delta_density += definite_integral(
        (get(density_on_slice) - get(neighbor_density_on_slice)) *
            weighted_inflow_mask,
        mesh.slice_away(sliced_dim));

    const auto energy_on_slice = data_on_slice(
        cons_energy_density, mesh.extents(), sliced_dim, index_of_slice);
    const auto& neighbor_energy_on_slice =
        get<NewtonianEuler::Tags::EnergyDensity>(neighbor_vars_on_slice);
    inflow_delta_energy += definite_integral(
        (get(energy_on_slice) - get(neighbor_energy_on_slice)) *
            weighted_inflow_mask,
        mesh.slice_away(sliced_dim));
  }

  if (not inflow_boundaries_present) {
    // No boundaries had inflow, so not a troubled cell
    return false;
  }

  // KXRCF take h to be the radius of the circumscribed circle
  const double h = 0.5 * magnitude(element_size);
  const double h_pow = pow(h, 0.5 * mesh.extents(0));

  ASSERT(inflow_area > 0.,
         "Sanity check failed: negative area of inflow boundaries");

  const double norm_squared_density = mean_value(
      get(det_logical_to_inertial_jacobian) * square(get(cons_mass_density)),
      mesh);
  ASSERT(norm_squared_density > 0.,
         "Sanity check failed: negative density norm over element");
  const double norm_density = sqrt(norm_squared_density);
  const double ratio_for_density =
      abs(inflow_delta_density) / (h_pow * inflow_area * norm_density);

  const double norm_squared_energy = mean_value(
      get(det_logical_to_inertial_jacobian) * square(get(cons_energy_density)),
      mesh);
  ASSERT(norm_squared_energy > 0.,
         "Sanity check failed: negative energy norm over element");
  const double norm_energy = sqrt(norm_squared_energy);
  const double ratio_for_energy =
      abs(inflow_delta_energy) / (h_pow * inflow_area * norm_energy);

  return (ratio_for_density > kxrcf_constant or
          ratio_for_energy > kxrcf_constant);
}

}  // namespace Tci
}  // namespace Limiters
}  // namespace NewtonianEuler