/*************************************************************************
    > File Name: RowMap.cpp
    > Author: Yibo Lin
    > Mail: yibolin@utexas.edu
    > Created Time: Wed Jun 24 21:19:32 2015
 ************************************************************************/

#include "RowMap.h"
#include "AlgoDB.h"
#include "AlgoHelpers.h"
#include "Iterators.h"
#include <algorithm>
#include <boost/algorithm/cxx11/is_sorted.hpp>

DREAMPLACE_BEGIN_NAMESPACE

/// helper object of ObsWraper for collectBlockageIntervals()
struct CollectFixedCellIntervalHelper
{
    typedef AlgoDB::index_type index_type;
    typedef AlgoDB::interval_type interval_type;
    typedef AlgoDB::box_type box_type;

    PlaceDB const& placeDB;
    SubRowMap const& subRowMap;
    std::vector<std::vector<interval_type> >& mBlkInterval;

    CollectFixedCellIntervalHelper(PlaceDB const& db, SubRowMap const& map, std::vector<std::vector<interval_type> >& mInterval)
        : placeDB(db)
        , subRowMap(map)
        , mBlkInterval(mInterval)
    {
    }
    CollectFixedCellIntervalHelper(CollectFixedCellIntervalHelper const& rhs)
        : placeDB(rhs.placeDB)
        , subRowMap(rhs.subRowMap)
        , mBlkInterval(rhs.mBlkInterval)
    {
    }
    inline void operator()(Node const& /*node*/, box_type const& box)
    {
        subRowMap.collectBlockageIntervals(placeDB, box, mBlkInterval);
    }
};

SubRowMap& SubRowMap::set(PlaceDB const& db)
{
    std::vector<Row> const& vRow = db.rows();
    index_type numRow = vRow.size();
    m_vSubRow.clear();
    m_mSubRowId.resize(numRow);
    // create sub rows
    // construct a 2D blockage map
    std::vector<std::vector<interval_type> > mBlkInterval (numRow);
    ObsWraper<CollectFixedCellIntervalHelper> cfciHelper (db, CollectFixedCellIntervalHelper(db, *this, mBlkInterval));
    for (FixedNodeConstIterator it = db.fixedNodeBegin(); it.inRange(); ++it)
    {
        Node const& node = *it;
        cfciHelper(node);
    }
    for (std::vector<box_type>::const_iterator it = db.placeBlockages().begin(); it != db.placeBlockages().end(); ++it)
        collectBlockageIntervals(db, *it, mBlkInterval);
    // clean blockage map so that there is no overlap between row slots
    for (std::vector<std::vector<Interval<coordinate_type> > >::iterator it1 = mBlkInterval.begin(), it1e = mBlkInterval.end(); it1 != it1e; ++it1)
    {
        std::vector<Interval<coordinate_type> > vMergedBlkInterval;
        // sort by low coordinates
        std::sort(it1->begin(), it1->end(), Interval<coordinate_type>::CompareByLow());
        // merge intervals
        for (index_type i = 0, ie = it1->size(); i < ie; ++i)
        {
            Interval<coordinate_type> const& icur = it1->at(i);
            if (!vMergedBlkInterval.empty() && intersects(vMergedBlkInterval.back(), icur, true))
                vMergedBlkInterval.back().encompass(icur);
            else
                vMergedBlkInterval.push_back(icur);
        }
        // apply vMergedBlkInterval to mBlkInterval
        it1->swap(vMergedBlkInterval);
    }
    // create sub rows in each row
    for (index_type i = 0; i < numRow; ++i)
    {
        Row const& row = db.rows().at(i);
        std::vector<Interval<coordinate_type> > const& vBlkInterval = mBlkInterval.at(i);
        std::vector<index_type>& vSubRowId = m_mSubRowId.at(i);
        vSubRowId.reserve(vBlkInterval.size()+1);
        for (index_type j = 0, je = vBlkInterval.size(); j <= je; ++j)
        {
            coordinate_type xl = (j == 0)? row.xl() : vBlkInterval[j-1].high();
            coordinate_type xh = (j < vBlkInterval.size())? vBlkInterval[j].low() : row.xh();

            if (xl+db.siteWidth() <= xh) // at least 1 site
            {
                // create and add sub row
                // set data that is not going to change
                m_vSubRow.push_back(SubRow());
                SubRow& srow = m_vSubRow.back();
                srow.setIndex1D(m_vSubRow.size()-1); // object id
                srow.set(xl, row.yl(), xh, row.yh());
                srow.setRowId(row.id()); // id for level-1 indexing
                srow.setSubRowId(vSubRowId.size()); // id for level-2 indexing
                vSubRowId.push_back(srow.index1D());
            }
        }
    }

    return *this;
}

void SubRowMap::collectBlockageIntervals(PlaceDB const& db, SubRowMap::box_type const& box, std::vector<std::vector<SubRowMap::interval_type> >& mBlkInterval) const
{
    std::vector<Row> const& vRow = db.rows();
    // collect intervals to blockage map
    // it is possible that the blockage may not align to sites
    // scale it up so that all the sub rows start and end to sites
    box_type adjustBox (
            db.rowXL()+floor((box.xl()-db.rowXL())/db.siteWidth())*db.siteWidth(),
            box.yl(),
            db.rowXL()+ceil((double)(box.xh()-db.rowXL())/db.siteWidth())*db.siteWidth(),
            box.yh()
            );
    Interval<index_type> idxRange (db.getRowIndexRange(adjustBox.yl(), adjustBox.yh()));
    for (index_type i = idxRange.low(); i <= idxRange.high(); ++i)
    {
        if (i < mBlkInterval.size() && intersects(vRow[i], adjustBox, false)) // only collect valid intervals
            mBlkInterval[i].push_back(adjustBox.get(kX));
    }
}

BinRowMap& BinRowMap::set(AlgoDB& algo)
{
    BinType bt = kSBin; // use sbin to initialize bin row
    SubRowMap const& subRowMap = algo.subRowMap();
    m_vBinRow.clear();
    m_mBinRowId.resize(subRowMap.numRows());
    // create bin rows according to sub row map and bin map
    // traverse through all sub rows
    for (SubRowMap1DConstIterator it = subRowMap.begin1D(), ite = subRowMap.end1D(); it != ite; ++it)
    {
        SubRow const& srow = *it;
        std::vector<index_type>& vBinRowId = m_mBinRowId.at(srow.rowId());
        Box<index_type> idxBox (algo.getBinIndexRange(bt, srow.xl(), srow.yl(), srow.xh(), srow.yh()));
        for (index_type iy = idxBox.yl(); iy <= idxBox.yh(); ++iy)
        {
            vBinRowId.reserve(idxBox.width()+1);
            for (index_type ix = idxBox.xl(); ix <= idxBox.xh(); ++ix)
            {
                Bin& bin = algo.getBinByIndex(bt, ix, iy);
                std::pair<Box<coordinate_type>, bool> intersectBox = intersection(srow, bin, false);
                if (intersectBox.second) // at least have intersection
                {
                    // create bin row
                    m_vBinRow.push_back(BinRow());
                    BinRow& brow = m_vBinRow.back();
                    brow.set(kX, intersectBox.first.get(kX)).set(kY, srow.get(kY));
                    brow.setIndex1D(m_vBinRow.size()-1); // object id
                    brow.setBinId(bin.index1D());
                    brow.setSubRowId(srow.index1D()); // id to find parent sub row
                    vBinRowId.push_back(brow.index1D());
                    // add to bin
                    // first step
                    bin.binRows().setPartial1(brow.index1D());
                }
            }
        }
    }
    // iterate through all bins to construct m_vBinRowId in bins
    for (BinMap1DIterator it = algo.binMap(bt).begin1D(), ite = algo.binMap(bt).end1D(); it != ite; ++it)
    {
        Bin& bin = *it;
        std::vector<index_type> vRowIdOfBinRows (bin.binRows().size()); // row id of bin rows, because row id is continuous vertically
        index_type count = 0;
        for (HrchyList<index_type>::const_iterator_type itBR = bin.binRows().begin(), itBRe = bin.binRows().end(); itBR != itBRe; ++itBR)
            vRowIdOfBinRows[count++] = algo.getSubRow(getBinRow(*itBR).subRowId()).rowId(); // assume sub row is initialized

        Interval<index_type> rowIdxInv = algo.placeDB().getRowIndexRange(bin.yl(), bin.yh()-1); // min, max row id
        for (std::vector<index_type>::iterator itSRID = vRowIdOfBinRows.begin(), itSRIDe = vRowIdOfBinRows.end(); itSRID != itSRIDe; ++itSRID)
        {
           *itSRID -= rowIdxInv.low(); // normalize to 0
        }
        std::vector<std::vector<index_type> > mData;
        // second step
        bin.binRows().setPartial2(vRowIdOfBinRows, mData, rowIdxInv.delta()+1);
        // the target is to sort bin rows from left to right within each row
        // considering that bin row id is ordered from bottom to top, left to right, by construction
        // it should be enough to sort by the indices
        for (std::vector<std::vector<index_type> >::iterator itd = mData.begin(), itde = mData.end(); itd != itde; ++itd)
            std::sort(itd->begin(), itd->end());
        // third step
        bin.binRows().setPartial3(mData);
    }
#ifdef DEBUG
    // maybe no longer reasonable check
    for (BinMap1DConstIterator it = algo.binMap(bt).begin1D(), ite = algo.binMap(bt).end1D(); it != ite; ++it)
    {
        Bin const& bin = *it;
        // check whether bin rows in a sbin are sorted
        dreamplaceAssert(boost::algorithm::is_sorted(bin.binRows().begin(), bin.binRows().end()));
    }
#endif

    return *this;
}

DREAMPLACE_END_NAMESPACE


#include "farm_ng/calibration/multi_view_apriltag_rig_calibrator.h"

#include <map>

#include <ceres/ceres.h>
#include <google/protobuf/util/time_util.h>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_traits.hpp>
#include <opencv2/highgui.hpp>  // TODO remove.
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <sophus/average.hpp>

#include "farm_ng/calibration/apriltag_rig_calibrator.h"
#include "farm_ng/calibration/calibrator.pb.h"
#include "farm_ng/calibration/camera_rig_apriltag_rig_cost_functor.h"
#include "farm_ng/calibration/local_parameterization.h"
#include "farm_ng/core/blobstore.h"
#include "farm_ng/core/event_log_reader.h"
#include "farm_ng/core/ipc.h"
#include "farm_ng/perception/apriltag.h"
#include "farm_ng/perception/apriltag.pb.h"
#include "farm_ng/perception/camera_model.h"
#include "farm_ng/perception/capture_video_dataset.pb.h"
#include "farm_ng/perception/image_loader.h"
#include "farm_ng/perception/image_utils.h"
#include "farm_ng/perception/pose_graph.h"
#include "farm_ng/perception/pose_utils.h"
#include "farm_ng/perception/sophus_protobuf.h"
#include "farm_ng/perception/time_series.h"

namespace farm_ng {
namespace calibration {
typedef farm_ng::core::Event EventPb;
using farm_ng::core::EventLogReader;
using farm_ng::core::GetUniqueArchiveResource;
using farm_ng::core::ReadProtobufFromResource;
using farm_ng::core::Resource;
using farm_ng::perception::ApriltagDetections;
using farm_ng::perception::ApriltagRig;
using farm_ng::perception::ApriltagsFilter;
using farm_ng::perception::CameraModel;
using farm_ng::perception::PoseEdge;

using farm_ng::perception::ConstructGridImage;
using farm_ng::perception::FrameNameNumber;
using farm_ng::perception::FrameRigTag;
using farm_ng::perception::Image;
using farm_ng::perception::ImageLoader;
using farm_ng::perception::NamedSE3Pose;
using farm_ng::perception::PoseGraph;
using farm_ng::perception::SE3Map;
using farm_ng::perception::SophusToProto;
using farm_ng::perception::TimeSeries;
using Sophus::SE3d;

using farm_ng::core::Event;
using farm_ng::perception::CaptureVideoDatasetResult;
using farm_ng::perception::MultiViewApriltagDetections;
using Sophus::SE3d;

void GetCameraRigPosesTagRig(const MultiViewApriltagRigModel& model,
                             PoseGraph* pose_graph) {
  std::string tag_rig_view_frame = model.apriltag_rig().name() + "/view/";
  std::map<int, SE3d> camera_rig_poses_tag_rig;
  for (const NamedSE3Pose& camera_rig_pose_tag_rig :
       model.camera_rig_poses_apriltag_rig()) {
    CHECK(camera_rig_pose_tag_rig.frame_b().rfind(tag_rig_view_frame) == 0)
        << camera_rig_pose_tag_rig.frame_b()
        << " does not start with: " << tag_rig_view_frame;
    CHECK_EQ(camera_rig_pose_tag_rig.frame_a(), model.camera_rig().name());
    int frame_n = std::stoi(
        camera_rig_pose_tag_rig.frame_b().substr(tag_rig_view_frame.size()));
    CHECK_GE(frame_n, 0);
    CHECK_LT(frame_n, model.multi_view_detections_size());
    pose_graph->AddPose(camera_rig_pose_tag_rig);
  }
}

PoseGraph PoseGraphFromModel(const MultiViewApriltagRigModel& model) {
  PoseGraph pose_graph;
  for (const ApriltagRig::Node& node : model.apriltag_rig().nodes()) {
    pose_graph.AddPose(node.pose());
  }
  pose_graph.AddPoses(model.camera_rig().camera_pose_rig());
  GetCameraRigPosesTagRig(model, &pose_graph);
  return pose_graph;
}
void UpdateModelFromPoseGraph(const PoseGraph& pose_graph,
                              MultiViewApriltagRigModel* model) {
  for (ApriltagRig::Node& node :
       *model->mutable_apriltag_rig()->mutable_nodes()) {
    pose_graph.UpdateNamedSE3Pose(node.mutable_pose());
  }
  pose_graph.UpdateNamedSE3Poses(
      model->mutable_camera_rig()->mutable_camera_pose_rig());
  pose_graph.UpdateNamedSE3Poses(
      model->mutable_camera_rig_poses_apriltag_rig());
}

void ModelError(MultiViewApriltagRigModel* model,
                bool output_reprojection_images) {
  model->set_rmse(0.0);
  model->clear_reprojection_images();
  model->clear_tag_stats();
  PoseGraph pose_graph = PoseGraphFromModel(*model);
  std::string root_tag_frame = FrameRigTag(model->apriltag_rig().name(),
                                           model->apriltag_rig().root_tag_id());
  std::string tag_rig_frame = model->apriltag_rig().name();
  std::string root_camera_frame = model->camera_rig().root_camera_name();
  std::string camera_rig_frame = model->camera_rig().name();
  int frame_num = -1;
  double total_rmse = 0.0;
  double total_count = 0.0;

  double total_depth_error = 0.0;
  double total_depth_count = 0.0;

  std::map<int, ApriltagRigTagStats> tag_stats;

  ImageLoader image_loader;

  for (const auto& mv_detections : model->multi_view_detections()) {
    frame_num++;
    std::string tag_rig_view_frame =
        tag_rig_frame + "/view/" + std::to_string(frame_num);

    if (!pose_graph.HasEdge(camera_rig_frame, tag_rig_view_frame)) {
      continue;
    }
    PoseEdge* camera_rig_to_tag_rig_view =
        pose_graph.MutablePoseEdge(camera_rig_frame, tag_rig_view_frame);
    std::vector<cv::Mat> images;

    for (const auto& detections_per_view :
         mv_detections.detections_per_view()) {
      const auto& camera_model = detections_per_view.image().camera_model();
      std::string camera_frame = camera_model.frame_name();

      PoseEdge* camera_to_camera_rig =
          pose_graph.MutablePoseEdge(camera_frame, camera_rig_frame);

      if (output_reprojection_images) {
        cv::Mat image = image_loader.LoadImage(detections_per_view.image());
        if (image.channels() == 1) {
          cv::Mat color;
          cv::cvtColor(image, color, cv::COLOR_GRAY2BGR);
          image = color;
        }

        for (const auto& node : model->apriltag_rig().nodes()) {
          PoseEdge* tag_to_tag_rig =
              pose_graph.MutablePoseEdge(node.frame_name(), tag_rig_frame);

          auto camera_pose_tag =
              camera_to_camera_rig->GetAPoseBMapped(camera_frame,
                                                    camera_rig_frame) *
              camera_rig_to_tag_rig_view->GetAPoseBMapped(camera_rig_frame,
                                                          tag_rig_view_frame) *
              tag_to_tag_rig->GetAPoseBMapped(tag_rig_frame, node.frame_name());
          for (int i = 0; i < 4; ++i) {
            Eigen::Vector3d point_tag(node.points_tag().Get(i).x(),
                                      node.points_tag().Get(i).y(),
                                      node.points_tag().Get(i).z());
            Eigen::Vector3d point_camera = camera_pose_tag * point_tag;
            if (point_camera.z() > 0.001) {
              Eigen::Vector2d rp =
                  ProjectPointToPixel(camera_model, point_camera);
              cv::circle(image, cv::Point(rp.x(), rp.y()), 3,
                         cv::Scalar(0, 0, 255), -1);
            }
          }
          for (const auto& detection : detections_per_view.detections()) {
            auto points_image = PointsImage(detection);
            for (size_t i = 0; i < points_image.size(); ++i) {
              cv::circle(image,
                         cv::Point(points_image[i].x(), points_image[i].y()), 5,
                         cv::Scalar(255, 0, 0));
            }
          }
        }
        images.push_back(image);
      }

      for (const auto& detection : detections_per_view.detections()) {
        std::string tag_frame = FrameRigTag(tag_rig_frame, detection.id());
        PoseEdge* tag_to_tag_rig =
            pose_graph.MutablePoseEdge(tag_frame, tag_rig_frame);
        auto points_image = PointsImage(detection);
        CameraRigApriltagRigCostFunctor cost(
            detections_per_view.image().camera_model(), detection,
            camera_to_camera_rig->GetAPoseBMap(camera_frame, camera_rig_frame),
            tag_to_tag_rig->GetAPoseBMap(tag_rig_frame, tag_frame),
            camera_rig_to_tag_rig_view->GetAPoseBMap(camera_rig_frame,
                                                     tag_rig_view_frame),
            1.0);
        Eigen::Matrix<double, 4, 3> residuals;
        CHECK(cost(camera_to_camera_rig->GetAPoseB().data(),
                   tag_to_tag_rig->GetAPoseB().data(),
                   camera_rig_to_tag_rig_view->GetAPoseB().data(),
                   residuals.data()));
        double depth_error = 0;
        int depth_count = 0;
        for (int i = 0; i < 4; ++i) {
          double d2 = residuals(i, 2) * residuals(i, 2);
          if (d2 > 0.0) {
            depth_error += residuals(i, 2);
            depth_count += 1;
          }
        }
        total_depth_count += depth_count;
        total_depth_error += depth_error;
        total_rmse += residuals.block<4, 2>(0, 0).squaredNorm();
        total_count += 8;

        ApriltagRigTagStats& stats = tag_stats[detection.id()];
        stats.set_tag_id(detection.id());
        stats.set_n_frames(stats.n_frames() + 1);
        stats.set_tag_rig_rmse(stats.tag_rig_rmse() +
                               residuals.block<4, 2>(0, 0).squaredNorm() / 8);
        stats.set_tag_rig_depth_error(stats.tag_rig_depth_error() +
                                      depth_error);
        stats.set_tag_rig_depth_count(stats.tag_rig_depth_count() +
                                      depth_count);

        PerImageRmse* image_rmse = stats.add_per_image_rmse();
        image_rmse->set_rmse(
            std::sqrt(residuals.block<4, 2>(0, 0).squaredNorm() / 8));
        image_rmse->set_frame_number(frame_num);
        image_rmse->set_camera_name(
            detections_per_view.image().camera_model().frame_name());
        if (depth_count > 0) {
          image_rmse->set_depth_error(  // std::sqrt
              (depth_error / depth_count));
        }
      }
    }
    if (output_reprojection_images && !images.empty()) {
      Image& reprojection_image = *model->add_reprojection_images();
      int n_cols = 3;
      int n_rows = std::ceil(float(images.size()) / n_cols);
      if (n_cols > int(images.size())) {
        n_cols = images.size();
        n_rows = 1;
      }

      int image_width = images[0].size().width * n_cols;
      int image_height = images[0].size().height * n_rows;
      reprojection_image.mutable_camera_model()->set_image_width(image_width);
      reprojection_image.mutable_camera_model()->set_image_height(image_height);
      auto resource_path = GetUniqueArchiveResource(
          FrameNameNumber(
              "reprojection-" + SolverStatus_Name(model->solver_status()),
              frame_num),
          "jpg", "image/jpeg");
      reprojection_image.mutable_resource()->CopyFrom(resource_path.first);
      LOG(INFO) << resource_path.second.string();
      CHECK(
          cv::imwrite(resource_path.second.string(),
                      ConstructGridImage(
                          images, cv::Size(image_width, image_height), n_cols)))
          << "Could not write: " << resource_path.second;
    }
  }
  std::stringstream summary;
  summary << "# tag_id frame_number camera_name rmse depth_error\n";
  for (auto& stats : tag_stats) {
    for (auto& per_image : stats.second.per_image_rmse()) {
      summary << stats.second.tag_id() << " " << per_image.frame_number() << " "
              << per_image.camera_name() << " " << per_image.rmse() << " "
              << per_image.depth_error() << "\n";
    }
  }
  for (auto& stats : tag_stats) {
    stats.second.set_tag_rig_rmse(
        std::sqrt(stats.second.tag_rig_rmse() / stats.second.n_frames()));
    if (stats.second.tag_rig_depth_count() > 0) {
      stats.second.set_tag_rig_depth_error(
          // std::sqrt
          (stats.second.tag_rig_depth_error() /
           stats.second.tag_rig_depth_count()));
    }

    auto debug_stats = stats.second;
    debug_stats.clear_per_image_rmse();
    summary << debug_stats.DebugString() << "\n\n";
    model->add_tag_stats()->CopyFrom(stats.second);
  }
  model->set_rmse(std::sqrt(total_rmse / total_count));
  summary << "model rmse (pixels): " << model->rmse() << "\n";
  if (total_depth_count > 0) {
    model->set_depth_error(  // std::sqrt
        (total_depth_error / total_depth_count));
    summary << "model depth error (meters): " << model->depth_error() << "\n";
  }
  LOG(INFO) << "Error Summary:\n" << summary.str();
}

std::vector<MultiViewApriltagDetections> LoadMultiViewApriltagDetections(
    const std::string& root_camera_name, const Resource& event_log,
    const CalibrateMultiViewApriltagRigConfiguration& config) {
  EventLogReader log_reader(event_log);

  std::set<std::string> allowed_cameras;

  for (auto camera_name : config.include_cameras()) {
    allowed_cameras.insert(camera_name);
    LOG(INFO) << "allowed camera: " << camera_name;
  }
  std::unordered_set<int> allowed_ids;

  for (auto id : config.tag_ids()) {
    allowed_ids.insert(id);
    LOG(INFO) << "allowed tag id: " << id;
  }

  CHECK(allowed_ids.count(config.root_tag_id()) == 1)
      << "Please ensure root_tag_id is in the tag_ids list";

  std::map<std::string, TimeSeries<Event>> apriltag_series;

  while (true) {
    EventPb event;
    try {
      event = log_reader.ReadNext();
    } catch (std::runtime_error& e) {
      break;
    }
    ApriltagDetections unfiltered_detections;
    if (event.data().UnpackTo(&unfiltered_detections)) {
      auto camera_name =
          unfiltered_detections.image().camera_model().frame_name();
      CHECK(!camera_name.empty()) << " camera_name is not set.";
      if (!allowed_cameras.empty() && !allowed_cameras.count(camera_name)) {
        LOG(INFO) << "skipping data from camera: " << camera_name;
        continue;
      }
      ApriltagDetections detections = unfiltered_detections;
      detections.clear_detections();
      for (const auto& detection : unfiltered_detections.detections()) {
        if (allowed_ids.count(detection.id())) {
          detections.add_detections()->CopyFrom(detection);
        }
      }
      event.mutable_data()->PackFrom(detections);

      event.set_name(detections.image().camera_model().frame_name() +
                     "/apriltags");
      apriltag_series[event.name()].insert(event);
    }
  }
  {
    std::stringstream ss;
    ss << "Raw detections\n";
    for (const auto& series : apriltag_series) {
      ss << series.first << " " << series.second.size() << " detections\n";
    }
    LOG(INFO) << ss.str();
  }

  ApriltagsFilter tag_filter;
  std::vector<MultiViewApriltagDetections> mv_detections_series;
  auto time_window =
      google::protobuf::util::TimeUtil::MillisecondsToDuration(1000.0 / 2);

  std::map<std::string, int> detection_counts;
  int steady_count = 5;
  if (config.has_steady_count()) {
    steady_count = config.steady_count().value();
  }
  LOG(INFO) << "Steady count is: " << steady_count;
  for (const Event& event : apriltag_series[root_camera_name + "/apriltags"]) {
    ApriltagDetections detections;
    CHECK(event.data().UnpackTo(&detections));
    if (!config.filter_stable_tags() ||
        tag_filter.AddApriltags(detections, steady_count, 7)) {
      MultiViewApriltagDetections mv_detections;
      for (auto name_series : apriltag_series) {
        auto nearest_event =
            name_series.second.FindNearest(event.stamp(), time_window);
        if (nearest_event) {
          CHECK(nearest_event->data().UnpackTo(
              mv_detections.add_detections_per_view()));
          detection_counts[nearest_event->name()]++;
        }
      }
      mv_detections_series.push_back(mv_detections);
    }
  }
  {
    std::stringstream ss;
    ss << "Stable multi-view detections: " << mv_detections_series.size()
       << "\n";
    for (const auto& name_count : detection_counts) {
      ss << name_count.first << " " << name_count.second
         << " stable detections\n";
    }
    LOG(INFO) << ss.str();
  }

  return mv_detections_series;
}

PoseGraph TagRigFromMultiViewDetections(std::string tag_rig_name,
                                        int root_tag_id,
                                        MultiViewApriltagRigModel* model) {
  model->mutable_apriltag_rig()->set_name(tag_rig_name);
  model->mutable_apriltag_rig()->set_root_tag_id(root_tag_id);

  PoseGraph tag_rig;
  std::map<int, ApriltagRig::Node> tag_rig_nodes;

  for (const auto& mv_detections : model->multi_view_detections()) {
    for (const auto& detections_per_view :
         mv_detections.detections_per_view()) {
      if (detections_per_view.detections_size() <= 1) {
        continue;
      }
      for (const auto& detection : detections_per_view.detections()) {
        if (tag_rig_nodes.count(detection.id())) {
          continue;
        }
        ApriltagRig::Node node;
        node.set_id(detection.id());
        node.set_frame_name(FrameRigTag(tag_rig_name, detection.id()));
        node.set_tag_size(detection.tag_size());
        for (const auto& v : PointsTag(detection)) {
          EigenToProto(v, node.add_points_tag());
        }
        tag_rig_nodes.emplace(detection.id(), std::move(node));
      }
      for (int i = 0; i < detections_per_view.detections_size() - 1; ++i) {
        for (int j = i + 1; j < detections_per_view.detections_size(); ++j) {
          const auto& detection_i = detections_per_view.detections().Get(i);
          const auto& detection_j = detections_per_view.detections().Get(j);
          Sophus::SE3d c_pose_i, c_pose_j;
          CHECK_EQ(detection_i.pose().frame_a(), detection_j.pose().frame_a());
          ProtoToSophus(detection_i.pose().a_pose_b(), &c_pose_i);
          ProtoToSophus(detection_j.pose().a_pose_b(), &c_pose_j);
          tag_rig.AddPose(detection_i.pose().frame_b(),
                          detection_j.pose().frame_b(),
                          c_pose_i.inverse() * c_pose_j);
        }
      }
    }
  }
  std::string root_tag = "tag/" + std::to_string(root_tag_id);

  auto tag_rig_small = tag_rig.AveragePoseGraph(root_tag);

  for (auto& node : tag_rig_nodes) {
    std::string tag = "tag/" + std::to_string(node.first);
    auto root_pose_tag = tag_rig_small.AverageAPoseB(root_tag, tag);
    if (!root_pose_tag) {
      continue;
    }
    SophusToProto(*root_pose_tag, tag_rig_name, node.second.frame_name(),
                  node.second.mutable_pose());
    model->mutable_apriltag_rig()->add_nodes()->CopyFrom(node.second);
  }
  return tag_rig_small;
}

void CameraRigFromMultiViewDetections(std::string camera_rig_name,
                                      std::string root_camera_name,
                                      const PoseGraph& tag_rig,
                                      MultiViewApriltagRigModel* model) {
  model->mutable_camera_rig()->set_root_camera_name(root_camera_name);
  model->mutable_camera_rig()->set_name(camera_rig_name);

  std::string root_tag_name =
      "tag/" + std::to_string(model->apriltag_rig().root_tag_id());
  PoseGraph camera_rig_inter;
  PoseGraph camera_rig_tags;

  int frame_num = -1;

  std::map<std::string, CameraModel> camera_models;
  for (const auto& mv_detections : model->multi_view_detections()) {
    frame_num++;
    PoseGraph camera_rig_step_i;

    camera_rig_step_i.AddPose(camera_rig_name, root_camera_name, SE3d::rotX(0));

    for (const auto& detections_per_view :
         mv_detections.detections_per_view()) {
      if (detections_per_view.detections_size() < 1) {
        continue;
      }
      camera_models.emplace(
          detections_per_view.image().camera_model().frame_name(),
          detections_per_view.image().camera_model());

      for (const auto& detection : detections_per_view.detections()) {
        Sophus::SE3d c_pose_tag;
        ProtoToSophus(detection.pose().a_pose_b(), &c_pose_tag);
        auto o_tag_pose_root_tag =
            tag_rig.AverageAPoseB(detection.pose().frame_b(), root_tag_name);
        if (!o_tag_pose_root_tag) {
          LOG(WARNING) << "Unable to compute pose for: "
                       << detection.pose().frame_b() << " <- " << root_tag_name;
          continue;
        }
        auto c_pose_root_tag = c_pose_tag * (*o_tag_pose_root_tag);
        camera_rig_step_i.AddPose(detection.pose().frame_a(), root_tag_name,
                                  c_pose_root_tag);
      }
    }
    if (!camera_rig_step_i.HasName(root_camera_name)) {
      continue;
    }
    auto camera_rig_i = camera_rig_step_i.AveragePoseGraph(camera_rig_name);

    for (auto es = camera_rig_i.Edges(); es.first != es.second; es.first++) {
      auto edge = camera_rig_i.PoseEdgeMap()[*es.first];
      std::string frame_a = edge.frame_a;
      std::string frame_b = edge.frame_b;
      if (frame_a == root_tag_name || frame_b == root_tag_name) {
        std::string rig_frame_name =
            model->apriltag_rig().name() + "/view/" + std::to_string(frame_num);
        if (frame_a == root_tag_name) {
          frame_a = rig_frame_name;
        }
        if (frame_b == root_tag_name) {
          frame_b = rig_frame_name;
        }
        camera_rig_tags.AddPose(frame_a, frame_b, edge.GetAPoseB());
      } else {
        camera_rig_inter.AddPose(frame_a, frame_b, edge.GetAPoseB());
      }
    }
  }
  for (auto camera : camera_models) {
    model->mutable_camera_rig()->add_cameras()->CopyFrom(camera.second);
  }

  model->mutable_camera_rig()->mutable_camera_pose_rig()->CopyFrom(
      camera_rig_inter.AveragePoseGraph(camera_rig_name).ToNamedSE3Poses());

  model->mutable_camera_rig_poses_apriltag_rig()->CopyFrom(
      camera_rig_tags.AveragePoseGraph(camera_rig_name).ToNamedSE3Poses());
}

MultiViewApriltagRigModel InitialMultiViewApriltagModelFromConfig(
    const CalibrateMultiViewApriltagRigConfiguration& config) {
  core::Resource event_log;
  switch (config.input_case()) {
    case CalibrateMultiViewApriltagRigConfiguration::InputCase::kVideoDataset: {
      auto dataset_result = ReadProtobufFromResource<CaptureVideoDatasetResult>(
          config.video_dataset());
      event_log = dataset_result.dataset();
      break;
    }
    case CalibrateMultiViewApriltagRigConfiguration::InputCase::kEventLog: {
      event_log = config.event_log();
      break;
    }
    default:
      LOG(FATAL) << "No input provided in config.";
  }
  MultiViewApriltagRigModel model;
  for (const auto& mv_detections : LoadMultiViewApriltagDetections(
           config.root_camera_name(), event_log, config)) {
    model.add_multi_view_detections()->CopyFrom(mv_detections);
  }
  auto tag_rig = TagRigFromMultiViewDetections(config.tag_rig_name(),
                                               config.root_tag_id(), &model);
  CameraRigFromMultiViewDetections(config.name(), config.root_camera_name(),
                                   tag_rig, &model);

  model.set_solver_status(SolverStatus::SOLVER_STATUS_INITIAL);
  return model;
}

MultiViewApriltagRigModel SolveMultiViewApriltagModel(
    MultiViewApriltagRigModel model) {
  PoseGraph pose_graph = PoseGraphFromModel(model);
  double depth_weight = 100.0;
  ceres::Problem problem;
  for (PoseEdge* pose_edge : pose_graph.MutablePoseEdges()) {
    LOG(INFO) << *pose_edge;
    problem.AddParameterBlock(pose_edge->GetAPoseB().data(),
                              SE3d::num_parameters,
                              new LocalParameterizationSE3);
  }
  std::string root_tag_frame = FrameRigTag(model.apriltag_rig().name(),
                                           model.apriltag_rig().root_tag_id());
  std::string tag_rig_frame = model.apriltag_rig().name();
  problem.SetParameterBlockConstant(
      pose_graph.MutablePoseEdge(root_tag_frame, tag_rig_frame)
          ->GetAPoseB()
          .data());

  std::string root_camera_frame = model.camera_rig().root_camera_name();
  std::string camera_rig_frame = model.camera_rig().name();

  problem.SetParameterBlockConstant(
      pose_graph.MutablePoseEdge(root_camera_frame, camera_rig_frame)
          ->GetAPoseB()
          .data());

  int frame_num = -1;
  for (const auto& mv_detections : model.multi_view_detections()) {
    frame_num++;

    std::string tag_rig_view_frame =
        tag_rig_frame + "/view/" + std::to_string(frame_num);

    if (!pose_graph.HasEdge(camera_rig_frame, tag_rig_view_frame)) {
      continue;
    }

    PoseEdge* camera_rig_to_tag_rig_view =
        pose_graph.MutablePoseEdge(camera_rig_frame, tag_rig_view_frame);

    for (const auto& detections_per_view :
         mv_detections.detections_per_view()) {
      if (detections_per_view.detections_size() < 1) {
        continue;
      }
      std::string camera_frame =
          detections_per_view.image().camera_model().frame_name();

      PoseEdge* camera_to_camera_rig =
          pose_graph.MutablePoseEdge(camera_frame, camera_rig_frame);

      for (const auto& detection : detections_per_view.detections()) {
        std::string tag_frame = FrameRigTag(tag_rig_frame, detection.id());
        PoseEdge* tag_to_tag_rig =
            pose_graph.MutablePoseEdge(tag_frame, tag_rig_frame);

        ceres::CostFunction* cost_function1 = new ceres::AutoDiffCostFunction<
            CameraRigApriltagRigCostFunctor, 12, Sophus::SE3d::num_parameters,
            Sophus::SE3d::num_parameters, Sophus::SE3d::num_parameters>(
            new CameraRigApriltagRigCostFunctor(
                detections_per_view.image().camera_model(), detection,
                camera_to_camera_rig->GetAPoseBMap(camera_frame,
                                                   camera_rig_frame),
                tag_to_tag_rig->GetAPoseBMap(tag_rig_frame, tag_frame),
                camera_rig_to_tag_rig_view->GetAPoseBMap(camera_rig_frame,
                                                         tag_rig_view_frame),
                depth_weight));
        problem.AddResidualBlock(
            cost_function1, new ceres::CauchyLoss(1.0),
            camera_to_camera_rig->GetAPoseB().data(),
            tag_to_tag_rig->GetAPoseB().data(),
            camera_rig_to_tag_rig_view->GetAPoseB().data());
      }
    }
  }

  // Set solver options (precision / method)
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::SPARSE_SCHUR;
  options.gradient_tolerance = 1e-18;
  options.function_tolerance = 1e-18;
  options.parameter_tolerance = 1e-18;
  options.max_num_iterations = 2000;

  // Solve
  ceres::Solver::Summary summary;
  options.logging_type = ceres::PER_MINIMIZER_ITERATION;
  options.minimizer_progress_to_stdout = true;
  ceres::Solve(options, &problem, &summary);
  LOG(INFO) << summary.FullReport() << std::endl;
  if (summary.termination_type == ceres::CONVERGENCE) {
    model.set_solver_status(SolverStatus::SOLVER_STATUS_CONVERGED);
  } else {
    model.set_solver_status(SolverStatus::SOLVER_STATUS_FAILED);
  }
  UpdateModelFromPoseGraph(pose_graph, &model);
  return model;
}

std::optional<NamedSE3Pose> CameraRigPoseApriltagRigEstimateAverage(
    const perception::MultiViewCameraRig& camera_rig,
    const perception::ApriltagRig& apriltag_rig,
    const perception::MultiViewApriltagDetections& mv_detections,
    PoseGraph* pose_graph) {
  perception::ApriltagRigIdMap tag_id_map;
  tag_id_map.AddRig(apriltag_rig);

  *pose_graph = PoseGraph();

  for (const ApriltagRig::Node& node : apriltag_rig.nodes()) {
    pose_graph->AddPose(node.pose());
  }
  pose_graph->AddPoses(camera_rig.camera_pose_rig());
  for (const auto& detections_per_view : mv_detections.detections_per_view()) {
    for (const auto& detection : detections_per_view.detections()) {
      auto frame_names = tag_id_map.GetFrameNames(detection.id());
      if (!frame_names) {
        continue;
      }
      CHECK_EQ(detections_per_view.image().camera_model().frame_name(),
               detection.pose().frame_a());

      Sophus::SE3d c_pose_tag;
      ProtoToSophus(detection.pose().a_pose_b(), &c_pose_tag);

      auto tag_pose_tag_rig = pose_graph->CheckAverageAPoseB(
          frame_names->tag_frame, frame_names->rig_frame);

      auto camera_rig_pose_camera = pose_graph->CheckAverageAPoseB(
          camera_rig.name(),
          detections_per_view.image().camera_model().frame_name());

      auto camera_rig_pose_tag_rig =
          camera_rig_pose_camera * c_pose_tag * tag_pose_tag_rig;
      pose_graph->AddPose(camera_rig.name(), apriltag_rig.name(),
                          camera_rig_pose_tag_rig);
    }
  }
  if (pose_graph->HasEdge(camera_rig.name(), apriltag_rig.name())) {
    auto edge =
        pose_graph->MutablePoseEdge(camera_rig.name(), apriltag_rig.name());

    LOG(INFO) << camera_rig.name() << " <- " << apriltag_rig.name()
              << " has: " << edge->a_poses_b.size() << " estimates.";
    edge->Collapse();
  }
  return pose_graph->AverageNamedSE3Pose(camera_rig.name(),
                                         apriltag_rig.name());
}

std::optional<std::tuple<perception::NamedSE3Pose, double,
                         std::vector<ApriltagRigTagStats>>>
EstimateMultiViewCameraRigPoseApriltagRig(
    const perception::MultiViewCameraRig& camera_rig,
    const perception::ApriltagRig& apriltag_rig,
    const perception::MultiViewApriltagDetections& mv_detections) {
  PoseGraph pose_graph;
  auto o_camera_rig_pose_tag_rig = CameraRigPoseApriltagRigEstimateAverage(
      camera_rig, apriltag_rig, mv_detections, &pose_graph);
  if (!o_camera_rig_pose_tag_rig) {
    return std::nullopt;
  }
  double depth_weight = 100.0;
  ceres::Problem problem;
  for (PoseEdge* pose_edge : pose_graph.MutablePoseEdges()) {
    VLOG(2) << *pose_edge;
    problem.AddParameterBlock(pose_edge->GetAPoseB().data(),
                              SE3d::num_parameters,
                              new LocalParameterizationSE3);
    if (!pose_edge->HasFrames(camera_rig.name(), apriltag_rig.name())) {
      problem.SetParameterBlockConstant(pose_edge->GetAPoseB().data());
    }
  }

  PoseEdge* camera_rig_to_tag_rig =
      pose_graph.MutablePoseEdge(camera_rig.name(), apriltag_rig.name());

  std::map<int, ApriltagRigTagStats> tag_stats;
  std::map<int, std::vector<std::tuple<PerImageRmse, ceres::ResidualBlockId>>>
      tag_id_to_per_image_rmse_block_id;

  for (const auto& detections_per_view : mv_detections.detections_per_view()) {
    if (detections_per_view.detections_size() < 1) {
      continue;
    }
    std::string camera_frame =
        detections_per_view.image().camera_model().frame_name();

    PoseEdge* camera_to_camera_rig =
        pose_graph.MutablePoseEdge(camera_frame, camera_rig.name());

    for (const auto& detection : detections_per_view.detections()) {
      std::string tag_frame = FrameRigTag(apriltag_rig.name(), detection.id());
      if (!pose_graph.HasEdge(tag_frame, apriltag_rig.name())) {
        continue;
      }
      ApriltagRigTagStats& stats = tag_stats[detection.id()];
      stats.set_tag_id(detection.id());
      stats.set_n_frames(stats.n_frames() + 1);

      PoseEdge* tag_to_tag_rig =
          pose_graph.MutablePoseEdge(tag_frame, apriltag_rig.name());

      ceres::CostFunction* cost_function1 = new ceres::AutoDiffCostFunction<
          CameraRigApriltagRigCostFunctor, 12, Sophus::SE3d::num_parameters,
          Sophus::SE3d::num_parameters, Sophus::SE3d::num_parameters>(
          new CameraRigApriltagRigCostFunctor(
              detections_per_view.image().camera_model(), detection,
              camera_to_camera_rig->GetAPoseBMap(camera_frame,
                                                 camera_rig.name()),
              tag_to_tag_rig->GetAPoseBMap(apriltag_rig.name(), tag_frame),
              camera_rig_to_tag_rig->GetAPoseBMap(camera_rig.name(),
                                                  apriltag_rig.name()),
              depth_weight));
      auto block_id =
          problem.AddResidualBlock(cost_function1, new ceres::CauchyLoss(1.0),
                                   camera_to_camera_rig->GetAPoseB().data(),
                                   tag_to_tag_rig->GetAPoseB().data(),
                                   camera_rig_to_tag_rig->GetAPoseB().data());
      PerImageRmse per_image_rmse;
      per_image_rmse.set_frame_number(0);
      per_image_rmse.set_camera_name(camera_frame);

      tag_id_to_per_image_rmse_block_id[detection.id()].push_back(
          std::make_tuple(per_image_rmse, block_id));
    }
  }

  // Set solver options (precision / method)
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::SPARSE_SCHUR;
  options.gradient_tolerance = 1e-18;
  options.function_tolerance = 1e-18;
  options.parameter_tolerance = 1e-18;
  options.max_num_iterations = 2000;

  // Solve
  ceres::Solver::Summary summary;
  // options.logging_type = ceres::PER_MINIMIZER_ITERATION;
  // options.minimizer_progress_to_stdout = true;
  ceres::Solve(options, &problem, &summary);
  VLOG(2) << summary.FullReport() << std::endl;
  if (!summary.IsSolutionUsable()) {
    return std::nullopt;
  }
  double total_depth_count = 0;
  double total_depth_error = 0;
  double total_rmse = 0.0;
  double total_count = 0;
  std::vector<ApriltagRigTagStats> out_tag_stats;
  for (auto it : tag_id_to_per_image_rmse_block_id) {
    int tag_id = it.first;
    auto& stats = tag_stats[tag_id];
    for (auto per_image_rmse_block : it.second) {
      auto [image_rmse, block] = per_image_rmse_block;
      double cost;
      Eigen::Matrix<double, 4, 3> residuals;
      problem.EvaluateResidualBlockAssumingParametersUnchanged(
          block, false, &cost, residuals.data(), nullptr);
      double depth_error = 0;
      int depth_count = 0;
      for (int i = 0; i < 4; ++i) {
        double d2 = residuals(i, 2) * residuals(i, 2);
        if (d2 > 0.0) {
          depth_error += residuals(i, 2) / depth_weight;
          depth_count += 1;
        }
      }
      total_depth_count += depth_count;
      total_depth_error += depth_error;
      total_rmse += residuals.block<4, 2>(0, 0).squaredNorm();
      total_count += 8;

      stats.set_tag_rig_rmse(stats.tag_rig_rmse() +
                             residuals.block<4, 2>(0, 0).squaredNorm() / 8);
      stats.set_tag_rig_depth_error(stats.tag_rig_depth_error() + depth_error);
      stats.set_tag_rig_depth_count(stats.tag_rig_depth_count() + depth_count);

      image_rmse.set_rmse(
          std::sqrt(residuals.block<4, 2>(0, 0).squaredNorm() / 8));

      if (depth_count > 0) {
        image_rmse.set_depth_error(  // std::sqrt
            (depth_error / depth_count));
      }
      stats.add_per_image_rmse()->CopyFrom(image_rmse);
    }
    stats.set_tag_rig_rmse(std::sqrt(stats.tag_rig_rmse() / stats.n_frames()));
    if (stats.tag_rig_depth_count() > 0) {
      stats.set_tag_rig_depth_error(
          // std::sqrt
          (stats.tag_rig_depth_error() / stats.tag_rig_depth_count()));
    }
    out_tag_stats.push_back(stats);
  }
  double rmse = std::sqrt(total_rmse / total_count);
  std::stringstream ss;
  ss << "model rmse (pixels): " << rmse << "\n";
  double depth_error = 0;
  if (total_depth_count > 0) {
    depth_error = (total_depth_error / total_depth_count);
    ss << "model depth error (meters): " << depth_error << "\n";
  }

  auto pose = pose_graph.CheckAverageNamedSE3Pose(camera_rig.name(),
                                                  apriltag_rig.name());

  LOG(INFO) << "Solved pose:" << pose.ShortDebugString() << "\n" << ss.str();
  return {{pose, rmse, out_tag_stats}};
}
}  // namespace calibration
}  // namespace farm_ng


#pragma once
#include <memory>
#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <OpenMesh/Core/Mesh/PolyConnectivity.hh>
#include "../../container_cast.hh"
#include "geodesic/geodesic_algorithm_dijkstra.h"
#include "geodesic/geodesic_algorithm_exact.h"
#include "geodesic/geodesic_algorithm_subdivision.h"

namespace kt84 {

    struct GeodesicPoint {
        OpenMesh::PolyConnectivity::VHandle vhandle;
        OpenMesh::PolyConnectivity::EHandle ehandle;
        OpenMesh::PolyConnectivity::FHandle fhandle;
        Eigen::Vector2d ecoord;         // barycentric coord for edge
        Eigen::Vector3d fcoord;         // barycentric coord for face
        GeodesicPoint(){}
        GeodesicPoint(OpenMesh::PolyConnectivity::VHandle vhandle) : vhandle(vhandle) {}
        GeodesicPoint(OpenMesh::PolyConnectivity::EHandle ehandle, const Eigen::Vector2d& ecoord) : ehandle(ehandle), ecoord(ecoord) {}
        GeodesicPoint(OpenMesh::PolyConnectivity::FHandle fhandle, const Eigen::Vector3d& fcoord) : fhandle(fhandle), fcoord(fcoord) {}
        GeodesicPoint(const geodesic::SurfacePoint& p) {
            switch (p.type()) {
            case geodesic::VERTEX:
                vhandle = OpenMesh::PolyConnectivity::VHandle(p.base_element()->id());
                break;
            case geodesic::EDGE:
                ehandle = OpenMesh::PolyConnectivity::EHandle(p.base_element()->id());
                break;
            }
        }
        operator geodesic::SurfacePoint() const {
            geodesic::SurfacePoint p;
            if (is_vertex()) {

            }
            //...
            return p;
        }
        bool is_vertex() const { return vhandle.is_valid(); }
        bool is_edge  () const { return ehandle.is_valid(); }
        bool is_face  () const { return fhandle.is_valid(); }
    };

    template <class TMeshBase, class TMesh>
    struct Geodesic : public DerivedPtrHolder<TMesh, Geodesic <TMeshBase, TMesh>> {
        void geodesic_init() {
            auto mesh = get_mesh();
            std::vector<double> points;
            points.reserve(mesh->n_vertices() * 3);
            for (auto v : mesh->vertices()) {
                auto p = mesh->point(v);
                points.push_back(p[0]);
                points.push_back(p[1]);
                points.push_back(p[2]);
            }
            std::vector<unsigned> faces;
            faces.reserve(mesh->n_faces() * 3);
            for (auto f : mesh->faces()) {
                if (mesh->valence(f) != 3) {
                    std::cerr << "Error: geodesic algorithm does not work on polygonal meshes!\n";
                    assert(false);
                }
                for (auto v : mesh->fv_range(f))
                    faces.push_back(v.idx());
            }
            geodesic.mesh.initialize_mesh_data(points, faces);
            geodesic_set_algorithm_exact();      // default setting
        }
        void geodesic_set_algorithm_exact() {
            geodesic.algorithm = std::make_shared<geodesic::GeodesicAlgorithmExact>(&geodesic.mesh);
        }
        void geodesic_set_algorithm_dijkstra() {
            geodesic.algorithm = std::make_shared<geodesic::GeodesicAlgorithmDijkstra>(&geodesic.mesh);
        }
        void geodesic_set_algorithm_subdivision(int subdivision_level) {
            geodesic.algorithm = std::make_shared<geodesic::GeodesicAlgorithmSubdivision>(&geodesic.mesh, subdivision_level);
        }
        std::vector<GeodesicPoint> geodesic_compute(const GeodesicPoint& source, const GeodesicPoint& target) {
            assert(geodesic.algorithm);
            std::vector<geodesic::SurfacePoint> path;
            geodesic::SurfacePoint source_(source), target_(target);
            geodesic.algorithm->geodesic(source_, target_, path);
            return container_cast<GeodesicPoint>(path);
        }
        struct Data {
            geodesic::Mesh mesh;
            std::shared_ptr<geodesic::GeodesicAlgorithmBase> algorithm;
            Data() {}
            Data(const Data& src)
                : mesh(src.mesh)
            {
                switch (src.algorithm->type()) {
                case geodesic::GeodesicAlgorithmBase::EXACT:
                    algorithm = std::make_shared<geodesic::GeodesicAlgorithmExact>(&mesh);
                    break;
                case geodesic::GeodesicAlgorithmBase::DIJKSTRA:
                    algorithm = std::make_shared<geodesic::GeodesicAlgorithmDijkstra>(&mesh);
                    break;
                case geodesic::GeodesicAlgorithmBase::SUBDIVISION:
                {
                    auto subdivision_level = static_cast<geodesic::GeodesicAlgorithmSubdivision*>(algorithm.get())->subdivision_level();
                    algorithm = std::make_shared<geodesic::GeodesicAlgorithmSubdivision>(&mesh, subdivision_level);
                    break;
                }
                default:
                    assert(false);
                }
            }
            Data& Data::operator=(const Data& src) {
                mesh = src.mesh;
                switch (src.algorithm->type()) {
                case geodesic::GeodesicAlgorithmBase::EXACT:
                    algorithm = std::make_shared<geodesic::GeodesicAlgorithmExact>(&mesh);
                    break;
                case geodesic::GeodesicAlgorithmBase::DIJKSTRA:
                    algorithm = std::make_shared<geodesic::GeodesicAlgorithmDijkstra>(&mesh);
                    break;
                case geodesic::GeodesicAlgorithmBase::SUBDIVISION:
                {
                    auto subdivision_level = static_cast<geodesic::GeodesicAlgorithmSubdivision*>(algorithm.get())->subdivision_level();
                    algorithm = std::make_shared<geodesic::GeodesicAlgorithmSubdivision>(&mesh, subdivision_level);
                    break;
                }
                default:
                    assert(false);
                }
                return *this;
            }
        } geodesic;
    private:
        const TMesh* get_mesh() const { return DerivedPtrHolder<TMesh, Geodesic<TMeshBase, TMesh>>::derived_ptr; }
    };
}


// Copyright (c) 2019-2021 The Bigbang developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "crypto.h"

#include <boost/test/unit_test.hpp>
#include <sodium.h>

#include "crypto.h"
#include "curve25519/curve25519.h"
#include "test_big.h"
#include "util.h"

using namespace bigbang::crypto;
using namespace curve25519;
using namespace std;

BOOST_FIXTURE_TEST_SUITE(crypto_tests, BasicUtfSetup)

BOOST_AUTO_TEST_CASE(multisign)
{
    srand(time(0));

    int64_t signCount = 0, signTime = 0, verifyTime = 0;
    int count = 5;
    for (int i = 0; i < count; i++)
    {
        uint32_t nKey = CryptoGetRand32() % 256 + 1;
        uint32_t nPartKey = CryptoGetRand32() % nKey + 1;

        CCryptoKey* keys = new CCryptoKey[nKey];
        std::set<uint256> setPubKey;
        for (int i = 0; i < nKey; i++)
        {
            CryptoMakeNewKey(keys[i]);
            setPubKey.insert(keys[i].pubkey);
        }

        uint256 msg;
        CryptoGetRand256(msg);

        std::vector<uint8_t> vchSig;
        for (int i = 0; i < nPartKey; i++)
        {
            boost::posix_time::ptime t0 = boost::posix_time::microsec_clock::universal_time();
            BOOST_CHECK(CryptoMultiSign(setPubKey, keys[i], msg.begin(), msg.size(), vchSig));
            boost::posix_time::ptime t1 = boost::posix_time::microsec_clock::universal_time();
            signTime += (t1 - t0).ticks();
            ++signCount;
        }

        std::set<uint256> setPartKey;
        boost::posix_time::ptime t0 = boost::posix_time::microsec_clock::universal_time();
        BOOST_CHECK(CryptoMultiVerify(setPubKey, msg.begin(), msg.size(), vchSig, setPartKey) && (setPartKey.size() == nPartKey));
        boost::posix_time::ptime t1 = boost::posix_time::microsec_clock::universal_time();
        verifyTime += (t1 - t0).ticks();
    }

    std::cout << "multisign count : " << count << ", key count: " << signCount << std::endl;
    std::cout << "multisign sign count : " << signCount << "; average time : " << signTime / signCount << "us." << std::endl;
    std::cout << "multisign verify count : " << count << "; time per count : " << verifyTime / count << "us.; time per key: " << verifyTime / signCount << "us." << std::endl;
}

BOOST_AUTO_TEST_CASE(multisign_defect)
{
    srand(time(0));

    int64_t signCount = 0, signTime1 = 0, verifyTime1 = 0, signTime2 = 0, verifyTime2 = 0;
    int count = 5;
    for (int i = 0; i < count; i++)
    {
        uint32_t nKey = CryptoGetRand32() % 256 + 1;
        uint32_t nPartKey = CryptoGetRand32() % nKey + 1;

        CCryptoKey* keys = new CCryptoKey[nKey];
        std::set<uint256> setPubKey;
        for (int i = 0; i < nKey; i++)
        {
            CryptoMakeNewKey(keys[i]);
            setPubKey.insert(keys[i].pubkey);
        }

        uint256 msg;
        CryptoGetRand256(msg);
        uint256 seed;
        CryptoGetRand256(seed);

        std::vector<uint8_t> vchSig;
        for (int i = 0; i < nPartKey; i++)
        {
            boost::posix_time::ptime t0 = boost::posix_time::microsec_clock::universal_time();
            BOOST_CHECK(CryptoMultiSignDefect(setPubKey, keys[i], seed.begin(), seed.size(), msg.begin(), msg.size(), vchSig));
            boost::posix_time::ptime t1 = boost::posix_time::microsec_clock::universal_time();
            signTime1 += (t1 - t0).ticks();
            ++signCount;
        }

        {
            boost::posix_time::ptime t0 = boost::posix_time::microsec_clock::universal_time();
            std::set<uint256> setPartKey;
            BOOST_CHECK(CryptoMultiVerifyDefect(setPubKey, seed.begin(), seed.size(), msg.begin(), msg.size(), vchSig, setPartKey) && (setPartKey.size() == nPartKey));
            boost::posix_time::ptime t1 = boost::posix_time::microsec_clock::universal_time();
            verifyTime1 += (t1 - t0).ticks();
        }
    }

    std::cout << "multisign count : " << count << ", key count: " << signCount << std::endl;
    std::cout << "multisign sign1 count : " << signCount << "; average time : " << signTime1 / signCount << "us." << std::endl;
    std::cout << "multisign verify1 count : " << count << "; time per count : " << verifyTime1 / count << "us.; time per key: " << verifyTime1 / signCount << "us." << std::endl;
    std::cout << "multisign sign2 count : " << signCount << "; average time : " << signTime2 / signCount << "us." << std::endl;
    std::cout << "multisign verify2 count : " << count << "; time per count : " << verifyTime2 / count << "us.; time per key: " << verifyTime2 / signCount << "us." << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/photo.hpp>

#include "envitools/EnviUtils.hpp"

namespace fs = boost::filesystem;
namespace po = boost::program_options;
namespace et = envitools;

constexpr static float DEFAULT_GAMMA = 2.2f;
constexpr static uint8_t MAX_INTENSITY_8BPC = std::numeric_limits<uint8_t>::max();
constexpr static uint16_t MAX_INTENSITY_16BPC =
    std::numeric_limits<uint16_t>::max();

enum class BitDepth { Unsigned8 = 8, Unsigned16 = 16 };

int main(int argc, char* argv[])
{
    ///// Parse the cmd line /////
    fs::path imgDir, outputPath;
    std::cout.width(10);

    // clang-format off
    po::options_description options("Options");
    options.add_options()
        ("help,h","Show this message")
        ("input-dir,i",po::value<std::string>()->required(),
            "Input directory")
        ("output-dir,o", po::value<std::string>()->required(),
            "Output directory")
        ("bpc,b",po::value<int>()->default_value(16),
            "Output bit depth:\n"
            " 8  = 8bpc (unsigned)\n"
            " 16 = 16bpc (unsigned)")
        ("gamma", po::value<float>()->default_value(DEFAULT_GAMMA),
            "Gamma for floating point image tone mapping");
    // clang-format on

    // parsedOptions will hold the values of all parsed options as a Map
    po::variables_map parsedOptions;
    po::store(
        po::command_line_parser(argc, argv).options(options).run(),
        parsedOptions);

    // show the help message
    if (parsedOptions.count("help") || argc < 3) {
        std::cout << argv[0] << std::endl;
        std::cout.fill('-');
        std::cout.width(20);
        std::cout << "\n";
        std::cout << "A utility for converting a directory of 32bpc, "
                     "floating-point TIFFs into" << std::endl;
        std::cout << "8 or 16bpc TIFFs." << std::endl;
        std::cout << "\n";
        std::cout << options << std::endl;
        return EXIT_SUCCESS;
    }

    // warn of missing options
    try {
        po::notify(parsedOptions);
    } catch (po::error& e) {
        std::cerr << "ERROR: " << e.what() << std::endl;
        return EXIT_FAILURE;
    }

    // Get the output options
    outputPath = parsedOptions["output-dir"].as<std::string>();
    auto depth = static_cast<BitDepth>(parsedOptions["bpc"].as<int>());

    ///// Collect the tif files /////
    imgDir = parsedOptions["input-dir"].as<std::string>();
    auto imgPaths = et::FindByExtension(imgDir, ".tif");
    if (imgPaths.empty()) {
        std::cerr << "Error opening/parsing image directory. No files found."
                  << std::endl;
        return EXIT_FAILURE;
    }

    ///// Convert /////
    cv::Mat image;
    std::cout << "Converting to " << static_cast<int>(depth) << " bpc..."
              << std::endl;
    for (auto path : imgPaths) {
        // Get wavelength for key
        std::string id = path.stem().string();
        std::cout << "Image: " << id << "\r" << std::flush;

        // Load the image
        image = cv::imread(path.string(), -1);
        if (!image.data) {
            std::cout << "Could not open or find the image: ";
            std::cout << path.string() << std::endl;
            continue;
        }

        if (image.depth() != CV_32F) {
            std::cout << "Input image is not floating point: ";
            std::cout << path.string() << std::endl;
            continue;
        }

        // Tone map for contrast
        auto gamma = parsedOptions["gamma"].as<float>();
        image = et::ToneMap(image, gamma);

        // Scale to output bit depth
        switch (depth) {
            case BitDepth::Unsigned8:
                image *= MAX_INTENSITY_8BPC;
                image.convertTo(image, CV_8UC(image.channels()));
                break;
            case BitDepth::Unsigned16:
                image *= MAX_INTENSITY_16BPC;
                image.convertTo(image, CV_16UC(image.channels()));
                break;
        }

        // Save the output image
        fs::path outpath = outputPath / (id + ".tif");
        cv::imwrite(outpath.string(), image);
    }
    std::cout << std::endl;
}


#include "simInternal.h"
#include "meshWrapper.h"
#include "mesh.h"
#include "tt.h"
#include "global.h"
#include "viewableBase.h"
#include "app.h"
#include <Eigen/Eigenvalues>
#include <iostream>

CMeshWrapper::CMeshWrapper()
{
    _mass=1.0f;
    _name="sub__0";

    _dynMaterialId_old=-1; // not used anymore since V3.4.0

    _convex=false;

    setDefaultInertiaParams();

    _transformationsSinceGrouping.setIdentity();
}

CMeshWrapper::~CMeshWrapper()
{
    for (size_t i=0;i<childList.size();i++)
        delete childList[i];
}

C7Vector CMeshWrapper::getTransformationsSinceGrouping()
{
    return(_transformationsSinceGrouping);
}

void CMeshWrapper::setTransformationsSinceGrouping(const C7Vector& tr)
{
    _transformationsSinceGrouping=tr;
}

void CMeshWrapper::display(CShape* geomData,int displayAttrib,CColorObject* collisionColor,int dynObjFlag_forVisualization,int transparencyHandling,bool multishapeEditSelected)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->display(geomData,displayAttrib,collisionColor,dynObjFlag_forVisualization,transparencyHandling,multishapeEditSelected);
}

void CMeshWrapper::display_extRenderer(CShape* geomData,int displayAttrib,const C7Vector& tr,int shapeHandle,int& componentIndex)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->display_extRenderer(geomData,displayAttrib,tr,shapeHandle,componentIndex);
}

void CMeshWrapper::display_colorCoded(CShape* geomData,int objectId,int displayAttrib)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->display_colorCoded(geomData,objectId,displayAttrib);
}

void CMeshWrapper::displayGhost(CShape* geomData,int displayAttrib,bool originalColors,bool backfaceCulling,float transparency,const float* newColors)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->displayGhost(geomData,displayAttrib,originalColors,backfaceCulling,transparency,newColors);
}

void CMeshWrapper::performSceneObjectLoadingMapping(const std::vector<int>* map)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->performSceneObjectLoadingMapping(map);
}

void CMeshWrapper::performTextureObjectLoadingMapping(const std::vector<int>* map)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->performTextureObjectLoadingMapping(map);
}

void CMeshWrapper::performDynMaterialObjectLoadingMapping(const std::vector<int>* map)
{
    _dynMaterialId_old=CWorld::getLoadingMapping(map,_dynMaterialId_old);
}

void CMeshWrapper::announceSceneObjectWillBeErased(int objectID)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->announceSceneObjectWillBeErased(objectID);
}

void CMeshWrapper::setTextureDependencies(int shapeID)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->setTextureDependencies(shapeID);
}

int CMeshWrapper::getTextureCount()
{ // function has virtual/non-virtual counterpart!
    int retVal=0;
    for (size_t i=0;i<childList.size();i++)
        retVal+=childList[i]->getTextureCount();
    return(retVal);
}

bool CMeshWrapper::hasTextureThatUsesFixedTextureCoordinates()
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
    {
        if (childList[i]->hasTextureThatUsesFixedTextureCoordinates())
            return(true);
    }
    return(false);
}

void CMeshWrapper::removeAllTextures()
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->removeAllTextures();
}

void CMeshWrapper::getColorStrings(std::string& colorStrings)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->getColorStrings(colorStrings);
}

bool CMeshWrapper::getContainsTransparentComponents()
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
    {
        if (childList[i]->getContainsTransparentComponents())
            return(true);
    }
    return(false);
}

float CMeshWrapper::getGouraudShadingAngle()
{ // function has virtual/non-virtual counterpart!
    return(childList[0]->getGouraudShadingAngle()); // we just return the first angle we encounter! Normally never used
}

void CMeshWrapper::setGouraudShadingAngle(float angle)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->setGouraudShadingAngle(angle);
}

float CMeshWrapper::getEdgeThresholdAngle()
{ // function has virtual/non-virtual counterpart!
    return(childList[0]->getEdgeThresholdAngle()); // we just return the first angle we encounter! Normally never used
}

void CMeshWrapper::setEdgeThresholdAngle(float angle)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->setEdgeThresholdAngle(angle);
}

void CMeshWrapper::setHideEdgeBorders(bool v)
{  // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->setHideEdgeBorders(v);
}

bool CMeshWrapper::getHideEdgeBorders()
{  // function has virtual/non-virtual counterpart!
    return(childList[0]->getHideEdgeBorders()); // we just return the first setting we encounter! Normally never used
}

CMeshWrapper* CMeshWrapper::copyYourself()
{ // function has virtual/non-virtual counterpart!
    CMeshWrapper* newIt=new CMeshWrapper();
    copyWrapperInfos(newIt);
    return(newIt);
}

void CMeshWrapper::copyWrapperInfos(CMeshWrapper* target)
{
    target->_mass=_mass;
    target->_name=_name;
    target->_convex=_convex;

    target->_localInertiaFrame=_localInertiaFrame;
    target->_principalMomentsOfInertia=_principalMomentsOfInertia;

    target->_transformationsSinceGrouping=_transformationsSinceGrouping;

    target->_dynMaterialId_old=_dynMaterialId_old;

    for (size_t i=0;i<childList.size();i++)
        target->childList.push_back(childList[i]->copyYourself());
}

void CMeshWrapper::setMass(float m)
{
    _mass=tt::getLimitedFloat(0.000000001f,100000.0f,m);
}

float CMeshWrapper::getMass()
{
    return(_mass);
}

void CMeshWrapper::setDefaultInertiaParams()
{
    _localInertiaFrame.setIdentity();
    _principalMomentsOfInertia(0)=0.001f;
    _principalMomentsOfInertia(1)=0.001f;
    _principalMomentsOfInertia(2)=0.001f;
}

void CMeshWrapper::setName(std::string newName)
{
    _name=newName;
}

std::string CMeshWrapper::getName()
{
    return(_name);
}

int CMeshWrapper::getDynMaterialId_old()
{
    return(_dynMaterialId_old);
}

void CMeshWrapper::setDynMaterialId_old(int id)
{
    _dynMaterialId_old=id;
}

C7Vector CMeshWrapper::getLocalInertiaFrame()
{
    return (_localInertiaFrame);
}

void CMeshWrapper::setLocalInertiaFrame(const C7Vector& li)
{
    _localInertiaFrame=li;
}

C3Vector CMeshWrapper::getPrincipalMomentsOfInertia()
{
    return (_principalMomentsOfInertia);
}

void CMeshWrapper::setPrincipalMomentsOfInertia(const C3Vector& inertia)
{
    _principalMomentsOfInertia=inertia;
    _principalMomentsOfInertia(0)=tt::getLimitedFloat(0.0f,10000.0f,_principalMomentsOfInertia(0));
    _principalMomentsOfInertia(1)=tt::getLimitedFloat(0.0f,10000.0f,_principalMomentsOfInertia(1));
    _principalMomentsOfInertia(2)=tt::getLimitedFloat(0.0f,10000.0f,_principalMomentsOfInertia(2));
    if (_principalMomentsOfInertia.getLength()==0.0f)
        _principalMomentsOfInertia(0)=0.001f; // make sure we don't have a zero vector (problems with Bullet? and CoppeliaSim!)
}

void CMeshWrapper::scale(float xVal,float yVal,float zVal)
{ // function has virtual/non-virtual counterpart!
    // iso-scaling for compound shapes!! (should normally already be xVal=yVal=zVal)
    scaleWrapperInfos(xVal,xVal,xVal);
}

void CMeshWrapper::prepareVerticesIndicesNormalsAndEdgesForSerialization()
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->prepareVerticesIndicesNormalsAndEdgesForSerialization();
}

void CMeshWrapper::scaleWrapperInfos(float xVal,float yVal,float zVal)
{
    scaleMassAndInertia(xVal,yVal,zVal);

    _localInertiaFrame.X(0)*=xVal;
    _localInertiaFrame.X(1)*=yVal;
    _localInertiaFrame.X(2)*=zVal;

    _transformationsSinceGrouping.X(0)*=xVal;
    _transformationsSinceGrouping.X(1)*=yVal;
    _transformationsSinceGrouping.X(2)*=zVal;

    for (size_t i=0;i<childList.size();i++)
        childList[i]->scale(xVal,yVal,zVal);
    if ((xVal<0.0f)||(yVal<0.0f)||(zVal<0.0f)) // that effectively flips faces!
        checkIfConvex();
}

void CMeshWrapper::scaleMassAndInertia(float xVal,float yVal,float zVal)
{
    _mass*=xVal*yVal*zVal;
    _principalMomentsOfInertia(0)*=yVal*zVal;
    _principalMomentsOfInertia(1)*=xVal*zVal;
    _principalMomentsOfInertia(2)*=xVal*yVal;
}

void CMeshWrapper::setPurePrimitiveType(int theType,float xOrDiameter,float y,float zOrHeight)
{ // function has virtual/non-virtual counterpart!
    // Following added on 14/03/2011 because a compound shape composed by pure and non pure shapes would decompose as pure shapes with wrong orientation!
    if (theType==sim_pure_primitive_none)
    {
        for (size_t i=0;i<childList.size();i++)
            childList[i]->setPurePrimitiveType(theType,xOrDiameter,y,zOrHeight);
        // _convex=false;  NO!!!
    }
}

int CMeshWrapper::getPurePrimitiveType()
{ // function has virtual/non-virtual counterpart!
    return(childList[0]->getPurePrimitiveType()); // we just return the first type we encounter! Normally never used
}

bool CMeshWrapper::isMesh()
{ // function has virtual/non-virtual counterpart!
    return(false);
}

bool CMeshWrapper::isPure()
{ // function has virtual/non-virtual counterpart!
    return(childList[0]->isPure());
}

bool CMeshWrapper::isConvex()
{ // function has virtual/non-virtual counterpart!
    return(_convex);
}

bool CMeshWrapper::containsOnlyPureConvexShapes()
{ // function has virtual/non-virtual counterpart!
    bool retVal=true;
    for (size_t i=0;i<childList.size();i++)
        retVal=retVal&&childList[i]->containsOnlyPureConvexShapes();
    if (retVal)
        _convex=retVal; // needed since there was a bug where pure planes and pure discs were considered as convex
    return(retVal);
}

bool CMeshWrapper::checkIfConvex()
{ // function has virtual/non-virtual counterpart!
    _convex=true;
    for (size_t i=0;i<childList.size();i++)
        _convex=_convex&&childList[i]->checkIfConvex();
    setConvex(_convex);
    return(_convex);
}

void CMeshWrapper::setConvex(bool convex)
{ // function has virtual/non-virtual counterpart!
    _convex=convex; // This is just for the wrapper!
    /* removed on 24/3/2013
    if (_convex)
    { // convex shape handling in Bullet includes a very large margin. We can:
        // 1. shrink the dynamic model using _bulletAutoShrinkConvexMesh. This adds some initial preprocessing time, can lead to crashes, and edges and points appear shifted inwards. Faces react correctly.
        // 2. reduce the margin (what we do here). Erwin from Bullet doesn't recommend it (doesn't say why), but I got the best (still not good!!) results with it
        _bulletNonDefaultCollisionMarginFactor=0.002f;
        _bulletNonDefaultCollisionMargin=true;
    }
    else
        _bulletNonDefaultCollisionMargin=false;
        */
}

void CMeshWrapper::getCumulativeMeshes(std::vector<float>& vertices,std::vector<int>* indices,std::vector<float>* normals)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->getCumulativeMeshes(vertices,indices,normals);
}

void CMeshWrapper::setColor(const char* colorName,int colorComponent,const float* rgbData,int& rgbDataOffset)
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->setColor(colorName,colorComponent,rgbData,rgbDataOffset);
}

bool CMeshWrapper::getColor(const char* colorName,int colorComponent,float* rgbData,int& rgbDataOffset)
{ // function has virtual/non-virtual counterpart!
    bool retVal=false;
    for (size_t i=0;i<childList.size();i++)
        retVal=childList[i]->getColor(colorName,colorComponent,rgbData,rgbDataOffset)||retVal;
    return(retVal);
}

void CMeshWrapper::getAllShapeComponentsCumulative(std::vector<CMesh*>& shapeComponentList)
{   // function has virtual/non-virtual counterpart!
    // needed by the dynamics routine. We return ALL shape components!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->getAllShapeComponentsCumulative(shapeComponentList);
}

CMesh* CMeshWrapper::getShapeComponentAtIndex(int& index)
{ // function has virtual/non-virtual counterpart!
    if (index<0)
        return(nullptr);
    for (size_t i=0;i<childList.size();i++)
    {
        CMesh* retVal=childList[i]->getShapeComponentAtIndex(index);
        if (retVal!=nullptr)
            return(retVal);
        if (index<0)
            return(nullptr);
    }
    return(nullptr);
}

int CMeshWrapper::getComponentCount() const
{ // function has virtual/non-virtual counterpart!
    int retVal=0;
    for (size_t i=0;i<childList.size();i++)
        retVal+=childList[i]->getComponentCount();
    return(retVal);
}

void CMeshWrapper::preMultiplyAllVerticeLocalFrames(const C7Vector& preTr)
{ // function has virtual/non-virtual counterpart!

    _transformationsSinceGrouping=preTr*_transformationsSinceGrouping;
    _localInertiaFrame=preTr*_localInertiaFrame;

    for (size_t i=0;i<childList.size();i++)
        childList[i]->preMultiplyAllVerticeLocalFrames(preTr);
}

void CMeshWrapper::flipFaces()
{ // function has virtual/non-virtual counterpart!
    for (size_t i=0;i<childList.size();i++)
        childList[i]->flipFaces();
    checkIfConvex();
}

void CMeshWrapper::serialize(CSer& ar,const char* shapeName)
{ // function has virtual/non-virtual counterpart!
    serializeWrapperInfos(ar,shapeName);
}

void CMeshWrapper::serializeWrapperInfos(CSer& ar,const char* shapeName)
{
    if (ar.isBinary())
    {
        if (ar.isStoring())
        {       // Storing
            ar.storeDataName("Nme");
            ar << _name;
            ar.flush();

            ar.storeDataName("Mas");
            ar << _mass;
            ar.flush();

            ar.storeDataName("Dmi");
            ar << _dynMaterialId_old;
            ar.flush();

            ar.storeDataName("Ine");
            ar << _localInertiaFrame(0) << _localInertiaFrame(1) << _localInertiaFrame(2) << _localInertiaFrame(3);
            ar << _localInertiaFrame(4) << _localInertiaFrame(5) << _localInertiaFrame(6);
            ar << _principalMomentsOfInertia(0) << _principalMomentsOfInertia(1) << _principalMomentsOfInertia(2);
            ar.flush();

            ar.storeDataName("Vtb");
            ar << _transformationsSinceGrouping(0) << _transformationsSinceGrouping(1) << _transformationsSinceGrouping(2) << _transformationsSinceGrouping(3);
            ar << _transformationsSinceGrouping(4) << _transformationsSinceGrouping(5) << _transformationsSinceGrouping(6);
            ar.flush();

            ar.storeDataName("Var");
            unsigned char nothing=0;
            // reserved (12/9/2013) SIM_SET_CLEAR_BIT(nothing,0,_bulletNonDefaultCollisionMargin);
            // reserved (12/9/2013) SIM_SET_CLEAR_BIT(nothing,1,_bulletStickyContact);
            SIM_SET_CLEAR_BIT(nothing,2,_convex);
            // reserved (12/9/2013) SIM_SET_CLEAR_BIT(nothing,3,!_bulletAutoShrinkConvexMesh);
            // reserved (12/9/2013) SIM_SET_CLEAR_BIT(nothing,4,_bulletNonDefaultCollisionMargin_forConvexAndNonPureShape); // since 24/3/2013
            SIM_SET_CLEAR_BIT(nothing,5,true); // means: we do not have to make the convectivity test for this shape (was already done). Added this on 28/1/2013
            ar << nothing;
            ar.flush();

            for (size_t i=0;i<childList.size();i++)
            {
                if (childList[i]->isMesh())
                    ar.storeDataName("Geo");
                else
                    ar.storeDataName("Wrp");
                ar.setCountingMode();
                childList[i]->serialize(ar,shapeName);
                if (ar.setWritingMode())
                    childList[i]->serialize(ar,shapeName);
            }

            ar.storeDataName(SER_END_OF_OBJECT);
        }
        else
        {       // Loading
            int byteQuantity;
            std::string theName="";
            while (theName.compare(SER_END_OF_OBJECT)!=0)
            {
                theName=ar.readDataName();
                if (theName.compare(SER_END_OF_OBJECT)!=0)
                {
                    bool noHit=true;
                    if (theName.compare("Nme")==0)
                    {
                        noHit=false;
                        ar >> byteQuantity;
                        ar >> _name;
                    }
                    if (theName.compare("Mas")==0)
                    {
                        noHit=false;
                        ar >> byteQuantity;
                        ar >> _mass;
                        if (_mass==0.0f) // to catch an old bug
                            _mass=0.001f;
                    }
                    if (theName.compare("Dmi")==0)
                    {
                        noHit=false;
                        ar >> byteQuantity;
                        ar >> _dynMaterialId_old;
                    }
                    if (theName.compare("Ine")==0)
                    {
                        noHit=false;
                        ar >> byteQuantity;
                        ar >> _localInertiaFrame(0) >> _localInertiaFrame(1) >> _localInertiaFrame(2) >> _localInertiaFrame(3);
                        ar >> _localInertiaFrame(4) >> _localInertiaFrame(5) >> _localInertiaFrame(6);
                        ar >> _principalMomentsOfInertia(0) >> _principalMomentsOfInertia(1) >> _principalMomentsOfInertia(2);
                    }

                    if (theName.compare("Vtb")==0)
                    {
                        noHit=false;
                        ar >> byteQuantity;
                        ar >> _transformationsSinceGrouping(0) >> _transformationsSinceGrouping(1) >> _transformationsSinceGrouping(2) >> _transformationsSinceGrouping(3);
                        ar >> _transformationsSinceGrouping(4) >> _transformationsSinceGrouping(5) >> _transformationsSinceGrouping(6);
                    }

                    if (theName=="Var")
                    {
                        noHit=false;
                        ar >> byteQuantity;
                        unsigned char nothing;
                        ar >> nothing;
                        // 0 reserved
                        // 1 reserved
                        _convex=SIM_IS_BIT_SET(nothing,2);
                        // 3 reserved
                        // 4 reserved
                        // 5 reserved
                    }
                    if (theName.compare("Geo")==0)
                    {
                        noHit=false;
                        ar >> byteQuantity; // never use that info, unless loading unknown data!!!! (undo/redo stores dummy info in there)
                        CMesh* it=new CMesh();
                        it->serialize(ar,shapeName);
                        childList.push_back(it);
                    }
                    if (theName.compare("Wrp")==0)
                    {
                        noHit=false;
                        ar >> byteQuantity; // never use that info, unless loading unknown data!!!! (undo/redo stores dummy info in there)
                        CMeshWrapper* it=new CMeshWrapper();
                        it->serialize(ar,shapeName);
                        childList.push_back(it);
                    }
                    if (noHit)
                        ar.loadUnknownData();
                }
            }
        }
    }
    else
    {
        if (ar.isStoring())
        {
            ar.xmlPushNewNode("common");

            ar.xmlAddNode_string("name",_name.c_str());

            ar.xmlPushNewNode("dynamics");
            ar.xmlAddNode_float("mass",_mass);
            ar.xmlPushNewNode("localInertiaFrame");
            ar.xmlAddNode_floats("position",_localInertiaFrame.X.data,3);
            ar.xmlAddNode_floats("quaternion",_localInertiaFrame.Q.data,4);
            ar.xmlAddNode_floats("principalMomentsOfInertia",_principalMomentsOfInertia.data,3);
            ar.xmlPopNode();
            ar.xmlPopNode();

            ar.xmlPushNewNode("transformationSinceGrouping");
            ar.xmlAddNode_floats("position",_transformationsSinceGrouping.X.data,3);
            ar.xmlAddNode_floats("quaternion",_transformationsSinceGrouping.Q.data,4);
            ar.xmlPopNode();

            ar.xmlPushNewNode("switches");
            ar.xmlAddNode_bool("convex",_convex);
            ar.xmlPopNode();

            for (size_t i=0;i<childList.size();i++)
            {
                ar.xmlPushNewNode("child");
                if (childList[i]->isMesh())
                    ar.xmlPushNewNode("mesh");
                else
                    ar.xmlPushNewNode("compound");
                childList[i]->serialize(ar,shapeName);
                ar.xmlPopNode();
                ar.xmlPopNode();
            }
            ar.xmlPopNode();
        }
        else
        {
            if (ar.xmlPushChildNode("common"))
            {
                ar.xmlGetNode_string("name",_name);

                if (ar.xmlPushChildNode("dynamics"))
                {
                    ar.xmlGetNode_float("mass",_mass);
                    if (ar.xmlPushChildNode("localInertiaFrame"))
                    {
                        ar.xmlGetNode_floats("position",_localInertiaFrame.X.data,3);
                        ar.xmlGetNode_floats("quaternion",_localInertiaFrame.Q.data,4);
                        _localInertiaFrame.Q.normalize(); // just in case
                        ar.xmlGetNode_floats("principalMomentsOfInertia",_principalMomentsOfInertia.data,3);
                        ar.xmlPopNode();
                    }
                    ar.xmlPopNode();
                }

                if (ar.xmlPushChildNode("transformationSinceGrouping"))
                {
                    ar.xmlGetNode_floats("position",_transformationsSinceGrouping.X.data,3);
                    ar.xmlGetNode_floats("quaternion",_transformationsSinceGrouping.Q.data,4);
                    _transformationsSinceGrouping.Q.normalize(); // just in case
                    ar.xmlPopNode();
                }

                if (ar.xmlPushChildNode("switches"))
                {
                    ar.xmlGetNode_bool("convex",_convex);
                    ar.xmlPopNode();
                }
                if (ar.xmlPushChildNode("child",false))
                {
                    while (true)
                    {
                        if (ar.xmlPushChildNode("mesh",false))
                        {
                            CMesh* it=new CMesh();
                            it->serialize(ar,shapeName);
                            childList.push_back(it);
                            ar.xmlPopNode();
                        }
                        else
                        {
                            if (ar.xmlPushChildNode("compound"))
                            {
                                CMeshWrapper* it=new CMeshWrapper();
                                it->serialize(ar,shapeName);
                                childList.push_back(it);
                                ar.xmlPopNode();
                            }
                        }
                        if (!ar.xmlPushSiblingNode("child",false))
                            break;
                    }
                    ar.xmlPopNode();
                }
                ar.xmlPopNode();
            }
        }
    }
}

void CMeshWrapper::findPrincipalMomentOfInertia(const C3X3Matrix& tensor,C4Vector& rotation,C3Vector& principalMoments)
{
    //*
    Eigen::Matrix3f m;
    m(0,0)=tensor(0,0);
    m(1,1)=tensor(1,1);
    m(2,2)=tensor(2,2);
    m(0,1)=tensor(0,1);
    m(1,0)=tensor(0,1);
    m(0,2)=tensor(0,2);
    m(2,0)=tensor(0,2);
    m(1,2)=tensor(1,2);
    m(2,1)=tensor(1,2);
    Eigen::EigenSolver<Eigen::Matrix3f> es(m);
    Eigen::Vector3cf eigenVals=es.eigenvalues();
    principalMoments.set(eigenVals(0).real(),eigenVals(1).real(),eigenVals(2).real());
    Eigen::Vector3cf eigenVect1=es.eigenvectors().col(0);
    C3Vector eVect1(eigenVect1(0).real(),eigenVect1(1).real(),eigenVect1(2).real());
    Eigen::Vector3cf eigenVect2=es.eigenvectors().col(1);
    C3Vector eVect2(eigenVect2(0).real(),eigenVect2(1).real(),eigenVect2(2).real());
    Eigen::Vector3cf eigenVect3=es.eigenvectors().col(2);
    C3Vector eVect3(eigenVect3(0).real(),eigenVect3(1).real(),eigenVect3(2).real());
    C3X3Matrix matr;
    matr.axis[0]=eVect1;
    matr.axis[1]=eVect2;
    matr.axis[2]=eVect3;
    rotation=matr.getQuaternion();
    //*/
    /*
    // This routine is iterative and not elegant. But we do not need speed here anyway
    C3X3Matrix rot;
    C3X3Matrix tens(tensor);
    rot.setIdentity();
    float w=_getTensorNonDiagonalMeasure(tensor);

    // With below settings, we will get an orientational precision of about 0.000128 degrees
    float stepSize=10.0f*degToRad_f;
    const float stepSizeMultiplier=0.2f;
    const int resolutionScalePasses=8;

    C3X3Matrix drot;
    for (int scale=0;scale<resolutionScalePasses;scale++)
    { // resolution loop
        for (int axis=0;axis<3;axis++)
        { // axis loop (x, y, z)
            if (axis==0)
                drot.buildXRotation(stepSize);
            if (axis==1)
                drot.buildYRotation(stepSize);
            if (axis==2)
                drot.buildZRotation(stepSize);
            int dirChangeCnt=0;
            while (dirChangeCnt<2)
            { // error reduction loop for a given axis
                rot=rot*drot;
                tens=drot.getTranspose()*tens*drot;
                float w2=_getTensorNonDiagonalMeasure(tens);
                if (w2>=w)
                { // it got worse
                    dirChangeCnt++;
                    drot.transpose();
                    rot=rot*drot; // reverse change
                    tens=drot.getTranspose()*tens*drot;
                }
                else
                    w=w2; // it got better
            }
        }
        stepSize*=stepSizeMultiplier;
    }

    principalMoments(0)=tens.axis[0](0);
    principalMoments(1)=tens.axis[1](1);
    principalMoments(2)=tens.axis[2](2);
    rotation=rot.getQuaternion();
    //*/
}

float CMeshWrapper::_getTensorNonDiagonalMeasure(const C3X3Matrix& tensor)
{
    C3Vector v(tensor.axis[1](0),tensor.axis[2](0),tensor.axis[2](1));
    return(v*v);
}

C3X3Matrix CMeshWrapper::getNewTensor(const C3Vector& principalMoments,const C7Vector& newFrame)
{ // remember that we always work with a massless tensor. The tensor is multiplied with the mass in the dynamics module!
    C3X3Matrix tensor;
    tensor.clear();
    tensor.axis[0](0)=principalMoments(0);
    tensor.axis[1](1)=principalMoments(1);
    tensor.axis[2](2)=principalMoments(2);
    // 1. reorient the frame:
    tensor=newFrame.Q.getMatrix()*tensor*newFrame.Q.getInverse().getMatrix();
    // 2. shift the frame:
    C3X3Matrix D;
    D.setIdentity();
    D*=(newFrame.X*newFrame.X);
    D.axis[0]-=newFrame.X*newFrame.X(0);
    D.axis[1]-=newFrame.X*newFrame.X(1);
    D.axis[2]-=newFrame.X*newFrame.X(2);
    tensor+=D;
    return(tensor);
}


#include "abstract_sph.hpp"
#include "generic/is_finite.hpp"
#include "generic/eigen.hpp"

#include <boost/log/trivial.hpp>

namespace GooBalls {

namespace d2 {

namespace Physics {

#define INLINE_KERNELS 1

void AbstractSph::initFluid(Scene& scene) {
    BOOST_LOG_TRIVIAL(trace) << "AbstractSph: initializing";
    const auto& pos = scene.fluid->particles_position();
    int PN = pos.rows();
    BOOST_LOG_TRIVIAL(trace) << "Found " << PN << " fluid particles.";
    if(PN != scene.fluid->particles_velocity().rows()){
        // by default, give the particles no speed
        BOOST_LOG_TRIVIAL(warning) << "No proper fluid particle velocity set, setting to zero.";
        scene.fluid->particles_velocity().setOnes(PN, Eigen::NoChange);
    }
    if(PN != scene.fluid->particles_mass().rows()){
        BOOST_LOG_TRIVIAL(warning) << "No proper fluid particle mass set, setting to ones.";
        scene.fluid->particles_mass().setOnes(PN);
    }
    if(PN != scene.fluid->particles_velocity_correction().rows()){
        BOOST_LOG_TRIVIAL(warning) << "No proper fluid particle velocity correction coefficients set, setting to 1.";
        scene.fluid->particles_velocity_correction().resize(PN, Eigen::NoChange);
        scene.fluid->particles_velocity_correction().array() = 0.001;
    }
    if(PN != scene.fluid->particles_external_force().rows()){
        BOOST_LOG_TRIVIAL(warning) << "No proper external particle force set, setting zero.";
        scene.fluid->particles_external_force().setZero(PN, 2);
    }

    scene.fluid->particles_density().setZero(PN, 1);
    scene.fluid->particles_total_force().setZero(PN, 2);
    scene.fluid->particles_pressure().setZero(PN, 1);

    scene.fluid->boundary_force().setZero(scene.fluid->boundary_position().rows(), 2);

    FGravity.setZero(PN, 2);
    FPressure.setZero(PN, 2);
    FSurface.setZero(PN, 2);
    FViscosity.setZero(PN, 2);

    // initialize with some rough values such that the first frame looks ok
    assert(scene.fluid->rest_density() > 0);
    scene.fluid->particles_density().array() = scene.fluid->rest_density();
    computeFluidPressure(scene);
    BOOST_LOG_TRIVIAL(trace) << "Abstract fluid initialized";
}

void AbstractSph::prepareFluid(Scene& scene) const {
    assert(scene.fluid.get() != nullptr);
    assert(scene.fluid->sanity_check());
    const auto& pos = scene.fluid->particles_position();
    assert(is_finite(pos));
    scene.fluid->fluid_neighborhood->inRange(pos, scene.fluid->h());
}

void AbstractSph::prepareBoundary(Scene& scene) const {
    Coordinates1d& psi = scene.fluid->boundary_psi();
    Float rho0 = scene.fluid->rest_density();
    psi = rho0 * scene.fluid->boundary_volume();
    auto h = scene.fluid->h();
    scene.fluid->boundary_neighborhood->inRange(scene.fluid->particles_position(), scene.fluid->boundary_position(), h);
}

void AbstractSph::advance(Scene& scene, TimeStep dt){
    if(scene.fluid.get() == nullptr){
        return;
    }
    computeTotalForce(scene, dt);
    // a_i = f_i / rho_i
    const auto& rho = scene.fluid->particles_density();
    const auto& Ftotal = scene.fluid->particles_total_force();
    Coordinates2d a(rho.rows(), 2);
    a.col(0) = Ftotal.col(0).array() / rho.array();
    a.col(1) = Ftotal.col(1).array() / rho.array();
    auto& pos = scene.fluid->particles_position();
    auto& vs = scene.fluid->particles_velocity();
    vs = vs + dt * a;
    scene.room.restrictFluid(* scene.fluid);
    pos = pos + dt * vs;
    limitVelocity(scene);
    assert(is_finite(pos));
    assert(is_finite(vs));
}

void AbstractSph::computeGravityForce(const Scene& scene) {
    const auto& rho = scene.fluid->particles_density();
    FGravity.col(0) = scene.gravity[0] * rho;
    FGravity.col(1) = scene.gravity[1] * rho;
}

void AbstractSph::computeFluidPressure(Scene& scene) const {
    auto rho0 = scene.fluid->rest_density();
    auto pressure_gamma = scene.fluid->pressure_gamma();
    const auto& rho = scene.fluid->particles_density();
    auto& ps = scene.fluid->particles_pressure();
    auto K = scene.fluid->stiffnessConstant();
    //ps = K * (rho.array() - rho0);
    ps = K * rho0 / pressure_gamma * ((rho.array()/rho0).pow(pressure_gamma) - 1.0);
    ps = ps.array().max(0);
    assert(is_finite(ps));
}

void AbstractSph::computeStandardPressureForce(const Scene& scene, const Kernel& pressureKernel){
    const auto& pos = scene.fluid->particles_position();
    const auto& ms = scene.fluid->particles_mass();
    const auto& rho = scene.fluid->particles_density();
    const auto& ps = scene.fluid->particles_pressure();
    int PN = pos.rows();
    const auto& fluid_index = scene.fluid->fluid_neighborhood->indexes();
    Coordinates2d jpos, jGrad, jPress;
    for(int i = 0; i < PN; ++i){
        const auto& index = fluid_index[i];
        minSize(jPress, index.size());
        pickRows(pos, index, jpos);
        auto xij = -(jpos.rowwise() - pos.row(i));
        pressureKernel.compute(xij, nullptr, &jGrad, nullptr);
        for(size_t j = 0; j < index.size(); ++j){
            int jj = index[j];
            jPress.row(j) = ms[jj] / rho[jj] * (ps[jj] + ps[i]) * jGrad.row(j);
        }
        // f_i^pressure = - sum_j m_j*(p_i + p_j) / (2 rho_j) \nabla W(r_i - r_j, h)
        FPressure.row(i) = (- 1.0/2.0) * jPress.colwise().sum();
    }
}

void AbstractSph::computeMomentumPreservingPressureForce(const Scene& scene, const Kernel& pressureKernel) {
    const auto& pos = scene.fluid->particles_position();
    const auto& ms = scene.fluid->particles_mass();
    const auto& rho = scene.fluid->particles_density();
    const auto& ps = scene.fluid->particles_pressure();
    assert(is_finite(pos));
    assert(is_finite(ms));
    assert(is_finite(rho));
    assert(is_finite(ps));
    int PN = pos.rows();
    const auto& fluid_index = scene.fluid->fluid_neighborhood->indexes();
#if INLINE_KERNELS
    Float h = scene.fluid->h();
    Float kernelScale = -3*pressureKernel.scale2d();
    Float epsilon = 0.000001;
#endif
    for(int i = 0; i < PN; ++i){
        const auto& index = fluid_index[i];
        int N = index.size();
        Float A = ps[i]/(rho[i] * rho[i]);
        TranslationVector vi = pos.row(i);
        TranslationVector jPress;
        jPress.setZero();
        for(int j = 0; j < N; ++j){
            int jj = index[j];
            TranslationVector xij = vi - pos.row(jj);
#if INLINE_KERNELS
            Float r1 = xij.norm();
            Float hr = h - r1;
            Float hr2 = hr * hr;
            TranslationVector jGrad = kernelScale * (xij * ( hr2 / (r1 + h*epsilon)));
#else
            TranslationVector jGrad = pressureKernel.computeGradient(xij);
#endif
            jPress += ms[jj] * (A + ps[jj] / (rho[jj] * rho[jj])) * jGrad;
        }
        // f_i^pressure = - m_i * sum_j m_j*(p_i/rho_i^2 + p_j/rho_j^2) \nabla W(r_i - r_j, h)
        FPressure.row(i) = - ms[i] * jPress;
    }
    assert(is_finite(FPressure));
}

void AbstractSph::computeStandardViscosityForce(const Scene& scene, const Kernel& viscosityKernel) {
    const auto& pos = scene.fluid->particles_position();
    const auto& ms = scene.fluid->particles_mass();
    const auto& vs = scene.fluid->particles_velocity();
    const auto mu = scene.fluid->fluid_viscosity();
    const auto& rho = scene.fluid->particles_density();
    int PN = pos.rows();
    const auto& fluid_index = scene.fluid->fluid_neighborhood->indexes();
#if INLINE_KERNELS
    Float h = scene.fluid->h();
    Float kernelScale = viscosityKernel.scale2d();
#endif
    for(int i = 0; i < PN; ++i){
        const auto& index = fluid_index[i];
        int N = index.size();
        TranslationVector vi = pos.row(i);
        TranslationVector vvi = vs.row(i);
        TranslationVector jVisc;
        jVisc.setZero();
        for(int j = 0; j < N; ++j){
            int jj = index[j];
            auto xij = vi - pos.row(jj);
#if INLINE_KERNELS
            Float jLap = kernelScale * (h - xij.norm());
#else
            Float jLap = viscosityKernel.computeLaplacian(xij);
#endif
            jVisc += ms[jj] / rho[jj] * (vs.row(jj) - vvi) * jLap;
        }
        // f_i^viscosity = mu * sum_j m_j (v_j - v_i) / rho_j \nabla^2 W(r_i - r_j, h)
        FViscosity.row(i) = mu * jVisc;
    }
}

void AbstractSph::computeStandardSurfaceTensionForce(const Scene& scene, const Kernel& kernel, Float color_relevant_normal_size) {
    const auto& pos = scene.fluid->particles_position();
    const auto& ms = scene.fluid->particles_mass();
    const auto& rho = scene.fluid->particles_density();
    auto color_sigma = scene.fluid->surface_tension();
    int PN = pos.rows();
    const auto& fluid_index = scene.fluid->fluid_neighborhood->indexes();
    for(int i = 0; i < PN; ++i){
        const auto& index = fluid_index[i];
        Coordinates2d jpos;
        Coordinates2d jGrad;
        Coordinates1d jCLap;
        Coordinates2d jColGrad(index.size(), 2);
        Coordinates1d jColLap(index.size());
        pickRows(pos, index, jpos);
        // c_s(r_i) = sum_j m_j/rho-j W(r_i - r_j, h)
        auto xij = -(jpos.rowwise() - pos.row(i));
        kernel.compute(xij, nullptr, &jGrad, &jCLap);
        for(size_t j = 0; j < index.size(); ++j){
            int jj = index[j];
            Float a = ms[jj] / rho[jj];
            jColGrad.row(j) = a * jGrad.row(j);
            jColLap[j] = a * jCLap[j];
        }
        TranslationVector colorN = jColGrad.colwise().sum();
        Float colorLap = jColLap.sum();
        Float colorNNorm = colorN.norm();
        if(colorNNorm > color_relevant_normal_size){
            Float aa = (-color_sigma * colorLap / colorNNorm);
            FSurface.row(i) = aa * colorN;
        }
    }
}

/// computes rho_i += sum_j m_j W_ij
void AbstractSph::computeFluidDensity(Scene& scene, const Kernel& densityKernel) const {
    const auto& pos = scene.fluid->particles_position();
    int PN = pos.rows();
    const auto& ms = scene.fluid->particles_mass();
    assert(ms.minCoeff() > 0.0);
    auto& rho = scene.fluid->particles_density();
    const auto& fluid_index = scene.fluid->fluid_neighborhood->indexes();
#if INLINE_KERNELS
    Float h = scene.fluid->h();
    Float kernelScale = densityKernel.scale2d();
    Float h2 = h * h;
#endif
    for(int i = 0; i < PN; ++i){
        // density from fluid<->fluid
        const auto& index = fluid_index[i];
        int N = index.size();
        TranslationVector vi = pos.row(i);
        Float jW = 0.0;
        for(int j = 0; j < N; ++j){
            int jj = index[j];
            TranslationVector xij = vi - pos.row(jj);
#if INLINE_KERNELS
            Float d = h2 - xij.squaredNorm();
            Float v = kernelScale * d*d*d;
#else
            Float v = densityKernel.computeValue(xij);
#endif
            jW += ms[jj] * v;
        }
        rho[i] = jW;
    }
    assert(rho.minCoeff() > 0.0);
    assert(is_finite(rho));
}

/// based on CFL condition: dt = lambda * h /max(sqrt(K), v_max)
void AbstractSph::limitVelocity(const Scene& scene) const {
    const Float vMax = std::sqrt(scene.fluid->stiffnessConstant());
    auto& vs = scene.fluid->particles_velocity();

    Coordinates1d vsMag2 = vs.rowwise().squaredNorm();
    Float maxV2 = vsMag2.maxCoeff();
    if(maxV2 > vMax*vMax){
        BOOST_LOG_TRIVIAL(info) << "Limiting particle speed from " << std::sqrt(maxV2) << " to " << vMax;
        for(int i = 0; i < vs.rows(); ++i){
            if(vsMag2[i] > vMax*vMax){
                vs.row(i) = vs.row(i) / std::sqrt(vsMag2[i]) * vMax;
            }
        }
        vs.array().min(vMax).max(-vMax);
    }
}

} // Physics

} // d2

} // GooBalls