#include <frovedis.hpp>
#include <frovedis/matrix/sliced_matrix.hpp>
#include <frovedis/matrix/blas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // creating a colmajor matrix local from file
    colmajor_matrix_local<float> cm (
           make_rowmajor_matrix_local_load<float>("./sample_4x4"));

    // creating slices from matrix
    auto sm1 = make_sliced_colmajor_matrix_local<float>(cm,0,0,2,2);
    auto sm2 = make_sliced_colmajor_matrix_local<float>(cm,2,2,2,2);
    auto sm3 = make_sliced_colmajor_matrix_local<float>(cm,0,2,2,2);

    // <cm> top-right = top-left * bottom-right
    gemm<float>(sm1,sm2,sm3);
    cm.to_rowmajor().save("./out_4x4");

    // checking whether slices have been created properly and
    // top-right sliced square is correctly modified with result
    BOOST_CHECK (system("diff ./out_4x4 ./ref_4x4") == 0);
    system("rm -f ./out_4x4");
}


// ROS includes.
#include <ros/ros.h>
#include <ros/time.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <tf_conversions/tf_eigen.h>
#include <eigen_conversions/eigen_msg.h>

#include <Eigen/Geometry>

#include <string.h>
#include <iostream>
#include <fstream>
#include <math.h>

#include "rwth_visual_odometry/VisualOdometry.h"

ros::Publisher pub_message;

void callback(const nav_msgs::OdometryConstPtr& odom) {
    Eigen::Affine3d eigenTr;
    tf::poseMsgToEigen(odom->pose.pose, eigenTr);

    // transpose, just to have P=[R|t] format
    Eigen::Matrix4d m = eigenTr.matrix();
    ROS_WARN_STREAM("m: \n" << m );

    // for ros coordinate system to tracker coordinate system, rotate 270° around y and 90° around (then) x counter-clock-wise
    Eigen::Affine3d coordinateTransform = Eigen::Affine3d::Identity();
    coordinateTransform.rotate( Eigen::AngleAxisd(270.0 / 180.0 * M_PI, Eigen::Vector3d::UnitY() ) );
    coordinateTransform.rotate( Eigen::AngleAxisd(90.0  / 180.0 * M_PI, Eigen::Vector3d::UnitX() ) );

    Eigen::Matrix4d ros2tracker_R = coordinateTransform.matrix();
    //ros2tracker_R << 0,-1,0,0, 0,0,-1,0, 1,0,0,0, 0,0,0,1;

    ROS_WARN_STREAM("R: \n" << ros2tracker_R );

    Eigen::Affine3d rotatedOdometry(ros2tracker_R * m);

    ROS_WARN_STREAM("rotatedOdometry: \n" << rotatedOdometry.matrix() );

    // and mirror at x-z-plane (turns a +0 in a -0 in t and does sth. to R, but still...)
    Eigen::Matrix3d A_rh = rotatedOdometry.linear();
    Eigen::Vector3d b_rh = rotatedOdometry.translation();

    Eigen::Matrix3d S_y; S_y << 1, 0, 0,   0, -1, 0,  0, 0, 1;

    Eigen::Matrix3d A_lh = S_y * A_rh * S_y;
    Eigen::Vector3d b_lh = S_y * b_rh;

    Eigen::Affine3d reflectedOdometry;
    reflectedOdometry.linear() = A_lh;
    reflectedOdometry.translation() = b_lh;

    ROS_WARN_STREAM("reflectedOdometry: \n" << reflectedOdometry.matrix() );

    //DEBUG
    /*for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){
            if(i==j){
                m(i,j) = 1;
            }
            else{
                m(i,j) = 0;
            }
        }
    }*/
    // switch y,z in t
    /*float temp_val = m(3,1);
    m(3,1) = m(3,2);
    m(3,2) = temp_val;

    // switch y,z in R
    temp_val = m(2,2);
    m(2,2) = m(1,1);
    m(1,1) = temp_val;

    temp_val = m(2,0);
    m(2,0) = m(1,0);
    m(1,0) = temp_val;

    temp_val = m(0,2);
    m(0,2) = m(0,1);
    m(0,1) = temp_val;

    temp_val = m(1,2);
    m(1,2) = m(2,1);
    m(2,1) = temp_val;

    // switch x,z in t
    temp_val = m(3,0);
    m(3,0) = m(3,2);
    m(3,2) = temp_val;

    // switch x,z in R
    temp_val = m(2,2);
    m(2,2) = m(0,0);
    m(0,0) = temp_val;

    temp_val = m(2,0);
    m(2,0) = m(0,2);
    m(0,2) = temp_val;

    temp_val = m(1,0);
    m(1,0) = m(1,2);
    m(1,2) = temp_val;

    temp_val = m(0,1);
    m(0,1) = m(2,1);
    m(2,1) = temp_val;*/

    // negate x in t
    //m(3,0) = -1.0*m(3,0);

   // "negate" x in R (roatate around ? 180°)
   //m(2,0) = -1.0*m(2,0);
   //m(0,0) = -1.0*m(0,0);
   //m(1,1) = -1.0*m(1,1);
   //m(0,2) = -1.0*m(0,2);

    /*temp_val = m(3,0);
    m(3,0) = m(3,2);
    m(3,2) = temp_val;
    //DEBUG-END*/

    // transpose for correct array-filling below
    Eigen::Matrix4d finalOdometry = reflectedOdometry.matrix().transpose();

    //ROS_WARN_STREAM("R*m^-1: \n" << mi );

    /*for(int i = 0; i < 4*4; i++){
        printf("%.2f ",mi.data()[i]);
        if((i+1)%4 == 0)
        printf("\n");
    }*/

    rwth_visual_odometry::VisualOdometry fovis_info_msg;
    fovis_info_msg.header = odom->header;
    fovis_info_msg.motion_estimate_valid = true;
    fovis_info_msg.transformation_matrix.resize(4*4);
    for(int i = 0; i < 4*4; i++)
        fovis_info_msg.transformation_matrix[i] = finalOdometry.data()[i];
     pub_message.publish(fovis_info_msg);
}

// Connection callback that unsubscribes from the tracker if no one is subscribed.
void connectCallback(ros::Subscriber &sub_odom,
                     ros::NodeHandle &n,
                     std::string odom_topic){
    if(!pub_message.getNumSubscribers()) {
        ROS_DEBUG("Odom: No subscribers. Unsubscribing.");
        sub_odom.shutdown();
    } else {
        ROS_DEBUG("Odom: New subscribers. Subscribing.");
        sub_odom = n.subscribe(odom_topic.c_str(), 1, &callback);
    }
}

int main(int argc, char **argv)
{
    // Set up ROS.
    ros::init(argc, argv, "odom2visual");
    ros::NodeHandle n;

    // Declare variables that can be modified by launch file or command line.
    std::string odom_topic;
    std::string pub_topic;

    // Initialize node parameters from launch file or command line.
    // Use a private node handle so that multiple instances of the node can be run simultaneously
    // while using different parameters.
    ros::NodeHandle private_node_handle_("~");
    private_node_handle_.param("odom", odom_topic, std::string("/odom"));

    // Create a subscriber.
    // Set queue size to 1 because generating a queue here will only pile up images and delay the output by the amount of queued images
    ros::Subscriber sub_odom; //Subscribers have to be defined out of the if scope to have effect.

    ros::SubscriberStatusCallback con_cb = boost::bind(&connectCallback,
                                                       boost::ref(sub_odom),
                                                       boost::ref(n),
                                                       odom_topic);

    private_node_handle_.param("motion_parameters", pub_topic, std::string("/spencer/sensors/visual_odometry/motion_matrix"));
    pub_message = n.advertise<rwth_visual_odometry::VisualOdometry>(pub_topic.c_str(), 10, con_cb, con_cb);

    ros::spin();

    return 0;
}


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.
//
// gDLS*: Generalized Pose-and-Scale Estimation Given Scale and Gravity Priors
//
// Victor Fragoso, Joseph DeGol, Gang Hua.
// Proc. of the IEEE/CVF Conf. on Computer Vision and Pattern Recognition 2020.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@microsoft.com)

#include <stdlib.h>
#include <cmath>
#include <random>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include "gtest/gtest.h"
#include "gdls_star/camera_feature_correspondence_2d_3d.h"
#include "gdls_star/estimate_similarity_transformation.h"
#include "gdls_star/pinhole_camera.h"
#include "math/utils.h"

namespace msft {
namespace {

// Random seed.
constexpr size_t kSeed = 67;
// Focal length.
static const double kFocalLength = 1000.0;
// Number of cameras.
static const int kNumCameras = 4;
// Number of points.
static const int kNumPoints = 100;
// Min. angle in degrees.
static const double kMinAngleDeg = 5.0;
// Max. angle in degrees.
static const double kMaxAngleDeg = 40.0;
// Max. num of trials.
static const int kNumTrials = 8;
// Minimum and maximum limits for generating 3D points.
static const double kMinXY = -5.0;
static const double kMaxXY = 5.0;
static const double kMinZ = 10;
static const double kMaxZ = 20;

// Similarity transformation.
struct SimilarityTransformation {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  Eigen::Quaterniond rotation = Eigen::Quaterniond::Identity();
  Eigen::Vector3d translation = Eigen::Vector3d::Zero();
  double scale = 1.0;
};

struct TestParameters {
  // Num. of points for the test.
  int num_points = 4;
  // Noise level to points in 3d.
  double noise_std_dev = 0.0;
  // Inlier ratio. 1.0: All inliers, 0.0: All outliers.
  double inlier_ratio = 1.0;
  // Rotation error threshold in radians.
  double rotation_thresh = DegToRad(1.0);
  // Translation error threshold.
  double translation_thresh = 0.5;
  // Scale error threshold.
  double scale_thresh = 0.1;
  // Rotation generation parameters.
  double min_angle_deg = 1.0;
  double max_angle_deg = 45.0;
  // Translation generation parameters.
  double min_translation_value = 0.0;
  double max_translation_value = 10.0;
  // Scale generation parameters.
  double min_scale_value = 0.5;
  double max_scale_value = 5.0;
  // Ransac parameters.
  GdlsStarRobustEstimator::RansacParameters ransac_params;
};

class EstimateSimilarityTransformationTest : public ::testing::Test {
 public:
  static void SetUpTestCase() {
    rng = new std::mt19937(kSeed);
    srand(kSeed);
  }

  static void TearDownTestCase() {
    delete rng;
  }

  double GenerateRandomScale(const double min_value,
                             const double max_value) {
    CHECK_LT(min_value, max_value);
    std::mt19937& prng = *rng;
    std::uniform_real_distribution<double> dist(min_value, max_value);
    return dist(prng);
  }

  Eigen::Vector3d GenerateRandomVector3d(const double min_value,
                                         const double max_value) {
    CHECK_LT(min_value, max_value);
    std::mt19937& prng = *rng;
    std::uniform_real_distribution<double> dist(min_value, max_value);
    const Eigen::Vector3d point(dist(prng),
                                dist(prng),
                                dist(prng));
    return point;
  }

  Eigen::Quaterniond GenerateRandomRotation(const double min_angle_deg,
                                            const double max_angle_deg) {
    CHECK_LT(min_angle_deg, max_angle_deg);
    std::uniform_real_distribution<double> angle_dist(min_angle_deg,
                                                      max_angle_deg);
    std::mt19937& prng = *rng;
    const Eigen::Quaterniond rotation(
        Eigen::AngleAxisd(DegToRad(angle_dist(prng)),
                          Eigen::Vector3d::Random().normalized()));
    return rotation;
  }

  PinholeCamera GenerateRandomCamera() {
    const Eigen::Vector2d principal_point(kFocalLength / 2.0,
                                          kFocalLength / 2.0);
    const Eigen::Quaterniond rotation = GenerateRandomRotation(kMinAngleDeg,
                                                               kMaxAngleDeg);
    const Eigen::Vector3d translation = GenerateRandomVector3d(-10, 10);
    PinholeCamera camera(kFocalLength, principal_point, rotation, translation);
    return camera;
  }

  Eigen::Vector3d GenerateRandomPoint3d(const double min_xy,
                                        const double max_xy,
                                        const double min_z,
                                        const double max_z) {
    std::mt19937& prng = *rng;
    std::uniform_real_distribution<double> xy_dist(min_xy, max_xy);
    std::uniform_real_distribution<double> z_dist(min_z, max_z);
    Eigen::Vector3d point(xy_dist(prng), xy_dist(prng), z_dist(prng));
    return point;
  }

  Eigen::Vector3d AddNoiseToPoint3d(const double noise_std_dev,
                                    Eigen::Vector3d* point) {
    std::mt19937& prng = *rng;
    std::normal_distribution<double> noise(0.0, noise_std_dev);
    *CHECK_NOTNULL(point) += Eigen::Vector3d(noise(prng),
                                             noise(prng),
                                             noise(prng));
    return *point;
  }

  SimilarityTransformation GenerateRandomSimilarityTransformation(
      const TestParameters& test_params) {
    SimilarityTransformation transformation;
    transformation.rotation = GenerateRandomRotation(test_params.min_angle_deg,
                                                     test_params.max_angle_deg);
    transformation.translation =
        GenerateRandomVector3d(test_params.min_translation_value,
                               test_params.max_translation_value);
    transformation.scale = GenerateRandomScale(test_params.min_scale_value,
                                               test_params.max_scale_value);
    return transformation;
  }

  std::vector<CameraFeatureCorrespondence2D3D> GenerateCorrespondences(
      const TestParameters& test_params,
      const SimilarityTransformation& expected_transformation) {
    CHECK_GT(test_params.inlier_ratio, 0.0);
    CHECK_LE(test_params.inlier_ratio, 1.0);
    std::vector<CameraFeatureCorrespondence2D3D> correspondences(
        test_params.num_points);

    // Generate cameras.
    std::vector<PinholeCamera> cameras(kNumCameras);
    for (int i = 0; i < kNumCameras; ++i) {
      cameras[i] = GenerateRandomCamera();
    }

    // Generate synthetic correspondences.
    for (int i = 0; i < test_params.num_points; ++i) {
      CameraFeatureCorrespondence2D3D& correspondence = correspondences[i];
      correspondence.camera = cameras[i % cameras.size()];
      double depth = -1;
      do {
        // Create a random 3D point that is in front of the camera.
        correspondence.point =
            GenerateRandomPoint3d(kMinXY, kMaxXY, kMinZ, kMaxZ);
        // Project point and see if it is infront of camera.
        depth = correspondence.camera.ProjectPoint(correspondence.point,
                                                   &correspondence.observation);
      } while(depth < 0);

      // Add zero-mean Gaussian noise.
      if (test_params.noise_std_dev > 0.0) {
        AddNoiseToPoint3d(test_params.noise_std_dev, &correspondence.point);
      }
    }

    // Add outliers and apply inv. transformation.
    for (int i = 0; i < correspondences.size(); ++i) {
      // Add outlier.
      if (i > test_params.inlier_ratio * correspondences.size()) {
        correspondences[i].observation =
            kFocalLength * Eigen::Vector2d::Random();
      }

      // Apply inv. transformation.
      const Eigen::Vector3d new_point =
          expected_transformation.rotation.conjugate() *
          (expected_transformation.scale * correspondences[i].point -
           expected_transformation.translation);
      correspondences[i].point = std::move(new_point);
    }

    return correspondences;
  }

  bool EvaluateSolution(const TestParameters& test_params,
                        const SimilarityTransformation& expected_transformation,
                        const GdlsStar::Solution& estimated_transformations) {
    // Check that we get a single solution.
    EXPECT_GE(estimated_transformations.rotations.size(), 1);
    EXPECT_GE(estimated_transformations.rotations.size(),
              estimated_transformations.translations.size());
    EXPECT_GE(estimated_transformations.rotations.size(),
              estimated_transformations.scales.size());

    // Estimated solution.
    const int num_solutions = estimated_transformations.rotations.size();
    const Eigen::Quaterniond& expected_rotation =
        expected_transformation.rotation;
    const Eigen::Vector3d& expected_translation =
        expected_transformation.translation;
    const double& expected_scale = expected_transformation.scale;
    int num_trials_passed = 0;
    // Evaluation of solutions.
    for (int i = 0; i < num_solutions; ++i) {
      const Eigen::Quaterniond& estimated_rotation =
          estimated_transformations.rotations[i];
      const Eigen::Vector3d& estimated_translation =
          estimated_transformations.translations[i];
      const double& estimated_scale = estimated_transformations.scales[i];
      // Compute errors.
      const double rotation_error =
          expected_rotation.angularDistance(estimated_rotation);
      const double translation_error =
          (expected_translation - estimated_translation).norm();
      const double scale_error = std::abs(expected_scale - estimated_scale);

      const bool good_rotation = rotation_error < test_params.rotation_thresh;
      const bool good_translation =
           translation_error < test_params.translation_thresh;
      const bool good_scale = scale_error < test_params.scale_thresh;
      const bool good_run = good_rotation && good_translation && good_scale;

      VLOG_IF(3, !good_run) << "Solution: " << (i + 1) << "/" << num_solutions
                            << "\nRotation error: " << rotation_error
                            << " thresh: " << test_params.rotation_thresh
                            << " good: " << good_rotation
                            << "\nTranslation error: " << translation_error
                            << " thresh: " << test_params.translation_thresh
                            << " good: " << good_translation
                            << "\nScale error: " << scale_error
                            << " thresh: " << test_params.scale_thresh
                            << " good: " << good_scale;

      if (good_run) {
        ++num_trials_passed;
      }
    }

    return num_trials_passed > 0;
  }

  void ExecuteRandomTest(
      const TestParameters& test_params,
      const SimilarityTransformation& expected_transformation,
      const GdlsStar::Priors& priors) {
    // Compute 2D-3D correspondences.
    const std::vector<CameraFeatureCorrespondence2D3D> correspondences =
        GenerateCorrespondences(test_params, expected_transformation);

    // Estimate similarity transformation.
    const GdlsStar::Solution solution =
        EstimateSimilarityTransformation(priors, correspondences);

    // Evaluate solution.
    EXPECT_TRUE(EvaluateSolution(test_params,
                                 expected_transformation,
                                 solution));
  }

  void ExecuteRansacRandomTest(
      const TestParameters& test_params,
      const SimilarityTransformation& expected_transformation,
      const GdlsStar::Priors& priors) {
    // Compute 2D-3D correspondences.
    const std::vector<CameraFeatureCorrespondence2D3D> correspondences =
        GenerateCorrespondences(test_params, expected_transformation);

    // Estimate similarity transformation.
    GdlsStarRobustEstimator::RansacSummary ransac_summary;
    const GdlsStar::Solution solution =
        EstimateSimilarityTransformation(test_params.ransac_params,
                                         priors,
                                         correspondences,
                                         &ransac_summary);

    // Evaluate solution.
    EXPECT_TRUE(EvaluateSolution(test_params,
                                 expected_transformation,
                                 solution));
  }

  // Random number generator.
  static std::mt19937* rng;
};

std::mt19937* EstimateSimilarityTransformationTest::rng = nullptr;

// Minimal estimation tests.
TEST_F(EstimateSimilarityTransformationTest,
       MinimalBasicEstimationNoPriorsNoNoise) {
  TestParameters test_params;
  const GdlsStar::Priors priors;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    ExecuteRandomTest(test_params, expected_transformation, priors);
  }
}

TEST_F(EstimateSimilarityTransformationTest,
       MinimalBasicEstimationWithNoiseNoPriors) {
  TestParameters test_params;
  test_params.noise_std_dev = 1e-3;
  test_params.rotation_thresh = DegToRad(5.0);
  test_params.translation_thresh = 1.0;
  test_params.scale_thresh = 0.5;
  const GdlsStar::Priors priors;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    ExecuteRandomTest(test_params, expected_transformation, priors);
  }
}

// Non-minimal estimation tests.
TEST_F(EstimateSimilarityTransformationTest,
       NonMinimalBasicEstimationNoPriorsNoNoise) {
  TestParameters test_params;
  test_params.num_points = kNumPoints;
  const GdlsStar::Priors priors;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    ExecuteRandomTest(test_params, expected_transformation, priors);
  }
}

TEST_F(EstimateSimilarityTransformationTest,
       NonMinimalBasicEstimationWithNoiseNoPriors) {
  TestParameters test_params;
  test_params.num_points = kNumPoints;
  test_params.noise_std_dev = 1e-3;
  test_params.rotation_thresh = DegToRad(5.0);
  test_params.translation_thresh = 1.0;
  test_params.scale_thresh = 0.5;
  const GdlsStar::Priors priors;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    ExecuteRandomTest(test_params, expected_transformation, priors);
  }
}

// Minimal estimatio tests w/ priors.
TEST_F(EstimateSimilarityTransformationTest,
       MinimalBasicEstimationWithPriorsNoNoise) {
  const TestParameters test_params;
  GdlsStar::Priors priors;
  priors.scale_penalty = 1.0;
  priors.gravity_penalty = 1.0;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    priors.scale_prior = expected_transformation.scale;
    priors.world_down_direction = Eigen::Vector3d::UnitZ();
    priors.query_down_direction =
        expected_transformation.rotation * priors.world_down_direction;
    ExecuteRandomTest(test_params, expected_transformation, priors);
  }
}

TEST_F(EstimateSimilarityTransformationTest,
       MinimalBasicEstimationWithPriorsAndNoise) {
  TestParameters test_params;
  test_params.noise_std_dev = 1e-3;
  test_params.rotation_thresh = DegToRad(5.0);
  test_params.translation_thresh = 1.0;
  test_params.scale_thresh = 0.5;
  GdlsStar::Priors priors;
  priors.scale_penalty = 1.0;
  priors.gravity_penalty = 1.0;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    priors.scale_prior = expected_transformation.scale;
    priors.world_down_direction = Eigen::Vector3d::UnitZ();
    priors.query_down_direction =
        expected_transformation.rotation * priors.world_down_direction;
    ExecuteRandomTest(test_params, expected_transformation, priors);
  }
}

// Non-minimal estimatio tests w/ priors.
TEST_F(EstimateSimilarityTransformationTest,
       NonMinimalBasicEstimationWithPriorsNoNoise) {
  TestParameters test_params;
  test_params.num_points = kNumPoints;
  GdlsStar::Priors priors;
  priors.scale_penalty = 1.0;
  priors.gravity_penalty = 1.0;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    priors.scale_prior = expected_transformation.scale;
    priors.world_down_direction = Eigen::Vector3d::UnitZ();
    priors.query_down_direction =
        expected_transformation.rotation * priors.world_down_direction;
    ExecuteRandomTest(test_params, expected_transformation, priors);
  }
}

TEST_F(EstimateSimilarityTransformationTest,
       NonMinimalBasicEstimationWithPriorsAndNoise) {
  TestParameters test_params;
  test_params.num_points = kNumPoints;
  test_params.noise_std_dev = 1e-3;
  test_params.rotation_thresh = DegToRad(5.0);
  test_params.translation_thresh = 1.0;
  test_params.scale_thresh = 0.5;
  GdlsStar::Priors priors;
  priors.scale_penalty = 1.0;
  priors.gravity_penalty = 1.0;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    priors.scale_prior = expected_transformation.scale;
    priors.world_down_direction = Eigen::Vector3d::UnitZ();
    priors.query_down_direction =
        expected_transformation.rotation * priors.world_down_direction;
    ExecuteRandomTest(test_params, expected_transformation, priors);
  }
}

// Test RANSAC estimators all inliers.
TEST_F(EstimateSimilarityTransformationTest, AllInliersWithNoiseNoPriors) {
  TestParameters test_params;
  test_params.num_points = kNumPoints;
  test_params.noise_std_dev = 1e-3;
  test_params.rotation_thresh = DegToRad(5.0);
  test_params.translation_thresh = 1.0;
  test_params.scale_thresh = 0.5;
  const GdlsStar::Priors priors;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    ExecuteRansacRandomTest(test_params, expected_transformation, priors);
  }
}

// RANSAC w/ inliers and outliers and noise but w/o priors.
TEST_F(EstimateSimilarityTransformationTest,
       InliersAndOutliersWithNoiseNoPriors) {
  TestParameters test_params;
  test_params.num_points = kNumPoints;
  test_params.noise_std_dev = 1e-3;
  test_params.rotation_thresh = DegToRad(5.0);
  test_params.translation_thresh = 1.0;
  test_params.scale_thresh = 0.5;
  test_params.inlier_ratio = 0.8;
  const GdlsStar::Priors priors;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    ExecuteRansacRandomTest(test_params, expected_transformation, priors);
  }
}

// RANSAC w/ inliers and outliers, noise, and priors.
TEST_F(EstimateSimilarityTransformationTest,
       InliersAndOutliersWithNoiseAndPriors) {
  TestParameters test_params;
  test_params.num_points = kNumPoints;
  test_params.noise_std_dev = 1e-3;
  test_params.rotation_thresh = DegToRad(5.0);
  test_params.translation_thresh = 1.0;
  test_params.scale_thresh = 0.5;
  test_params.inlier_ratio = 0.8;
  GdlsStar::Priors priors;
  priors.scale_penalty = 1.0;
  priors.gravity_penalty = 1.0;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    priors.scale_prior = expected_transformation.scale;
    priors.world_down_direction = Eigen::Vector3d::UnitZ();
    priors.query_down_direction =
        expected_transformation.rotation * priors.world_down_direction;
    ExecuteRansacRandomTest(test_params, expected_transformation, priors);
  }
}

// RANSAC w/ inliers and outliers, noise, and gravity prior.
TEST_F(EstimateSimilarityTransformationTest,
       InliersAndOutliersWithNoiseAndGravityPrior) {
  TestParameters test_params;
  test_params.num_points = kNumPoints;
  test_params.noise_std_dev = 1e-3;
  test_params.rotation_thresh = DegToRad(5.0);
  test_params.translation_thresh = 1.0;
  test_params.scale_thresh = 0.5;
  test_params.inlier_ratio = 0.8;
  GdlsStar::Priors priors;
  priors.scale_penalty = 0.0;  // Disabling scale prior.
  priors.gravity_penalty = 1.0;
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    priors.world_down_direction = Eigen::Vector3d::UnitZ();
    priors.query_down_direction =
        expected_transformation.rotation * priors.world_down_direction;
    ExecuteRansacRandomTest(test_params, expected_transformation, priors);
  }
}

// RANSAC w/ inliers and outliers, noise, and scale prior.
TEST_F(EstimateSimilarityTransformationTest,
       InliersAndOutliersWithNoiseAndScalePrior) {
  TestParameters test_params;
  test_params.num_points = kNumPoints;
  test_params.noise_std_dev = 1e-3;
  test_params.rotation_thresh = DegToRad(5.0);
  test_params.translation_thresh = 1.0;
  test_params.scale_thresh = 0.5;
  test_params.inlier_ratio = 0.8;
  GdlsStar::Priors priors;
  priors.scale_penalty = 1.0;
  priors.gravity_penalty = 0.0;  // Disabling gravity prior.
  for (int i = 0; i < kNumTrials; ++i) {
    const SimilarityTransformation expected_transformation =
        GenerateRandomSimilarityTransformation(test_params);
    priors.scale_prior = expected_transformation.scale;
    ExecuteRansacRandomTest(test_params, expected_transformation, priors);
  }
}

}  // namespace
}  // namespace msft


#include <boost/numeric/conversion/cast.hpp>
#include <cradle/imaging/foreach.hpp>
#include <cradle/imaging/statistics.hpp>

namespace cradle {

struct discretize_pixel_fn
{
    double a, b;
    template<class Discrete, class Continuous>
    void
    operator()(Discrete& dst, Continuous& src)
    {
        dst = Discrete(src * a + b);
    }
};

template<unsigned N, class DiscreteT, class ContinuousT, class SourceSP>
void
discretize(
    image<N, DiscreteT, shared>& result,
    image<N, ContinuousT, SourceSP> const& src,
    unsigned result_max)
{
    double src_min, src_max;
    auto min_max = get(image_min_max(src));
    src_min = min_max.min;
    src_max = min_max.max;
    double scale = (src_max - src_min) / result_max;

    discretize_pixel_fn fn;
    fn.a = src.value_mapping.slope / scale;
    fn.b = (src.value_mapping.intercept - src_min) / scale + 0.5;

    image<N, DiscreteT, unique> tmp;
    create_image(tmp, src.size);
    foreach_pixel2(tmp, src, fn);

    copy_spatial_mapping(tmp, src);
    tmp.value_mapping = linear_function<double>(src_min, scale);
    tmp.units = src.units;

    result = share(tmp);
}

template<unsigned N, class DiscreteT, class ContinuousT, class SourceSP>
void
discretize(
    image<N, DiscreteT, shared>& result,
    image<N, ContinuousT, SourceSP> const& src,
    linear_function<double> const& value_mapping)
{
    discretize_pixel_fn fn;
    fn.a = src.value_mapping.slope / value_mapping.slope,
    fn.b = (src.value_mapping.intercept - value_mapping.intercept)
               / value_mapping.slope
           + 0.5;

    image<N, DiscreteT, unique> tmp;
    create_image(tmp, src.size);
    foreach_pixel2(tmp, src, fn);

    copy_spatial_mapping(tmp, src);
    tmp.value_mapping = value_mapping;
    tmp.units = src.units;

    result = share(tmp);
}

namespace impl {

template<unsigned N, class DiscreteT>
struct manual_discretize_functor
{
    image<N, DiscreteT, shared>* result;
    linear_function<double> value_mapping;
    template<class ContinuousT, class SourceSP>
    void
    operator()(image<N, ContinuousT, SourceSP> const& src)
    {
        discretize(*result, src, value_mapping);
    }
};
} // namespace impl

template<unsigned N, class DiscreteT, class SourceSP>
void
discretize(
    image<N, DiscreteT, shared>& result,
    image<N, variant, SourceSP> const& src,
    linear_function<double> const& value_mapping)
{
    impl::manual_discretize_functor<N, DiscreteT> fn;
    fn.result = &result;
    fn.value_mapping = value_mapping;
    apply_fn_to_gray_variant(fn, src);
}

} // namespace cradle


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2020, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 * Created on June 8, 2020, 9:17 AM
 */

#include <gtest/gtest.h>

#include <Eigen/Eigen>
#include <boost/math/constants/constants.hpp>

#include "MaterialLib/MPL/Medium.h"
#include "MaterialLib/MPL/Properties/CreatePermeabilityMohrCoulombFailureIndexModel.h"
#include "MaterialLib/MPL/Properties/PermeabilityMohrCoulombFailureIndexModel.h"
#include "MaterialLib/MPL/Utils/FormEigenTensor.h"
#include "MathLib/KelvinVector.h"
#include "ParameterLib/ConstantParameter.h"
#include "TestMPL.h"
#include "Tests/TestTools.h"

TEST(MaterialPropertyLib, PermeabilityMohrCoulombFailureIndexModel)
{
    ParameterLib::ConstantParameter<double> const k0("k0", 1.e-20);
    double const kr = 1.0e-19;
    double const b = 3.0;
    double const c = 1.0e+6;
    double const phi = 15.0;
    double const k_max = 1.e-10;
    double const t_sigma_max = c;

    auto const k_model = MPL::PermeabilityMohrCoulombFailureIndexModel<3>(
        "k_f", k0, kr, b, c, phi, k_max, t_sigma_max, nullptr);

    const int symmetric_tensor_size = 6;
    using SymmetricTensor = Eigen::Matrix<double, symmetric_tensor_size, 1>;

    // Under failure, i,e stress beyond the yield.
    SymmetricTensor stress;
    stress[0] = -1.36040e+7;
    stress[1] = -1.78344e+7;
    stress[2] = -1.36627e+7;
    stress[3] = -105408;
    stress[4] = -25377.2;
    stress[5] = -1.39944e+7;

    MPL::VariableArray vars;
    vars[static_cast<int>(MPL::Variable::total_stress)].emplace<SymmetricTensor>(
        stress);

    ParameterLib::SpatialPosition const pos;
    double const t = std::numeric_limits<double>::quiet_NaN();
    double const dt = std::numeric_limits<double>::quiet_NaN();

    auto const k = MPL::formEigenTensor<3>(k_model.value(vars, pos, t, dt));

    double const k_expected = 1.1398264890628033e-15;

    ASSERT_LE(std::fabs(k_expected - k(0, 0)) / k_expected, 1e-10)
        << "for expected changed permeability " << k_expected
        << " and for computed changed permeability." << k(0, 0);

    // Stress outside of the yield surface. No change in permeability
    stress[0] = -1.2e+7;
    stress[1] = -1.5e+7;
    stress[2] = -1.2e+7;
    stress[3] = -1e+5;
    stress[4] = -2200.2;
    stress[5] = -8e+5;
    vars[static_cast<int>(MPL::Variable::total_stress)] = stress;
    auto const k_non_f =
        MPL::formEigenTensor<3>(k_model.value(vars, pos, t, dt));

    auto const k_non_f_expected = k0(t, pos)[0];

    ASSERT_LE(std::fabs(k_non_f_expected - k_non_f(0, 0)) / k_non_f_expected,
              1e-19)
        << "for expected untouched permeability" << k_non_f_expected
        << " and for computed untouched permeability." << k_non_f(0, 0);

    // Stress at the apex. No change in permeability.
    const double val =
        2.0 * c / std::tan(phi * boost::math::constants::degree<double>());
    stress[0] = 0.7 * val;
    stress[1] = 0.3 * val;
    stress[2] = 0.3 * val;
    stress[3] = 0.0;
    stress[4] = 0.0;
    stress[5] = 0.0;
    vars[static_cast<int>(MPL::Variable::total_stress)] = stress;
    auto const k_apex_f =
        MPL::formEigenTensor<3>(k_model.value(vars, pos, t, dt));

    double const k_apex_expected = 7.2849707418474819e-15;
    ASSERT_LE(std::fabs(k_apex_expected - k_apex_f(0, 0)) / k_apex_expected,
              5e-16)
        << "for expected untouched permeability" << k_non_f_expected
        << " and for computed untouched permeability." << k_apex_f(0, 0);
}


/*******************************************************************************
 * This domain is a simplified version of the paper:
 *
 * "A Partial-Order Approach to Array Content Analysis" by
 *  Gange, Navas, Schachte, Sondergaard, and Stuckey
 *  available here http://arxiv.org/pdf/1408.1754v1.pdf.
 *
 * It reasons about array contents and the idea is to compute all the
 * feasible partial orderings between array indexes. It keeps a single
 * graph where vertices are the array indexes (potentially all scalar
 * variables) and edges are labelled with weights (that includes
 * scalar and array variables).  An edge (i,j) with weight w denotes
 * that the property w holds for the all elements in the array between
 * [i,j).
 ******************************************************************************/

/* Limitations:

  - The implementation is just a proof-of-concept so it is horribly
    inefficient. I have not tried to make it more efficient yet or
    ran even with real programs.
  - Assume all array accesses are aligned wrt to the size of the array
    element (e.g., if the size of the array element is 4 bytes then
    all array accesses must be multiple of 4).
  - Assume that the size of the array element is always
    1. Therefore, if the array indexes are incremented or decremented
    by 2,4,... we will lose all the precision.

  FIXMEs:

  - Use AdaptGraph instead of boost::graph
  - Have a flag is_normalized and normalize only if the flag is false
  - Perform incremental Floyd-Warshall by keeping track of changed edges.
  - array_graph::widening is normalizing both operands. The first
    operand cannot be normalized.
  - perform common renaming before binary operations (join/widening/narrowing/meet)
  - Do no use shared_ptr because we are doing deep copies Use magic
    move semantics and also shared references with copy-on-write to
    avoid unnecessary copies.
  - etc.

 */

#ifndef ARRAY_GRAPH_HPP
#define ARRAY_GRAPH_HPP

#include <boost/optional.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/copy.hpp>
#include <boost/lexical_cast.hpp>

#include <crab/common/types.hpp>
#include <crab/common/debug.hpp>
#include <crab/common/stats.hpp>
#include <crab/domains/patricia_trees.hpp>
#include <crab/domains/operators_api.hpp>
#include <crab/domains/domain_traits.hpp>

using namespace std;
using namespace boost;
using namespace ikos;

namespace crab {

  namespace domains {

     /*
       A weighted array graph is a graph (V,E,L) where V is the set of
       vertices, E the edges and L is a label function: E -> W such that:
       - If there is an edge e from vertex i to j whose weight is not
       bottom (i.e., L(e) != bot) then it means that i < j.
       - It is possible one non-bottom edge from i to j and another
       non-bottom one from j to i. This means that both i < j and j < i
       are possible.
       - If the both edges from i to j and from j to i are bottom then it
       must be that (i>=j) && (j>=i) = (i==j)
     */
     template< typename VertexName, typename Weight,
               typename ScalarNumDomain, bool IsDistWeight >
     class array_graph: public writeable{

       typedef VertexName VertexNameKey;

       template < typename Any1, typename Any2, bool Any5>
       friend class array_graph_domain;

       typedef index_t key_t;
       typedef boost::shared_ptr<VertexName> VertexNamePtr;
       typedef boost::shared_ptr<Weight>     WeightPtr;
       struct  graph_vertex_t { VertexNamePtr name; };
       struct  graph_edge_t   { WeightPtr weight; };

      public:
       typedef array_graph<VertexName,Weight,ScalarNumDomain,IsDistWeight> array_graph_t;
       typedef boost::tuple<VertexName, VertexName, Weight> edge_t;

      private:
       typedef adjacency_list<listS,listS,bidirectionalS,graph_vertex_t,graph_edge_t> graph_t;

       typedef typename graph_traits<graph_t>::edge_iterator     edge_iterator;
       typedef typename graph_traits<graph_t>::vertex_iterator   vertex_iterator;
       typedef typename graph_traits<graph_t>::edge_descriptor   edge_descriptor_t;
       typedef typename graph_traits<graph_t>::vertex_descriptor vertex_descriptor_t;
       typedef typename graph_traits<graph_t>::out_edge_iterator out_edge_iterator;
       typedef typename graph_traits<graph_t>::in_edge_iterator  in_edge_iterator;

       typedef typename ScalarNumDomain::linear_constraint_t     linear_constraint_t;
       typedef typename ScalarNumDomain::variable_t              variable_t;

       typedef boost::unordered_map<key_t, vertex_descriptor_t >  vertex_map_t;

       typedef std::set<VertexName> vertex_names_set_t;

       typedef boost::shared_ptr <graph_t> graph_ptr;
       typedef boost::shared_ptr< vertex_map_t > vertex_map_ptr;
       typedef boost::shared_ptr< vertex_names_set_t > vertex_names_set_ptr;

       bool _is_bottom;
       graph_ptr  _graph;
       vertex_map_ptr _vertex_map;   //! map a VertexName to a graph vertex
       vertex_names_set_ptr _vertices_set;

       bool find_vertex_map (VertexName v) {
         return (_vertex_map->find(v.index()) != _vertex_map->end());
       }

       void insert_vertex_map (VertexName key, vertex_descriptor_t value)
       {
         if (find_vertex_map(key))
           CRAB_ERROR (key," already in the vertex map");

         _vertex_map->insert (make_pair(key.index(), value));
         _vertices_set->insert (key);
       }

       void remove_vertex_map (VertexName key)
       {
         _vertex_map->erase (key.index());
         _vertices_set->erase (key);
       }

       vertex_descriptor_t lookup_vertex_map (VertexName key) const
       {
         auto it = _vertex_map->find(key.index());
         if (it != _vertex_map->end())
           return it->second;

         CRAB_ERROR ("No vertex with name ",key," found in the graph");
       }

       // All methods that add new vertices should call this one.
       void add (vector<VertexName> vertices, vector<edge_t> edges)
       {
         for(auto v: vertices)
         {
           vertex_descriptor_t u = add_vertex(*_graph);
           (*_graph)[u].name = VertexNamePtr (new VertexName(v));
           insert_vertex_map(v, u);
         }

         for(auto e: edges)
         {
           vertex_descriptor_t u =  lookup_vertex_map(e.template get<0>());
           vertex_descriptor_t v =  lookup_vertex_map(e.template get<1>());
           edge_descriptor_t k; bool b;
           boost::tie(k,b) = add_edge(u, v, *_graph);
           if (!b)
             CRAB_ERROR ("edge is already in the graph");

           (*_graph)[k].weight = WeightPtr(new Weight(e.template get<2>()));
         }

         canonical();
       }

       // All methods that remove vertices should call this one.
       void remove (VertexName v)
       {
         if (!find_vertex_map (v)) return ;

         canonical();
         vertex_descriptor_t u = lookup_vertex_map(v);

         // remove all in and out edges to/from u
         clear_vertex(u, *_graph);

         // remove the vertex
         remove_vertex(u, *_graph);
         remove_vertex_map(v);
       }

       ///////////////////////////////////////////////////////////////////////
       // For our canonical form, we would like to compute the greatest
       // fixed point to the set of inequalities:
       //    \forall i,j,k. G[i,j] \subseteq G[i,k] \cup G[k,j]
       // If the weight domain is distributive we can solve this set of
       // inequations by solving:
       //    \forall i,j,k. G[i,j] = G[i,j] \cap G[i,k] \cup G[k,j]
       // The Floyd-Warshall algorithm does exactly that.
       // Otherwise, we iterate the Floyd-Warshall algorithm until no change.
       ///////////////////////////////////////////////////////////////////////
       bool oneStep()
       {
         binary_join_op join;
         binary_meet_op meet;
         vertex_iterator ki, ke, ii, ie, ji, je;
         bool change = false;
         for (tie(ki, ke) = vertices(*_graph); ki != ke; ++ki)
         {
           for (tie(ii, ie) = vertices(*_graph); ii != ie; ++ii)
           {
             if (edge(*ii, *ki, *_graph).second)
             {
               for (tie(ji, je) = vertices(*_graph); ji != je; ++ji)
               {
                 if (edge(*ii, *ji, (*_graph)).second &&
                     edge(*ki, *ji, (*_graph)).second)
                 {
                   auto e_ij = edge(*ii, *ji, (*_graph)).first;
                   auto e_ik = edge(*ii, *ki, (*_graph)).first;
                   auto e_kj = edge(*ki, *ji, (*_graph)).first;
                   auto Old = (*_graph)[e_ij].weight;
                   auto New = meet((*_graph)[e_ij].weight,
                                   (join((*_graph)[e_ik].weight,
                                         (*_graph)[e_kj].weight)));
                   change |= (!(*Old <= *New && *New <= *Old));
                   (*_graph)[e_ij].weight = New;
                 }
               }
             }
           }
         }
         return change;
       }

       void canonical()
       {
         if (IsDistWeight)
           oneStep ();
         else
         {
           bool change = true;
           while (change)
             change = oneStep ();
         }
       }

       void insert_vertex (VertexName u, Weight val = Weight::top())
       {
         if (!is_bottom () && !find_vertex_map(u))
         {
           vector<VertexName> new_vertices;
           new_vertices.push_back(u);
           vector<edge_t> new_edges;
           vertex_iterator i, e;
           for (tie(i, e) = vertices(*_graph); i != e; ++i)
           {
             VertexNamePtr v = (*_graph)[*i].name;
             // add two edges in both directions
             new_edges.push_back(edge_t(u, *v, val));
             new_edges.push_back(edge_t(*v, u, val));
           }
           add(new_vertices, new_edges);
         }
       }

       template<typename Iterator>
       void insert_vertices(array_graph_t &g, Iterator begin, Iterator end)
       {
         for(;begin!=end;++begin)
           g.insert_vertex(begin->name());
       }

       // pre: caller must ensure the graph is in canonical form
       void set_incoming (const VertexName &v, const Weight &weight)
       {
         if (!is_bottom ())
         {
           vertex_descriptor_t u = lookup_vertex_map(v);
           in_edge_iterator in_it, in_et;
           for (tie(in_it, in_et) = in_edges(u, *_graph); in_it != in_et; ++in_it)
             (*_graph)[*in_it].weight = WeightPtr( new Weight(weight));
         }
       }

       // pre: caller must ensure the graph is in canonical form
       void set_outgoing (const VertexName &v, const Weight &weight)
       {
         if (!is_bottom ())
         {
           vertex_descriptor_t u = lookup_vertex_map(v);
           out_edge_iterator out_it, out_et;
           for (tie(out_it, out_et) = out_edges(u, *_graph);
                out_it != out_et; ++out_it)
             (*_graph)[*out_it].weight = WeightPtr( new Weight(weight));
         }
       }

       struct binary_join_op{
         WeightPtr operator()(WeightPtr w1, WeightPtr w2) {
           return WeightPtr(new Weight(*w1 | *w2));
         }
       };

       struct binary_meet_op{
         WeightPtr operator()(WeightPtr w1, WeightPtr w2) {
           return WeightPtr(new Weight(*w1 & *w2));
         }
       };

       struct binary_widening_op{
         WeightPtr operator()(WeightPtr w1, WeightPtr w2) {
           return WeightPtr(new Weight(*w1 || *w2));
         }
       };

       struct binary_narrowing_op{
         WeightPtr operator()(WeightPtr w1, WeightPtr w2) {
           return WeightPtr(new Weight(*w1 && *w2));
         }
       };

       template<typename Op>
       void pointwise_binop_helper (array_graph_t &g1,
                                    const array_graph_t &g2)
       {
         // pre: g1 and g2 have the same adjacency structure
         edge_iterator it_1, et_1;
         for(tie(it_1,et_1) = edges(*g1._graph); it_1 != et_1; ++it_1)
         {
           edge_descriptor_t e_1   = *it_1;
           vertex_descriptor_t u_1 = source(e_1, *g1._graph);
           vertex_descriptor_t v_1 = target(e_1, *g1._graph);
           VertexNamePtr u_name_1  = (*g1._graph)[u_1].name;
           VertexNamePtr v_name_1  = (*g1._graph)[v_1].name;
           WeightPtr     weight_1  = (*g1._graph)[e_1].weight;
           vertex_descriptor_t u_2 = g2.lookup_vertex_map(*u_name_1);
           vertex_descriptor_t v_2 = g2.lookup_vertex_map(*v_name_1);
           if (edge(u_2, v_2, *g2._graph).second)
           {
             Op op;
             edge_descriptor_t e_2 = edge(u_2, v_2, *g2._graph).first;
             (*g1._graph)[e_1].weight =
                 op((*g1._graph)[e_1].weight, (*g2._graph)[e_2].weight);
           }
           else
             CRAB_ERROR("unreachable");
         }
       }

       template<typename Op>
       array_graph_t pointwise_binop (array_graph_t g1,
                                      array_graph_t g2)
       {
         g1.canonical();
         g2.canonical();

         // if (*(g1._vertices_set) != *(g2._vertices_set)){
         //   set<VertexName> all_vs;
         //   set_union(g1._vertices_set->begin(), g1._vertices_set->end(),
         //             g2._vertices_set->begin(), g2._vertices_set->end(), inserter(all_vs, all_vs.end()));
         //   vector<VertexName> new_g1, new_g2;
         //   set_difference(all_vs.begin(), all_vs.end(),
         //                  g1._vertices_set->begin(), g1._vertices_set->end(), inserter(new_g1, new_g1.end()));
         //   set_difference(all_vs.begin(), all_vs.end(),
         //                  g2._vertices_set->begin(), g2._vertices_set->end(), inserter(new_g2, new_g2.end()));
         //   insert_vertices<typename vector<VertexName>::iterator>(g1, new_g1.begin(), new_g1.end());
         //   insert_vertices<typename vector<VertexName>::iterator>(g2, new_g2.begin(), new_g2.end());
         // }

         // pre: g1 and g2 have the same set of vertices and edges at this
         // point
         pointwise_binop_helper<Op>(g1,g2);
         return g1;
       }

       array_graph(bool is_bot):
           _is_bottom(is_bot),
           _graph(new graph_t(0)),
           _vertex_map(new vertex_map_t()),
           _vertices_set(new vertex_names_set_t())
       {  }

      public:

       static array_graph_t bottom()  { return array_graph(true); }

       static array_graph_t top() { return array_graph(false); }

       // Deep copy of the array graph
       array_graph(const array_graph_t &other):
           writeable(),
           _is_bottom(other._is_bottom),
           //_graph(new graph_t(*other._graph)),
           //_vertex_map(new vertex_map_t(*other._vertex_map)),
           //_vertices_set( new vertex_names_set_t(*other._vertices_set))
           _graph (new graph_t(0)),
           _vertex_map (new vertex_map_t()),
           _vertices_set (new vertex_names_set_t())
       {
         crab::CrabStats::count ("Domain.count.copy");
         crab::ScopedCrabStats __st__("Domain.copy");

         if (!is_bottom ())
         {
           // copy vertices, _vertex_map and _vertices_set
           vertex_iterator i, e;
           for (tie(i, e) = vertices(*other._graph); i != e; ++i)
           {
             vertex_descriptor_t u  = add_vertex(*_graph);
             VertexName u_name = *((*other._graph)[*i].name);
             (*_graph)[u].name = VertexNamePtr(new VertexName(u_name));
             insert_vertex_map(u_name, u);
           }

           // copy edges
           edge_iterator ie, ee;
           for(tie(ie,ee) = edges(*other._graph); ie != ee; ++ie)
           {
             VertexName u = *((*other._graph)[source(*ie, *other._graph)].name);
             VertexName v = *((*other._graph)[target(*ie, *other._graph)].name);
             Weight     w = *((*other._graph)[*ie].weight);
             vertex_descriptor_t _u = lookup_vertex_map(u);
             vertex_descriptor_t _v = lookup_vertex_map(v);
             edge_descriptor_t _e; bool b;
             boost::tie(_e,b) = add_edge(_u, _v, *_graph);
             (*_graph)[_e].weight = WeightPtr(new Weight(w));
           }
         }
       }

       array_graph_t& operator=(const array_graph_t &other)
       {
         crab::CrabStats::count ("Domain.count.copy");
         crab::ScopedCrabStats __st__("Domain.copy");
         if (this != &other)
         {
           _is_bottom      = other._is_bottom;
           _graph          = other._graph;
           _vertex_map     = other._vertex_map;
           _vertices_set   = other._vertices_set;
         }
         return *this;
       }

       bool is_bottom() { return this->_is_bottom; }

       bool is_top()
       {
         if (this->is_bottom())
           return false;
         else
         {
           // FIXME: speedup this operation
           canonical();
           edge_iterator it, et;
           for(tie(it,et) = edges(*_graph); it != et; ++it)
           {
             edge_descriptor_t e = *it;
             if (!(*(*_graph)[e].weight).is_top ())
               return false;
           }
           return true;
         }
       }

       void reduce(ScalarNumDomain scalar)
       {
         if (is_bottom ()) return;

         canonical();

         edge_iterator it, et;
         for(tie(it,et) = edges(*_graph); it != et; ++it)
         {
           edge_descriptor_t e   = *it;
           VertexNamePtr u = (*_graph)[source(e, *_graph)].name;
           VertexNamePtr v = (*_graph)[target(e, *_graph)].name;
           ScalarNumDomain tmp(scalar);
           linear_constraint_t cst ( variable_t(*u) <= variable_t(*v) - 1);
           tmp += cst;
           if (tmp.is_bottom())
             (*_graph)[e].weight = WeightPtr(new Weight(Weight::bottom()));
         }

         canonical();
       }

       // Point-wise application of <= in the weight domain
       bool operator <=(array_graph_t other)
       {
         if (is_bottom())
           return true;
         else if (other.is_bottom())
           return false;
         else
         {
           canonical();
           other.canonical();
           edge_iterator it_1, et_1;
           edge_iterator it_2, et_2;
           for(tie(it_1,et_1) = edges(*_graph); it_1 != et_1; ++it_1)
           {
             edge_descriptor_t e_1 = *it_1;
             vertex_descriptor_t u_1 = source(e_1, *_graph);
             vertex_descriptor_t v_1 = target(e_1, *_graph);
             VertexNamePtr u_name_1 = (*_graph)[u_1].name;
             VertexNamePtr v_name_1 = (*_graph)[v_1].name;
             WeightPtr     weight_1 = (*_graph)[e_1].weight;
             vertex_descriptor_t u_2 = other.lookup_vertex_map(*u_name_1);
             vertex_descriptor_t v_2 = other.lookup_vertex_map(*v_name_1);
             if (edge(u_2,v_2, *other._graph).second)
             {
               edge_descriptor_t e_2 = edge(u_2,v_2, *other._graph).first;
               WeightPtr weight_2 = (*other._graph)[e_2].weight;
               if (!(*weight_1 <= *weight_2))
                 return false;
             }
             else
               CRAB_ERROR ("operator<= with graphs with different adjacency structure");
           }
           return true;
         }
       }

       bool operator==(array_graph_t other)
       {
         if (is_bottom()) return other.is_bottom();
         else
           return (*(_vertices_set) == *(other._vertices_set) &&
                   ( *this <= other && other <= *this));
       }

       void operator-=(VertexName v)
       {
         if (!is_bottom ())
           remove (v);
       }

       void operator|=(array_graph_t other) {
         *this = *this | other;
       }

       // Point-wise join in the weight domain
       array_graph_t operator|(array_graph_t other)
       {
         if (is_bottom())
           return other;
         else if (other.is_bottom())
           return *this;
         else
           return pointwise_binop<binary_join_op>(*this, other);
       }

       // Point-wise widening in the weight domain
       array_graph_t operator||(array_graph_t other)
       {
         if (is_bottom())
           return other;
         else if (other.is_bottom())
           return *this;
         else
           return pointwise_binop<binary_widening_op>(*this, other);
       }

       // Point-wise meet in the weight domain
       array_graph_t operator&(array_graph_t other)
       {
         if (this->is_bottom())
           return *this;
         else if (other.is_bottom())
           return other;
         else {
           return pointwise_binop<binary_meet_op>(*this, other);
         }
       }

       // Point-wise narrowing in the weight domain
       array_graph_t operator&&(array_graph_t other)
       {
         if (this->is_bottom())
           return *this;
         else if (other.is_bottom())
           return other;
         else {
           return pointwise_binop<binary_narrowing_op>(*this, other);
         }
       }

       void meet_weight (const VertexName &src, const VertexName &dest,
                         Weight weight)
       {
         if (find_vertex_map(src) && find_vertex_map(dest))
         {
           vertex_descriptor_t u = lookup_vertex_map(src);
           vertex_descriptor_t v = lookup_vertex_map(dest);
           if (edge(u,v,*_graph).second) {
             edge_descriptor_t e = edge(u,v,*_graph).first;
             Weight meet = weight & (*(*_graph)[e].weight);
             (*_graph)[e].weight = WeightPtr(new Weight(meet));
           }
           else {
             vector<VertexName> vertices;
             vector<edge_t>     edges;
             edges.push_back(edge_t(src,dest,weight));
             add(vertices,edges);
           }
         }
       }

       void set_weight (const VertexName &src, const VertexName &dest,
                        Weight weight)
       {
         if (find_vertex_map(src) && find_vertex_map(dest))
         {
           vertex_descriptor_t u = lookup_vertex_map(src);
           vertex_descriptor_t v = lookup_vertex_map(dest);
           if (edge(u,v,*_graph).second) {
             edge_descriptor_t e = edge(u,v,*_graph).first;
             (*_graph)[e].weight = WeightPtr(new Weight(weight));
           }
           else {
             vector<VertexName> vertices;
             vector<edge_t>     edges;
             edges.push_back(edge_t(src,dest,weight));
             add(vertices,edges);
           }
         }
       }

       Weight& get_weight (const VertexName &src, const VertexName &dest)
       {
         if (find_vertex_map(src) && find_vertex_map(dest))
         {
           vertex_descriptor_t u = lookup_vertex_map(src);
           vertex_descriptor_t v = lookup_vertex_map(dest);
           if (edge(u,v,*_graph).second)
           {
             edge_descriptor_t e = edge(u,v,*_graph).first;
             return *((*_graph)[e].weight);
           }
         }
         CRAB_ERROR ("No edge found with given vertices");
       }

       void write(crab_os& o)
       {
         if (is_bottom())
           o << "_|_";
         else
         {
           {
             vertex_iterator it, et;
             o << "(V={";
             for (tie(it, et) = vertices(*_graph); it != et; ++it){
               vertex_descriptor_t u = *it;
               VertexNamePtr u_name  = (*_graph)[u].name;
               o << *u_name << " ";
             }
             o << "},";
           }
           {
             edge_iterator it, et;
             o << "E={";
             for(tie(it,et) = edges(*_graph); it!= et; ++it)
             {
               edge_descriptor_t e   = *it;
               vertex_descriptor_t u = source(e, *_graph);
               vertex_descriptor_t v = target(e, *_graph);
               VertexNamePtr u_name = (*_graph)[u].name;
               VertexNamePtr v_name = (*_graph)[v].name;
               WeightPtr     weight = (*_graph)[e].weight;
               if (!weight->is_bottom())
                 o << "(" << *u_name << "," << *v_name << "," << *weight << ") ";
             }
             o << "})";
           }
         }
       }
     }; // end class array_graph

    /*
      Reduced product of a scalar numerical domain with a weighted array
      graph.
    */
    template<typename ScalarNumDomain, typename WeightDomain, bool IsDistWeight = false>
    class array_graph_domain:
        public writeable,
        public numerical_domain<typename ScalarNumDomain::number_t,
                                typename ScalarNumDomain::varname_t>,
        public bitwise_operators<typename ScalarNumDomain::number_t,
                                 typename ScalarNumDomain::varname_t>,
        public division_operators<typename ScalarNumDomain::number_t,
                                  typename ScalarNumDomain::varname_t>,
        public array_operators<typename ScalarNumDomain::number_t,
                               typename ScalarNumDomain::varname_t >,
        public pointer_operators<typename ScalarNumDomain::number_t,
                                 typename ScalarNumDomain::varname_t > {

      template<typename Key, typename Value>
      class merge_op_check_equal: public patricia_tree< Key, Value >::binary_op_t {
        boost::optional< Value > apply(Value x, Value y) {
          if (x == y) return x;
          else
            CRAB_ERROR("merging a key with two different values");
        };
        bool default_is_absorbing() { return false; }
      };

      template<typename Key, typename Value>
      class merge_op_first: public patricia_tree< Key, Value >::binary_op_t {
        boost::optional< Value > apply(Value x, Value y)  {
          return x;
        };
        bool default_is_absorbing() { return false; }
      };

      template<typename Key, typename Value>
      class merge_op_second: public patricia_tree< Key, Value >::binary_op_t {
        boost::optional< Value > apply(Value x, Value y)  {
          return y;
        };
        bool default_is_absorbing() { return false; }
      };

     template < typename Key, typename Value,
                typename MergeOp = merge_op_check_equal <Key, Value> >
     class mergeable_map: public writeable {

      private:
       typedef patricia_tree< Key, Value > patricia_tree_t;
       typedef typename patricia_tree_t::binary_op_t binary_op_t;

      public:
       typedef mergeable_map< Key, Value > mergeable_map_t;
       typedef typename patricia_tree_t::iterator iterator;

      private:
       patricia_tree_t _tree;

       static patricia_tree_t do_union(patricia_tree_t t1, patricia_tree_t t2) {
         MergeOp o;
         t1.merge_with(t2, o);
         return t1;
       }

       mergeable_map(patricia_tree_t t): _tree(t) { }

      public:

       mergeable_map(): _tree(patricia_tree_t()) { }

       mergeable_map(const mergeable_map_t& e): writeable(), _tree(e._tree) { }

       mergeable_map_t& operator=(mergeable_map_t e) {
         _tree = e._tree;
         return *this;
       }

       iterator begin() { return _tree.begin(); }

       iterator end() { return _tree.end(); }

       std::size_t size(){ return _tree.size(); }

       void set(Key k, Value v) { _tree.insert(k, v); }

       mergeable_map_t& operator-=(Key k) {
         _tree.remove(k);
         return *this;
       }
       mergeable_map_t operator|(mergeable_map_t e) {
         mergeable_map_t u(do_union(_tree, e._tree));
         return u;
       }

       boost::optional<Value> operator[](Key k) { return _tree.lookup(k); }
       void clear() { _tree = patricia_tree_t(); }

       void write(crab_os& o) {
         o << "{";
         for (auto it = _tree.begin(); it != _tree.end(); ) {
           Key k = it->first;
           k.write(o);
           o << " -> ";
           Value v = it->second;
           o << v;
           ++it;
           if (it != _tree.end()) {
             o << "; ";
           }
         }
         o << "}";
       }
     }; // class mergeable_map

     public:
      typedef typename ScalarNumDomain::number_t Number;
      typedef typename ScalarNumDomain::varname_t VariableName;

      // WARNING: assumes ScalarNumDomain::number_t = WeightDomain::number_t and
      //                  ScalarNumDomain::varname_t = WeightDomain::varname_t
      using typename numerical_domain< Number, VariableName>::linear_expression_t;
      using typename numerical_domain< Number, VariableName>::linear_constraint_t;
      using typename numerical_domain< Number, VariableName>::linear_constraint_system_t;
      using typename numerical_domain< Number, VariableName>::variable_t;
      using typename numerical_domain< Number, VariableName>::number_t;
      using typename numerical_domain< Number, VariableName>::varname_t;
      typedef WeightDomain content_domain_t;
      typedef ScalarNumDomain index_domain_t;

     private:
      typedef array_graph< VariableName,WeightDomain,ScalarNumDomain,IsDistWeight> array_graph_t;
      typedef array_graph_domain<ScalarNumDomain,WeightDomain,IsDistWeight> array_graph_domain_t;

      typedef mergeable_map<VariableName,VariableName> succ_index_map_t;
      typedef boost::shared_ptr< succ_index_map_t > succ_index_map_ptr;

      bool  _is_bottom;
      ScalarNumDomain _scalar;
      array_graph_t _g;
      // for each array index i we keep track of a special index that
      // represent i+1
      succ_index_map_ptr _succ_idx_map;

      void abstract (VariableName v)
      {
        if (_g.find_vertex_map(v))
        {
          _g.set_incoming(v , WeightDomain::top());
          _g.set_outgoing(v , WeightDomain::top());
          optional<VariableName> succ_v = get_succ_idx(v);
          if (succ_v)
          {
            _g.set_incoming(*succ_v, WeightDomain::top());
            _g.set_outgoing(*succ_v, WeightDomain::top());
          }
        }
      }

      optional<VariableName> get_succ_idx (VariableName v) const
      {
        return (*_succ_idx_map)[v];
      }

      template <typename VariableFactory>
      VariableName add_variable (Number n, VariableFactory &vfac)
      {
        // FIXME: really big assumption that the variable factory
        // understands strings. For instance, this is not true if the
        // factory is created by Crab-llvm.
        VariableName var_n = vfac["#C" + n.get_str()];

        if (n >= 0)
        {
          _g.insert_vertex(var_n);
          _scalar.assign(var_n, n);
        }
        return var_n;
      }

      void add_variable (VariableName v)
      {
        if (is_array_index(v))
        {
          /*assign to v_succ a fresh var must be always the same*/
          VariableName v_succ = v.get_var_factory().get (v.index());

          _g.insert_vertex (v);
          _g.insert_vertex (v_succ);
          _succ_idx_map->set (v, v_succ);

          /// FIXME: assume that the array element size is 1.

          // --- Enforce: i+ == i+1
          _scalar += linear_constraint_t( variable_t (v_succ) == variable_t(v) + 1);
          // needed if scalar domain is non-relational:
          _g.set_weight (v_succ,v,WeightDomain::bottom ());
        }
      }

      void meet_weight (VariableName i, VariableName j, WeightDomain w)
      {
        add_variable (i);
        add_variable (j);
        _g.meet_weight (i,j,w);
        reduce();
      }

      template <typename VariableFactory>
      void meet_weight (Number i, Number j, WeightDomain w, VariableFactory &vfac)
      {
        _g.meet_weight (add_variable (i, vfac),
                        add_variable (j, vfac) ,
                        w);
        reduce();
      }

      void meet_weight (Number i, VariableName j, WeightDomain w)
      {
        add_variable (j);
        _g.meet_weight (add_variable(i, j.get_var_factory ()),j,w);
        reduce();
      }

      void meet_weight (VariableName i, Number j, WeightDomain w)
      {
        add_variable(i);
        _g.meet_weight(i,add_variable(j, i.get_var_factory ()),w);
        reduce();
      }

      bool IsDefiniteOne (Number x)  {
        return x == Number (1);
      }

      bool IsDefiniteOne (VariableName x)  {
        auto n = _scalar [x].singleton ();
        if (n) return *n == Number (1);
        else return false;
      }

      // x := x op k
      // Most of the magic happens here.
      template<typename VarNum>
      void apply_helper (operation_t op, VariableName x, VarNum k)
      {
        if (is_bottom()) return;

        /// step 1: add x_old in the graph
        VariableName x_old = x.get_var_factory ().get (); /*fresh var*/
        VariableName x_old_succ = x.get_var_factory ().get (); /*fresh var*/
        _g.insert_vertex(x_old);
        _g.insert_vertex(x_old_succ);
        _succ_idx_map->set(x_old, x_old_succ);

        /// --- Enforce the following relationships:
        ///     { x_old = x, x_old+ = x+, x_old+ = x_old + 1}

        /// x_old = x
        _scalar.assign(x_old, linear_expression_t(x));
        // needed if scalar domain is non-relational:
        // enforcing x_old = x
        _g.set_weight (x_old,x,WeightDomain::bottom ());
        _g.set_weight (x,x_old,WeightDomain::bottom ());

        /// x_old+ = x_old +1
        _scalar += linear_constraint_t(variable_t(x_old_succ) == variable_t(x_old) + 1);
        // needed if scalar domain is non-relational:
        // enforcing x_old+ < x_old is false
        _g.set_weight (x_old_succ,x_old,WeightDomain::bottom ());
        // enforcing x_old+ < x is false
        _g.set_weight (x_old_succ,x,WeightDomain::bottom ());

        /// x_old+ = x+
        optional<VariableName> x_succ = get_succ_idx(x);
        if (x_succ) {
          _scalar += linear_constraint_t( variable_t(x_old_succ) == variable_t(*x_succ));
          // needed if scalar domain is non-relational:
          // enforcing x_old + = x+
          _g.set_weight (x_old_succ,*x_succ,WeightDomain::bottom ());
          _g.set_weight (*x_succ,x_old_succ,WeightDomain::bottom ());
          // enforcing x+ < x_old is false
          _g.set_weight (*x_succ,x_old,WeightDomain::bottom ());
        }
        // propagate the scalar constraints to the graph
        reduce();

        /// step 2: abstract all incoming/outgoing edges of x
        abstract(x);

        /// step 3: update the graph with the scalar domain after applying
        ///         x = x op k.
        _scalar.apply(op, x, x, k);

#if 1
        // This is not needed at all if the scalar domain is relational.
        // Otherwise, we would like to keep the relationship between
        // x_old+ and x. We do it in a completely adhoc way but at least
        // we cover common cases when the array is traversed forward or
        // backwards one element by one.
        if (op == OP_ADDITION && IsDefiniteOne (k)) {
          _g.set_weight (x,x_old_succ,WeightDomain::bottom ());
          _g.set_weight (x_old_succ,x,WeightDomain::bottom ());
        }
        else if (op == OP_SUBTRACTION && IsDefiniteOne (k)) {
          _g.set_weight (x_old,*x_succ,WeightDomain::bottom ());
          _g.set_weight (*x_succ,x_old,WeightDomain::bottom ());
        }
#endif

        if (x_succ){
          _scalar -= *x_succ;
          /// --- Enforce x+ == x + 1
          _scalar += linear_constraint_t(variable_t(*x_succ) == variable_t(x) + 1);
          // needed if scalar domain is non-relational:
          _g.set_weight (*x_succ,x,WeightDomain::bottom ());
        }

        /* { x = x op k, x+ = x+1} */
        reduce();

        /// step 4: delete x_old
        _g -= x_old;
        _g -= x_old_succ;
        (*_succ_idx_map) -= x_old;
        _scalar -= x_old;
        _scalar -= x_old_succ;

        //this->reduce();
      }

      //! In case we can statically determine which variables should be
      //  considered array indexes. Note that any subset is sound but it
      //  might be imprecise. By default we consider all.
      bool is_array_index(VariableName v) const {
        return true;
      }

      // model array reads: return the weight from the edge i to i+
      WeightDomain array_read (VariableName i)
      {
        if (is_bottom())
          return WeightDomain::bottom();

        if (!is_array_index(i))
          return WeightDomain::top();

        //this->reduce();
        optional<VariableName> i_succ = get_succ_idx(i);
        if (i_succ)
          return _g.get_weight(i, *i_succ);

        CRAB_ERROR ("There is no successor index associated with ",i);
      }

      // model array writes
      void array_write (VariableName arr, VariableName i, WeightDomain w)
      {
        if (is_bottom()) return;
        //this->reduce();

        //--- strong update
        optional<VariableName> i_succ = get_succ_idx(i);
        if (!i_succ)
          CRAB_ERROR ("There is no successor index associated with ",i);

        WeightDomain& old_w = _g.get_weight(i, *i_succ);
        old_w -= arr;
        _g.meet_weight(i, *i_succ, w);
        WeightDomain new_w = _g.get_weight(i, *i_succ);

        //--- weak update:
        // An edge (p,q) must be weakened if p <= i <= q and p < q
        typename array_graph_t::edge_iterator it, et;
        for(tie(it,et) = edges(*_g._graph); it!= et; ++it)
        {
          typename array_graph_t::edge_descriptor_t e = *it;
          typename array_graph_t::VertexNamePtr  p = (*_g._graph)[source(e, *_g._graph)].name;
          typename array_graph_t::VertexNamePtr  q = (*_g._graph)[target(e, *_g._graph)].name;
          typename array_graph_t::WeightPtr weight = (*_g._graph)[e].weight;
          if ( ((*p == i) &&  (*q == *i_succ)) || weight->is_bottom())
            continue;
          // p < q
          ScalarNumDomain tmp(_scalar);
          tmp += linear_constraint_t( variable_t(*p) <= variable_t(i));
          tmp += linear_constraint_t( variable_t(*i_succ)  <= variable_t(*q));
          if (tmp.is_bottom())
            continue;
          // p <= i <= q and p < q
          typename array_graph_t::binary_join_op join;
          (*_g._graph)[e].weight = join (weight,
                                         typename array_graph_t::WeightPtr (new WeightDomain(new_w)));
        }
        _g.canonical();

      }

      void set_to_bottom()
      {
        _is_bottom = true;
        _scalar = ScalarNumDomain::bottom();
        _g = array_graph_t::bottom();
        _succ_idx_map->clear();
      }

      array_graph_domain(ScalarNumDomain scalar,
                         array_graph_t g,
                         succ_index_map_ptr map):
          writeable(),
          _is_bottom(false),
          _scalar(scalar),
          _g(g),
          _succ_idx_map(new succ_index_map_t(*map))
      {
        if (_scalar.is_bottom() || _g.is_bottom())
          set_to_bottom();
        else
          reduce();
      }

     public:

      array_graph_domain():
          writeable(),
          _is_bottom(false),
          _scalar(ScalarNumDomain::top()),
          _g(array_graph_t::top()),
          _succ_idx_map(new succ_index_map_t())
      { }

      static array_graph_domain_t top()
      {
        return array_graph_domain(ScalarNumDomain::top(),
                                  array_graph_t::top(),
                                  succ_index_map_ptr(new succ_index_map_t()));
      }

      static array_graph_domain_t bottom()
      {
        return array_graph_domain(ScalarNumDomain::bottom(),
                                  array_graph_t::bottom(),
                                  succ_index_map_ptr(new succ_index_map_t()));
      }

      array_graph_domain(const array_graph_domain_t& other):
          writeable(),
          _is_bottom(other._is_bottom),
          _scalar(other._scalar) ,
          _g(other._g),
          _succ_idx_map(new succ_index_map_t(*other._succ_idx_map))
      {
        crab::CrabStats::count (getDomainName() + ".count.copy");
        crab::ScopedCrabStats __st__(getDomainName() + ".copy");
      }

      array_graph_domain_t& operator=(array_graph_domain_t other)
      {
        crab::CrabStats::count (getDomainName() + ".count.copy");
        crab::ScopedCrabStats __st__(getDomainName() + ".copy");

        if (this != &other)
        {
          this->_is_bottom = other._is_bottom;
          this->_scalar = other._scalar;
          this->_g = other._g;
          this->_succ_idx_map = other._succ_idx_map;
        }
        return *this;
      }

      bool is_bottom() { return _is_bottom; }

      // --- top operation in the graph is expensive because we need to
      //     traverse the whole graph and check each edge.
      // bool is_top() { return (_scalar.is_top() && _g.is_top()); }

      bool is_top() { return (_scalar.is_top()); }

      void reduce ()
      {
        crab::CrabStats::count (getDomainName() + ".count.reduce");
        crab::ScopedCrabStats __st__(getDomainName() + ".reduce");

        if (is_bottom ()) return;

        domain_traits<ScalarNumDomain>::normalize(_scalar);

        if (_scalar.is_bottom() || _g.is_bottom())
          set_to_bottom();
        else
          _g.reduce(_scalar);
      }

      bool operator<=(array_graph_domain_t other)
      {
        crab::CrabStats::count (getDomainName() + ".count.leq");
        crab::ScopedCrabStats __st__(getDomainName() + ".leq");

        if (is_bottom ()) {
          return true;
        } else if (other.is_bottom ()) {
          return false;
        } else {
          return (_scalar <= other._scalar && _g <= other._g);
        }
      }

      void operator|=(array_graph_domain_t other)  {
        *this = *this | other;
      }

      array_graph_domain_t operator|(array_graph_domain_t other)
      {
        crab::CrabStats::count (getDomainName() + ".count.join");
        crab::ScopedCrabStats __st__(getDomainName() + ".join");

        if (is_bottom ()) {
          return other;
        } else if (other.is_bottom ()) {
          return *this;
        } else {
          succ_index_map_ptr map(new succ_index_map_t(*(_succ_idx_map) |
                                                      *(other._succ_idx_map)));
          return array_graph_domain_t(_scalar | other._scalar,
                                      _g | other._g, map);
        }
      }

      array_graph_domain_t operator&(array_graph_domain_t other)
      {
        crab::CrabStats::count (getDomainName() + ".count.meet");
        crab::ScopedCrabStats __st__(getDomainName() + ".meet");

        if (is_bottom () || other.is_bottom ()) {
          return bottom();
        } else {
          succ_index_map_ptr map(new succ_index_map_t(*(_succ_idx_map) |
                                                      *(other._succ_idx_map)));
          return array_graph_domain_t(_scalar & other._scalar,
                                      _g & other._g, map);
        }
      }

      array_graph_domain_t operator||(array_graph_domain_t other)
      {
        crab::CrabStats::count (getDomainName() + ".count.widening");
        crab::ScopedCrabStats __st__(getDomainName() + ".widening");

        if (is_bottom ())  return other;
        else if (other.is_bottom ())  return *this;
        else
        {
          succ_index_map_ptr map (new succ_index_map_t(*(_succ_idx_map) |
                                                       *(other._succ_idx_map)));
          array_graph_domain_t widen (_scalar || other._scalar,
                                      _g || other._g, map);
          CRAB_LOG("array-graph" , crab::outs() << "Widening: " << *this<<"\n";);
          return widen;
        }
      }

      template<typename Thresholds>
      array_graph_domain_t widening_thresholds (array_graph_domain_t other,
                                                const Thresholds & /*ts*/) {
        return (*this || other);
      }

      array_graph_domain_t operator&& (array_graph_domain_t other)
      {
        crab::CrabStats::count (getDomainName() + ".count.narrowing");
        crab::ScopedCrabStats __st__(getDomainName() + ".narrowing");

        if (is_bottom ()|| other.is_bottom ())
          return bottom();
        else
        {
          succ_index_map_ptr map (new succ_index_map_t(*(_succ_idx_map) |
                                                       *(other._succ_idx_map)));
          return array_graph_domain_t (_scalar && other._scalar,
                                       _g && other._g, map);
        }
      }

      void operator-=(VariableName var)
      {
        crab::CrabStats::count (getDomainName() + ".count.forget");
        crab::ScopedCrabStats __st__(getDomainName() + ".forget");

        if (is_bottom ()) return;

        // scalar domain
        _scalar -= var;
        _g -= var;
        optional<VariableName> var_succ = get_succ_idx(var);
        if (var_succ) {
          _scalar -= *var_succ;
          _g -= *var_succ;
          (*_succ_idx_map) -= var;
        }

        // graph domain
        typename array_graph_t::edge_iterator it, et;
        for(tie(it,et) = edges(*(_g._graph)); it!= et; ++it) {
          auto e  = *it;
          auto weight = (*(_g._graph))[e].weight;
          (*weight) -= var;
        }
        // this->reduce();
      }

      /////
      // Transfer functions
      /////

      void operator += (linear_constraint_system_t csts)
      {
        crab::CrabStats::count (getDomainName() + ".count.add_constraints");
        crab::ScopedCrabStats __st__(getDomainName() + ".add_constraints");

        if (is_bottom()) return;

        // graph domain: make sure that all the relevant variables
        // (included special "0") are inserted in the graph
        for (auto cst : csts) {
          // TODO
          //Number n = cst.expression().constant();
          //if (n == 0) add_variable(n, vfac);
          for (auto v : cst.variables())
            add_variable (v.name());
        }

        _scalar += csts;
        reduce();

        CRAB_LOG("array-graph",
                 crab::outs() << "Assume("<< csts<< ") --- "<< *this<<"\n";);
      }

      void assign (VariableName x, linear_expression_t e)
      {
        crab::CrabStats::count (getDomainName() + ".count.assign");
        crab::ScopedCrabStats __st__(getDomainName() + ".assign");

        if (is_bottom()) return;

        if (optional<variable_t> y = e.get_variable())
        {
          if ((*y).name() == x) return;
        }

        // scalar domain
        _scalar.assign(x, e);

        // graph domain
        if (e.is_constant() && (e.constant() == 0))
          add_variable (e.constant(), x.get_var_factory ());

        if (_g.find_vertex_map(x))
        {
          abstract(x);
          // wrong results if we do not restore the relationship between x
          // and x+ in the scalar domain
          optional<VariableName> x_succ = get_succ_idx(x);
          if (x_succ){
            _scalar -= *x_succ;
            /// --- Enforce x+ == x+1
            _scalar += linear_constraint_t( variable_t(*x_succ) == variable_t(x) + 1);
            // needed if scalar domain is non-relational:
            _g.set_weight (*x_succ,x,WeightDomain::bottom ());
          }
        }
        else
          add_variable(x);

        reduce();

        CRAB_LOG("array-graph",
                 crab::outs() << "Assign "<<x<<" := "<<e<<" ==> "<<*this<<"\n";);
      }

      void apply (operation_t op, VariableName x, VariableName y, Number z)
      {
        crab::CrabStats::count (getDomainName() + ".count.apply");
        crab::ScopedCrabStats __st__(getDomainName() + ".apply");

        assign (x, linear_expression_t(y));
        apply_helper<Number> (op, x, z);

        CRAB_LOG("array-graph",
                 crab::outs() << "Apply "<<x<<" := "<<y<<" "<<op<<" "<<z<<" ==> "<<*this<<"\n";);
      }

      void apply(operation_t op, VariableName x, VariableName y, VariableName z)
      {
        crab::CrabStats::count (getDomainName() + ".count.apply");
        crab::ScopedCrabStats __st__(getDomainName() + ".apply");

        assign (x, linear_expression_t(y));
        apply_helper<VariableName> (op, x, z);

        CRAB_LOG("array-graph",
                 crab::outs() << "Apply "<<x<<" := "<<y<<" "<<op<<" "<<z<<" ==> "<<*this<<"\n";);
      }

      void apply(operation_t op, VariableName x, Number k)
      {
        crab::CrabStats::count (getDomainName() + ".count.apply");
        crab::ScopedCrabStats __st__(getDomainName() + ".apply");

        apply_helper<Number> (op, x, k);

        CRAB_LOG("array-graph",
                 crab::outs() << "Apply "<<x<<" := "<<x<<" "<<op<<" "<<k<<" ==> "<<*this<<"\n";);
      }

      // bitwise_operators_api
      void apply(conv_operation_t op, VariableName x, VariableName y, unsigned width) {
        assign (x, variable_t (y));
      }

      void apply(conv_operation_t op, VariableName x, Number k, unsigned width) {
        assign (x, k);
      }

      void apply(bitwise_operation_t op, VariableName x, VariableName y, VariableName z) {
        CRAB_WARN ("bitwise operations not implemented in array_graph");
      }

      void apply(bitwise_operation_t op, VariableName x, VariableName y, Number k) {
        CRAB_WARN ("bitwise operations not implemented in array_graph");
      }

      // division_operators_api
      void apply(div_operation_t op, VariableName x, VariableName y, VariableName z) {
        CRAB_WARN ("division operations not implemented in array_graph");
      }

      void apply(div_operation_t op, VariableName x, VariableName y, Number k) {
        CRAB_WARN ("division operations not implemented in array_graph");
      }

      // array_operators_api

      virtual void array_init (VariableName a, const vector<ikos::z_number> &values)  override {
        CRAB_WARN ("array_graph_domain init not implemented");
      }

      virtual void array_assume (VariableName a,
                                 boost::optional<Number> lb, boost::optional<Number> ub) override {
        CRAB_WARN ("array_graph_domain assume not implemented");
      }

      virtual void array_load (VariableName lhs, VariableName arr, VariableName idx,
                               z_number /*bytes*/) override {
        crab::CrabStats::count (getDomainName() + ".count.load");
        crab::ScopedCrabStats __st__(getDomainName() + ".load");

        WeightDomain w = array_read (idx);
        // --- Simplification wrt Gange et.al.:
        //     Only non-relational invariants are passed from the graph
        //     domain to the scalar domain.
        //     We use operator[] as the conversion function
        _scalar.set (lhs, w [arr]);

        CRAB_LOG("array-graph",
                 crab::outs() << "Array read "<<lhs<<" := "<< arr<<"["<<idx<<"] ==> "
                           << *this <<"\n";);
      }

      virtual void array_store (VariableName arr, VariableName idx, linear_expression_t val,
                                 z_number /*n_bytes*/, bool /*is_singleton*/) override {

        crab::CrabStats::count (getDomainName() + ".count.store");
        crab::ScopedCrabStats __st__(getDomainName() + ".store");

        // --- Simplification wrt Gange et.al.:
        //     Only non-relational invariants are passed from the scalar
        //     domain to the graph domain.
        //     We use operator[] as the conversion function
        WeightDomain w = WeightDomain::top ();
        if (val.is_constant ())
          w.assign (arr, val);
        else if (auto v = val.get_variable ()){
          w.set (arr, _scalar[(*v).name()]);
        }
        else {
          // If you see this warning you can switch to intervals.
          crab::outs() << "Warning: scalar domain does not support assignments with arbitrary rhs.\n";
        }
        array_write (arr, idx, w);

        CRAB_LOG("array-graph",
                 crab::outs() << "Array write "<<arr<<"["<<idx<<"] := "<<val<< " ==> "<< *this <<"\n";);
      }

      void write(crab_os& o)
      {
        o << "(" ;
#if 1
        // less verbose: remove the special variables i+ from the scalar
        // domain
        ScalarNumDomain inv (_scalar);
        for(auto p : *_succ_idx_map) {
          inv -= p.second;
        }
        o << inv;
#else
        o << _scalar;
#endif
        o << "," << _g;
        o << ")";
      }

      linear_constraint_system_t to_linear_constraint_system (){
        CRAB_ERROR ("array_graph: to_linear_constraint_system not implemented");
      }

      static string getDomainName () {
        string name ("ArrayGraph(" +
                     ScalarNumDomain::getDomainName () +  "," +
                     WeightDomain::getDomainName () +
                     ")");
        return name;
      }

    }; // end array_graph_domain

    }// namespace domain_traits

} // namespace crab

#endif