/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: Erik Nelson            ( eanelson@eecs.berkeley.edu )
 *          David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

#include <camera/camera.h>
#include <camera/camera_extrinsics.h>
#include <camera/camera_intrinsics.h>
#include <geometry/eight_point_algorithm_solver.h>
#include <geometry/essential_matrix_solver.h>
#include <geometry/pose_estimator_2d3d.h>
#include <geometry/rotation.h>
#include <matching/distance_metric.h>
#include <matching/feature_matcher_options.h>
#include <matching/pairwise_image_match.h>
#include <matching/naive_matcher_2d2d.h>
#include <matching/naive_matcher_2d3d.h>
#include <math/random_generator.h>
#include <pose/pose.h>
#include <sfm/view.h>
#include <slam/landmark.h>
#include <slam/observation.h>
#include <util/types.h>

#include <Eigen/Core>
#include <gtest/gtest.h>

namespace bsfm {

using Eigen::Matrix3d;
using Eigen::Vector3d;

class TestSimpleNoiselessSfm : public ::testing::Test {
 public:
  // Clean up before and after running tests.
  TestSimpleNoiselessSfm() {
    Landmark::ResetLandmarks();
    View::ResetViews();
  }

  ~TestSimpleNoiselessSfm() {
    Landmark::ResetLandmarks();
    View::ResetViews();
  }

 protected:

  // Create a bunch of 3D points, and cameras that are able to see at least one of
  // those points. Also generate a random descriptor for each point.
  void GenerateGroundTruthPointsAndCameras() {
    points_.clear();
    descriptors_.clear();
    cameras_.clear();

    // Generate a bunch of 3D points, and then a bunch of cameras that each
    // observe some subset of the points. Run a simple incremental SfM pipeline
    // to estimate the 3D points of all cameras and images, and check that they
    // are all in the right places.
    math::RandomGenerator rng(0);

    // Generate random points in 3D, and give them each a unique descriptor.
    std::vector<Descriptor> descriptors;
    for (int ii = 0; ii < kNumPoints_; ++ii) {
      const double x = rng.DoubleUniform(-2.0, 2.0);
      const double y = rng.DoubleUniform(-2.0, 2.0);
      const double z = rng.DoubleUniform(-2.0, 2.0);
      points_.emplace_back(x, y, z);
      descriptors_.push_back(Descriptor::Random(64));
    }

    // Use the same intrinsics for all cameras.
    CameraIntrinsics intrinsics = DefaultIntrinsics();

    // Generate the first camera at identity for easy testing.
    cameras_.push_back(Camera());
    cameras_.back().SetIntrinsics(intrinsics);

    // Generate cameras such that each camera sees at least 8 points.
    while (cameras_.size() < kNumCameras_) {
      Camera camera;
      CameraExtrinsics extrinsics;
      const double x = rng.DoubleUniform(-10.0, 10.0);
      const double y = rng.DoubleUniform(-10.0, 10.0);
      const double z = rng.DoubleUniform(-10.0, 10.0);

      // Don't get too close to the 3D points.
      if ((x > -3.0 && x < 3.0) || (y > -3.0 && y < 3.0) || (z > -3.0 && z < 3.0))
        continue;

      extrinsics.SetTranslation(x, y, z);
      Vector3d euler_angles(Vector3d::Random()*D2R(20.0));
      extrinsics.Rotate(EulerAnglesToMatrix(euler_angles));
      camera.SetExtrinsics(extrinsics);
      camera.SetIntrinsics(intrinsics);

      // Make sure this camera can see at least eight 3D points.
      bool sees_at_least_eight = false;
      unsigned int count = 0;
      for (const auto& point : points_) {
        double u = 0.0, v = 0.0;
        if (camera.WorldToImage(point.X(), point.Y(), point.Z(), &u, &v)) {
          count++;
        }

        if (count == 8) {
          sees_at_least_eight = true;
          break;
        }
      }

      if (sees_at_least_eight)
        cameras_.push_back(camera);
    }

    // Set the global scale to be the distance between the first two cameras.
    global_scale_ =
        (cameras_[0].Translation() - cameras_[1].Translation()).norm();
  }

  // Simulate feature extraction from the camera at a specified index.
  void SimulateFeatureExtraction(unsigned int camera_index,
                                 std::vector<Feature>& features,
                                 std::vector<Descriptor>& descriptors) {
    features.clear();
    descriptors.clear();

    // Get this camera.
    Camera camera = cameras_[camera_index];

    // Try to project each point into the camera.
    for (size_t ii = 0; ii < points_.size(); ++ii) {
      Point3D p = points_[ii];
      double u = 0.0, v = 0.0;

      // If any points project successfully, store the projected point and its
      // corresponding descriptor as output.
      if (camera.WorldToImage(p.X(), p.Y(), p.Z(), &u, &v)) {
        features.push_back(Feature(u, v));
        descriptors.push_back(descriptors_[ii]);
      }
    }
  }

  CameraIntrinsics DefaultIntrinsics() const {
    CameraIntrinsics intrinsics;
    intrinsics.SetImageLeft(0);
    intrinsics.SetImageTop(0);
    intrinsics.SetImageWidth(kImageWidth_);
    intrinsics.SetImageHeight(kImageHeight_);
    intrinsics.SetVerticalFOV(kVerticalFov_);
    intrinsics.SetFU(intrinsics.f_v());
    intrinsics.SetCU(0.5 * kImageWidth_);
    intrinsics.SetCV(0.5 * kImageHeight_);
    intrinsics.SetK(0.0, 0.0, 0.0, 0.0, 0.0);
    return intrinsics;
  }

  int kImageWidth_ = 1920;
  int kImageHeight_ = 1080;
  int kVerticalFov_ = D2R(90.0);

  int kNumPoints_ = 100;
  int kNumCameras_ = 20;

  // The distance between the first two cameras' centers.
  double global_scale_ = 0.0;

  std::vector<Point3D> points_;
  std::vector<Descriptor> descriptors_;
  std::vector<Camera> cameras_;
};

TEST_F(TestSimpleNoiselessSfm, TestNoBundleAdjustment) {

  // Make some fake 3D points, a random descriptor for each point, and cameras
  // that are able to observe at least 1 point.
  GenerateGroundTruthPointsAndCameras();

  // Initialize using the first two cameras.
  std::vector<Feature> features1;
  std::vector<Feature> features2;
  std::vector<Descriptor> descriptors1;
  std::vector<Descriptor> descriptors2;
  SimulateFeatureExtraction(0, features1, descriptors1);
  SimulateFeatureExtraction(1, features2, descriptors2);

  // Match features between first 2 cameras.
  NaiveMatcher2D2D feature_matcher_2d2d;
  FeatureMatcherOptions matcher_options;
  matcher_options.distance_metric = "SCALED_L2";

  // Get exact feature matches so that we isolate SfM testing.
  matcher_options.min_num_feature_matches = 8;
  matcher_options.enforce_maximum_descriptor_distance = true;
  matcher_options.maximum_descriptor_distance = 1e-8;
  matcher_options.threshold_image_distance= false;
  feature_matcher_2d2d.AddImageFeatures(features1, descriptors1);
  feature_matcher_2d2d.AddImageFeatures(features2, descriptors2);
  PairwiseImageMatchList image_matches;
  ASSERT_TRUE(feature_matcher_2d2d.MatchImages(matcher_options, image_matches));
  PairwiseImageMatch image_match = image_matches[0];
  FeatureMatchList feature_matches = image_match.feature_matches_;

  // Get fundamental matrix of first 2 cameras.
  Matrix3d E, F;
  EightPointAlgorithmSolver f_solver;
  FundamentalMatrixSolverOptions solver_options;
  f_solver.SetOptions(solver_options);
  ASSERT_TRUE(f_solver.ComputeFundamentalMatrix(feature_matches, F));

  // Get essential matrix of first 2 cameras.
  CameraIntrinsics intrinsics = DefaultIntrinsics();
  EssentialMatrixSolver e_solver;
  E = e_solver.ComputeEssentialMatrix(F, intrinsics, intrinsics);

  // Get extrinsics from essential matrix.
  Pose relative_pose;
  ASSERT_TRUE(e_solver.ComputeExtrinsics(E, feature_matches, intrinsics,
                                         intrinsics, relative_pose));

  // Now we know the transformation between the first two cameras up to scale.
  Camera camera1, camera2;
  CameraExtrinsics extrinsics1, extrinsics2;
  extrinsics1.SetWorldToCamera(Pose());
  extrinsics2.SetWorldToCamera(relative_pose);
  camera1.SetExtrinsics(extrinsics1);
  camera2.SetExtrinsics(extrinsics2);
  camera1.SetIntrinsics(intrinsics);
  camera2.SetIntrinsics(intrinsics);

  // Triangulate features in 3D. This not needed in a real implementation. We
  // are doing it so that we can check if landmarks automatically triangulate
  // points.
  Point3DList initial_points;
  double uncertainty = 0.0;
  ASSERT_TRUE(Triangulate(feature_matches,
                          camera1,
                          camera2,
                          initial_points,
                          uncertainty));
  ASSERT_EQ(initial_points.size(), feature_matches.size());

  // Initialize views and landmarks.
  View::Ptr view1 = View::Create(camera1);
  View::Ptr view2 = View::Create(camera2);
  for (size_t ii = 0; ii < initial_points.size(); ++ii) {
    Landmark::Ptr landmark = Landmark::Create();

    Descriptor d1 = descriptors1[image_match.descriptor_indices1_[ii]];
    Descriptor d2 = descriptors2[image_match.descriptor_indices2_[ii]];
    ASSERT_TRUE(d1.isApprox(d2));

    // Creating observations automatically adds them to their views.
    Observation::Ptr observation1 =
        Observation::Create(view1, feature_matches[ii].feature1_, d1);
    Observation::Ptr observation2 =
        Observation::Create(view2, feature_matches[ii].feature2_, d2);

    landmark->IncorporateObservation(observation1);
    landmark->IncorporateObservation(observation2);

    // The landmark's 3D position should automatically be triangulated from the
    // observations.
    EXPECT_NEAR(initial_points[ii].X(), landmark->Position().X(), 1e-8);
    EXPECT_NEAR(initial_points[ii].Y(), landmark->Position().Y(), 1e-8);
    EXPECT_NEAR(initial_points[ii].Z(), landmark->Position().Z(), 1e-8);
  }

  // Loop over remaining cameras as if we are receiving their images
  // incrementally.
  for (int ii = 2; ii < kNumCameras_; ++ii) {
    // Get features and descriptors for the new frame.
    std::vector<Feature> features;
    std::vector<Descriptor> descriptors;
    SimulateFeatureExtraction(ii, features, descriptors);

    // Create a new view with identity camera extrinsics.
    Camera new_camera;
    new_camera.SetIntrinsics(intrinsics);
    View::Ptr new_view = View::Create(new_camera);

    // Add features and descriptors as observations to the view.
    for (size_t jj = 0; jj < features.size(); ++jj) {
      // Creating observations automatically adds them to their views.
      Observation::Create(new_view, features[jj], descriptors[jj]);
    }

    // Get a list of all existing landmarks.
    std::vector<LandmarkIndex> existing_landmarks;
    for (LandmarkIndex jj = 0; jj < Landmark::NumExistingLandmarks(); ++jj)
      existing_landmarks.push_back(jj);

    // Match features seen by this new view with existing landmarks.
    NaiveMatcher2D3D feature_matcher_2d3d;
    if (!feature_matcher_2d3d.Match(matcher_options,
                                    new_view->Index(),
                                    existing_landmarks)) {
      continue;
    }

    // Get 2D and 3D points from the match.
    FeatureList points_2d;
    Point3DList points_3d;
    for (const auto& observation : new_view->Observations()) {
      CHECK_NOTNULL(observation.get());
      if (!observation->IsMatched())
        continue;

      points_2d.push_back(observation->Feature());
      points_3d.push_back(observation->GetLandmark()->Position());
    }

    // Calculate the 3D pose of this camera.
    Pose calculated_pose;
    PoseEstimator2D3D pose_estimator;
    pose_estimator.Initialize(points_2d, points_3d, intrinsics);
    ASSERT_TRUE(pose_estimator.Solve(calculated_pose));

    // Set the extrinsic parameters of the camera in the new view.
    CameraExtrinsics extrinsics(calculated_pose);
    new_view->MutableCamera().SetExtrinsics(extrinsics);

    const Matrix3d expected_R = cameras_[ii].Rotation();
    const Matrix3d actual_R = new_view->Camera().Rotation();
    const Vector3d expected_t = cameras_[ii].Translation() / global_scale_;
    const Vector3d actual_t = new_view->Camera().Translation();

    // Did SfM work? :)
    EXPECT_TRUE(expected_R.isApprox(actual_R, 1e-8));
    EXPECT_TRUE(expected_t.isApprox(actual_t, 1e-8));
  }
}

}  //\namespace bsfm


/*
# Copyright (c) 2014-2020 Reed A. Cartwright <reed@cartwright.ht>
#
# This file is part of the Ultimate Source Code Project.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
*/

#ifndef MUTK_MEMORY_HPP
#define MUTK_MEMORY_HPP

#include <cstdint>
#include <cfloat>
#include <algorithm>
#include <initializer_list>

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <unsupported/Eigen/KroneckerProduct>
#include <unsupported/Eigen/CXX11/Tensor>

namespace mutk {

using tensor_index_t = Eigen::DenseIndex;

inline
constexpr tensor_index_t num_diploids(tensor_index_t n) {
    return n*(n+1)/2;
}

template<int N>
constexpr tensor_index_t dim_width(tensor_index_t n) {
    static_assert(N == 1 || N == 2);
    if constexpr(N == 1) {
        return n;
    } else {
        return num_diploids(n);
    }
}

template<std::size_t N>
using Tensor = Eigen::Tensor<float,N>;

template<typename Arg1, typename ...Args>
typename Eigen::array<Eigen::DenseIndex,1+sizeof...(Args)>
tensor_dims(Arg1 arg1, Args... args) {
    return {arg1,args...};
}

template<typename T, typename ...Args>
auto wrap_tensor(T *p, Args... args) {
    return Eigen::TensorMap<Eigen::Tensor<T,sizeof...(Args)>>(p, args...);
}

template<typename T, std::size_t N>
auto wrap_tensor(T *p, Eigen::array<Eigen::DenseIndex,N> arg) {
    return Eigen::TensorMap<Eigen::Tensor<T,N>>(p, arg);
}

template<typename A, typename B>
inline auto kronecker_product_coef(const A &a, const B &b, std::size_t i,
                              std::size_t j) {
    assert(i < a.rows()*b.rows() && j < a.cols()*b.cols());
    return a(i / b.rows(), j / b.cols()) * b(i % b.rows(), j % b.cols());
}

// From http://stackoverflow.com/a/16287999
template <typename T, int R, int C>
inline T sum_kahan(const Eigen::Array<T, R, C> &xs) {
    if(xs.size() == 0) {
        return 0;
    }
    T sumP(0), sumN(0), tP(0), tN(0);
    T cP(0), cN(0), yP(0), yN(0);
    for(size_t i = 0, size = xs.size(); i < size; i++) {
        T temporary = (*(xs.data() + i));
        if(temporary > 0) {
            yP = temporary - cP;
            tP = sumP + yP;
            cP = (tP - sumP) - yP;
            sumP = tP;
        } else {
            yN = temporary - cN;
            tN = sumN + yN;
            cN = (tN - sumN) - yN;
            sumN = tN;
        }
    }
    return sumP + sumN;
}

} // namespace mutk

#endif // MUTK_MEMORY_HPP


//
// Created by erik on 11/2/16.
//

#include "physics/PhysicsWorld.h"
#include <boost/test/unit_test.hpp>
#include <iostream>

using namespace spatacs::physics;

BOOST_AUTO_TEST_SUITE(Fixture_tests)
    BOOST_AUTO_TEST_CASE(Constructor)
    {
        // This test checks that the constructor initializes all values properly
        ObjectID parent{5};
        length_t radius = 0.1_m;
        Fixture f(parent, FixtureID{1}, radius);

        BOOST_CHECK_EQUAL(f.parent(), parent);
        BOOST_CHECK_EQUAL(f.radius(), radius);
        BOOST_CHECK_EQUAL(f.id(), FixtureID{1});
    }

    BOOST_AUTO_TEST_CASE(GetSet)
    {
        // This test checks that getters and setters work as expected
        ObjectID parent{5};
        length_t radius = 0.1_m;
        Fixture f(parent, FixtureID{1}, radius);

        radius = 1.0_m;
        f.setRadius(radius);
        BOOST_CHECK_EQUAL(f.radius(), radius);
        BOOST_CHECK_THROW(f.setRadius(-1.0_m), std::logic_error);

        f.setUserdata(10ul);
        BOOST_CHECK_EQUAL(f.userdata(), 10ul);

        ++parent;
        f.setParent(parent);
        BOOST_CHECK_EQUAL(f.parent(), parent);
    }

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(Object_tests)
    BOOST_AUTO_TEST_CASE(Constructor)
    {
        // This test checks that the constructor initializes all values properly
        length_vec p{5.0_m, 1.0_m, 4.8_m};
        velocity_vec v{1.0_kps, 4.5_kps, -2.5_kps};
        mass_t m = 12.5_kg;
        std::uint64_t ud = 42;

        Object o{p, v, m, ud};

        BOOST_CHECK_EQUAL(o.position(), p);
        BOOST_CHECK_EQUAL(o.velocity(), v);
        BOOST_CHECK_EQUAL(o.mass(), m);
        BOOST_CHECK_EQUAL(o.userdata(), ud);
        BOOST_CHECK(!(bool)o.id());
        BOOST_CHECK(o.begin() == o.end());
    }

    BOOST_AUTO_TEST_CASE(GetSet)
    {
        // This test checks that getters and setters work as expected
        length_vec p{5.0_m, 1.0_m, 4.8_m};
        velocity_vec v{1.0, 4.5, -2.5};
        mass_t m = 12.5_kg;
        std::uint64_t ud = 42;

        Object o;

        o.setPosition(p);
        BOOST_CHECK_EQUAL(o.position(), p);

        o.setVelocity(v);
        BOOST_CHECK_EQUAL(o.velocity(), v);

        o.setMass(m);
        BOOST_CHECK_EQUAL(o.mass(), m);

        o.setUserdata(ud);
        BOOST_CHECK_EQUAL(o.userdata(), ud);

        BOOST_CHECK_THROW(o.setID(ObjectID{}), std::logic_error);

        o.setID(ObjectID{12});
        BOOST_CHECK_EQUAL(o.id(), ObjectID{12});

        BOOST_CHECK_THROW(o.setID(ObjectID{12}), std::logic_error);
    }

    BOOST_AUTO_TEST_CASE(Fixtures)
    {
        // This test checks the functions for adding, removing and iterating fixtures
        Object o;

        auto& f1 = o.addFixture( 0.5_m );
        // check that there is a fixture
        BOOST_CHECK_EQUAL(&f1, &(*o.begin()));
        BOOST_CHECK_EQUAL(o.end() - o.begin(), 1);
        BOOST_CHECK_EQUAL(f1.radius(), 0.5_m);
        f1.setUserdata(42);
        BOOST_CHECK_EQUAL(o.begin()->userdata(), 42);

        // check that the ID is correctly updated
        BOOST_CHECK_EQUAL(f1.parent(), o.id());
        o.setID( ObjectID{5} );
        BOOST_CHECK_EQUAL(f1.parent(), o.id());

        // second fixture
        auto& f2 = o.addFixture( 0.25_m );
        BOOST_CHECK_EQUAL(o.end() - o.begin(), 2);
        BOOST_CHECK_EQUAL(f2.parent(), o.id());

        BOOST_CHECK(f1.id() != f2.id());
    }

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(PhysicsWorld_tests)
    BOOST_AUTO_TEST_CASE(Errors)
    {
        // create an empty world
        PhysicsWorld world;

        // check that we cannot get an Object that does not exist.
        BOOST_CHECK_THROW(world.getObject( ObjectID{5}), std::logic_error );
        BOOST_CHECK_NO_THROW(world.simulate(0.1_s));
    }

    BOOST_AUTO_TEST_CASE(Spawn)
    {
        PhysicsWorld world;
        Object obj;
        obj.setMass( 100.5_kg );

        auto spawned = world.spawn(obj);
        BOOST_REQUIRE(spawned);

        auto o2 = world.getObject(spawned);
        BOOST_CHECK_EQUAL( spawned, o2.id() );
        BOOST_CHECK_EQUAL( o2.mass(), obj.mass() );

        BOOST_CHECK_THROW(world.getObject( ObjectID{5}), std::logic_error );

        world.despawn(spawned);
        BOOST_CHECK_THROW(world.getObject( spawned ), std::logic_error );
    }

    BOOST_AUTO_TEST_CASE(SetMass)
    {
        PhysicsWorld world;
        Object obj;
        auto spawned = world.spawn(obj);
        world.setMass(spawned, 250.0_kg);

        BOOST_CHECK_EQUAL( world.getObject(spawned).mass(), 250.0_kg );
    }

    BOOST_AUTO_TEST_CASE(ApplyImpulse_Direct)
    {
        PhysicsWorld world;
        Object obj{length_vec{0,0,0}, velocity_vec{0,0,0}, 1.0_kg};
        auto spawned = world.spawn(obj);
        world.applyImpulse(spawned, impulse_vec{1, 0, 0});

        BOOST_CHECK_EQUAL( world.getObject(spawned).velocity(), (velocity_vec{1, 0, 0}) );

        world.simulate(1.0_s);
        BOOST_CHECK_EQUAL( world.getObject(spawned).position(), (length_vec{1, 0, 0}) );
    }

    BOOST_AUTO_TEST_CASE(CollisionCallback)
    {
        int called = 0;
        spatacs::physics::time_t ctime = 0.0_s;
        auto callback = [&](PhysicsWorld& world, const Object& A,
                            const Object& B, ImpactInfo info){ ++called; ctime = info.time; };
        PhysicsWorld world;
        world.setCollisionCallback(callback);

        Object obj{length_vec{0,0,0}, velocity_vec{0,0,0}, 1.0_kg};
        obj.addFixture(1.0_m);
        auto spawned = world.spawn(obj);
        BOOST_REQUIRE(spawned);
        world.simulate(0.1_s);
        BOOST_REQUIRE_EQUAL(called, 0);

        Object o2{length_vec{0,0,2}, velocity_vec{0,0,-2}, 1.0_kg};
        o2.addFixture(0.0_m);
        spawned = world.spawn(o2);
        BOOST_REQUIRE(spawned);
        world.simulate(1.0_s);
        BOOST_CHECK_EQUAL(called, 1);
        BOOST_CHECK_EQUAL(ctime, 0.5_s);
    }

    BOOST_AUTO_TEST_CASE(ApplyImpulse_MidStep)
    {
        PhysicsWorld world;
        Object obj{length_vec{0,0,0}, velocity_vec{1,0,0}, 1.0_kg};
        obj.addFixture(1.0_m);
        auto spawned = world.spawn(obj);
        int called = 0;
        spatacs::physics::time_t ip_time = 0.0_s;
        auto cb = [&](PhysicsWorld& world, const Object& A,
                      const Object& B, ImpactInfo info){
            ++called;
            if(called == 1) {
                world.applyImpulse(spawned, impulse_vec{1, 0, 0});
            } else
            {
                ip_time = info.time;
            }
        };
        world.setCollisionCallback(cb);

        // we cannot time event directly, so use a fake collision to do the timing
        {
            Object o1{length_vec{0, 0, 100}, velocity_vec{1, 0, 0}, 1.0_kg};
            o1.addFixture(1.0_m);
            world.spawn(o1);

            Object o2{length_vec{2.5, 0, 100}, velocity_vec{0, 0, 0}, 1.0_kg};
            o2.addFixture(1.0_m);
            world.spawn(o2);

            // these collide at .5s, this is our timer

            Object o3{length_vec{3, 0, 0}, velocity_vec{0, 0, 0}, 1.0_kg};
            o3.addFixture(1.0_m);
            world.spawn(o3);
        }

        world.simulate(1.0_s);
        BOOST_CHECK_EQUAL( world.getObject(spawned).position(), (length_vec{1.5, 0, 0}) );
        BOOST_REQUIRE_EQUAL(called, 2);
        BOOST_CHECK_EQUAL(ip_time, 0.75_s);

        /// \todo make an automatic test here, comparing a two-step simulation result to a single step simulation where
        /// the impulse is applied in the middle.
    }

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2022 - present Advanced Micro Devices, Inc. All rights reserved.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include <boost/scope_exit.hpp>
#include <gtest/gtest.h>
#include <math.h>
#include <stdexcept>
#include <utility>
#include <vector>

#include "../../shared/gpubuf.h"
#include "../rocfft_params.h"
#include "fftw_transform.h"
#include "rocfft.h"
#include "rocfft_accuracy_test.h"
#include "rocfft_against_fftw.h"

void fft_vs_reference(rocfft_params& params)
{
    switch(params.precision)
    {
    case fft_precision_single:
        fft_vs_reference_impl<float, rocfft_params>(params);
        break;
    case fft_precision_double:
        fft_vs_reference_impl<double, rocfft_params>(params);
        break;
    }
}

// Test for comparison between FFTW and rocFFT.
TEST_P(accuracy_test, vs_fftw)
{
    rocfft_params params(GetParam());

    params.validate();

    // Test that the tokenization works as expected.
    auto       token = params.token();
    fft_params tokentest;
    tokentest.from_token(token);
    auto token1 = tokentest.token();
    EXPECT_TRUE(token == token1);

    if(!params.valid(verbose))
    {
        if(verbose)
        {
            std::cout << "Invalid parameters, skip this test." << std::endl;
        }
        GTEST_SKIP();
    }

    fft_vs_reference(params);
    SUCCEED();
}


// These headers are deprecated.
//
// #include <boost/math/common_factor_ct.hpp>
// #include <boost/math/common_factor.hpp>
// #include <boost/math/common_factor_rt.hpp>

#include <boost/cstdfloat.hpp>
#include <boost/math/complex.hpp>
#include <boost/math/distributions.hpp>
#include <boost/math_fwd.hpp>
#include <boost/math/octonion.hpp>
#include <boost/math/quaternion.hpp>
#include <boost/math/special_functions.hpp>

// This header is used by the other drivers.
//
// #include <boost/math/tr1.hpp>

int
main ()
{
  return boost::math::prime (0) == 2 && boost::math::prime (9) == 29 ? 0 : 1;
}


#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

#include "Evaluation.h"
#include "Serialize.h"

using namespace klee;

namespace s2e {
namespace plugins {

#define DEBUG_CAPSUMMARY

// Capability related
void genSymbolValue(S2EExecutionState *state, Capability &cap, ref<Expr> &expr,
                    SymbolValue &value) {
    // length and offset should not more that 2048
    // value is a single byte
    auto pair =
        findMinMax(state, cap.constraints, expr, cap.assignment, 0, 2048);
    ref<Expr> res = cap.assignment->evaluate(expr);
    value.min = pair.first;
    value.max = pair.second;
    if (ConstantExpr *CE = dyn_cast<ConstantExpr>(res)) {
        value.value = CE->getZExtValue();
    }
}

void genCapSummary(S2EExecutionState *state, Capability &cap,
                   std::map<uint64_t, Summary> &summaries) {
    std::map<uint64_t, unsigned> indices;
    for (auto &op : cap.ops) {
        Summary sum;
        if (cap.loopguards.find(op.pc) != cap.loopguards.end()) {
            if (indices.find(op.pc) != indices.end()) {
                continue;
            }
            indices[op.pc] = 0;
            ref<Expr> offset =
                E_SUB(op.dst, E_CONST(cap.vuln.vul_base, Expr::Int64));
            ref<Expr> EEC = cap.guardtable[cap.loopguards[op.pc]]->expcount;
            ref<Expr> length = MulExpr::create(EEC, op.len);
            genSymbolValue(state, cap, offset, sum.offset);
            genSymbolValue(state, cap, length, sum.length);
            summaries[op.pc] = sum;
            // omit value for loops
            continue;
        }

        ref<Expr> offset =
            E_SUB(op.dst, E_CONST(cap.vuln.vul_base, Expr::Int64));
        genSymbolValue(state, cap, offset, sum.offset);
        genSymbolValue(state, cap, op.len, sum.length);
        uint64_t concrete_len = readExpr<uint64_t>(cap.assignment, op.len);
        for (uint64_t i = 0; i < concrete_len && i < 2048; i++) {
            ref<Expr> byte = E_EXTR(op.payload, i * CHAR_BIT, Expr::Int8);
            SymbolValue tmp;
            genSymbolValue(state, cap, byte, tmp);
            sum.values.push_back(tmp);
        }
        summaries[op.pc] = sum;
    }
#ifdef DEBUG_CAPSUMMARY
    std::stringstream ss;
    for (auto it : summaries) {
        ss << "PC: " << hexval(it.first) << "\n";
        it.second.print(ss);
    }
    g_s2e->getDebugStream() << ss.str();
#endif
}

#define UNKNOWN 3
int compareSymbolValue(SymbolValue &a, SymbolValue &b) {
    if (a.min == b.min && a.max == b.max) {
        return 1;
    }
    if (a.min >= b.min && a.max <= b.max) {
        return 0;
    }
    if (a.min <= b.min && a.max >= b.max) {
        return 2;
    }

    return UNKNOWN;
}
/*
 *	Return:
 *  -1: a is less than or equal to b
 * 1:   a is larger than b
 * 0: cannot compare
 */
int compareSummary(Summary &a, Summary &b) {
    // a < b
    if (compareSymbolValue(a.offset, b.offset) <= 1 &&
        compareSymbolValue(a.length, b.length) <= 1) {
        if (a.values.size() <= b.values.size()) {
            bool found = false;
            for (unsigned i = 0; i < a.values.size(); i++) {
                if (compareSymbolValue(a.values[i], b.values[i]) > 1) {
                    found = true;
                    break;
                }
            }
            if (!found) {
                // special case
                if (a.length.value > b.length.value) {
                    return 1;
                }
                return -1;
            }
        }
        return 0;
    }
    // a > b
    if (compareSymbolValue(a.offset, b.offset) >= 1 &&
        compareSymbolValue(a.length, b.length) >= 1) {
        if (a.values.size() >= b.values.size()) {
            bool found = false;
            for (unsigned i = 0; i < b.values.size(); i++) {
                if (compareSymbolValue(a.values[i], b.values[i]) <= 1) {
                    found = true;
                    break;
                }
            }
            if (!found) {
                return 1;
            }
        }
        return 0;
    }
    return 0;
}

bool isNewCapability(S2EExecutionState *state, Capability &cap, std::string workdir) {
    // Load database
    std::string database = workdir + "/database";
    CapSummary caps;
    std::map<uint64_t, Spot> spots;
    std::map<uint64_t, Summary> new_cap;
    {
        if (fileExists(database)) {
            std::ifstream ifs(database, std::ifstream::binary);
            deserialize(ifs, caps, spots);
        }
    }

    genCapSummary(state, cap, new_cap);
    bool isNew = false;
    for (auto &it : caps) {
        auto pos = new_cap.find(it.first);
        if (pos == new_cap.end()) {
            continue;
        }
        Summary new_sum = new_cap[it.first];
        bool insert = true;
        for (auto itt = it.second.begin(); itt != it.second.end();) {
            int res = compareSummary(new_sum, *itt);
            if (res < 0) {
                insert = false;
                break;
            } else if (res > 0) {
                it.second.erase(itt);
            } else {
                ++itt;
            }
        }
        if (insert) {
            isNew = true;
            it.second.push_back(new_sum);
        }
        new_cap.erase(pos);
    }

    // New vulnerability points
    for (auto it : new_cap) {
        std::vector<Summary> summaries;
        summaries.push_back(it.second);
        caps.insert({it.first, summaries});
        isNew = true;
    }

    for (auto it : cap.spots) {
        if (spots.find(it.first) == spots.end()) {
            spots.insert({it.first, it.second});
        }
    }

    {
        std::ofstream ofs(database, std::ofstream::binary);
        serialize(ofs, caps, spots);
        // save all vuln points
        // std::stringstream ss;
        // ss << "{\"Addrs\": [";
        // foreach2(it, spots.begin(), spots.end()) {
        //     if (it != spots.begin()) {
        //         ss << ", ";
        //     }
        //     ss << std::to_string(it->first);
        // }
        // ss << "]}";
        // std::ofstream vulns("addrs.json");
        // vulns.write(ss.str().c_str(), ss.str().size());
    }
    return isNew;
}

// save and restore state
bool saveState(S2EExecutionState *state, Capability &cap,
               ConstraintManager &manager, std::vector<OOB_Operation> &ops,
               std::map<uint64_t, uint64_t> &loopguards, GuardTable &guardtable,
               AllocObj vul_obj, uint64_t base_addr,
               std::map<uint64_t, Spot> &spots, std::string allocator,
               std::string filepath, std::string workdir) {

    cap.constraints = manager;
    selectSymbols(state, cap.symbols);
    cap.assignment = state->concolics;
    cap.ops = ops;
    cap.loopguards = loopguards;
    cap.guardtable = guardtable;
    cap.spots = spots;

    cap.vuln.symbolic = vul_obj.tag == AllocObj::Tag::SYMBOLIC;
    cap.vuln.allocator = allocator;
    cap.vuln.vul_base = base_addr;
    cap.vuln.width = vul_obj.width;
    cap.vuln.sym_width = vul_obj.sym_width;

    for (auto & it : cap.ops) {
        it.dst = rebuildExpr(it.dst);
    }

    if (!isNewCapability(state, cap, workdir)) {
        g_s2e->getDebugStream() << "The capability is duplicate!\n";
        return true;
    }

    {
        std::ofstream ofs(filepath, std::ofstream::binary);
        g_s2e->getDebugStream() << "start to serialize\n";
        serialize_init();
        serialize(ofs, cap);
    }

    {
        // std::ifstream ifs(filepath, std::ifstream::binary);
        // // ref<Expr> expr;
        // // deserialize(ifs, expr);
        // Capability temp;
        // deserialize(ifs, temp);
        // for (auto expr : temp.constraints) {
        // 	g_s2e->getDebugStream(state) << "Restore: " << expr << "\n";
        // }
    }
    return true;
}

bool restoreState(Capability &cap, std::string filepath) {
    std::ifstream ifs(filepath, std::ifstream::binary);
    deserialize_init();
    deserialize(ifs, cap);
    return true;
}

}
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Pavel Kharitonov <ipavrus@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE tiger_test

#include <nil/crypto3/hash/algorithm/hash.hpp>
#include <nil/crypto3/hash/adaptor/hashed.hpp>

#include <nil/crypto3/hash/tiger.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <boost/static_assert.hpp>

#include <iostream>
#include <string>
#include <unordered_map>

#include <cstdio>

using namespace nil::crypto3;
using namespace nil::crypto3::accumulators;

namespace boost {
    namespace test_tools {
        namespace tt_detail {
            template<template<typename, typename> class P, typename K, typename V>
            struct print_log_value<P<K, V>> {
                void operator()(std::ostream &, P<K, V> const &) {
                }
            };
        }    // namespace tt_detail
    }        // namespace test_tools
}    // namespace boost

BOOST_TEST_DONT_PRINT_LOG_VALUE(hashes::tiger<192>::digest_type)

template<std::size_t Size>
class fixture {
public:
    accumulator_set<hashes::tiger<Size>> acc;
    typedef hashes::tiger<Size> hash_t;
    virtual ~fixture() {
    }
};

const char *test_data = TEST_DATA;

boost::property_tree::ptree string_data(const char *child_name) {
    boost::property_tree::ptree root_data;
    boost::property_tree::read_json(test_data, root_data);
    boost::property_tree::ptree string_data = root_data.get_child(child_name);

    return string_data;
}

BOOST_AUTO_TEST_SUITE(tiger_test_suite1)

BOOST_DATA_TEST_CASE(tiger_algorithm_test, string_data("data_192"), array_element) {
    std::string out = hash<hashes::tiger<192>>(array_element.first);

    BOOST_CHECK_EQUAL(out, array_element.second.data());
}

BOOST_DATA_TEST_CASE(tiger_adaptor_test, string_data("data_192"), array_element) {
    std::string out = array_element.first | adaptors::hashed<hashes::tiger<192>>;

    BOOST_CHECK_EQUAL(out, array_element.second.data());
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(tiger_test_suite2)

BOOST_AUTO_TEST_CASE(tiger_iterator_hash1) {

    std::string input = "a";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("77befbef2e7ef8ab2ec8f93bf587a7fc613e247f5f247809", out);
}

BOOST_AUTO_TEST_CASE(tiger_iterator_hash2) {

    std::string input = "abc";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("2aab1484e8c158f2bfb8c5ff41b57a525129131c957b5f93", out);
}

BOOST_AUTO_TEST_CASE(tiger_iterator_hash3) {

    std::string input = "message digest";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("d981f8cb78201a950dcf3048751e441c517fca1aa55a29f6", out);
}

BOOST_AUTO_TEST_CASE(tiger_iterator_hash4) {

    std::string input = "abcdefghijklmnopqrstuvwxyz";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("1714a472eee57d30040412bfcc55032a0b11602ff37beee9", out);
}

BOOST_AUTO_TEST_CASE(tiger_iterator_hash5) {

    std::string input = "abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("0f7bf9a19b9c58f2b7610df7e84f0ac3a71c631e7b53f78e", out);
}

BOOST_AUTO_TEST_CASE(tiger_iterator_hash6) {

    std::string input = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("8dcea680a17583ee502ba38a3c368651890ffbccdc49a8cc", out);
}

BOOST_AUTO_TEST_CASE(tiger_iterator_hash7) {

    std::string input = "12345678901234567890123456789012345678901234567890123456789012345678901234567890";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("1c14795529fd9f207a958f84c52f11e887fa0cabdfd91bfd", out);
}

BOOST_AUTO_TEST_SUITE_END()


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file NoiseModel.cpp
 * @date Jan 13, 2010
 * @author Richard Roberts
 * @author Frank Dellaert
 */

#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/timing.h>

#include <boost/format.hpp>
#include <boost/make_shared.hpp>

#include <cmath>
#include <iostream>
#include <limits>
#include <stdexcept>
#include <typeinfo>

using namespace std;

namespace gtsam {
namespace noiseModel {

/* ************************************************************************* */
// update A, b
// A' \define A_{S}-ar and b'\define b-ad
// Linear algebra: takes away projection on latest orthogonal
// Graph: make a new factor on the separator S
// __attribute__ ((noinline))  // uncomment to prevent inlining when profiling
template<class MATRIX>
void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
  size_t n = Ab.cols()-1;
  Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose();
}

/* ************************************************************************* */
// check *above the diagonal* for non-zero entries
boost::optional<Vector> checkIfDiagonal(const Matrix M) {
  size_t m = M.rows(), n = M.cols();
  // check all non-diagonal entries
  bool full = false;
  size_t i, j;
  for (i = 0; i < m; i++)
    if (!full)
      for (j = i + 1; j < n; j++)
        if (std::abs(M(i, j)) > 1e-9) {
          full = true;
          break;
        }
  if (full) {
    return boost::none;
  } else {
    Vector diagonal(n);
    for (j = 0; j < n; j++)
      diagonal(j) = M(j, j);
    return diagonal;
  }
}

/* ************************************************************************* */
Vector Base::sigmas() const {
  throw("Base::sigmas: sigmas() not implemented for this noise model");
}

/* ************************************************************************* */
double Base::squaredMahalanobisDistance(const Vector& v) const {
  // Note: for Diagonal, which does ediv_, will be correct for constraints
  Vector w = whiten(v);
  return w.dot(w);
}

/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
  size_t m = R.rows(), n = R.cols();
  if (m != n)
    throw invalid_argument("Gaussian::SqrtInformation: R not square");
  if (smart) {
    boost::optional<Vector> diagonal = checkIfDiagonal(R);
    if (diagonal)
      return Diagonal::Sigmas(diagonal->array().inverse(), true);
  }
  // NOTE(frank): only reaches here if !(smart && diagonal)
  return shared_ptr(new Gaussian(R.rows(), R));
}

/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart) {
  size_t m = information.rows(), n = information.cols();
  if (m != n)
    throw invalid_argument("Gaussian::Information: R not square");
  boost::optional<Vector> diagonal = boost::none;
  if (smart)
    diagonal = checkIfDiagonal(information);
  if (diagonal)
    return Diagonal::Precisions(*diagonal, true);
  else {
    Eigen::LLT<Matrix> llt(information);
    Matrix R = llt.matrixU();
    return shared_ptr(new Gaussian(n, R));
  }
}

/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
    bool smart) {
  size_t m = covariance.rows(), n = covariance.cols();
  if (m != n)
    throw invalid_argument("Gaussian::Covariance: covariance not square");
  boost::optional<Vector> variances = boost::none;
  if (smart)
    variances = checkIfDiagonal(covariance);
  if (variances)
    return Diagonal::Variances(*variances, true);
  else {
    // NOTE: if cov = L'*L, then the square root information R can be found by
    // QR, as L.inverse() = Q*R, with Q some rotation matrix. However, R has
    // annoying sign flips with respect the simpler Information(inv(cov)),
    // hence we choose the simpler path here:
    return Information(covariance.inverse(), false);
  }
}

/* ************************************************************************* */
void Gaussian::print(const string& name) const {
  gtsam::print(thisR(), name + "Gaussian");
}

/* ************************************************************************* */
bool Gaussian::equals(const Base& expected, double tol) const {
  const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
  if (p == nullptr) return false;
  if (typeid(*this) != typeid(*p)) return false;
  return equal_with_abs_tol(R(), p->R(), sqrt(tol));
}

/* ************************************************************************* */
Matrix Gaussian::covariance() const {
  // Uses a fast version of `covariance = information().inverse();`
  const Matrix& R = this->R();
  Matrix I = Matrix::Identity(R.rows(), R.cols());
  // Fast inverse of upper-triangular matrix R using forward-substitution
  Matrix Rinv = R.triangularView<Eigen::Upper>().solve(I);
  // (R' * R)^{-1} = R^{-1} * R^{-1}'
  return Rinv * Rinv.transpose();
}

/* ************************************************************************* */
Vector Gaussian::sigmas() const {
  return Vector(covariance().diagonal()).cwiseSqrt();
}

/* ************************************************************************* */
Vector Gaussian::whiten(const Vector& v) const {
  return thisR() * v;
}

/* ************************************************************************* */
Vector Gaussian::unwhiten(const Vector& v) const {
  return backSubstituteUpper(thisR(), v);
}

/* ************************************************************************* */
Matrix Gaussian::Whiten(const Matrix& H) const {
  return thisR() * H;
}

/* ************************************************************************* */
void Gaussian::WhitenInPlace(Matrix& H) const {
  H = thisR() * H;
}

/* ************************************************************************* */
void Gaussian::WhitenInPlace(Eigen::Block<Matrix> H) const {
  H = thisR() * H;
}

/* ************************************************************************* */
// General QR, see also special version in Constrained
SharedDiagonal Gaussian::QR(Matrix& Ab) const {

  gttic(Gaussian_noise_model_QR);

  static const bool debug = false;

  // get size(A) and maxRank
  // TODO: really no rank problems ?
   size_t m = Ab.rows(), n = Ab.cols()-1;
   size_t maxRank = min(m,n);

  // pre-whiten everything (cheaply if possible)
  WhitenInPlace(Ab);

  if(debug) gtsam::print(Ab, "Whitened Ab: ");

  // Eigen QR - much faster than older householder approach
  inplace_QR(Ab);
  Ab.triangularView<Eigen::StrictlyLower>().setZero();

  // hand-coded householder implementation
  // TODO: necessary to isolate last column?
  // householder(Ab, maxRank);

  return noiseModel::Unit::Create(maxRank);
}

void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const {
  for(Matrix& Aj: A) { WhitenInPlace(Aj); }
  whitenInPlace(b);
}

void Gaussian::WhitenSystem(Matrix& A, Vector& b) const {
  WhitenInPlace(A);
  whitenInPlace(b);
}

void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const {
  WhitenInPlace(A1);
  WhitenInPlace(A2);
  whitenInPlace(b);
}

void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
  WhitenInPlace(A1);
  WhitenInPlace(A2);
  WhitenInPlace(A3);
  whitenInPlace(b);
}

/* ************************************************************************* */
// Diagonal
/* ************************************************************************* */
Diagonal::Diagonal() :
    Gaussian(1) // TODO: Frank asks: really sure about this?
{
}

/* ************************************************************************* */
Diagonal::Diagonal(const Vector& sigmas)
    : Gaussian(sigmas.size()),
      sigmas_(sigmas),
      invsigmas_(sigmas.array().inverse()),
      precisions_(invsigmas_.array().square()) {
}

/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
  if (smart) {
    // check whether all the same entry
    size_t n = variances.size();
    for (size_t j = 1; j < n; j++)
      if (variances(j) != variances(0)) goto full;
    return Isotropic::Variance(n, variances(0), true);
  }
  full: return shared_ptr(new Diagonal(variances.cwiseSqrt()));
}

/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
  if (smart) {
    size_t n = sigmas.size();
    if (n==0) goto full;
    // look for zeros to make a constraint
    for (size_t j=0; j< n; ++j)
      if (sigmas(j)<1e-8)
        return Constrained::MixedSigmas(sigmas);
    // check whether all the same entry
    for (size_t j = 1; j < n; j++)
      if (sigmas(j) != sigmas(0)) goto full;
    return Isotropic::Sigma(n, sigmas(0), true);
  }
  full: return Diagonal::shared_ptr(new Diagonal(sigmas));
}

/* ************************************************************************* */
void Diagonal::print(const string& name) const {
  gtsam::print(sigmas_, name + "diagonal sigmas");
}

/* ************************************************************************* */
Vector Diagonal::whiten(const Vector& v) const {
  return v.cwiseProduct(invsigmas_);
}

/* ************************************************************************* */
Vector Diagonal::unwhiten(const Vector& v) const {
  return v.cwiseProduct(sigmas_);
}

/* ************************************************************************* */
Matrix Diagonal::Whiten(const Matrix& H) const {
  return vector_scale(invsigmas(), H);
}

/* ************************************************************************* */
void Diagonal::WhitenInPlace(Matrix& H) const {
  vector_scale_inplace(invsigmas(), H);
}

/* ************************************************************************* */
void Diagonal::WhitenInPlace(Eigen::Block<Matrix> H) const {
  H = invsigmas().asDiagonal() * H;
}

/* ************************************************************************* */
// Constrained
/* ************************************************************************* */

namespace internal {
// switch precisions and invsigmas to finite value
// TODO: why?? And, why not just ask s==0.0 below ?
static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) {
  for (Vector::Index i = 0; i < sigmas.size(); ++i)
    if (!std::isfinite(1. / sigmas[i])) {
      precisions[i] = 0.0;
      invsigmas[i] = 0.0;
    }
}
}

/* ************************************************************************* */
Constrained::Constrained(const Vector& sigmas)
  : Diagonal(sigmas), mu_(Vector::Constant(sigmas.size(), 1000.0)) {
  internal::fix(sigmas, precisions_, invsigmas_);
}

/* ************************************************************************* */
Constrained::Constrained(const Vector& mu, const Vector& sigmas)
  : Diagonal(sigmas), mu_(mu) {
  internal::fix(sigmas, precisions_, invsigmas_);
}

/* ************************************************************************* */
Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu,
    const Vector& sigmas) {
  return shared_ptr(new Constrained(mu, sigmas));
}

/* ************************************************************************* */
bool Constrained::constrained(size_t i) const {
  // TODO why not just check sigmas_[i]==0.0 ?
  return !std::isfinite(1./sigmas_[i]);
}

/* ************************************************************************* */
void Constrained::print(const std::string& name) const {
  gtsam::print(sigmas_, name + "constrained sigmas");
  gtsam::print(mu_, name + "constrained mu");
}

/* ************************************************************************* */
Vector Constrained::whiten(const Vector& v) const {
  // If sigmas[i] is not 0 then divide v[i] by sigmas[i], as usually done in
  // other normal Gaussian noise model. Otherwise, sigmas[i] = 0 indicating
  // a hard constraint, we don't do anything.
  const Vector& a = v;
  const Vector& b = sigmas_;
  size_t n = a.size();
  assert (b.size()==a.size());
  Vector c(n);
  for( size_t i = 0; i < n; i++ ) {
    const double& ai = a(i), bi = b(i);
    c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_()
  }
  return c;
}

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/* ************************************************************************* */
double Constrained::error(const Vector& v) const {
  Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
  for (size_t i=0; i<dim_; ++i)  // add mu weights on constrained variables
    if (constrained(i)) // whiten makes constrained variables zero
      w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
  return 0.5 * w.dot(w);
}
#endif

/* ************************************************************************* */
double Constrained::squaredMahalanobisDistance(const Vector& v) const {
  Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
  for (size_t i=0; i<dim_; ++i)  // add mu weights on constrained variables
    if (constrained(i)) // whiten makes constrained variables zero
      w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
  return w.dot(w);
}

/* ************************************************************************* */
Matrix Constrained::Whiten(const Matrix& H) const {
  Matrix A = H;
  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
    if (!constrained(i)) // if constrained, leave row of A as is
      A.row(i) *= invsigmas_(i);
  return A;
}

/* ************************************************************************* */
void Constrained::WhitenInPlace(Matrix& H) const {
  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
    if (!constrained(i)) // if constrained, leave row of H as is
      H.row(i) *= invsigmas_(i);
}

/* ************************************************************************* */
void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
    if (!constrained(i)) // if constrained, leave row of H as is
      H.row(i) *= invsigmas_(i);
}

/* ************************************************************************* */
Constrained::shared_ptr Constrained::unit() const {
  Vector sigmas = Vector::Ones(dim());
  for (size_t i=0; i<dim(); ++i)
    if (constrained(i))
      sigmas(i) = 0.0;
  return MixedSigmas(mu_, sigmas);
}

/* ************************************************************************* */
// Special version of QR for Constrained calls slower but smarter code
// that deals with possibly zero sigmas
// It is Gram-Schmidt orthogonalization rather than Householder

// Check whether column a triggers a constraint and corresponding variable is deterministic
// Return constraint_row with maximum element in case variable plays in multiple constraints
template <typename VECTOR>
boost::optional<size_t> check_if_constraint(VECTOR a, const Vector& invsigmas, size_t m) {
  boost::optional<size_t> constraint_row;
  // not zero, so roundoff errors will not be counted
  // TODO(frank): that's a fairly crude way of dealing with roundoff errors :-(
  double max_element = 1e-9;
  for (size_t i = 0; i < m; i++) {
    if (!std::isinf(invsigmas[i]))
      continue;
    double abs_ai = std::abs(a(i,0));
    if (abs_ai > max_element) {
      max_element = abs_ai;
      constraint_row.reset(i);
    }
  }
  return constraint_row;
}

SharedDiagonal Constrained::QR(Matrix& Ab) const {
  static const double kInfinity = std::numeric_limits<double>::infinity();

  // get size(A) and maxRank
  size_t m = Ab.rows();
  const size_t n = Ab.cols() - 1;
  const size_t maxRank = min(m, n);

  // create storage for [R d]
  typedef boost::tuple<size_t, Matrix, double> Triple;
  list<Triple> Rd;

  Matrix rd(1, n + 1);  // and for row of R
  Vector invsigmas = sigmas_.array().inverse();
  Vector weights = invsigmas.array().square();  // calculate weights once

  // We loop over all columns, because the columns that can be eliminated
  // are not necessarily contiguous. For each one, estimate the corresponding
  // scalar variable x as d-rS, with S the separator (remaining columns).
  // Then update A and b by substituting x with d-rS, zero-ing out x's column.
  for (size_t j = 0; j < n; ++j) {
    // extract the first column of A
    Eigen::Block<Matrix> a = Ab.block(0, j, m, 1);

    // Check whether we need to handle as a constraint
    boost::optional<size_t> constraint_row = check_if_constraint(a, invsigmas, m);

    if (constraint_row) {
      // Handle this as a constraint, as the i^th row has zero sigma with non-zero entry A(i,j)

      // In this case, the row in [R|d] is simply the row in [A|b]
      // NOTE(frank): we used to divide by a[i] but there is no need with a constraint
      rd = Ab.row(*constraint_row);

      // Construct solution (r, d, sigma)
      Rd.push_back(boost::make_tuple(j, rd, kInfinity));

      // exit after rank exhausted
      if (Rd.size() >= maxRank)
        break;

      // The constraint row will be zeroed out, so we can save work by swapping in the
      // last valid row and decreasing m. This will save work on subsequent down-dates, too.
      m -= 1;
      if (*constraint_row != m) {
        Ab.row(*constraint_row) = Ab.row(m);
        weights(*constraint_row) = weights(m);
        invsigmas(*constraint_row) = invsigmas(m);
      }

      // get a reduced a-column which is now shorter
      Eigen::Block<Matrix> a_reduced = Ab.block(0, j, m, 1);
      a_reduced *= (1.0/rd(0, j)); // NOTE(frank): this is the 1/a[i] = 1/rd(0,j) factor we need!

      // Rank-1 down-date of Ab, expensive, using outer product
      Ab.block(0, j + 1, m, n - j).noalias() -= a_reduced * rd.middleCols(j + 1, n - j);
    } else {
      // Treat in normal Gram-Schmidt way
      // Calculate weighted pseudo-inverse and corresponding precision

      // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma)
      // For diagonal Sigma, inv(Sigma) = diag(precisions)
      double precision = 0;
      Vector pseudo(m);     // allocate storage for pseudo-inverse
      for (size_t i = 0; i < m; i++) {
        double ai = a(i, 0);
        if (std::abs(ai) > 1e-9) {  // also catches remaining sigma==0 rows
          pseudo[i] = weights[i] * ai;
          precision += pseudo[i] * ai;
        } else
          pseudo[i] = 0;
      }

      if (precision > 1e-8) {
        pseudo /= precision;

        // create solution [r d], rhs is automatically r(n)
        rd(0, j) = 1.0;  // put 1 on diagonal
        rd.block(0, j + 1, 1, n - j) = pseudo.transpose() * Ab.block(0, j + 1, m, n - j);

        // construct solution (r, d, sigma)
        Rd.push_back(boost::make_tuple(j, rd, precision));
      } else {
        // If precision is zero, no information on this column
        // This is actually not limited to constraints, could happen in Gaussian::QR
        // In that case, we're probably hosed. TODO: make sure Householder is rank-revealing
        continue;  // but even if not, no need to update if a==zeros
      }

      // exit after rank exhausted
      if (Rd.size() >= maxRank)
        break;

      // Rank-1 down-date of Ab, expensive, using outer product
      Ab.block(0, j + 1, m, n - j).noalias() -= a * rd.middleCols(j + 1, n - j);
    }
  }

  // Create storage for precisions
  Vector precisions(Rd.size());

  // Write back result in Ab, imperative as we are
  size_t i = 0;  // start with first row
  bool mixed = false;
  Ab.setZero();  // make sure we don't look below
  for (const Triple& t: Rd) {
    const size_t& j = t.get<0>();
    const Matrix& rd = t.get<1>();
    precisions(i) = t.get<2>();
    if (std::isinf(precisions(i)))
      mixed = true;
    Ab.block(i, j, 1, n + 1 - j) = rd.block(0, j, 1, n + 1 - j);
    i += 1;
  }

  // Must include mu, as the defaults might be higher, resulting in non-convergence
  return mixed ? Constrained::MixedPrecisions(mu_, precisions) : Diagonal::Precisions(precisions);
}

/* ************************************************************************* */
// Isotropic
/* ************************************************************************* */
Isotropic::shared_ptr Isotropic::Sigma(size_t dim, double sigma, bool smart)  {
  if (smart && std::abs(sigma-1.0)<1e-9) return Unit::Create(dim);
  return shared_ptr(new Isotropic(dim, sigma));
}

/* ************************************************************************* */
Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart)  {
  if (smart && std::abs(variance-1.0)<1e-9) return Unit::Create(dim);
  return shared_ptr(new Isotropic(dim, sqrt(variance)));
}

/* ************************************************************************* */
void Isotropic::print(const string& name) const {
  cout << boost::format("isotropic dim=%1% sigma=%2%") % dim() % sigma_ << endl;
}

/* ************************************************************************* */
double Isotropic::squaredMahalanobisDistance(const Vector& v) const {
  return v.dot(v) * invsigma_ * invsigma_;
}

/* ************************************************************************* */
Vector Isotropic::whiten(const Vector& v) const {
  return v * invsigma_;
}

/* ************************************************************************* */
Vector Isotropic::unwhiten(const Vector& v) const {
  return v * sigma_;
}

/* ************************************************************************* */
Matrix Isotropic::Whiten(const Matrix& H) const {
  return invsigma_ * H;
}

/* ************************************************************************* */
void Isotropic::WhitenInPlace(Matrix& H) const {
  H *= invsigma_;
}

/* ************************************************************************* */
void Isotropic::whitenInPlace(Vector& v) const {
  v *= invsigma_;
}

/* ************************************************************************* */
void Isotropic::WhitenInPlace(Eigen::Block<Matrix> H) const {
  H *= invsigma_;
}

/* ************************************************************************* */
// Unit
/* ************************************************************************* */
void Unit::print(const std::string& name) const {
  cout << name << "unit (" << dim_ << ") " << endl;
}

/* ************************************************************************* */
// Robust
/* ************************************************************************* */

void Robust::print(const std::string& name) const {
  robust_->print(name);
  noise_->print(name);
}

bool Robust::equals(const Base& expected, double tol) const {
  const Robust* p = dynamic_cast<const Robust*> (&expected);
  if (p == nullptr) return false;
  return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
}

void Robust::WhitenSystem(Vector& b) const {
  noise_->whitenInPlace(b);
  robust_->reweight(b);
}

void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const {
  noise_->WhitenSystem(A,b);
  robust_->reweight(A,b);
}

void Robust::WhitenSystem(Matrix& A, Vector& b) const {
  noise_->WhitenSystem(A,b);
  robust_->reweight(A,b);
}

void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const {
  noise_->WhitenSystem(A1,A2,b);
  robust_->reweight(A1,A2,b);
}

void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
  noise_->WhitenSystem(A1,A2,A3,b);
  robust_->reweight(A1,A2,A3,b);
}

Robust::shared_ptr Robust::Create(
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
  return shared_ptr(new Robust(robust,noise));
}

/* ************************************************************************* */

}
} // gtsam


#include "syntax_tree_evaluation.h"
#include "syntax_descriptors.h"
#include "unit_assert.h"
#include "type_primitive.h"
#include "string_utils.h"
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>
using namespace std;
using namespace compiler;
using namespace compiler::syntax;

template<typename T>
object_t eval_binary_common(const string &op, const object_t &lhs, const object_t &rhs)
{
	T l = lhs.cast<T>(), r = rhs.cast<T>();
	if (op == "*")
		return l * r;
	else if (op == "+")
		return l + r;
	else if (op == "-")
		return l - r;
	else if (op == "/")
		return l / r;
	else if (op == "==")
		return l == r;
	else if (op == "!=")
		return l != r;
	else if (op == ">")
		return l > r;
	else if (op == "<")
		return l < r;
	else if (op == "<=")
		return l <= r;
	else if (op == ">=")
		return l >= r;
	else
		return object_t();
}

template<typename T>
object_t eval_binary_integral(const string &op, const object_t &lhs, const object_t &rhs)
{
	T l = lhs.cast<T>(), r = rhs.cast<T>();
	auto last_res = eval_binary_common<T>(op, lhs, rhs);
	if (!last_res.empty())
		return last_res;
	else if (op == "<<")
		return l << r;
	else if (op == ">>")
		return l >> r;
	else if (op == "&&")
		return l && r;
	else if (op == "||")
		return l || r;
	else if (op == "^")
		return l ^ r;
	else if (op == "|")
		return l | r;
	else if (op == "&")
		return l & r;
	else if (op == "%")
		return l % r;
	else
		return object_t();
}

object_t eval_binary_bool(const string &op, const object_t &lhs, const object_t &rhs)
{
	bool l = lhs.cast<bool>(), r = rhs.cast<bool>();
	if (op == "&&")
		return l && r;
	else if (op == "||")
		return l || r;
	else if (op == "^")
		return l ^ r;
	else if (op == "|")
		return l | r;
	else if (op == "&")
		return l & r;
	else
		return object_t();
}

template<typename T>
object_t build_unary_common(const string &op, const object_t &obj)
{
	T val = obj.cast<T>();
	if (op == "-")
		return -val;
	else
		return object_t();
}

template<typename T>
object_t build_unary_integral(const string &op, const object_t &obj)
{
	T val = obj.cast<T>();
	auto res = build_unary_common<T>(op, obj);
	if (!res.empty())
		return res;
	else  if (op == "!")
		return !val;
	else if (op == "~")
		return ~val;
	else
		return object_t();
}

object_t build_unary_bool(const string &op, const object_t &obj)
{
	bool val = obj.cast<bool>();
	if (op == "!")
		return !val;
	else
		return object_t();
}

template<typename T>
object_t cast_primitive0(object_t obj, type_base_ptr from_type)
{
	string from = from_type->id;
	if (from == type_int->id)
		return (T)obj.cast<int>();
	else if (from == type_float->id)
		return (T)obj.cast<float>();
	else if (from == type_double->id)
		return (T)obj.cast<double>();
	else if (from == type_long_double->id)
		return (T)obj.cast<long double>();
	else if (from == type_long->id)
		return (T)obj.cast<long>();
	else if (from == type_int64->id)
		return (T)obj.cast<long long>();
	else if (from == type_bool->id)
		return (T)obj.cast<bool>();
	else if (from == type_char->id)
		return (T)obj.cast<char>();
	else if (from == type_short->id)
		return (T)obj.cast<short>();
	else
		throw std::runtime_error(string_format("primitive cast does not support type %s", from.c_str()));
}

object_t cast_primitive(object_t from, type_base_ptr from_type, type_base_ptr to_type)
{
	string to = to_type->id;
	if (to == type_int->id)
		return cast_primitive0<int>(from, from_type);
	else if (to == type_float->id)
		return cast_primitive0<float>(from, from_type);
	else if (to == type_double->id)
		return cast_primitive0<double>(from, from_type);
	else if (to == type_long_double->id)
		return cast_primitive0<long double>(from, from_type);
	else if (to == type_long->id)
		return cast_primitive0<long>(from, from_type);
	else if (to == type_int64->id)
		return cast_primitive0<long long>(from, from_type);
	else if (to == type_bool->id)
		return cast_primitive0<bool>(from, from_type);
	else if (to == type_char->id)
		return cast_primitive0<char>(from, from_type);
	else if (to == type_short->id)
		return cast_primitive0<short>(from, from_type);
	else
		throw std::runtime_error(string_format("primitive cast does not support type %s", to.c_str()));
}

object_t compiler::evaulate_expression(AST ast)
{
	if (ast->desc.type() == typeid(descriptor_binary_operator))
	{
		auto desc = any_cast<descriptor_binary_operator>(ast->desc);

		assert_eq(ast->children.size(), 2);

		auto lhs = evaulate_expression(ast->children[0]);
		auto rhs = evaulate_expression(ast->children[1]);

		auto type_name = ast->children[0]->type.base_type->id;
		assert_eq(ast->children[0]->type.base_type->id, ast->children[1]->type.base_type->id);

		object_t res;
		if (type_name == type_int->id)
			res = eval_binary_integral<int>(desc.op, lhs, rhs);
		else if (type_name == type_short->id)
			res = eval_binary_integral<short>(desc.op, lhs, rhs);
		else if (type_name == type_long->id)
			res = eval_binary_integral<long>(desc.op, lhs, rhs);
		else if (type_name == type_int64->id)
			res = eval_binary_integral<long long>(desc.op, lhs, rhs);
		else if (type_name == type_float->id)
			res = eval_binary_common<float>(desc.op, lhs, rhs);
		else if (type_name == type_double->id)
			res = eval_binary_common<double>(desc.op, lhs, rhs);
		else if (type_name == type_long_double->id)
			res = eval_binary_common<long double>(desc.op, lhs, rhs);
		else if (type_name == type_bool->id)
			res = eval_binary_bool(desc.op, lhs, rhs);

		if (res.empty())
			throw expression_evaulation_error();

		return res;
	}
	else if (ast->desc.type() == typeid(descriptor_unary_operator))
	{
		auto desc = any_cast<descriptor_unary_operator>(ast->desc);

		assert_eq(ast->children.size(), 1);

		object_t val = evaulate_expression(ast->children[0]);

		auto type_name = ast->children[0]->type.base_type->id;

		object_t res;
		if (type_name == type_int->id)
			res = build_unary_integral<int>(desc.op, val);
		else if (type_name == type_short->id)
			res = build_unary_integral<short>(desc.op, val);
		else if (type_name == type_long->id)
			res = build_unary_integral<long>(desc.op, val);
		else if (type_name == type_int64->id)
			res = build_unary_integral<long long>(desc.op, val);
		else if (type_name == type_bool->id)
			res = build_unary_bool(desc.op, val);
		else if (type_name == type_float->id)
			res = build_unary_common<float>(desc.op, val);
		else if (type_name == type_double->id)
			res = build_unary_common<double>(desc.op, val);
		else if (type_name == type_long_double->id)
			res = build_unary_common<long double>(desc.op, val);

		if (res.empty())
			throw expression_evaulation_error();

		return res;
	}
	else if (ast->desc.type() == typeid(descriptor_constant))
	{
		auto desc = any_cast<descriptor_constant>(ast->desc);

		if (boost::starts_with(desc.value, "'")) // a char
		{
			auto chars = parse_constant_string(desc.value.substr(1, desc.value.size() - 2));
			if (chars.size() != 1)
				throw expression_evaulation_error();
			return chars[0];
		}
		else if (desc.value == "true")
		{
			return 1;
		}
		else if (desc.value == "false")
		{
			return 0;
		}
		else
		{
			try
			{
				boost::to_lower(desc.value);
				if (desc.value.find("e") != string::npos)
				{
					vector<string> strs;
					boost::split(strs, desc.value, boost::is_any_of("e"));
					assert_eq(strs.size(), 2);
					int e = boost::lexical_cast<int>(strs[1]);
					double m = boost::lexical_cast<double>(strs[0]);
					return m * pow(10, e);
				}
				else if (desc.value.find(".") == string::npos)
				{
					if (boost::starts_with(desc.value, "0x"))
					{
						if (boost::ends_with(desc.value, "ll"))
							return boost::lexical_cast<lexical_hex<long long>>(desc.value.substr(0, desc.value.size() - 2));
						else if (boost::ends_with(desc.value, "l"))
							return boost::lexical_cast<lexical_hex<long>>(desc.value.substr(0, desc.value.size() - 1));
						else
							return boost::lexical_cast<lexical_hex<int>>(desc.value.substr(0, desc.value.size()));
					}
					else if (boost::starts_with(desc.value, "0"))
					{
						if (boost::ends_with(desc.value, "ll"))
							return boost::lexical_cast<lexical_oct<long long>>(desc.value.substr(0, desc.value.size() - 2));
						else if (boost::ends_with(desc.value, "l"))
							return boost::lexical_cast<lexical_oct<long>>(desc.value.substr(0, desc.value.size() - 1));
						else
							return boost::lexical_cast<lexical_oct<int>>(desc.value.substr(0, desc.value.size()));
					}
					else
					{
						if (boost::ends_with(desc.value, "ll"))
							return boost::lexical_cast<long long>(desc.value.substr(0, desc.value.size() - 2));
						else if (boost::ends_with(desc.value, "l"))
							return boost::lexical_cast<long>(desc.value.substr(0, desc.value.size() - 1));
						else
							return boost::lexical_cast<int>(desc.value.substr(0, desc.value.size()));
					}
				}
				else
				{
					if (boost::ends_with(desc.value, "l"))
						return boost::lexical_cast<long double>(desc.value.substr(0, desc.value.size() - 1));
					else if (boost::ends_with(desc.value, "f"))
						return boost::lexical_cast<float>(desc.value.substr(0, desc.value.size() - 1));
					else
						return boost::lexical_cast<double>(desc.value.substr(0, desc.value.size()));
				}
			}
			catch (boost::bad_lexical_cast&)
			{
				throw expression_evaulation_error();
			}
		}
		throw expression_evaulation_error();
	}
	else if (ast->desc.type() == typeid(descriptor_primitive_cast))
	{
		auto desc = any_cast<descriptor_primitive_cast>(ast->desc);
		assert_eq(ast->children.size(), 1);
		auto res = evaulate_expression(ast->children[0]);
		return cast_primitive(res, desc.from_type.base_type, desc.to_type.base_type);
	}
	else
		throw expression_evaulation_error();
}


#pragma once

// system includes ------------------------------------------------------------
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>
#include "aux/exceptions.h"

namespace boltzmann {
class CollisionTensor
{
 private:
  typedef Eigen::SparseMatrix<double> sparse_matrix_t;
  typedef std::shared_ptr<sparse_matrix_t> ptr_t;
  typedef Eigen::SparseLU<sparse_matrix_t> lu_t;

 public:
  CollisionTensor(int N);
  CollisionTensor()
      : N_(0)
  {
  }

  void apply(double* out, const double* in) const;
  void apply(double* out, const double* in, double* buffer) const;

  /**
   *
   *
   * @param out
   * @param in
   * @param L        #local phys. DoFs
   * @param buffer   external buffer of length N*L
   * @param local_bd_indicator true: is at boundary, do not apply scattering,
   *                           false: inner vertex, apply scattering
   */
  void apply(double* out,
             const double* in,
             const unsigned int L,
             double* buffer,
             const std::vector<bool>& local_bd_indicator = std::vector<bool>()) const;

  void apply_adaptive(double* out, const double* in, int nmax) const;
  void add(ptr_t& slice, unsigned int j);
  const sparse_matrix_t& get(int j);

  void set_mass_matrix(sparse_matrix_t& m);
  const lu_t& get_invm() const { return lu_; }
  const sparse_matrix_t& mass_matrix() const { return mass_matrix_; }

  const std::vector<ptr_t>& slices() const { return slices_; }

  /**
   * @brief read tensor from file
   *
   * @param fname Filename
   * @param N     #DoFs
   */
  void read_hdf5(const char* fname, const int N);

 private:
  /// basis size
  unsigned int N_;
  /// buffer
  mutable Eigen::VectorXd vtmp;
  /// tensor entries
  std::vector<ptr_t> slices_;
  /// mass matrix
  sparse_matrix_t mass_matrix_;
  /// inverse of mass matrix
  lu_t lu_;
};

// ------------------------------------------------------------
inline void
CollisionTensor::apply(double* out, const double* in) const
{
  assert(mass_matrix_.rows() > 0);
  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> const_vec_t;
  const_vec_t v_in(in, N_);
  vec_t v_out(out, N_);

#pragma omp parallel for
  for (unsigned int j = 0; j < N_; ++j) {
    vtmp[j] = v_in.dot((*slices_[j]) * v_in);
  }

  // this is not parallel
  v_out = lu_.solve(vtmp);
}

// ------------------------------------------------------------
inline void
CollisionTensor::apply_adaptive(double* out, const double* in, int nmax) const
{
  assert(mass_matrix_.rows() > 0);
  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> const_vec_t;
  const_vec_t v_in(in, nmax);
  vec_t v_out(out, N_);

// is this load-balanced? probably not
#pragma omp parallel for
  for (unsigned int j = 0; j < N_; ++j) {
    vtmp[j] = v_in.dot((*slices_[j]).topLeftCorner(nmax, nmax) * v_in);
  }

  // this is not parallel
  v_out = lu_.solve(vtmp);
}

// ------------------------------------------------------------
inline void
CollisionTensor::apply(double* out, const double* in, double* buffer) const
{
  assert(mass_matrix_.rows() > 0);
  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> const_vec_t;
  const_vec_t v_in(in, N_);
  vec_t vbuf(buffer, N_);
  vec_t v_out(out, N_);
#pragma omp parallel for schedule(guided)
  for (unsigned int j = 0; j < N_; ++j) {
    vbuf[j] = v_in.dot((*slices_[j]) * v_in);
  }
  v_out = lu_.solve(vbuf);
}

// ------------------------------------------------------------
inline void
CollisionTensor::apply(double* out,
                       const double* in,
                       const unsigned int L,
                       double* buffer,
                       const std::vector<bool>& local_bd_indicator) const
{
  assert(mass_matrix_.rows() > 0);
  typedef Eigen::Map<Eigen::VectorXd> vec_t;
  typedef Eigen::Map<const Eigen::VectorXd> const_vec_t;

  vec_t vbuf(buffer, N_ * L);

  if (local_bd_indicator.size() == 0) {
#pragma omp parallel
    {
#pragma omp for schedule(guided)
      for (unsigned int j = 0; j < N_; ++j) {
        for (unsigned int l = 0; l < L; ++l) {
          const_vec_t v_in(in + N_ * l, N_);
          vbuf[N_ * l + j] = v_in.dot((*slices_[j]) * v_in);
        }
      }

#pragma omp for schedule(static)
      for (unsigned int l = 0; l < L; ++l) {
        vec_t vout(out + N_ * l, N_);
        const_vec_t vbuf(buffer + N_ * l, N_);
        vout = lu_.solve(vbuf);
      }
    }
  } else if (local_bd_indicator.size() == L) {
#pragma omp parallel
    {
#pragma omp for schedule(guided)
      for (unsigned int j = 0; j < N_; ++j) {
        for (unsigned int l = 0; l < L; ++l) {
          if (local_bd_indicator[l]) continue;
          const_vec_t v_in(in + N_ * l, N_);
          vbuf[N_ * l + j] = v_in.dot((*slices_[j]) * v_in);
        }
      }

#pragma omp for schedule(static)
      for (unsigned int l = 0; l < L; ++l) {
        if (local_bd_indicator[l]) continue;
        vec_t vout(out + N_ * l, N_);
        const_vec_t vbuf(buffer + N_ * l, N_);
        vout = lu_.solve(vbuf);
      }
    }

  }
#ifdef DEBUG
  else {
    BAssertThrow(false, "something went wrong with bd_indicator");
  }
#endif
}

}  // end boltzmann