#include "CloudMapper.h"

#include <pcl/common/common.h>

#include <pcl/registration/registration.h>

#include <pcl/registration/icp.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/transformation_estimation_point_to_plane.h>
#include <pcl/registration/transformation_estimation_point_to_plane_weighted.h>
#include <pcl/registration/transformation_estimation_svd_scale.h>
#include <pcl/registration/correspondence_estimation_normal_shooting.h>
#include <pcl/visualization/cloud_viewer.h>

#include <pcl/features/normal_3d.h>

#include <pcl/correspondence.h>
#include <pcl/search/kdtree.h>

#include <boost/format.hpp>
#include <boost/thread.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/thread_time.hpp>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>

#include "UI/ShowCloud.h"
#include "Exception/HPEException.h"
#include "Filter/Filters.h"
#include "TransformationEstimationHPE.h"

#include <pcl/io/pcd_io.h>

namespace hpe
{
    float CalculateDistance(pcl::PointXYZRGBA &p1, pcl::PointXYZRGBA &p2)
    {
        Eigen::VectorXf d(3);
        d(0) = p1.x - p2.x;
        d(1) = p1.y - p2.y;
        d(2) = p1.z - p2.z;

        return d.norm();
    }

    CloudMapper::CloudMapper()
        : m_icpParams(CloudMapper::ICPParams(0.05, 0.05, 0.000001, 100000, 100)),
          m_useDynamicICP(false), m_usePointToPlaneMetric(false), m_useNormalShooting(false),
          m_estimateScale(false)

    {

    }

    void CloudMapper::AddCloud(CloudMapper::Cloud::Ptr &cloud, CloudMapper::Landmarks &landmarks)
    {
        m_pointClouds.push_back(cloud);
        m_landmarks.push_back(landmarks);
    }

    CloudMapper::Clouds CloudMapper::GetAlignedClouds(TransformationsPtr transformations)
    {
        return AlignAll(m_pointClouds, m_landmarks);
    }

    CloudMapper::Cloud::Ptr CloudMapper::GetMergedCloud()
    {
        CloudMapper::Clouds alignedClouds = AlignAll(m_pointClouds, m_landmarks);
        CloudMapper::Cloud::Ptr merged(new CloudMapper::Cloud);
        auto it = alignedClouds.begin();
        do
        {
            auto current = (*it);
            *merged += *current;
            ++it;
        }
        while (it != alignedClouds.end());

        return merged;
    }

    CloudMapper::TransformationsPtr CloudMapper::GetTransforms()
    {
        CloudMapper::TransformationsPtr transforms(new CloudMapper::Transformations);

        AlignAll(m_pointClouds, m_landmarks, transforms);

        return transforms;
    }

    void CloudMapper::SetICPParams(CloudMapper::ICPParams params)
    {
        m_icpParams = params;
    }

    void CloudMapper::SetDynamicICPParams(CloudMapper::DynamicICPParams &params)
    {
        m_dynamicICPParams = params;
        SetUseDynamicICP(true);
    }

    void CloudMapper::SetUseDynamicICP(bool use)
    {
        m_useDynamicICP = use;
    }

    void CloudMapper::SetUseNormalShooting(bool use)
    {
        m_useNormalShooting = use;
    }

    void CloudMapper::SetEstimateScale(bool estimateScale)
    {
        m_estimateScale = estimateScale;
    }

    void CloudMapper::SetUsePointToPlaneMetric(bool use)
    {
        m_usePointToPlaneMetric = use;
    }

    Eigen::Matrix4f CloudMapper::GetTransformForTwoCloudsPointToPlane(CloudMapper::Cloud::Ptr &source,
            CloudMapper::Cloud::Ptr &target,
            ICPParams params)
    {
        CloudMapper::NormalCloud::Ptr normalSource(new CloudMapper::NormalCloud);
        pcl::copyPointCloud(*source, *normalSource);

        CloudMapper::NormalCloud::Ptr normalTarget(new CloudMapper::NormalCloud);
        pcl::copyPointCloud(*target, *normalTarget);

        typedef pcl::search::KdTree<CloudMapper::PointNormal> Search2;
        Search2::Ptr search(new Search2);

        pcl::NormalEstimation<CloudMapper::PointNormal, CloudMapper::PointNormal> estimation;
        estimation.setSearchMethod(search);
        estimation.setKSearch(10);
        estimation.setInputCloud(normalTarget);
        estimation.compute(*normalTarget);

        typedef pcl::registration::CorrespondenceRejectorSampleConsensus<CloudMapper::PointNormal> Rejector;

        pcl::Registration<CloudMapper::PointNormal, CloudMapper::PointNormal>::Ptr registration(new pcl::IterativeClosestPoint<CloudMapper::PointNormal, CloudMapper::PointNormal>);

        pcl::registration::CorrespondenceRejectorSampleConsensus<CloudMapper::PointNormal>::Ptr rejector(new pcl::registration::CorrespondenceRejectorSampleConsensus<CloudMapper::PointNormal>);
        rejector->setInlierThreshold(params.RansacRejectionThreshold);
        rejector->setMaximumIterations(params.MaxRANSACIterations);
        rejector->setInputSource(normalSource);
        rejector->setInputTarget(normalTarget);
        registration->setInputSource(normalSource);
        registration->setInputTarget(normalTarget);
        registration->setMaxCorrespondenceDistance(params.MaxCorrespondenceDistance);
        registration->setRANSACOutlierRejectionThreshold(params.RansacRejectionThreshold);
        registration->setTransformationEpsilon(params.TransformationEpsilon);
        registration->setMaximumIterations(params.MaxICPIterations);
        registration->setRANSACIterations(params.MaxRANSACIterations);
        registration->addCorrespondenceRejector(rejector);

        if (m_useNormalShooting)
        {
            typedef pcl::registration::CorrespondenceEstimationNormalShooting<CloudMapper::PointNormal, CloudMapper::PointNormal, CloudMapper::PointNormal> NormalShooting;
            NormalShooting::Ptr ce(new NormalShooting);
            registration->setCorrespondenceEstimation(ce);
        }

        typedef TransformationEstimationHPE<CloudMapper::PointNormal, CloudMapper::PointNormal> Custom;
        Custom::Ptr metric(new Custom);
        metric->SetWeights(m_weights);
        registration->setTransformationEstimation(metric);

        CloudMapper::NormalCloud::Ptr c(new CloudMapper::NormalCloud);
        registration->align(*c);
        //std::cout << "Fitness Score " << registration->getFitnessScore() << std::endl;

        return registration->getFinalTransformation();
    }

    Eigen::Matrix4f CloudMapper::GetTransformForTwoCloudsPointToPoint(CloudMapper::Cloud::Ptr &source,
            CloudMapper::Cloud::Ptr &target,
            ICPParams params)
    {
        pcl::Registration<CloudMapper::PointType, CloudMapper::PointType>::Ptr registration(new pcl::IterativeClosestPoint<CloudMapper::PointType, CloudMapper::PointType>);
        typedef pcl::registration::CorrespondenceRejectorSampleConsensus<CloudMapper::PointType> Rejector;

        Rejector::Ptr rejector(new Rejector);
        rejector->setInlierThreshold(params.RansacRejectionThreshold);
        rejector->setMaximumIterations(params.MaxRANSACIterations);
        rejector->setInputSource(source);
        rejector->setInputTarget(target);

        registration->setInputSource(source);
        registration->setInputTarget(target);
        registration->setMaxCorrespondenceDistance(params.MaxCorrespondenceDistance);
        registration->setRANSACOutlierRejectionThreshold(params.RansacRejectionThreshold);
        registration->setTransformationEpsilon(params.TransformationEpsilon);
        registration->setMaximumIterations(params.MaxICPIterations);
        registration->setRANSACIterations(params.MaxRANSACIterations);
        registration->addCorrespondenceRejector(rejector);

        CloudMapper::Cloud::Ptr c(new CloudMapper::Cloud);
        registration->align(*c);

        std::cout << "Fitness Score " << registration->getFitnessScore() << std::endl;

        return registration->getFinalTransformation();
    }

    Eigen::Matrix4f CloudMapper::GetTransformForTwoCloudsDynamically(CloudMapper::Cloud::Ptr &source,
            CloudMapper::Cloud::Ptr &target,
            CloudMapper::DynamicICPParams &dynamicParams)
    {

        int icpRound = 1;

        CloudMapper::Cloud::Ptr result(new Cloud);
        pcl::copyPointCloud(*source, *result);
        Eigen::Matrix4f finalTransformationMatrix = Eigen::Matrix4f::Identity();
        for (auto it = dynamicParams.begin(); it != dynamicParams.end(); ++it)
        {
            auto params = *it;
            Eigen::Matrix4f stepTransform = Eigen::Matrix4f::Identity();

            if (m_usePointToPlaneMetric)
            {
                stepTransform = GetTransformForTwoCloudsPointToPlane(result, target, params);
            }
            else
            {
                stepTransform = GetTransformForTwoCloudsPointToPoint(result, target, params);
            }

            pcl::transformPointCloud(*result, *result, stepTransform);

            finalTransformationMatrix = finalTransformationMatrix * stepTransform;
        }

        return finalTransformationMatrix;
    }

    Eigen::Matrix4f CloudMapper::GetTransformHavingLandmarks(CloudMapper::Cloud::Ptr &source, Common<CloudMapper::PointType>::Landmarks &l1,
            CloudMapper::Cloud::Ptr &target, Common<CloudMapper::PointType>::Landmarks &l2)
    {
        pcl::CorrespondencesPtr correspondences = LandmarksToCorresponencies(source, l1, target, l2);
        return GetTransformHavingCorrespondences(source, target, correspondences);
    }

    Eigen::Matrix4f CloudMapper::GetTransformHavingCorrespondences(CloudMapper::Cloud::Ptr &source, CloudMapper::Cloud::Ptr &target, pcl::CorrespondencesPtr &correspondences)
    {
        Eigen::Matrix4f initialAlignmentMatrix = ComputeInitialAlignment(source, target, correspondences);
        bool initialAlignmentSucceed = IsNotNAN(initialAlignmentMatrix);

        CloudMapper::Cloud::Ptr sourceTransformed(new CloudMapper::Cloud);
        if (initialAlignmentSucceed)
        {
            pcl::transformPointCloud(*source, *sourceTransformed, initialAlignmentMatrix);
        }
        else
        {
            sourceTransformed = source;
        }

        if (m_estimateScale)
        {
            ShowTwoCloudsInDifferentColors<CloudMapper::PointType>(sourceTransformed, target, pcl::CorrespondencesPtr(), "After IA");
        }

        CloudMapper::DynamicICPParams dynamicParams = m_dynamicICPParams;
        if (m_useDynamicICP == false)
        {
            dynamicParams.push_back(m_icpParams);
        }

        CloudMapper::Cloud::Ptr result = sourceTransformed;
        Eigen::Matrix4f finalTransformationMatrix = GetTransformForTwoCloudsDynamically(sourceTransformed, target, dynamicParams);

        Eigen::Matrix4f totalTransform;
        if (initialAlignmentSucceed)
        {
            totalTransform = finalTransformationMatrix * initialAlignmentMatrix;
        }
        else
        {
            totalTransform = finalTransformationMatrix;
        }

        pcl::transformPointCloud(*source, *result, totalTransform);

        return totalTransform;
    }

    CloudMapper::Clouds CloudMapper::AlignAll(CloudMapper::Clouds &clouds, CloudMapper::LandmarksVector &landmarksVector, TransformationsPtr transformations)
    {
        TransformationsPtr allTransformations(new Transformations);
        if (transformations != nullptr)
        {
            allTransformations = transformations;
        }

        if (clouds.size() != landmarksVector.size())
        {
            throw HPEException("CloudMapper::AlignAll - clouds.size() != landmarksVector.size()");
        }

        std::vector<Eigen::Matrix4f> stepTransformations;

        pcl::PointCloud<CloudMapper::PointType>::Ptr sourceCloud = clouds[0];

        for (int cloudIndex = 1; cloudIndex < clouds.size() - 1; cloudIndex++)
        {
            int sourceIndex = cloudIndex;
            int targetIndex = sourceIndex + 1;

            pcl::PointCloud<CloudMapper::PointType>::Ptr targetCloud = clouds[targetIndex];

            auto totalTransform = GetTransformHavingLandmarks(sourceCloud, landmarksVector[sourceIndex],
                                  targetCloud, landmarksVector[targetIndex]);

            Eigen::Matrix3f rotationMatrix = totalTransform.block<3, 3>(0, 0);
            Eigen::Vector3f translationVector = totalTransform.block<3, 1>(0, 3);
            Eigen::Quaternionf quaternion(rotationMatrix);
            Eigen::Matrix3f rotation2(quaternion);

            pcl::transformPointCloud(*sourceCloud, *sourceCloud, totalTransform);
            pcl::PointCloud<CloudMapper::PointType>::Ptr newCloud(new pcl::PointCloud<CloudMapper::PointType>);
            *sourceCloud += *targetCloud;
            sourceCloud = Voxelize<CloudMapper::PointType>(sourceCloud, 0.006f);

            //ShowCloud<CloudMapper::PointType>(sourceCloud);

            if (IsNotNAN(totalTransform) == false)
            {
                int q = 42;
            }

            std::cout << cloudIndex << std::endl;

            stepTransformations.push_back(totalTransform);
        }

        ShowCloud<CloudMapper::PointType>(sourceCloud);

        CloudMapper::Clouds result;

        auto targetCloud = clouds[clouds.size() - 1];

        for (int cloudIndex = 0; cloudIndex < clouds.size() - 1; cloudIndex++)
        {
            Eigen::Matrix4f accumulatedMatrix = stepTransformations[cloudIndex];

            for (int matrixIndex = cloudIndex + 1; matrixIndex < stepTransformations.size(); matrixIndex++)
            {
                accumulatedMatrix = accumulatedMatrix * stepTransformations[matrixIndex];
            }

            CloudMapper::Cloud::Ptr transformed(new CloudMapper::Cloud);
            pcl::transformPointCloud(*clouds[cloudIndex], *transformed, accumulatedMatrix);
            allTransformations->push_back(accumulatedMatrix);

            result.push_back(transformed);
        }

        result.push_back(targetCloud);

        return result;
    }

    Eigen::Matrix4f CloudMapper::ComputeInitialAlignment(Cloud::Ptr &cloud1, Cloud::Ptr &cloud2, pcl::CorrespondencesPtr &correspondences)
    {
        if (correspondences->empty())
        {
            return Eigen::Matrix4f::Identity();
        }

        pcl::registration::CorrespondenceRejectorSampleConsensus<PointType> sac;
        sac.setInlierThreshold(0.5);
        sac.setInputSource(cloud1);
        sac.setInputTarget(cloud2);
        sac.setInputCorrespondences(correspondences);
        pcl::Correspondences inliers;
        sac.getCorrespondences(inliers);

        Eigen::Matrix4f transformation;
        if (m_estimateScale)
        {
            pcl::registration::TransformationEstimationSVDScale<PointType, PointType> transformationEstimation;
            transformationEstimation.estimateRigidTransformation(*cloud1, *cloud2, inliers, transformation);
        }
        else
        {
            pcl::registration::TransformationEstimationSVD<PointType, PointType> transformationEstimation;
            transformationEstimation.estimateRigidTransformation(*cloud1, *cloud2, inliers, transformation);
        }

        if (transformation == Eigen::Matrix4f::Identity() || IsNotNAN(transformation) == false)
        {
            std::cout << "Identity or NAN" << std::endl;
        }

        return transformation;
    }

    Common<CloudMapper::PointType>::Landmark CloudMapper::FindClosestPoint(CloudMapper::Cloud::Ptr &cloud, CloudMapper::PointType &p)
    {
        pcl::search::KdTree<CloudMapper::PointType> search;
        search.setInputCloud(cloud);

        std::vector<int> indices;
        std::vector<float> distances;
        search.nearestKSearch(p, 1, indices, distances);

        Common<CloudMapper::PointType>::Landmark result;
        result.index = indices[0];
        result.point = cloud->at(indices[0]);

        return result;
    }

    pcl::CorrespondencesPtr CloudMapper::LandmarksToCorresponencies(Cloud::Ptr &cloud1, CloudMapper::Landmarks &landmarks1, Cloud::Ptr &cloud2, CloudMapper::Landmarks &landmarks2)
    {
        pcl::CorrespondencesPtr result(new pcl::Correspondences);

        int resultSize = std::min(landmarks1.size(), landmarks2.size());

        for (int i = 0; i < resultSize; i++)
        {
            auto sourcePoint1 = landmarks1.at(i).point;
            auto sourcePoint2 = landmarks2.at(i).point;

            auto p1IsNotNan = IsNotNAN(sourcePoint1);
            auto p2IsNotNan = IsNotNAN(sourcePoint2);
            if (p1IsNotNan && p2IsNotNan)
            {
                CloudMapper::Landmarks::value_type pt1 = FindClosestPoint(cloud1, sourcePoint1);
                CloudMapper::Landmarks::value_type pt2 = FindClosestPoint(cloud2, sourcePoint2);

                float distance = CalculateDistance(pt1.point, pt2.point);
                pcl::Correspondence correspondence(pt1.index, pt2.index, distance);

                result->push_back(correspondence);
            }
        }

        return result;
    }

    bool CloudMapper::IsNotNAN(const Eigen::Matrix4f &x)
    {
        return ((x.array() == x.array())).all();
    }

    bool CloudMapper::IsNotNAN(CloudMapper::PointType &point)
    {
        return (boost::math::isnan(point.x) || boost::math::isnan(point.y) || boost::math::isnan(point.z)) == false;
    }
}


#pragma once

#include "WrappedExternalIO.hpp"
#include "decoder_module.h"
#include "audio_format.h"
#include "RationalValue.h"
#include <limits>
#include <cstdint>
#include <boost/rational.hpp>
#include <vector>
#include <memory>

class DecoderSubstream;
class Module;

class Decoder{
	std::int64_t position = 0;
protected:
	struct StreamRange{
		rational_t time_begin;
		rational_t time_length;
		std::int64_t sample_begin;
		std::int64_t sample_length;
		int frequency;
	};
	std::vector<StreamRange> stream_ranges;
	std::int64_t time_resolution = -1;
	Module *module;

	virtual AudioBuffer *internal_read(const AudioFormat &, size_t extra_data, int substream_index) = 0;
	std::int64_t time_to_sample(const rational_t &time) const{
		auto beg = this->stream_ranges.begin();
		auto end = this->stream_ranges.end();
		auto it = std::find_if(beg, end, [&time](const StreamRange &sr){ return sr.time_begin + sr.time_length >= time; });
		if (it == end){
			it = beg + (end - beg - 1);
			if (!(it->time_begin <= time && time < it->time_begin + it->time_length))
				return -1;
		}
		auto conv = (time - it->time_begin) * it->frequency;
		return it->sample_begin + conv.numerator() / conv.denominator();
	}
	rational_t sample_to_time(std::int64_t sample) const{
		if (sample < 0)
			return -1;
		rational_t accum = 0;
		for (auto &sr : this->stream_ranges){
			if (sr.sample_begin + sr.sample_length >= sample)
				return accum + rational_t(sample - sr.sample_begin, sr.frequency);
			accum += sr.time_length;
		}
		return -1;
	}
	virtual std::int64_t seek_to_sample_internal(std::int64_t pos, bool fast){
		return -1;
	}
public:
	Decoder(Module *module = nullptr): module(module){}
	virtual ~Decoder(){}
	AudioBuffer *read(const AudioFormat &af, size_t extra_data, int substream_index){
		auto ret = this->internal_read(af, extra_data, substream_index);
		if (!ret)
			return ret;
		this->position += ret->sample_count;
		return ret;
	}
	std::int64_t seek_to_sample(std::int64_t pos, bool fast){
		if (pos == this->position)
			return pos;
		auto ret = this->seek_to_sample_internal(pos, fast);
		if (ret < 0)
			return -1;
		this->position = ret;
		return ret;
	}
	virtual rational_t seek_to_second(const rational_t &second, bool fast){
		auto sample = this->time_to_sample(second);
		if (sample < 0)
			return rational_t(-1, 1);
		auto ret = this->seek_to_sample(sample, fast);
		if (ret < 0)
			return rational_t(-1, 1);
		return this->sample_to_time(ret);
	}
	virtual std::int64_t sample_tell(){
		return this->position;
	}
	virtual int get_substream_count(){
		return 1;
	}
	virtual DecoderSubstream *get_substream(int index) = 0;
	Module *get_module() const{
		return this->module;
	}
};

class DecoderSubstream{
protected:
	Decoder &parent;
	int index;
	std::uint64_t first_sample = 0;
	rational_t first_moment = {0, 1};
	std::uint64_t position = 0;
	std::uint64_t length = std::numeric_limits<std::uint64_t>::max();
	rational_t seconds_length = {-1, 1};
	AudioFormat format;
public:
	DecoderSubstream(
		Decoder &parent,
		int index = 0,
		std::uint64_t first_sample = 0,
		std::uint64_t length = std::numeric_limits<std::uint64_t>::max(),
		const rational_t &first_moment = 0,
		const rational_t &seconds_length = {-1, 1}
	):
		parent(parent),
		index(index),
		first_sample(first_sample),
		first_moment(first_moment),
		length(length),
		seconds_length(seconds_length){}
	virtual ~DecoderSubstream() = 0;
	virtual RationalValue get_length_in_seconds(){
		return to_RationalValue(this->seconds_length);
	}
	virtual std::uint64_t get_length_in_samples(){
		return this->length;
	}
	virtual std::int64_t seek_to_sample(std::int64_t pos, bool fast){
		if (pos < 0 || (std::uint64_t)pos >= this->length)
			return -1;
		auto new_pos = this->parent.seek_to_sample(this->first_sample + pos, fast);
		if (new_pos < 0)
			return new_pos;
		this->position = (std::uint64_t)new_pos - this->first_sample;
		return this->position;
	}
	virtual RationalValue seek_to_second(const RationalValue &pos, bool fast){
		auto r = to_rational(pos);
		RationalValue ret{-1, 1};
		if (r < this->seconds_length){
			auto new_pos = this->parent.seek_to_second(this->first_moment + r, fast);
			if (new_pos >= 0)
				this->position = (std::uint64_t)this->parent.sample_tell() - this->first_sample;
			ret = to_RationalValue(new_pos - this->first_moment);
		}
		return ret;
	}
	virtual AudioBuffer *read(size_t extra_data){
		if (this->position >= this->length)
			return nullptr;
		auto sample = this->position + this->first_sample;
		if (this->parent.seek_to_sample(sample, false) != sample)
			return nullptr;
		auto ret = this->parent.read(this->format, extra_data, this->index);
		if (!ret)
			return nullptr;
		if ((std::uint64_t)ret->sample_count > this->length - this->position)
			ret->sample_count = (size_t)(this->length - this->position);
		this->position += ret->sample_count;
		return ret;
	}
	virtual AudioFormat get_audio_format(){
		return this->format;
	}
	virtual Decoder &get_parent(){
		return this->parent;
	}
};

inline DecoderSubstream::~DecoderSubstream(){}


#ifndef INCLUDED_actor_BackboneHBondActor_HH
#define INCLUDED_actor_BackboneHBondActor_HH

#include <Eigen/Dense>
#include <scheme/io/dump_pdb_atom.hh>
#include <scheme/chemical/HBondRay.hh>

#include <core/conformation/Residue.hh>
#include <numeric/xyzVector.hh>

namespace scheme {
namespace actor {

// The hbond_rays_ holds the position information

	struct BackboneHBondActor {

		typedef ::Eigen::Transform<float,3,Eigen::AffineCompact> EigenXform;

		///@brief Position type, leave out to make actor "Fixed"
		typedef EigenXform Position;
		typedef typename Position::Scalar Float;
		typedef BackboneHBondActor THIS;
		typedef Eigen::Matrix<Float,3,1> V3;

		std::vector<::scheme::chemical::HBondRay> hbond_rays_;
		bool is_donor_;
		int index_;

		BackboneHBondActor() : is_donor_(false), index_(0)  {}

		BackboneHBondActor(std::vector<::scheme::chemical::HBondRay> const & hbond_rays, bool is_donor, int i=0) :
			hbond_rays_(hbond_rays),
			is_donor_(is_donor),
			index_(i)
		{}

		BackboneHBondActor(
			BackboneHBondActor const & actor0,
			Position const & to_moveby
		){
			hbond_rays_ = actor0.hbond_rays_;
			is_donor_ = actor0.is_donor_;
			index_ = actor0.index_;

			moveby(to_moveby);
		}

		std::vector<::scheme::chemical::HBondRay> const &
		hbond_rays() const {
			return hbond_rays_;
		}

		bool
		is_donor() const {
			return is_donor_;
		}

		int
		index() const {
			return index_;
		}

		// void
		// set_position(
		// 	Position const & pos
		// ){  }

		void
		moveby(
			Position const & pos
		){
			for ( ::scheme::chemical::HBondRay & hbray : hbond_rays_ ) {
				hbray.apply_xform( pos );
			}
		}

		// Position const &
		// position() const { return Position::Identity(); }

		bool operator==(THIS const & o) const {
			return o.hbond_rays_==hbond_rays_ &&
			       o.is_donor_  ==is_donor_   &&
			       o.index_     ==index_;
		}

		///@brief necessary for testing only
		// bool operator<(THIS const & o) const { return std::make_pair(position_,aa_) < std::make_pair(o.position_,o.aa_); }

		///@brief necessary for testing only
	  	// template<class Archive> void serialize(Archive & ar, const unsigned int ){
	  	// 	ar & position_;
	  	// 	ar & aa_;
	  	// 	ar & ss_;
	  	// 	ar & index_;
	  	// }

	};

template< class MetaData >
void write_pdb( std::ostream & out, BackboneHBondActor const & a, MetaData const & meta ){

	// AtomData(
	// 	std::string const & _atomname = AtomData::default_atomname(),
	// 	std::string const & _resname  = AtomData::default_resname(),
	// 	char                _chain    = AtomData::default_chain(),
	// 	int                 _resnum   = AtomData::default_resnum(),
	// 	int                 _atomnum  = AtomData::default_atomnum(),
	// 	std::string const & _elem     = AtomData::default_elem(),
	// 	bool                _ishet    = AtomData::default_ishet(),
	// 	float               _occ      = AtomData::default_occ(),
	// 	float               _bfac     = AtomData::default_bfac()
	// );

}

inline
std::ostream & operator<<(std::ostream & out,BackboneHBondActor const& a){
	return out << "BackboneHBondActor " << (a.is_donor_ ? "donor" : "acceptor" ) << " " << a.index_;
}

}
}

#endif


#include <iostream>
#include <iomanip>
#include <random>
#include <algorithm>
#include <sstream>
#include <math.h>
#include <boost/format.hpp>
#include <spdlog/spdlog.h>
#include <spdlog/logger.h>
#include <memory>

#include "population.h"
#include "statistics.h"
#include "defines.h"
#include "timer.h"
#include "globals.h"
#include "parallel_random.h"

using namespace CTModels;

extern CTModels::Timer timer;

namespace CTModels {

/**
* Population destructor.  Normally not reached if we run a single population and then exit from main(),
* but in situations where we might run simulations in a loop, we don't want to leak memory for whole
* populations.
*/
Population::~Population() {
	SPDLOG_TRACE(clog,"deallocating block prev_population_traits {:p}", (void*)prev_population_traits);
	SPDLOG_TRACE(clog,"deallocating block population_traits {:p}", (void*)population_traits);
	SPDLOG_TRACE(clog,"deallocating block indiv_to_copy {:p}", (void*)indiv_to_copy);
	FREE(prev_population_traits);
	FREE(population_traits);
	FREE(indiv_to_copy);
}

void Population::initialize() {
	timer.start("population::initialize");
	// Initialize needed random number generators
	std::random_device rd_mt;
	std::mt19937_64 t_mt(rd_mt());

	this->mt = t_mt;

	// in debug printing, we want fixed columns, with the number of digits appropriate given the
	// max number of traits or population size
	char buffer[32];
	pop_digits_printing = sprintf(buffer, "%ld", (long)popsize);
	//SPDLOG_DEBUG(clog,"Using {} digits to print population individual ID's", pop_digits_printing);

	// Construct a uniform integer distribution which will draw individuals from the population
	// std::random is not necessarily thread safe, so if we use this with OpenMP tasks or loops,
	// the strategy will be to draw as many as we need from the loop in a #pragma openmp single s
	// section, and then block them to the worker team to use.
	std::uniform_int_distribution<int> u{0, this->popsize - 1};
	this->uniform_pop = u;

	// Construct a uniform distribution for choosing a random locus
	std::uniform_int_distribution<int> l{0, this->numloci - 1};
	this->uniform_locus = l;

	// Construct a Poisson distribution for innovation rates, with mean popsize * innovrate
	double mutation_rate = static_cast<double>(this->popsize) * this->innovation_rate;
	//SPDLOG_DEBUG(log,"Constructing Poisson for innovation with mean {:0.4f}", mutation_rate);
	std::poisson_distribution<int> p{mutation_rate};
	this->poisson_dist = p;

	// next_trait stores the next new mutation/innovation for each locus/dimension, and each slot
	// is incremented when a new trait is handed out.  Since there are "inittraits" in the initial
	// population, we initialize this array of values to inittraits + 1.
	for(int i = 0; i < this->numloci; i++) {
		next_trait.push_back(this->inittraits + 1);
	}

	auto trait_bufsize = (numloci * popsize) * sizeof(int);
	population_traits = (int*) ALIGNED_MALLOC(trait_bufsize);
	prev_population_traits = (int*) ALIGNED_MALLOC(trait_bufsize);
	//SPDLOG_DEBUG(clog, "Pop initializing pop traits array {:p} as {}x{} block with size {}", (void*)population_traits, popsize, numloci, trait_bufsize);
	//SPDLOG_DEBUG(clog, "Pop initializing prev pop array {:p} as {}x{} block with size {}", (void*)prev_population_traits, popsize, numloci, trait_bufsize);

	int num_variates = popsize * numloci;
	generate_uniform_int(0, inittraits - 1, num_variates, population_traits);

	// Initialize a buffer to hold random numbers indicating which individuals are copied
	// in each time step
	auto indiv_bufsize = popsize * sizeof(int);
	indiv_to_copy = (int*) ALIGNED_MALLOC(indiv_bufsize);
	//SPDLOG_DEBUG(clog, "Pop initializing indiv_to_copy array {:p} as {} block with size {}", (void*)indiv_to_copy, popsize, indiv_bufsize);

	// For the first generation only, the previous population is the same as the initial population
	memcpy(prev_population_traits, population_traits, trait_bufsize);
	timer.end("population::initialize");
}

std::shared_ptr<TraitFrequencies> Population::tabulate_trait_counts() {
	timer.start("population::tabulate_trait_counts");
	// allocate space for the largest value in any locus
	// array of counts will be a rectangular array numloci * largest_locus_value
	// technically, the largest_locus_value is 1 greater than any trait value seen at any locus,
	// so we trimmed by one
	// MEM:  dynamically allocated locus_counts is freed in the destructor of TraitFrequencies

	auto result = std::max_element(next_trait.begin(), next_trait.end());
	int largest_locus_value = *result - 1;

	// declared as a std::unique_ptr, because we want the TF object from the last time tabulate was called
	// to clean itsetf up once there isn't a reference anymore.
	std::shared_ptr<TraitFrequencies> tf;
	tf.reset(new TraitFrequencies(numloci,largest_locus_value));
	int* locus_counts = tf->trait_counts;

#pragma vector always
	for(int indiv = 0; indiv < popsize; indiv++) {
		for(int locus = 0; locus < numloci; locus++) {
			int trait_at_locus = population_traits[indiv * numloci + locus];
			++locus_counts[locus * largest_locus_value + trait_at_locus];
		}
	}
	timer.end("population::tabulate_trait_counts");
	return tf;
}

void Population::step_basicwf() {
	// Prepare by copying current state to previous state, before doing transmission
	// algorithm
	swap_population_arrays();

	generate_uniform_int(0, popsize, popsize, indiv_to_copy);

	// Basic Wright-Fisher dynamics without innovation
#pragma omp parallel
{
	int indiv;
	#pragma omp for private(indiv)
	for(indiv = 0; indiv < popsize; indiv++) {
		int tocopy = indiv_to_copy[indiv];

		for(int locus = 0; locus < numloci; locus++) {
			population_traits[indiv * numloci + locus] = prev_population_traits[tocopy * numloci + locus];
		}

	}
}

}

void Population::step_wfia() {
	// Prepare by copying current state to previous state, before doing transmission
	// algorithm
	swap_population_arrays();

	generate_uniform_int(0, popsize, popsize, indiv_to_copy);

	// Wright-Fisher dynamics - in order to optimize performance, first we do all the copying so that
	// most of the copies will be vectorized and broken into work units.  Then, we come back for a small
	// number of individuals and give them mutated traits.

#pragma omp parallel
{
	int indiv;
	#pragma omp for private(indiv)
	for(indiv = 0; indiv < popsize; indiv++) {
		int tocopy = indiv_to_copy[indiv];

		for(int locus = 0; locus < numloci; locus++) {
			population_traits[indiv * numloci + locus] = prev_population_traits[tocopy * numloci + locus];
		}

	}
}

	// Now, we create innovations given the innovation rate, randomly throughout the population
	int num_mutations = poisson_dist(this->mt);
	//SPDLOG_TRACE(clog,"WFIA: num mutations this step: {}", num_mutations);

	for(int j = 0; j < num_mutations; j++) {
		int indiv_to_mutate = uniform_pop(this->mt);
		int locus_to_mutate = uniform_locus(this->mt);
		int new_trait = next_trait[locus_to_mutate];
		++next_trait[locus_to_mutate];
		population_traits[indiv_to_mutate * numloci + locus_to_mutate] = new_trait;
	}

}

void Population::swap_population_arrays() {
	// Start by swapping the population trait arrays, so that we capture the previous state for use

	int* tmp = prev_population_traits;
	prev_population_traits = population_traits;
	population_traits = tmp;

	// Clear the population_traits to zero so we can fill it by transmission from prev_population_traits
	int size_pop_array = numloci * popsize;
	memset(population_traits, 0, (size_pop_array * sizeof(int)));
}

std::string Population::dbg_params() {
	boost::format fmt("[Population %4% | popsize: %1% numloci: %2% inittraits: %3%  innovation_rate: %5%]");
	fmt % this->popsize % this->numloci % this->inittraits % this % this->innovation_rate;
	return fmt.str();
}

void Population::dbg_log_population() {
	SPDLOG_TRACE(clog, "population state: (rows are individuals, columns are loci)");

	//print with indiv as rows, columns as loci
	for(int indiv = 0; indiv < popsize; indiv++) {
		std::stringstream s;
		s << "indiv: " <<  std::setw(pop_digits_printing) << indiv << ": ";
		for(int locus = 0; locus < numloci; locus++) {
			s << population_traits[indiv * numloci + locus] << " ";
		}
		SPDLOG_TRACE(clog,"{}",s.str());
	}
}

};


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_VMX_ALTIVEC_ROL_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_VMX_ALTIVEC_ROL_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_VMX_SUPPORT

#include <boost/simd/bitwise/functions/rol.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/operator/functions/details/assert_utils.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( rol_
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , ((simd_ < integer_<A0>
                                              , boost::simd::tag::vmx_
                                              >
                                      ))
                                      ((simd_ < integer_<A0>
                                              , boost::simd::tag::vmx_
                                              >
                                      ))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1) const
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "rol : rotation is out of range");

      typedef typename boost::dispatch::meta::as_unsigned<A0>::type c_t;

      return vec_rl(a0(), bitwise_cast<c_t>(a1)());
    }
  };
} } }

#endif
#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SPLIT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SPLIT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

// #include <boost/simd/meta/hierarchy/simd.hpp>
// #include <boost/simd/function/simd/combine.hpp>
// #include <boost/simd/function/simd/extract.hpp>
// #include <boost/simd/function/simd/split_high.hpp>
// #include <boost/simd/function/simd/split_low.hpp>

// namespace boost { namespace simd { namespace ext
// {
//    namespace bd = boost::dispatch;
//    namespace bs = boost::simd;
//    BOOST_DISPATCH_OVERLOAD_IF(split_

//                              , (typename A0, typename A1, typename X, typename Y)
//                              , bd::cpu_
//                              , bs::pack_<bd::arithmetic_<A0>, X>
//                              , bs::pack_<bd::arithmetic_<A1>, X>
//                              , bs::pack_<bd::arithmetic_<A1>, X>
//                              )
//    {
//       using result = ;
//      BOOST_FORCEINLINE result operator()(A0 const& a0,A1& a1, A1& a2) const
//       {
//         a1 = split_low(a0);
//         a2 = split_high(a0);
//       }
//    };

//    BOOST_DISPATCH_OVERLOAD(split_

//                           , (typename A0, typename X)
//                           , bd::cpu_
//                           , bs::pack_<bd::arithmetic_<A0>, X>
//                           )
//    {
//   using  = ;
//       BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
//       {
//         return eval(a0, simd::meta::is_upgradable<A0>());
//       }
//       BOOST_FORCEINLINE result eval2(A0 const& a0, boost::mpl::true_) const
//       {
//         typename simd::meta::vector_of< base_t
//                                       , A0::static_size/2
//                                       >::type a00,a01;
//         split(a0, a00, a01);
//         return bs::combine(a00,a01);
//       }
//       BOOST_FORCEINLINE result eval2(A0 const& a0, boost::mpl::false_) const
//       {
//         return make<result>( static_cast<base_t>( extract<0>(a0) )
//                                 , static_cast<base_t>( extract<1>(a0) )
//                                 );
//       }
//       BOOST_FORCEINLINE result eval(A0 const& a0, boost::mpl::false_) const
//       {
//         return a0;
//       }
//       BOOST_FORCEINLINE result eval(A0 const& a0, boost::mpl::true_) const
//       {
//         return eval2(a0, boost::mpl::bool_<(A0::static_size>=4)>());
//       }
//    };

// } } }

#endif


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

#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__ACTION_NODE_HPP_
#define TRAFFIC_SIMULATOR__BEHAVIOR__ACTION_NODE_HPP_

#include <behaviortree_cpp_v3/action_node.h>

#include <boost/algorithm/clamp.hpp>
#include <memory>
#include <openscenario_msgs/msg/obstacle.hpp>
#include <openscenario_msgs/msg/waypoints_array.hpp>
#include <string>
#include <traffic_simulator/entity/entity_base.hpp>
#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
#include <traffic_simulator/helper/stop_watch.hpp>
#include <traffic_simulator/math/catmull_rom_spline.hpp>
#include <traffic_simulator/traffic_lights/traffic_light_manager.hpp>
#include <unordered_map>
#include <vector>

namespace entity_behavior
{
class ActionNode : public BT::ActionNodeBase
{
public:
  ActionNode(const std::string & name, const BT::NodeConfiguration & config);
  ~ActionNode() override = default;
  bool foundConflictingEntity(const std::vector<std::int64_t> & following_lanelets) const;
  boost::optional<double> getDistanceToConflictingEntity(
    const std::vector<std::int64_t> & route_lanelets,
    const traffic_simulator::math::CatmullRomSpline & spline);
  boost::optional<std::string> getFrontEntityName(
    const traffic_simulator::math::CatmullRomSpline & spline);
  double calculateStopDistance() const;
  boost::optional<double> getDistanceToFrontEntity(
    const traffic_simulator::math::CatmullRomSpline & spline);
  boost::optional<double> getDistanceToStopLine(
    const std::vector<std::int64_t> & route_lanelets,
    const std::vector<geometry_msgs::msg::Point> & waypoints);
  boost::optional<double> getDistanceToTrafficLightStopLine(
    const std::vector<std::int64_t> & route_lanelets,
    const std::vector<geometry_msgs::msg::Point> & waypoints);
  std::vector<openscenario_msgs::msg::EntityStatus> getRightOfWayEntities();
  std::vector<openscenario_msgs::msg::EntityStatus> getRightOfWayEntities(
    const std::vector<std::int64_t> & following_lanelets);
  boost::optional<double> getYieldStopDistance(
    const std::vector<std::int64_t> & following_lanelets);
  std::vector<openscenario_msgs::msg::EntityStatus> getOtherEntityStatus(std::int64_t lanelet_id);
  openscenario_msgs::msg::EntityStatus stopAtEndOfRoad();
  double getHorizon() const;

  /// throws if the derived class return RUNNING.
  BT::NodeStatus executeTick() override;

  /// You don't need to override this
  void halt() override { setStatus(BT::NodeStatus::IDLE); }

  static BT::PortsList providedPorts()
  {
    return {
      BT::InputPort<std::string>("request"),
      BT::InputPort<std::shared_ptr<hdmap_utils::HdMapUtils>>("hdmap_utils"),
      BT::InputPort<openscenario_msgs::msg::EntityStatus>("entity_status"),
      BT::InputPort<double>("current_time"),
      BT::InputPort<double>("step_time"),
      BT::InputPort<boost::optional<double>>("target_speed"),
      BT::OutputPort<openscenario_msgs::msg::EntityStatus>("updated_status"),
      BT::OutputPort<std::string>("request"),
      BT::InputPort<std::unordered_map<std::string, openscenario_msgs::msg::EntityStatus>>(
        "other_entity_status"),
      BT::InputPort<std::unordered_map<std::string, openscenario_msgs::msg::EntityType>>(
        "entity_type_list"),
      BT::InputPort<std::vector<std::int64_t>>("route_lanelets"),
      BT::InputPort<std::shared_ptr<traffic_simulator::TrafficLightManager>>(
        "traffic_light_manager"),
      BT::OutputPort<boost::optional<openscenario_msgs::msg::Obstacle>>("obstacle"),
      BT::OutputPort<openscenario_msgs::msg::WaypointsArray>("waypoints")};
  }
  void getBlackBoardValues();
  std::string request;
  std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils;
  std::shared_ptr<traffic_simulator::TrafficLightManager> traffic_light_manager;
  openscenario_msgs::msg::EntityStatus entity_status;
  double current_time;
  double step_time;
  boost::optional<double> target_speed;
  openscenario_msgs::msg::EntityStatus updated_status;
  std::unordered_map<std::string, openscenario_msgs::msg::EntityStatus> other_entity_status;
  std::unordered_map<std::string, openscenario_msgs::msg::EntityType> entity_type_list;
  std::vector<std::int64_t> route_lanelets;
  openscenario_msgs::msg::EntityStatus getEntityStatus(const std::string target_name) const;
  boost::optional<double> getDistanceToTargetEntityPolygon(
    const traffic_simulator::math::CatmullRomSpline & spline, const std::string target_name);

private:
  boost::optional<double> getDistanceToTargetEntityOnCrosswalk(
    const traffic_simulator::math::CatmullRomSpline & spline,
    const openscenario_msgs::msg::EntityStatus & status);
  boost::optional<double> getDistanceToTargetEntityPolygon(
    const traffic_simulator::math::CatmullRomSpline & spline,
    const openscenario_msgs::msg::EntityStatus & status);
  boost::optional<openscenario_msgs::msg::EntityStatus> getConflictingEntityStatus(
    const std::vector<std::int64_t> & following_lanelets) const;
  std::vector<openscenario_msgs::msg::EntityStatus> getConflictingEntityStatusOnCrossWalk(
    const std::vector<std::int64_t> & route_lanelets) const;
  std::vector<openscenario_msgs::msg::EntityStatus> getConflictingEntityStatusOnLane(
    const std::vector<std::int64_t> & route_lanelets) const;
};
}  // namespace entity_behavior

#endif  // TRAFFIC_SIMULATOR__BEHAVIOR__ACTION_NODE_HPP_


#include <genCBMC.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <bitset>
#include <cmath>
#include <algorithm>

namespace ila
{

    static std::string getMask(const int& width);
    static std::string getSignExtMask(const int& width);
    static std::string getSignBit(const int& width);
    static CVerifGen::CVarMap* maskPtr = NULL;
    // ---------------------------------------------------------------------- //
    int CVar::varCnt = 0;

    std::string CVar::boolStr  = "bool";
    std::string CVar::bvStr    = "BIT_VEC";
    std::string CVar::memStr   = "type_mem";
    std::string CVar::voidStr  = "void";

    // ---------------------------------------------------------------------- //
    CVar::CVar(nptr_t nptr, const std::string& name)
    {
        init(nptr);
        if (name != "") _name = name;
    }

    CVar::CVar(const Node* node, const std::string& name)
    {
        init(node);
        if (name != "") _name = name;
    }

    CVar::CVar(int width)
    {
        memberVar = false;
        _type = bvStr;
        _width = width;
        _name = "cVar_" + boost::lexical_cast<std::string>(varCnt++);
        _isConst = false;
    }

    CVar::CVar(const std::string& name, const std::string& val)
        : _val(val), memberVar(false)
    {
        _name = "cMask_" + name + "_";
        _type = bvStr;
        _isConst = false;
    }

    CVar::CVar(const CVar* var)
        : _type(var->_type)
        , _width(var->_width)
        , _idxWidth(var->_idxWidth)
        , memberVar(var->memberVar)
    {
        _name = "cVar_" + boost::lexical_cast<std::string>(varCnt++);
        _isConst = var->_isConst;
        _val = var->_val;
    }

    CVar::~CVar()
    {
    }

    // ---------------------------------------------------------------------- //

    std::string CVar::memdef() const
    {
        ILA_ASSERT(_type == memStr, "cannot generate memdef of non-mem variable");
        return  "( " + ctype() + " ) malloc( sizeof(BV" + boost::lexical_cast<std::string>(_width)  + ") * " +
            boost::lexical_cast<std::string>( std::pow( 2, _idxWidth ) ) + " )" ;
    }
    std::string CVar::ctype() const // only for def
    {

        if (_type != memStr)
            return _type;
        else
            return  "BV" + boost::lexical_cast<std::string>(_width)  + " *";
    }

    std::string CVar::def() const
    {
        if (_name != "") {
            if(_type == memStr)
                return (ctype() + " " + _name);
            return (_type + " " + _name);
        } else {
            return (_type);
        }
    }

    std::string CVar::refDef() const
    {
        return (_type + "& " + _name);
    }

    std::string CVar::use() const
    {
        if (_isConst) {
            return _val;
        } else {
            if (!memberVar)
                return _name;
            return "( this-> "+_name + " )";
        }
    }

    std::string CVar::exactUse() const
    {
        ILA_ASSERT(_type == bvStr, "Wrong type");
        if (_isConst) {
            return _val;
        } else {
            return ("(" + _name + " & " + getMask(_width) + ")");
        }
    }

    std::string CVar::signedUse() const
    {
        boost::format signUseFmt(
            "((%1% & %2%) ? (%3% | %4%) : %5%)");
        if (_isConst) {
            return _val;
        } else {
            signUseFmt % _name % getSignBit(_width)
                       % _name % getSignExtMask(_width)
                       % _name;
            return signUseFmt.str();
        }
    }

    std::string CVar::castUse() const
    {
        ILA_ASSERT(_type == bvStr, "Wrong type");
        if (_isConst) {
            return _val;
        } else {
            return ("static_cast<unsigned>(" + _name + ")");
        }
    }
    // ---------------------------------------------------------------------- //
    void CVar::init(nptr_t n)
    {
        init(n.get());
    }

    void CVar::init(const Node* n)
    {
        memberVar = false;
        _name = "cVar_" + boost::lexical_cast<std::string>(varCnt++);
        if (n->type.isBool()) {
            _type = boolStr;
            _width = 0;
        } else if (n->type.isBitvector()) {
            _type = bvStr;
            _width = n->type.bitWidth;
        } else if (n->type.isMem()) {
            _type = memStr;
            _width = n->type.dataWidth;
            _idxWidth = n->type.addrWidth;
        } else if (n->type.isFunc()) {
            // Not neccessary.
            _name = n->getName();
            _type = bvStr;
            _width = n->type.bitWidth;
        } else {
            ILA_ASSERT(false, "Unknonw type.");
        }

        const BoolConst* boolconst = NULL;
        const BitvectorConst* bvconst = NULL;

        if ((boolconst = dynamic_cast<const BoolConst*>(n))) {
            _isConst = true;
            _val = (boolconst->val()) ? "true" : "false";
        } else if ((bvconst = dynamic_cast<const BitvectorConst*>(n))) {
            _isConst = true;
            _val = bvconst->vstr();
        } else {
            _isConst = false;
            _val = "";
        }

    }

    // ---------------------------------------------------------------------- //
    CFun::CFun(const std::string& name)
        : _name(name)
    {
        _ret = NULL;
    }

    CFun::~CFun()
    {
    }

    // ---------------------------------------------------------------------- //
    void CFun::addArg(const CVar* arg)
    {
        _args.push_back(arg);
    }

    void CFun::addBody(const std::string& code)
    {
        _codeList.push_back(code);
    }

    // ---------------------------------------------------------------------- //
    // Print the function header outside the class to the output stream. (also uninterpreted functions)
    void CFun::dumpDec(std::ostream& out,
                          const std::string& modelName,
                          const int& indent, bool decl) const
    {
        std::string ind = "";
        for (int i = 0; i < indent; i++) {
            ind = ind + "\t";
        }

        std::string type = (_ret != NULL) ? _ret->_type : CVar::voidStr;
        std::string name = (modelName == "") ?  // it is an uninterpreted function ?
                           _name : (_name);//_name : (modelName + "::" + _name);
        std::string tail = (modelName == "" || decl) ? ");\n" : (")\n" + ind + "{\n");

        out << ind << type << " " << name << "(";
        bool hasThis = false;
        if(modelName != "") {
            out << "struct " + modelName + "* this";
            hasThis = true;
        }
        for (unsigned i = 0; i<_args.size(); i++) {
            out << ((i == 0 && !hasThis)? "" : ", ") << _args[i]->def();
        }
        out << tail;
    }

    void CFun::dumpCode(std::ostream& out, const int& indent) const
    {
        std::string ind = "";
        for (int i = 0; i < indent; i++) {
            ind = ind + "\t";
        }
        for (unsigned i = 0; i != _codeList.size(); i++) {
            out << ind << "\t" << _codeList[i] << "\n";
        }
        out << ind << "}\n";
    }

    void CFun::dumpVarDec(std::ostream& out, const int& indent) const
    {
        std::string ind = "";
        for (int i = 0; i < indent; i++) {
            ind = ind + "\t";
        }
        for (unsigned i = 0; i != _varList.size(); i++) {
            out << ind << "\t" << _varList[i] << "\n";
        }
        return;
    }

    // ---------------------------------------------------------------------- //
    CVerifGen::CVerifGen(const std::string& prefix)
        : _modelName("model_" + prefix)
    {
    }

    CVerifGen::~CVerifGen()
    {
    }

    // ---------------------------------------------------------------------- //
    CVar* CVerifGen::addInput(const std::string& name, nptr_t nptr, bool ms)
    {
        // FIXME Need to make sure name is valid.
        CVar* ip = new CVar(nptr, name);
        checkAndInsert(_inputs, name, ip, ms);
        return ip;
    }

    CVar* CVerifGen::addState(const std::string& name, nptr_t nptr, bool ms)
    {
        // FIXME Need to make sure name is valid.
        CVar* var = new CVar(nptr, name);
        var->memberVar = true;
        checkAndInsert(_states, name, var, ms);
        return var;
    }

    CFun* CVerifGen::addFun(const std::string& name, bool ms)
    {
        // Create new fun and put it in funMap.
        CFun* fun = new CFun(name);
        checkAndInsert(_funMap, name, fun, ms);
        // Insert inputs and global state into the function's varmap
        // Also set the input as fun's arguments.
        CVarMap* varMap = new CVarMap;
        for (auto it = _inputs.begin(); it != _inputs.end(); it++) {
            checkAndInsert(*varMap, it->first, it->second, ms);
            fun->addArg(it->second);
        }
        for (auto it = _states.begin(); it != _states.end(); it++) {
            checkAndInsert(*varMap, it->first, it->second, ms);
        }
        for (auto it = _unitpFuncVarMap.begin();
             it != _unitpFuncVarMap.end(); it++) {
            checkAndInsert(*varMap, it->first, it->second, ms);
        }
        _varInFun[fun] = varMap;
        return fun;
    }

    CVar* CVerifGen::addFuncVar(const std::string& name, nptr_t node, bool ms)
    {
        CVar* var = new CVar(node, name);
        checkAndInsert(_unitpFuncVarMap, name, var, ms);

        CFun* fun = new CFun(name);
        fun->_ret = new CVar(node->type.bitWidth);
        for (unsigned i = 0; i != node->type.argsWidth.size(); i++) {
            CVar* arg = new CVar(node->type.argsWidth[i]);
            fun->_args.push_back(arg);
        }
        checkAndInsert(_unitpFuncMap, name, fun, ms);
        return var;
    }

    // ---------------------------------------------------------------------- //
    void CVerifGen::operator() (const Node* n)
    {
        CVarMap::iterator it = _curVarMap->find(n->getName());
        if (it != _curVarMap->end()) {
            return;
        }
        const BoolVar* boolvar = NULL;
        const BoolConst* boolconst = NULL;
        const BoolOp* boolop = NULL;

        const BitvectorVar* bvvar = NULL;
        const BitvectorConst* bvconst = NULL;
        const BitvectorOp* bvop = NULL;

        const MemVar* memvar = NULL;
        const MemConst* memconst = NULL;
        const MemOp*  memop = NULL;

        const FuncVar* funcvar = NULL;

        const BitvectorChoice* bvchoice = NULL;
        const BoolChoice* boolchoice = NULL;
        const MemChoice* memchoice = NULL;
        const WriteSlice* writeslice = NULL;
        const ReadSlice* readslice = NULL;

        CVar* res = NULL;

        //// Bool ////
        if ((boolvar = dynamic_cast<const BoolVar*>(n))) {
            res = getBoolVarC(boolvar);
        } else if ((boolconst = dynamic_cast<const BoolConst*>(n))) {
            res = getBoolConstC(boolconst);
            _localConstVar[n->getName()] = res;
        } else if ((boolop = dynamic_cast<const BoolOp*>(n))) {
            res = getBoolOpC(boolop);
        //// Bitvector ////
        } else if ((bvvar = dynamic_cast<const BitvectorVar*>(n))) {
            res = getBvVarC(bvvar);
        } else if ((bvconst = dynamic_cast<const BitvectorConst*>(n))) {
            res = getBvConstC(bvconst);
            _localConstVar[n->getName()] = res;
        } else if ((bvop = dynamic_cast<const BitvectorOp*>(n))) {
            res = getBvOpC(bvop);
        //// Mem ////
        } else if ((memvar = dynamic_cast<const MemVar*>(n))) {
            res = getMemVarC(memvar);
        } else if ((memconst = dynamic_cast<const MemConst*>(n))) {
            res = getMemConstC(memconst);
        } else if ((memop = dynamic_cast<const MemOp*>(n))) {
            res = getMemOpC(memop);
        //// Func ////
        } else if ((funcvar = dynamic_cast<const FuncVar*>(n))) {
            res = getFuncVarC(funcvar);
        //// Choice ////
        } else if ((boolchoice = dynamic_cast<const BoolChoice*>(n))) {
            ILA_ASSERT(false, "BoolChoice not implemented.");
        } else if ((bvchoice = dynamic_cast<const BitvectorChoice*>(n))) {
            ILA_ASSERT(false, "BitvectorChoice not implemented.");
        } else if ((memchoice = dynamic_cast<const MemChoice*>(n))) {
            ILA_ASSERT(false, "MemChoice not implemented.");
        } else if ((writeslice = dynamic_cast<const WriteSlice*>(n))) {
            ILA_ASSERT(false, "WriteSlice not implemented.");
        } else if ((readslice = dynamic_cast<const ReadSlice*>(n))) {
            ILA_ASSERT(false, "ReadSlice not implemented.");
        } else {
            ILA_ASSERT(false, "Unknown node type.");
        }

        checkAndInsert(*_curVarMap, n->getName(), res);
        _curVar = res;
    }

    // ---------------------------------------------------------------------- //
    // Build function body with ast node.
    void CVerifGen::buildFun(CFun* f, nptr_t nptr)
    {
        if (nptr == NULL) { return; }
        auto it = _varInFun.find(f);
        if (it == _varInFun.end()) {
            ILA_ASSERT(false, "Function not defined yet.");
        }
        _curVarMap = it->second;
        _curFun = f;
        maskPtr = &_masks;
        _localConstVar.clear();

        depthFirstTraverse(nptr);

        // Add var decl (no global states)
        for (auto it = _curVarMap->begin(); it != _curVarMap->end(); it++) {
          if (_states.find(it->first) == _states.end() &&
              _inputs.find(it->first) == _inputs.end() &&
              _unitpFuncVarMap.find(it->first) == _unitpFuncVarMap.end() &&
              _localConstVar.find(it->first) == _localConstVar.end()
              ) {
                std::string code;
                if(it->second->_type == CVar::memStr) {
                    code = it->second->def() + " = " + it->second->memdef() + ";"; // define of variables, for memory need to take care of
                    _localArray[it->first] = it->second; // record and later may need to free it if it is not the return variable
                }
                else
                    code = it->second->def() + ";";  // define of variables
                //don't insert the same if you export to a single file (in a single update fun)
                if(std::find(_curFun->_varList.begin(),_curFun->_varList.end(),code) == _curFun->_varList.end())
                    _curFun->_varList.push_back(code);
                //else
                //    std::cout<<"red:"<<code<<std::endl;
          }
        }

        _curVarMap = NULL;
        _curFun = NULL;
        return;
    }

    // Set the return variable for the function.
    void CVerifGen::setFunReturn(CFun* f, nptr_t nptr)
    {
        if (nptr == NULL) { return; }
        auto it = _varInFun.find(f);
        if (it == _varInFun.end()) {
            ILA_ASSERT(false, "Function not defined yet.");
        }

        f->_ret = findVar(*(it->second), nptr->getName());
        ILA_ASSERT(f->retSet(), "Return not set.");

        // now let's clean the _localArray
        for (auto it = _localArray.begin(); it != _localArray.end(); ++it ) {
            if( nptr->getName() == it->first ) // don't free the return variable
                continue;
            std::string code = "free( " + it->second->use() + " );";
            f->addBody(code);
        }
        _localArray.clear();
    }

    // Terminate the function building.
    void CVerifGen::endFun(CFun* f)
    {
        std::string code = "NOT SPECIFIED";
        // Update the states.
        for (unsigned i = 0; i < f->_updates.size(); i++) {
            if(f->_updates[i].first->_type == CVar::memStr) {
                code = "__CPROVER_array_copy( " + f->_updates[i].first->use() + "," +
                       f->_updates[i].second->use() + " );";
                f->addBody(code);
                code = "free( " + f->_updates[i].second->use() +" );";
                f->addBody(code);
            }
            else {
                code = f->_updates[i].first->use() + " = " +
                       f->_updates[i].second->use() + ";";
                f->addBody(code);
            }
        }
        // Return the value.
        if (f->_ret != NULL) {
            code = "return " + f->_ret->use() + ";";
            f->addBody(code);
        } else {
            code = "return;";
            f->addBody(code);
        }
    }
    // Add a variable that should be update at the end of the function.
    void CVerifGen::addFunUpdate(CFun* f, nptr_t lhs, nptr_t rhs)
    {
        if (lhs == NULL || rhs == NULL) { return; }
        auto it = _varInFun.find(f);
        if (it == _varInFun.end()) {
            ILA_ASSERT(false, "Function not defined yet.");
        }

        CVar* argL = findVar(*(it->second), lhs->getName());
        CVar* argR = findVar(*(it->second), rhs->getName());

        f->_updates.push_back(std::make_pair(argL, argR));
    }

    // Add a variable that should be update at the end of the function.
    void CVerifGen::addFunUpdate(CFun* f, nptr_t lhs, CVar* rhs)
    {
        if (lhs == NULL || rhs == NULL) { return ; }
        auto it = _varInFun.find(f);
        if (it == _varInFun.end()) {
            ILA_ASSERT(false, "Function not defined yet.");
        }

        CVar* argL = findVar(*(it->second), lhs->getName());

        f->_updates.push_back(std::make_pair(argL, rhs));
    }

    CVar* CVerifGen::appFun(CFun* appFun, CFun* envFun)
    {
        auto it = _varInFun.find(envFun);
        if (it == _varInFun.end()) {
            ILA_ASSERT(false, "Function not defined yet.");
        }
        _curVarMap = it->second;

        ILA_ASSERT(appFun->_ret != NULL,
                appFun->_name + " return value not specified");
        CVar* retVar = new CVar(appFun->_ret); // FIXME: NOTE: no allocation is needed
        (*_curVarMap)[retVar->_name] = retVar;

        std::string code = retVar->def() + " = " + appFun->_name + "( this";  // FIXME: need to add its struct's name
        for (unsigned i = 0; i < appFun->_args.size(); i++) {
            code = code + (", ") + appFun->_args[i]->use();
        }
        code = code + ");";
        envFun->addBody(code);

        _curVarMap = NULL;
        return retVar;
    }

    // ---------------------------------------------------------------------- //
    // Export all code to the output stream.
    void CVerifGen::exportAllToFile(const std::string& fileName) const
    {
        std::ofstream out(fileName.c_str());
        ILA_ASSERT(out.is_open(), "File " + fileName + " not open.");

        // Put common headers, type def, ...
        createCommon(out);

        // Put model headers, constant memory, uninterpreted function.
        genModel(out);

        // Model member functions.
        for (auto it = _funMap.begin(); it != _funMap.end(); it++) {
            it->second->dumpDec(out, _modelName, 0);
            it->second->dumpVarDec(out, 0);
            it->second->dumpCode(out, 0);
        }

        out.close();
    }

    void CVerifGen::exportAllToDir(const std::string& dirName) const
    {
        std::vector<std::string> fileNames;

        std::string commonFile = "common.h";
        std::string modelHeader = _modelName + "_class.h";

        std::ofstream out;
        // Common headers
        out.open(dirName + "/" + commonFile.c_str());
        ILA_ASSERT(out.is_open(), "File " + commonFile + " not open.");
        createCommon(out);
        out.close();

        // Model headers
        out.open(dirName + "/" + modelHeader.c_str());
        ILA_ASSERT(out.is_open(), "File " + modelHeader + " not open.");
        out << "#include \"" << commonFile << "\"\n";
        genModel(out);
        out.close();

        // Update functions
        for (auto it = _funMap.begin(); it != _funMap.end(); it++) {
            std::string fileName = dirName + "/subfun_" + it->first + ".c";
            out.open(fileName.c_str());
            ILA_ASSERT(out.is_open(), "File " + fileName + " not open.");
            fileNames.push_back("subfun_" + it->first + ".c");

            out << "#include \"" << commonFile << "\"\n";
            out << "#include \"" << modelHeader << "\"\n\n";
            it->second->dumpDec(out, _modelName, 0);
            it->second->dumpVarDec(out, 0);
            it->second->dumpCode(out, 0);

            out.close();
        }

        out.open(dirName + "/funlist.txt");
        out << "SRCS =";
        for (unsigned i  = 0; i < fileNames.size(); i++) {
            out << " " << fileNames[i];
        }
        out << "\n";
        out.close();
    }

    // ---------------------------------------------------------------------- //
    void CVerifGen::createCommon(std::ostream& out) const
    {
        // Include headers
        out << "#include <stdint.h>\n";

        // type define
        out << "\n";
        out << "#ifndef CPP_BITVEC_TYPE\n";
        out << "#define CPP_BITVEC_TYPE\n";
        out << "typedef uint64_t "
            << CVar::bvStr << ";\n";
        out << "#endif\n";

        // Mem type class
        out << "\n/****************************************************/\n";
        out << "#ifndef TYPE_MEM_CLASS\n";
        out << "#define TYPE_MEM_CLASS\n";
        defMemClass(out);
        out << "#endif\n";
        out << "/****************************************************/\n";

    }

    // ---------------------------------------------------------------------- //
    CVar* CVerifGen::getBoolVarC(const BoolVar* n)
    {
        CVar* var = new CVar(n);
        var->memberVar = true;
        return var;
    }

    CVar* CVerifGen::getBoolConstC(const BoolConst* n)
    {
        CVar* var = new CVar(n);
        return var;
    }

    CVar* CVerifGen::getBoolOpC(const BoolOp* n)
    {
        CVar* var = new CVar(n);
        std::string code = "boolOp NOT IMPLEMENTED";
        //// Unary ////
        if (n->op == BoolOp::Op::NOT) {
            CVar* arg0 = findVar(*_curVarMap, n->arg(0)->getName());
            code = var->use() + " = !" + arg0->use() + ";";
            _curFun->addBody(code);
        //// Binary ////
        } else if (n->op >= BoolOp::Op::AND &&
                   n->op <= BoolOp::Op::DISTINCT) {
            CVar* arg0 = findVar(*_curVarMap, n->arg(0)->getName());
            CVar* arg1 = findVar(*_curVarMap, n->arg(1)->getName());
            if (n->op == BoolOp::Op::AND) {
                code = var->use() + " = " +
                       arg0->use() + " && " + arg1->use() + ";";
            } else if (n->op == BoolOp::Op::OR) {
                code = var->use() + " = " +
                       arg0->use() + " || " + arg1->use() + ";";
            } else if (n->op == BoolOp::Op::XOR) {
                code = var->use() + " = " +
                       arg0->use() + " ^ " + arg1->use() + ";";
            } else if (n->op == BoolOp::Op::XNOR) {
                code = var->use() + " = !( " +
                       arg0->use() + " ^ " + arg1->use() + ");";
            } else if (n->op == BoolOp::Op::NAND) {
                code = var->use() + " = !(" +
                       arg0->use() + " && " + arg1->use() + ");";
            } else if (n->op == BoolOp::Op::NOR) {
                code = var->use() + " = !(" +
                       arg0->use() + " || " + arg1->use() + ");";
            } else if (n->op == BoolOp::Op::IMPLY) {
                code = var->use() + " = !" +
                       arg0->use() + " || " + arg1->use() + ";";
            } else if (n->op == BoolOp::Op::SLT) {
                code = var->use() + " = " +
                       arg0->signedUse() + " < " + arg1->signedUse() + ";";
            } else if (n->op == BoolOp::Op::SGT) {
                code = var->use() + " = " +
                       arg0->signedUse() + " > " + arg1->signedUse() + ";";
            } else if (n->op == BoolOp::Op::SLE) {
                code = var->use() + " = " +
                       arg0->signedUse() + " <= " + arg1->signedUse() + ";";
            } else if (n->op == BoolOp::Op::SGE) {
                code = var->use() + " = " +
                       arg0->signedUse() + " >= " + arg1->signedUse() + ";";
            } else if (n->op == BoolOp::Op::ULT) {
                code = var->use() + " = " +
                       arg0->use() + " < " + arg1->use() + ";";
            } else if (n->op == BoolOp::Op::UGT) {
                code = var->use() + " = " +
                       arg0->use() + " > " + arg1->use() + ";";
            } else if (n->op == BoolOp::Op::ULE) {
                code = var->use() + " = " +
                       arg0->use() + " <= " + arg1->use() + ";";
            } else if (n->op == BoolOp::Op::UGE) {
                code = var->use() + " = " +
                       arg0->use() + " >= " + arg1->use() + ";";
            } else if (n->op == BoolOp::Op::EQUAL) {
                code = var->use() + " = (" +
                       arg0->use() + " == " + arg1->use() + ");";
            } else if (n->op == BoolOp::Op::DISTINCT) {
                code = var->use() + " = (" +
                       arg0->use() + " != " + arg1->use() + ");";
            }
            _curFun->addBody(code);
        //// Ternary ////
        } else if (n->op == BoolOp::Op::IF) {
            CVar* arg0 = findVar(*_curVarMap, n->arg(0)->getName());
            CVar* arg1 = findVar(*_curVarMap, n->arg(1)->getName());
            CVar* arg2 = findVar(*_curVarMap, n->arg(2)->getName());
            code = var->use() + " = " + arg0->use() + " ? " +
                   arg1->use() + " : " + arg2->use() + ";";
            _curFun->addBody(code);
        } else {
            ILA_ASSERT(false, "Unknown bool op.");
        }
        return var;
    }

    CVar* CVerifGen::getBvVarC(const BitvectorVar* n)
    {
        CVar* var = new CVar(n);
        var->memberVar = true;
        return var;
    }

    CVar* CVerifGen::getBvConstC(const BitvectorConst* n)
    {
        CVar* var = new CVar(n);
        return var;
    }

    CVar* CVerifGen::getBvOpC(const BitvectorOp* n)
    {
        CVar* var = new CVar(n);
        std::string code = "bvOp NOT IMPLEMENTED.";
        //// Unary ////
        if (n->op >= BitvectorOp::Op::NEGATE &&
            n->op <= BitvectorOp::Op::EXTRACT) {
            CVar* arg0 = findVar(*_curVarMap, n->arg(0)->getName());
            if (n->op == BitvectorOp::Op::NEGATE) {
                code = var->use() + " = -" + arg0->signedUse() + ";";
            } else if (n->op == BitvectorOp::Op::COMPLEMENT) {
                code = var->use() + " = ~" + arg0->use() + ";";
            } else if (n->op == BitvectorOp::Op::LROTATE) {
                int par0 = n->param(0);
                int total = var->_width;
                std::string l = boost::lexical_cast<std::string>(par0);
                std::string r = boost::lexical_cast<std::string>(total-par0);
                // int var = x & 0xff
                // var = ((var << l) | (var >> r)) & 0xff
                code = var->use() + " = " + arg0->exactUse() + ";";
                _curFun->addBody(code);
                code = var->use() + " = ((" +
                       var->use() + " << " + l + ") | ( " +
                       var->use() + " >> " + r + ")) & " +
                       getMask(var->_width) + ";";
            } else if (n->op == BitvectorOp::Op::RROTATE) {
                int par0 = n->param(0);
                int total = var->_width;
                std::string r = boost::lexical_cast<std::string>(par0);
                std::string l = boost::lexical_cast<std::string>(total-par0);
                // int var = x & 0xff
                // var = ((var << l) | (var >> r)) & 0xff
                code = var->use() + " = " + arg0->exactUse() + ";";
                _curFun->addBody(code);
                code = var->use() + " = ((" +
                       var->use() + " << " + l + ") | ( " +
                       var->use() + " >> " + r + ")) & " +
                       getMask(var->_width) + ";";
            } else if (n->op == BitvectorOp::Op::Z_EXT) {
                code = var->use() + " = " + arg0->exactUse() + ";";
            } else if (n->op == BitvectorOp::Op::S_EXT) {
                code = var->use() + " = " + arg0->signedUse() + ";";
            } else if (n->op == BitvectorOp::Op::EXTRACT) {
                int msb = n->param(0);
                int lsb = n->param(1);
                std::string r = boost::lexical_cast<std::string>(lsb);
                // (x >> r) & 0xfff..(-r)
                code = var->use() + " = (" + arg0->use() + " >> " + r +
                       ") & " + getMask(msb-lsb+1) + ";";
            }
        //// Binary ////
        } else if (n->op >= BitvectorOp::Op::ADD &&
                   n->op <= BitvectorOp::Op::READMEM) {
            CVar* arg0 = findVar(*_curVarMap, n->arg(0)->getName());
            CVar* arg1 = findVar(*_curVarMap, n->arg(1)->getName());
            if (n->op == BitvectorOp::Op::ADD) {
                code = var->use() + " = " +
                       arg0->use() + " + " + arg1->use() + ";";
                _curFun->addBody(code);
                code = var->use() + " = " + var->exactUse() + ";";
            } else if (n->op == BitvectorOp::Op::SUB) {
                code = var->use() + " = " +
                       arg0->use() + " - " + arg1->use() + ";";
                _curFun->addBody(code);
                code = var->use() + " = " + var->exactUse() + ";";
//                code = getSignedCCode(var);
            } else if (n->op == BitvectorOp::Op::AND) {
                code = var->use() + " = " +
                       arg0->use() + " & " + arg1->use() + ";";
            } else if (n->op == BitvectorOp::Op::OR) {
                code = var->use() + " = " +
                       arg0->use() + " | " + arg1->use() + ";";
            } else if (n->op == BitvectorOp::Op::XOR) {
                code = var->use() + " = " +
                       arg0->use() + " ^ " + arg1->use() + ";";
            } else if (n->op == BitvectorOp::Op::XNOR) {
                code = var->use() + " = ~(" +
                       arg0->use() + " ^ " + arg1->use() + ");";
            } else if (n->op == BitvectorOp::Op::NAND) {
                code = var->use() + " = ~(" +
                       arg0->use() + " & " + arg1->use() + ");";
            } else if (n->op == BitvectorOp::Op::NOR) {
                code = var->use() + " = ~(" +
                       arg0->use() + " | " + arg1->use() + ");";
            } else if (n->op == BitvectorOp::Op::SDIV) {
                code = var->use() + " = " +
                       arg0->signedUse() + " / " + arg1->signedUse() + ";";
                _curFun->addBody(code);
                code = var->use() + " = " + var->exactUse() + ";";
            } else if (n->op == BitvectorOp::Op::UDIV) {
                code = var->use() + " = " +
                       arg0->use() + " / " + arg1->use() + ";";
                _curFun->addBody(code);
                code = var->use() + " = " + var->exactUse() + ";";
            } else if (n->op == BitvectorOp::Op::SREM) {
                code = var->use() + " = " +
                       arg0->use() + " % " + arg1->use() + ";";
                _curFun->addBody(code);
                code = var->use() + " = " + var->exactUse() + ";";
            } else if (n->op == BitvectorOp::Op::UREM) {
                code = var->use() + " = " +
                       arg0->use() + " % " + arg1->use() + ";";
                _curFun->addBody(code);
                code = var->use() + " = " + var->exactUse() + ";";
            } else if (n->op == BitvectorOp::Op::SMOD) {
                // FIXME C++ has no modulo operator, equal to % when positive
                code = var->use() + " = " +
                       arg0->use() + " % " + arg1->use() + ";";
                _curFun->addBody(code);
                code = var->use() + " = " + var->exactUse() + ";";
            } else if (n->op == BitvectorOp::Op::SHL) {
                code = var->use() + " = " +
                       arg0->use() + " << " + arg1->castUse() + ";";
                _curFun->addBody(code);
                code = var->use() + " = " + var->exactUse() + ";";
            } else if (n->op == BitvectorOp::Op::LSHR) {
                code = var->use() + " = " +
                       arg0->use() + " >> " + arg1->castUse() + ";";
            } else if (n->op == BitvectorOp::Op::ASHR) {
                code = var->use() + " = " +
                       arg0->signedUse() + " >> " + arg1->castUse() + ";";
                _curFun->addBody(code);
                code = var->use() + " = " + var->exactUse() + ";";
            } else if (n->op == BitvectorOp::Op::MUL) {
                code = var->use() + " = " +
                       arg0->use() + " * " + arg1->use() + ";";
                _curFun->addBody(code);
                code = var->use() + " = " + var->exactUse() + ";";
            } else if (n->op == BitvectorOp::Op::CONCAT) {
                std::string w = boost::lexical_cast<std::string>(arg1->_width);
                // |arg0| << arg1.width | |arg1|
                code = var->use() + " = (" +
                       arg0->use() + " << " + w +
                       ") | " + arg1->use() + ";";
                _curFun->addBody(code);
                code = var->use() + " = " + var->exactUse() + ";";
            } else if (n->op == BitvectorOp::Op::GET_BIT) {
                code = var->use() + " = (" +
                       arg0->use() + " >> " + arg1->castUse() + ") & 0x1;";
            } else if (n->op == BitvectorOp::Op::READMEM) {
                code = var->use() + " = " +
                       arg0->use() + "[" + arg1->use() + "];";
            }
        //// Ternary ////
        } else if (n->op == BitvectorOp::READMEMBLOCK) {
            ILA_ASSERT(n->nArgs() == 2, "Two parameters expected.");
            CVar* mem = findVar(*_curVarMap, n->arg(0)->getName());
            CVar* addr = findVar(*_curVarMap, n->arg(1)->getName());
            ILA_ASSERT(n->nParams() == 2, "Two parameters expected.");
            int blxSize = mem->_width;
            int blxNum = n->param(0);
            endianness_t e = (endianness_t) n->param(1);
            // int64_t var = (mem.rd(addr) & 0xff);
            // for (int i = 0; i < blxNum; i++) {
            //     little:
            //     var = var | ((mem.rd(addr+i) & 0xff) << (width * i));
            //     big:
            //     var = (var << width) | (mem.rd(addr+i) & 0xff);
            // }
            // var = (var & 0x8) ? (var | 0xfff000) : var;
            boost::format defFmt(
                "%1% = (%2%[%3%] & %4%);");
            boost::format loopProlog(
                "for (int %1% = 1; %2% < %3%; %4%++) {");
            boost::format litFmt(
                "\t%1% = %2% | ((%3%[%4% + %5%] & %6%) << (%7% * %8%));");
            boost::format bigFmt(
                "\t%1% = (%2% << %3%) | (%4%[%5% + %6%] & %7%);");

            defFmt %var->use() %mem->use() %addr->use() %getMask(blxSize);
            _curFun->addBody(defFmt.str());

            CVar* idx = new CVar(32);
            loopProlog %idx->use() %idx->use() %blxNum %idx->use();
            _curFun->addBody(loopProlog.str());

            if (e == LITTLE_E) {
                litFmt % var->use()         // %1%
                       % var->use()         // %2%
                       % mem->use()         // %3%
                       % addr->use()        // %4%
                       % idx->use()         // %5%
                       % getMask(blxSize)   // %6%
                       % blxSize            // %7%
                       % idx->use();        // %8%
                code = litFmt.str();
                _curFun->addBody(code);
            } else {
                bigFmt % var->use()         // %1%
                       % var->use()         // %2%
                       % blxSize            // %3%
                       % mem->use()         // %4%
                       % addr->use()        // %5%
                       % idx->use()         // %6%
                       % getMask(blxSize);  // %7%
                code = bigFmt.str();
                _curFun->addBody(code);
            }
            _curFun->addBody("}");

            code = var->use() + " = " + var->exactUse() + ";";
        } else if (n->op == BitvectorOp::Op::IF) {
            CVar* arg0 = findVar(*_curVarMap, n->arg(0)->getName());
            CVar* arg1 = findVar(*_curVarMap, n->arg(1)->getName());
            CVar* arg2 = findVar(*_curVarMap, n->arg(2)->getName());
            code = var->use() + " = (" + arg0->use() + ") ? " +
                   arg1->use() + " : " + arg2->use() + ";";
        } else if (n->op == BitvectorOp::Op::APPLY_FUNC) {
            CVar* fun = findVar(*_curVarMap, n->arg(0)->getName());
            std::vector<CVar*> argVec;
            for (unsigned i = 1; i != n->nArgs(); i++) {
                CVar* arg = findVar(*_curVarMap, n->arg(i)->getName());
                argVec.push_back(arg);
            }
            code = var->use() + " = " + fun->use() + "(";
            for (unsigned i = 0; i != argVec.size(); i++) {
                code = code + ((i == 0)? "" : ", ") + argVec[i]->use();
            }
            code = code + ");";
        } else {
            ILA_ASSERT(false, "Unknown BitvectorOp.");
        }

        if (code != "") {
            _curFun->addBody(code);
        }
        return var;
    }

    CVar* CVerifGen::getMemVarC(const MemVar* n)
    {
        CVar* var = new CVar(n);
        var->memberVar = true;
        return var;
    }

    CVar* CVerifGen::getMemConstC(const MemConst* n)
    {
        // Should update to _states and _curVarMap.
        CVar* var = NULL;
        auto it = _states.find(n->getName());
        if (it != _states.end()) {
            var = it->second;
        } else {
            var = new CVar(n);
            checkAndInsert(_states, n->getName(), var);
        }

        _memConst[var] = n;
        return var;
    }

    CVar* CVerifGen::getMemOpC(const MemOp* n) // FIXME: need to change
    {
        CVar* var = NULL;
        CVar* arg0 = findVar(*_curVarMap, n->arg(0)->getName());
        CVar* arg1 = findVar(*_curVarMap, n->arg(1)->getName());
        CVar* arg2 = findVar(*_curVarMap, n->arg(2)->getName());

        std::string code = "MEM NOT IMPLEMENTED";
        if (n->op == MemOp::Op::STORE) {
            ILA_ASSERT(arg0->_type == CVar::memStr,
                       "Write to non-mem variable.");
            var = new CVar(n);
            code = var->use() + " = " + var->memdef() + ";";
            _curFun->addBody(code);
            //code = var->use() + " = " + arg0->use() + ";";
            code = "__CPROVER_array_copy( " + var->use() + " , " +  arg0->use() + " );";
            _curFun->addBody(code);
            code = var->use() + "[" + arg1->use() +
                   "] = ( " + arg2->use() + " );";
        } else if (n->op == MemOp::Op::ITE) {
            ILA_ASSERT(arg0->_type == CVar::boolStr,
                       "Condition not bool.");
            var = new CVar(n);
            code = var->use() + " = " + var->memdef() + ";";
            _curFun->addBody(code);

            boost::format memite(
                "if(%1%) \n\t__CPROVER_array_copy( %2% ,  %3%); \nelse\n\t__CPROVER_array_copy( %2% ,  %4%);");

            memite  % arg0->use()
                    % var->use()
                    % arg1->use()
                    % arg2->use();

            code = memite.str();

        } else if (n->op == MemOp::Op::STOREBLOCK) {
            ILA_ASSERT(arg0->_type == CVar::memStr,
                        "Write to non-mem variable.");
            var = new CVar(n);
            code = var->use() + " = " + var->memdef() + ";";
            _curFun->addBody(code);

            int chunkSize = arg0->_width;
            int chunkNum = arg2->_width / chunkSize;
            ILA_ASSERT(arg2->_width % chunkSize == 0,
                        "Block store size mismatch.");
            bool isLittle = n->endian == LITTLE_E;

            // var chunk_i;
            // Mem var = mem;
            // for (int i=0; i<chunkNum; i++) {
            //     endian dependent code
            //     chunk_i = chunk_i & 0xff
            //     var.wr(addr+i, chunk_i);
            // }
            //     little:
            //     chunk_i = data >> (i * chunkSize);
            //     big:
            //     chunk_i = data >> ((chunkNum - 1 - i) * chunkSize);

            CVar* idx = new CVar(32);
            CVar* chunk_i = new CVar(chunkSize);
            _curFun->addBody(chunk_i->def() + ";");
            code = "__CPROVER_array_copy( " + var->use() + " , " +  arg0->use() + " );";
            _curFun->addBody(code);
            //_curFun->addBody(var->use() + " = " + arg0->use() + ";");

            boost::format loopProlog(
                "for (int %1% = 0; %2% < %3%; %4%++) {");
            boost::format litFmt(
                "\t%1% = %2% >> (%3% * %4%);");
            boost::format bigFmt(
                "\t%1% = %2% >> ((%3% - 1 - %4%) * %5%);");
            boost::format uniFmt(
                "\t%1% = %2%;");
            boost::format writeFmt(
                "\t%1%[%2% + %3%] =(%4%);");

            loopProlog % idx->use()             // 1
                       % idx->use()             // 2
                       % chunkNum               // 3
                       % idx->use();            // 4

            litFmt     % chunk_i->use()         // 1
                       % arg2->use()            // 2
                       % idx->use()             // 3
                       % chunkSize;             // 4

            bigFmt     % chunk_i->use()         // 1
                       % arg2->use()            // 2
                       % chunkNum               // 3
                       % idx->use()             // 4
                       % chunkSize;             // 5

            uniFmt     % chunk_i->use()         // 1
                       % chunk_i->exactUse();   // 2

            writeFmt   % var->use()             // 1
                       % arg1->use()            // 2
                       % idx->use()             // 3
                       % chunk_i->use();        // 4

            _curFun->addBody(loopProlog.str());
            _curFun->addBody(isLittle ? litFmt.str() : bigFmt.str());
            _curFun->addBody(uniFmt.str());
            _curFun->addBody(writeFmt.str());
            code = "}";
        } else {
            ILA_ASSERT(false, "Invalid MemOp.");
        }

        if (code != "") {
            _curFun->addBody(code);
        }

        return var;
    }

    CVar* CVerifGen::getFuncVarC(const FuncVar* n)
    {
        CVar* var = new CVar(n);
        return var;
    }

    bool CVerifGen::isITE(nptr_t n)
    {
        const BoolOp* boolop = NULL;
        const BitvectorOp* bvop = NULL;
        const MemOp* memop = NULL;

        if ((boolop = dynamic_cast<const BoolOp*>(n.get()))) {
            return (boolop->op == BoolOp::Op::IF);
        } else if ((bvop = dynamic_cast<const BitvectorOp*>(n.get()))) {
            return (bvop->op == BitvectorOp::Op::IF);
        } else if ((memop = dynamic_cast<const MemOp*>(n.get()))) {
            return (memop->op == MemOp::Op::ITE);
        } else {
            return false;
        }
    }

    // ---------------------------------------------------------------------- //
    void CVerifGen::defMemClass(std::ostream& out) const
    {
        std::string bvStr = CVar::bvStr;
        out<<"typedef unsigned char BV1;\n";
        out<<"typedef unsigned char BV8;\n";
        out<<"typedef unsigned short BV16;\n";
        out<<"typedef uint32_t BV32;\n";
        out<<"typedef uint64_t BV64;\n";
    }

    // ---------------------------------------------------------------------- //
    void CVerifGen::setMemConst(std::ostream& out) const
    {
        out << "\n/****************************************************/\n";
        for (auto it = _memConst.begin(); it != _memConst.end(); it++) {
            out << "\n";
            out << _modelName << "::" <<  it->first->use() << ".setDef("
                << it->second->memvalues.def_value << ");\n";
            for (auto p : it->second->memvalues.values) {
                out << it->first->use()
                    << "[" << p.first << "] = ( " << p.second << ");\n";
            }
        }
        out << "/****************************************************/\n";
    }

    // ---------------------------------------------------------------------- //
    void CVerifGen::genModel(std::ostream& out) const
    {
        out << "\n/****************************************************/\n";
        out << "#ifndef " << boost::to_upper_copy<std::string>(_modelName)
            << "_CLASS\n";
        out << "#define " << boost::to_upper_copy<std::string>(_modelName)
            << "_CLASS\n";

        defUnitpFunc(out);

        // Model struct prolog
        out << "struct " <<  _modelName << "\n"
            << "{\n";

        // States variables
        out << "\t// State variables.\n";
        for (auto it = _states.begin(); it != _states.end(); it++) {
            out << "\t" << it->second->def() << ";\n";
        }
        // Model struct epilog
        out << "\n};\n";

        // Bit masks
        out << "\t// Bitvector masks.\n"; // let's make it global
        for (auto it = _masks.begin(); it != _masks.end(); it++) {
            out << it->second->def() <<  " = " <<  it->second->_val << ";\n";
        }

        // Constructor/destructor should be put in a separate file
        out << "void " << _modelName << "_init( struct "<<  _modelName <<" * this ) {\n";
        for (auto it = _states.begin(); it != _states.end(); it++) {
            if(it->second->_type == CVar::memStr)
                out << "\t" + it->second->use() + " = " + it->second->memdef() + ";\n";
        }
        out << "};\n";

        out << "" << _modelName << "_destroy( struct "<<  _modelName <<" * this ) {\n";
        for (auto it = _states.begin(); it != _states.end(); it++) {
            if(it->second->_type == CVar::memStr)
                out << "\tfree( " + it->second->use() + " );\n";
        }
        out << "};\n";

        // Public: functions (fetch, decode, update ... etc.)
        out << "\t// Public functions: fetch, decode, update, ...\n";
        for (auto it = _funMap.begin(); it != _funMap.end(); it++) {
            it->second->dumpDec(out, _modelName, 0, true);  // even we give modelname, we still want it to be only declaration not definition
        }

        //setMemConst(out);
        out << "#endif\n";
        out << "/****************************************************/\n";
    }

    void CVerifGen::defUnitpFunc(std::ostream& out) const
    {
        for (auto it = _unitpFuncMap.begin(); it != _unitpFuncMap.end(); it++) {
            out << "extern ";
            it->second->dumpDec(out, "", 0);
        }
    }

    // ---------------------------------------------------------------------- //
    void CVerifGen::depthFirstTraverse(nptr_t node)
    {
        CVarMap::iterator it = _curVarMap->find(node->getName());
        if (it != _curVarMap->end()) {
            return;
        }

        if (!isITE(node)) {
            unsigned n = node->nArgs();
            for (unsigned i = 0; i != n; i++) {
                const nptr_t arg_i = node->arg(i);
                depthFirstTraverse(arg_i);
            }
            (*this)(node.get());
        } else {
            ILA_ASSERT(node->nArgs() == 3, "ITE should have 3 args");

            CVar* var = new CVar(node);

            // condition
            depthFirstTraverse(node->arg(0));
            CVar* cond = findVar(*_curVarMap, node->arg(0)->getName());
            std::string condCode = "if (" + cond->use() + ") {";
            _curFun->addBody(condCode);

            // then
            depthFirstTraverse(node->arg(1));
            CVar* tExp = findVar(*_curVarMap, node->arg(1)->getName());
            std::string thenCode = var->use() + " = " + tExp->use() + ";";
            _curFun->addBody(thenCode);

            // else
            _curFun->addBody("} else {");
            depthFirstTraverse(node->arg(2));
            CVar* fExp = findVar(*_curVarMap, node->arg(2)->getName());
            std::string elseCode = var->use() + " = " + fExp->use() + ";";
            _curFun->addBody(elseCode);
            _curFun->addBody("}");

            checkAndInsert(*_curVarMap, node->getName(), var);
            _curVar = var;
        }
    }

    // ---------------------------------------------------------------------- //
    std::string CVerifGen::getSignedCCode(CVar* var)
    {
        static std::string signTempVarName = "c_signTempVar";
        static std::string unsignTempVarName = "c_unsignTempVar";
        CVar* signTempVar = NULL;
        CVar* unsignTempVar = NULL;
        std::string signStr;
        std::string unsignStr;

        auto signIt = _curVarMap->find(signTempVarName);
        if (signIt == _curVarMap->end()) {
            signTempVar = new CVar(32);
            unsignTempVar = new CVar(32);
            checkAndInsert(*_curVarMap, signTempVarName, signTempVar);
            checkAndInsert(*_curVarMap, unsignTempVarName, unsignTempVar);
            signStr = signTempVar->def() + " = ";
            unsignStr = unsignTempVar->def() + " = ";
        } else {
            signTempVar = (*_curVarMap)[signTempVarName];
            unsignTempVar = (*_curVarMap)[unsignTempVarName];
            signStr = signTempVar->use() + " = ";
            unsignStr = unsignTempVar->use() + " = ";
        }

        signStr += var->use() + " | " + getSignExtMask(var->_width) + ";";
        unsignStr += var->use() + " & " + getMask(var->_width) + ";";
        _curFun->addBody(signStr);
        _curFun->addBody(unsignStr);
        boost::format signUseFmt(
            "%1% = ((%2% >> %3%) & 0x1) ? %4% : %5%;");
        signUseFmt % var->use()
                   % var->use()
                   % (var->_width-1)
                   % signTempVar->use()
                   % unsignTempVar->use();
        return signUseFmt.str();
    }

    // ---------------------------------------------------------------------- //
    template <class T>
    void CVerifGen::checkAndInsert(std::map<std::string, T*>& mp,
                                   const std::string& name,
                                   T* var,
                                   bool force)
    {
        auto it = mp.find(name);
        if (it != mp.end()) {
            if (!force)
                ILA_ASSERT(false, "Variable " + name + " has been defined.");
            else
                return;
        }
        mp[name] = var;
    }

    CVar* CVerifGen::findVar(CVarMap& mp, const std::string& name)
    {
        auto it = mp.find(name);
        ILA_ASSERT(it != mp.end(), "Variable " + name + " not defined yet.");
        return it->second;
    }

    static std::string getMask(const int& width)
    {
        ILA_ASSERT(width > 0, "Negative width.");

        boost::format unMaskStr("((%1%)1 << %2%) - 1");
        unMaskStr % CVar::bvStr
                  % width;
        std::string val = unMaskStr.str();

        auto it = maskPtr->find(val);
        if (it == maskPtr->end()) {
            std::string name = "un_"+boost::lexical_cast<std::string>(width);
            CVar* var = new CVar(name, val);
            (*maskPtr)[val] = var;
            return var->use();
        } else {
            return it->second->use();
        }
        return val;
    }

    static std::string getSignExtMask(const int& width)
    {
        ILA_ASSERT(width > 0, "Negative width.");
        std::string str = "~" + getMask(width);
        auto it = maskPtr->find(str);
        if (it == maskPtr->end()) {
            std::string name = "sign_"+boost::lexical_cast<std::string>(width);
            CVar* var = new CVar(name, str);
            (*maskPtr)[str] = var;
            return var->use();
        } else {
            return it->second->use();
        }
        return str;
    }

    static std::string getSignBit(const int& width)
    {
        ILA_ASSERT(width > 0, "Negative width.");
        std::string str = "(" + CVar::bvStr + ")1 << (" +
            boost::lexical_cast<std::string>(width) + " - 1)";
        auto it = maskPtr->find(str);
        if (it == maskPtr->end()) {
            std::string name = "bit_"+boost::lexical_cast<std::string>(width);
            CVar* var = new CVar(name, str);
            (*maskPtr)[str] = var;
            return var->use();
        } else {
            return it->second->use();
        }
        return str;
    }

}