#ifndef __CRAB_LLVM_HPP_
#define __CRAB_LLVM_HPP_

/*
 * Infer invariants using Crab.
 */

#include "llvm/Pass.h"
#include "llvm/ADT/DenseMap.h"

#include <boost/shared_ptr.hpp>

#include "crab_llvm/crab_cfg.hh"

// forward declarations

namespace llvm {
  class TargetLibraryInfo;
}

namespace crab {
  namespace checker {
     class checks_db;
  }
}

namespace crab_llvm {
  class HeapAbstraction;
  struct GenericAbsDomWrapper;
  class IntraCrabLlvm_Impl;
}

namespace crab_llvm {
   ////
   // Base numerical domains for user options
   ////
  enum CrabDomain
     {   INTERVALS
       , INTERVALS_CONGRUENCES
       , BOXES
       , DIS_INTERVALS
       , ZONES_SPARSE_DBM
       , ZONES_SPLIT_DBM
       , TERMS_INTERVALS
       , TERMS_DIS_INTERVALS
         //TERMS_INTERVALS x  ZONES_SPLIT_DBM
       , TERMS_ZONES
         //(#live vars<threshold ? TERMS_INTERVALSxZONES_SPLIT_DBM, INTERVALS)
       , ADAPT_TERMS_ZONES
       , OPT_OCT_APRON
       , PK_APRON
     };

  ////
  // Kind of checker
  ////
  enum assert_check_kind_t
    { NOCHECKS = 0,
      ASSERTION = 1,
      NULLITY = 2
    };
}

namespace crab_llvm {

  /**
   * Class to set analysis options
   **/
  struct AnalysisParams {
    CrabDomain dom;
    CrabDomain sum_dom;
    bool run_backward;
    bool run_liveness;
    bool run_inter;
    unsigned relational_threshold;
    unsigned widening_delay;
    unsigned narrowing_iters;
    unsigned widening_jumpset;
    bool stats;
    bool print_invars;
    bool print_preconds;
    bool print_assumptions;
    bool keep_shadow_vars;
    assert_check_kind_t check;
    unsigned check_verbose;

    AnalysisParams()
      : dom(INTERVALS), sum_dom(ZONES_SPLIT_DBM),
	run_backward(false), run_liveness(false), run_inter(false),
	relational_threshold(10000),
	widening_delay(1), narrowing_iters(10), widening_jumpset(0),
	stats(false),
	print_invars(false), print_preconds(false), print_assumptions(false),
	keep_shadow_vars(false),
	check(NOCHECKS), check_verbose(0) {}
  };

  /**
   * Intra-procedural analysis of a function
   **/
  class IntraCrabLlvm {

  public:

    typedef boost::shared_ptr<GenericAbsDomWrapper> wrapper_dom_ptr;
    typedef llvm::DenseMap<const llvm::BasicBlock*, wrapper_dom_ptr> invariant_map_t;
    typedef llvm::DenseMap<const llvm::BasicBlock*, lin_cst_sys_t> assumption_map_t;
    typedef crab::checker::checks_db checks_db_t;
    typedef boost::shared_ptr<checks_db_t> checks_db_ptr;
    typedef boost::shared_ptr<HeapAbstraction> heap_abs_ptr;

  private:

    std::unique_ptr<IntraCrabLlvm_Impl> m_impl;
    variable_factory_t m_vfac;
    invariant_map_t m_pre_map;
    invariant_map_t m_post_map;

  public:

    /**
     * Constructor that builds a crab CFG
     **/
    IntraCrabLlvm(llvm::Function &fun,
		  const llvm::TargetLibraryInfo &tli,
		  const crab::cfg::tracked_precision cfg_precision = crab::cfg::NUM,
		  heap_abs_ptr heap_abs = nullptr);

    ~IntraCrabLlvm();

    /**
     * Call crab analysis on the CFG under assumptions.
     **/
    void analyze(AnalysisParams &params, const assumption_map_t &assumptions);

    /**
     * Return invariants that hold at the entry of b
     **/
    wrapper_dom_ptr get_pre(const llvm::BasicBlock *b, bool keep_shadows=false) const;

    /**
     * return invariants that hold at the exit of b
     **/
    wrapper_dom_ptr get_post(const llvm::BasicBlock *b, bool keep_shadows=false) const;

  };

  /**
   * LLVM Module pass that computes invariants using Crab.
   **/
  class CrabLlvmPass : public llvm::ModulePass {

    typedef typename IntraCrabLlvm::wrapper_dom_ptr wrapper_dom_ptr;
    typedef typename IntraCrabLlvm::invariant_map_t invariant_map_t;
    typedef typename IntraCrabLlvm::checks_db_ptr checks_db_ptr;
    typedef typename IntraCrabLlvm::heap_abs_ptr heap_abs_ptr;
    typedef llvm::DenseMap<llvm::Function*, cfg_ptr_t> cfg_map_t;

    invariant_map_t m_pre_map;
    invariant_map_t m_post_map;
    heap_abs_ptr m_mem;
    variable_factory_t m_vfac;
    cfg_map_t m_cfg_map;
    checks_db_ptr m_checks_db;

   public:

    static char ID;

    CrabLlvmPass();

    /* begin ModulePass API */
    virtual void releaseMemory();

    virtual bool runOnModule(llvm::Module& M);

    virtual bool runOnFunction(llvm::Function &F);

    virtual void getAnalysisUsage(llvm::AnalysisUsage &AU) const ;

    virtual const char* getPassName() const {return "CrabLlvm";}
    /* end ModulePass API */

    variable_factory_t& get_var_factory() { return m_vfac; }

    heap_abs_ptr get_heap_abstraction() { return m_mem; }

    cfg_ptr_t get_cfg(llvm::Function* F);

    /**
     * return invariants that hold at the entry of BB
     **/
    wrapper_dom_ptr get_pre(const llvm::BasicBlock *BB, bool KeepShadows=false) const;

    /**
     * return invariants that hold at the exit of BB
     **/
    wrapper_dom_ptr get_post(const llvm::BasicBlock *BB, bool KeepShadows=false) const;

    /**
     * To query and view the analysis results
     **/

    /* return total number of checks if assertion checker enabled,
       otherwise 0 */
    unsigned get_total_checks() const;
    /* return total number of safe checks if assertion checker
       enabled, otherwise 0 */
    unsigned get_total_safe_checks() const;
    /* return total number of definite error checks if assertion
       checker enabled, otherwise 0 */
    unsigned get_total_error_checks() const;
    /* return total number of possibly error checks if assertion
       checker enabled, otherwise 0 */
    unsigned get_total_warning_checks() const;

    void print_checks(llvm::raw_ostream &o) const;
  };

} // end namespace

#endif


// Copyright (C) 2019 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:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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 of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

// This file was created by Steffen Urban (urbste@googlemail.com) or
// company address (steffen.urban@zeiss.com)
// May 2019

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include <algorithm>
#include <vector>

#include "gtest/gtest.h"

#include "theia/matching/feature_correspondence.h"
#include "theia/math/util.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/estimators/estimate_radial_distortion_homography.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/sfm/pose/util.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/test/test_utils.h"

namespace theia {

using Eigen::AngleAxisd;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;

RandomNumberGenerator rng(53);

static constexpr double kFocalLength1 = 1000.0;
static constexpr double kFocalLength2 = 1500.0;
static constexpr double kRadialDistortion1 = -1e-7;
static constexpr double kRadialDistortion2 = -5e-7;
static constexpr double kReprojectionError = 3.0;

// allowing 10 percent error for radial distortion parameters
static constexpr double kRadDistThreshold1 =
    0.1 * std::abs(kRadialDistortion1 * std::pow(kFocalLength1, 2));
static constexpr double kRadDistThreshold2 =
    0.1 * std::abs(kRadialDistortion2 * std::pow(kFocalLength2, 2));

// Generate points on a plane so that a homography can accurately estimate the
// motion.
void GeneratePoints(std::vector<Vector3d>* points) {
  static const int kDepth = 3.0;
  for (int i = -4; i <= 4; i++) {
    for (int j = -4; j <= 4; j++) {
      points->emplace_back(Vector3d(i, j, kDepth));
    }
  }
}

void GenerateDistortedImagePoint(const Vector3d& point_3d,
                                 const double projection_noise_std_dev,
                                 const Matrix3d& expected_rotation,
                                 const Vector3d& expected_translation,
                                 const double focal_length1,
                                 const double focal_length2,
                                 const double radial_distortion1,
                                 const double radial_distortion2,
                                 const bool outlier,
                                 Vector3d& image_1_point_normalized,
                                 Vector3d& image_2_point_normalized,
                                 Vector2d& image_1_point,
                                 Vector2d& image_2_point) {
  Vector3d point3_cam2 = expected_rotation * point_3d + expected_translation;

  DistortPoint(point_3d, focal_length1, radial_distortion1, image_1_point);
  DistortPoint(point3_cam2, focal_length2, radial_distortion2, image_2_point);

  if (outlier) {
    image_1_point[0] += rng.RandDouble(-100.0, 100.0);
    image_2_point[0] += rng.RandDouble(-100.0, 100.0);
    image_1_point[1] += rng.RandDouble(-100.0, 100.0);
    image_2_point[1] += rng.RandDouble(-100.0, 100.0);
  }

  if (projection_noise_std_dev) {
    AddNoiseToProjection(projection_noise_std_dev, &rng, &image_1_point);
    AddNoiseToProjection(projection_noise_std_dev, &rng, &image_2_point);
  }

  // normalize points with focal length (and principal point) estimate
  UndistortPoint(image_1_point, focal_length1, 0.0, image_1_point_normalized);
  UndistortPoint(image_2_point, focal_length2, 0.0, image_2_point_normalized);
}

// Creates a test scenario from ground truth 3D points and ground truth rotation
// and translation. Projection noise is optional (set to 0 for no
// noise).
void GenerateDistortedImagePoints(
    const std::vector<Vector3d>& points_3d,
    const double projection_noise_std_dev,
    const Matrix3d& expected_rotation,
    const Vector3d& expected_translation,
    const double focal_length1,
    const double focal_length2,
    const double radial_distortion1,
    const double radial_distortion2,
    const double inlier_ratio,
    std::vector<Vector2d>* image_1_points_normalized,
    std::vector<Vector2d>* image_2_points_normalized,
    std::vector<Vector2d>* image_1_points,
    std::vector<Vector2d>* image_2_points) {
  for (int i = 0; i < points_3d.size(); i++) {
    Vector2d distorted_point1, distorted_point2;
    Vector3d normalized_points2d_1, normalized_points2d_2;
    bool outlier = false;
    if (i > inlier_ratio * points_3d.size()) {
      outlier = true;
    }
    GenerateDistortedImagePoint(points_3d[i],
                                projection_noise_std_dev,
                                expected_rotation,
                                expected_translation,
                                focal_length1,
                                focal_length2,
                                radial_distortion1,
                                radial_distortion2,
                                outlier,
                                normalized_points2d_1,
                                normalized_points2d_2,
                                distorted_point1,
                                distorted_point2);

    image_1_points->push_back(distorted_point1);
    image_2_points->push_back(distorted_point2);

    image_1_points_normalized->push_back(normalized_points2d_1.hnormalized());
    image_2_points_normalized->push_back(normalized_points2d_2.hnormalized());
  }
}

void ExecuteRandomTest(const RansacParameters& options,
                       const Matrix3d& rotation,
                       const Vector3d& position,
                       const double radial_distortion_1,
                       const double radial_distortion_2,
                       const double focal_length_1,
                       const double focal_length_2,
                       const double inlier_ratio,
                       const double noise) {
  // Create feature correspondences (inliers and outliers) and add noise if
  // appropriate.
  std::vector<Vector3d> points3d;
  GeneratePoints(&points3d);

  std::vector<Vector2d> image_1_points_normalized, image_2_points_normalized;
  std::vector<Vector2d> image_1_points, image_2_points;

  // Generate distorted points
  GenerateDistortedImagePoints(points3d,
                               noise,
                               rotation,
                               position,
                               focal_length_1,
                               focal_length_2,
                               radial_distortion_1,
                               radial_distortion_2,
                               inlier_ratio,
                               &image_1_points_normalized,
                               &image_2_points_normalized,
                               &image_1_points,
                               &image_2_points);
  // Get correspondences
  std::vector<RadialDistortionFeatureCorrespondence> correspondences;
  for (size_t i = 0; i < image_1_points.size(); ++i) {
    RadialDistortionFeatureCorrespondence correspondence;
    correspondence.feature_left = image_1_points[i];
    correspondence.feature_right = image_2_points[i];
    correspondence.normalized_feature_left = image_1_points_normalized[i];
    correspondence.normalized_feature_right = image_2_points_normalized[i];
    correspondence.focal_length_estimate_left = focal_length_1;
    correspondence.focal_length_estimate_right = focal_length_2;
    correspondences.push_back(correspondence);
  }
  // Estimate the radial distortion homography.
  RadialHomographyResult radial_homography_result;
  RansacSummary ransac_summary;
  EXPECT_TRUE(EstimateRadialHomographyMatrix(options,
                                             RansacType::RANSAC,
                                             correspondences,
                                             &radial_homography_result,
                                             &ransac_summary));
  // Expect that the radial distortion values are close to the ground truth
  EXPECT_NEAR(radial_homography_result.l1,
              radial_distortion_1 * (focal_length_1 * focal_length_1),
              kRadDistThreshold1);
  EXPECT_NEAR(radial_homography_result.l2,
              radial_distortion_2 * (focal_length_2 * focal_length_2),
              kRadDistThreshold2);
}

TEST(EstimateRadialHomographyMatrix, AllInliersNoNoise) {
  RansacParameters options;
  options.rng = std::make_shared<RandomNumberGenerator>(rng);
  options.use_mle = true;
  options.error_thresh = kReprojectionError;
  options.failure_probability = 0.01;
  options.max_iterations = 1000;
  options.min_iterations = 1;
  const double kInlierRatio = 1.0;
  const double kNoise = 0.0;

  const std::vector<Matrix3d> rotations = {
      Matrix3d::Identity(),
      AngleAxisd(DegToRad(12.0), Vector3d::UnitY()).toRotationMatrix(),
      AngleAxisd(DegToRad(-9.0), Vector3d(1.0, 0.2, -0.8).normalized())
          .toRotationMatrix()};
  const std::vector<Vector3d> positions = {Vector3d(-1.3, 0, 0),
                                           Vector3d(0, 1.3, 0.0)};
  for (int i = 0; i < rotations.size(); i++) {
    for (int j = 0; j < positions.size(); j++) {
      ExecuteRandomTest(options,
                        rotations[i],
                        positions[j],
                        kRadialDistortion1,
                        kRadialDistortion2,
                        kFocalLength1,
                        kFocalLength2,
                        kInlierRatio,
                        kNoise);
    }
  }
}

TEST(EstimateRadialHomographyMatrix, AllInliersNoise) {
  RansacParameters options;
  options.rng = std::make_shared<RandomNumberGenerator>(rng);
  options.use_mle = true;
  options.error_thresh = kReprojectionError;
  options.failure_probability = 0.01;
  options.max_iterations = 1000;
  options.min_iterations = 1;
  const double kInlierRatio = 1.0;
  const double kNoise = 1.0;

  const std::vector<Matrix3d> rotations = {
      Matrix3d::Identity(),
      AngleAxisd(DegToRad(12.0), Vector3d::UnitY()).toRotationMatrix(),
      AngleAxisd(DegToRad(-9.0), Vector3d(1.0, 0.2, -0.8).normalized())
          .toRotationMatrix()};
  const std::vector<Vector3d> positions = {Vector3d(-1.3, 0, 0),
                                           Vector3d(0, 1.3, 0.0)};
  for (int i = 0; i < rotations.size(); i++) {
    for (int j = 0; j < positions.size(); j++) {
      ExecuteRandomTest(options,
                        rotations[i],
                        positions[j],
                        kRadialDistortion1,
                        kRadialDistortion2,
                        kFocalLength1,
                        kFocalLength2,
                        kInlierRatio,
                        kNoise);
    }
  }
}

TEST(EstimateRadialHomographyMatrix, OutliersNoNoise) {
  RansacParameters options;
  options.rng = std::make_shared<RandomNumberGenerator>(rng);
  options.use_mle = true;
  options.error_thresh = kReprojectionError;
  options.failure_probability = 0.01;
  options.max_iterations = 1000;
  options.min_iterations = 1;
  const double kInlierRatio = 0.6;
  const double kNoise = 0.0;

  const std::vector<Matrix3d> rotations = {
      Matrix3d::Identity(),
      AngleAxisd(DegToRad(12.0), Vector3d::UnitY()).toRotationMatrix(),
      AngleAxisd(DegToRad(-9.0), Vector3d(1.0, 0.2, -0.8).normalized())
          .toRotationMatrix()};
  const std::vector<Vector3d> positions = {Vector3d(-1.3, 0, 0),
                                           Vector3d(0, 1.3, 0.0)};
  for (int i = 0; i < rotations.size(); i++) {
    for (int j = 0; j < positions.size(); j++) {
      ExecuteRandomTest(options,
                        rotations[i],
                        positions[j],
                        kRadialDistortion1,
                        kRadialDistortion2,
                        kFocalLength1,
                        kFocalLength2,
                        kInlierRatio,
                        kNoise);
    }
  }
}

TEST(EstimateRadialHomographyMatrix, OutliersNoise) {
  RansacParameters options;
  options.rng = std::make_shared<RandomNumberGenerator>(rng);
  options.use_mle = true;
  options.error_thresh = kReprojectionError;
  options.failure_probability = 0.01;
  options.max_iterations = 1000;
  options.min_iterations = 1;
  const double kInlierRatio = 0.6;
  const double kNoise = 1.0;

  const std::vector<Matrix3d> rotations = {
      Matrix3d::Identity(),
      AngleAxisd(DegToRad(12.0), Vector3d::UnitY()).toRotationMatrix(),
      AngleAxisd(DegToRad(-9.0), Vector3d(1.0, 0.2, -0.8).normalized())
          .toRotationMatrix()};
  const std::vector<Vector3d> positions = {Vector3d(-1.3, 0, 0),
                                           Vector3d(0, 1.3, 0.0)};
  for (int i = 0; i < rotations.size(); i++) {
    for (int j = 0; j < positions.size(); j++) {
      ExecuteRandomTest(options,
                        rotations[i],
                        positions[j],
                        kRadialDistortion1,
                        kRadialDistortion2,
                        kFocalLength1,
                        kFocalLength2,
                        kInlierRatio,
                        kNoise);
    }
  }
}

}  // namespace theia


#include <boost/accumulators/statistics/covariance.hpp>


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <memory>
#include <cassert>
#include <boost/noncopyable.hpp>
#include "Eigen/Core"
#include "snptest/FinitelySupportedFunctionSet.hpp"

namespace snptest {
	FinitelySupportedFunctionSet::FinitelySupportedFunctionSet(
		FinitelySupportedFunctionSet::Vector const& support,
		Matrix const& matrix
	):
		m_support( support ),
		m_matrix( matrix )
	{
		assert( m_support.size() == m_matrix.cols() ) ;
	}

	FinitelySupportedFunctionSet::FinitelySupportedFunctionSet(
		FinitelySupportedFunctionSet const& other
	):
		m_support( other.m_support ),
		m_matrix( other.m_matrix )
	{}

	FinitelySupportedFunctionSet& FinitelySupportedFunctionSet::operator=(
		FinitelySupportedFunctionSet const& other
	) {
		m_support = other.m_support ;
		m_matrix = other.m_matrix ;
		return *this ;
	}

}