#include <boost/hana.hpp>
#include <mpdef/list.hpp>
#include <nbdl.hpp>
#include <string>

namespace hana = boost::hana;

int main()
{
  auto my_promise = nbdl::promise([](auto& resolver, auto value)
  {
    if (value > hana::size_c<10>)
      resolver.reject(value);
    else
      resolver.resolve(value + hana::size_c<1>);
  });

  volatile std::size_t result;
#if defined(METABENCH)
  nbdl::run_sync(
    hana::make_tuple(
      hana::make_tuple(
        <%= (0..n).map { |i| "my_promise" }.join(', ') %>
      )
    , [&](auto value) { result = decltype(value)::value; }
    , nbdl::catch_([](auto&&) { })
    )
  , hana::size_c<0>
  );
#endif
}


//        Copyright Maarten L. Hekkelman 2013-2021
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <pinch/pinch.hpp>

#include <regex>

#include <pinch/connection.hpp>
#include <pinch/detail/ssh_agent_impl.hpp>
#include <pinch/ssh_agent.hpp>
#include <pinch/ssh_agent_channel.hpp>

#include <cryptopp/base64.h>
#include <cryptopp/osrng.h>
#include <cryptopp/rsa.h>
#include <cryptopp/modes.h>
#include <cryptopp/aes.h>
#include <cryptopp/camellia.h>
#include <cryptopp/des.h>
#include <cryptopp/hex.h>
#include <cryptopp/idea.h>
#define CRYPTOPP_ENABLE_NAMESPACE_WEAK 1
#include <cryptopp/md5.h>

#include <boost/algorithm/string.hpp>

using namespace CryptoPP;
namespace ba = boost::algorithm;

namespace pinch
{

// --------------------------------------------------------------------
// ssh_private_key_impl

ssh_private_key_impl::ssh_private_key_impl(const blob &b)
	: m_blob(b)
	, m_refcount(1)
{
}

ssh_private_key_impl::~ssh_private_key_impl()
{
	assert(m_refcount == 0);
}

void ssh_private_key_impl::reference()
{
	++m_refcount;
}

void ssh_private_key_impl::release()
{
	if (--m_refcount == 0)
		delete this;
}

// --------------------------------------------------------------------
// ssh_basic_private_key_impl

class ssh_basic_private_key_impl : public ssh_private_key_impl
{
  public:
	ssh_basic_private_key_impl(RSA::PrivateKey &rsa, const blob &b, const std::string &comment)
		: ssh_private_key_impl(b)
		, mPrivateKey(rsa)
		, mComment(comment)
	{
	}

	virtual blob sign(const blob &session_id, const opacket &p);

	virtual std::string get_type() const { return "ssh-rsa"; }
	virtual blob get_hash() const { return blob(); }
	virtual std::string get_comment() const { return mComment; }

  private:
	RSA::PrivateKey mPrivateKey;
	std::string mComment;
};

blob ssh_basic_private_key_impl::sign(const blob &session_id, const opacket &p)
{
	AutoSeededRandomPool rng;

	blob message(session_id);
	const blob &data(p);
	message.insert(message.end(), data.begin(), data.end());

	RSASSA_PKCS1v15_SHA_Signer signer(mPrivateKey);
	size_t length = signer.MaxSignatureLength();
	blob digest(length);

	signer.SignMessage(rng, message.data(), message.size(), digest.data());

	opacket signature;
	signature << get_type() << digest;
	return signature;
}

// --------------------------------------------------------------------
// ssh_private_key

ssh_private_key::ssh_private_key(ssh_private_key_impl *impl)
	: m_impl(impl)
{
}

ssh_private_key::ssh_private_key(const ssh_private_key &inKey)
	: m_impl(inKey.m_impl)
{
	m_impl->reference();
}

ssh_private_key::~ssh_private_key()
{
	m_impl->release();
}

ssh_private_key &ssh_private_key::operator=(const ssh_private_key &inKey)
{
	if (this != &inKey)
	{
		m_impl->release();
		m_impl = inKey.m_impl;
		m_impl->reference();
	}

	return *this;
}

blob ssh_private_key::sign(const blob &session_id, const opacket &data)
{
	return m_impl->sign(session_id, data);
}

std::string ssh_private_key::get_type() const
{
	return m_impl->get_type();
}

blob ssh_private_key::get_blob() const
{
	return m_impl->get_blob();
}

blob ssh_private_key::get_hash() const
{
	return m_impl->get_hash();
}

std::string ssh_private_key::get_comment() const
{
	return m_impl->get_comment();
}

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

ssh_agent &ssh_agent::instance()
{
	static ssh_agent s_instance;
	return s_instance;
}

ssh_agent::ssh_agent()
{
	update();
}

ssh_agent::~ssh_agent()
{
	m_private_keys.clear();
}

void ssh_agent::process_agent_request(ipacket &in, opacket &out)
{
	switch ((message_type)in)
	{
		case SSH_AGENTC_REQUEST_RSA_IDENTITIES:
			out = opacket(SSH_AGENT_RSA_IDENTITIES_ANSWER) << uint32_t(0);
			break;

		case SSH2_AGENTC_REQUEST_IDENTITIES:
		{
			out = opacket(SSH2_AGENT_IDENTITIES_ANSWER) << uint32_t(m_private_keys.size());

			for (auto &key : m_private_keys)
				out << key.get_blob() << key.get_comment();
			break;
		}

		case SSH2_AGENTC_SIGN_REQUEST:
		{
			ipacket blob, data;
			in >> blob >> data;

			ssh_private_key key = get_key(blob);

			if (key)
				out = opacket(SSH2_AGENT_SIGN_RESPONSE) << key.sign(data, opacket());
			else
				out = opacket(SSH_AGENT_FAILURE);
			break;
		}

		default:
			out = opacket(SSH_AGENT_FAILURE);
			break;
	}
}

void ssh_agent::update()
{
	std::list<blob> deleted;

	for (ssh_private_key &key : m_private_keys)
		deleted.push_back(key.get_hash());

	m_private_keys.clear();
	ssh_private_key_impl::create_list(m_private_keys);

	for (ssh_private_key &key : m_private_keys)
		deleted.erase(remove(deleted.begin(), deleted.end(), key.get_hash()), deleted.end());

	connection_list connections(m_registered_connections);

	for (blob &hash : deleted)
	{
		for (std::shared_ptr<basic_connection> connection : connections)
		{
			if (connection->uses_private_key(hash))
				connection->close();
		}
	}
}

void ssh_agent::register_connection(std::shared_ptr<basic_connection> connection)
{
	if (find(m_registered_connections.begin(), m_registered_connections.end(), connection) == m_registered_connections.end())
		m_registered_connections.push_back(connection);
}

void ssh_agent::unregister_connection(std::shared_ptr<basic_connection> connection)
{
	m_registered_connections.erase(
		remove(m_registered_connections.begin(), m_registered_connections.end(), connection),
		m_registered_connections.end());
}

void ssh_agent::expose_pageant(bool expose)
{
#if defined(_MSC_VER)
	pinch::expose_pageant(expose);
#endif
}

struct ssh_known_cipher_for_private_key
{
	std::string name;
	uint32_t key_size;
	uint32_t iv_size;
	std::function<SymmetricCipher *()> factory;
} kKnownCiphers[] = {
	{"AES-256-CBC", 32, 16, []() -> SymmetricCipher * { return new CBC_Mode<AES>::Decryption; }},
	{"AES-192-CBC", 24, 16, []() -> SymmetricCipher * { return new CBC_Mode<AES>::Decryption; }},
	{"AES-128-CBC", 16, 16, []() -> SymmetricCipher * { return new CBC_Mode<AES>::Decryption; }},
	{"CAMELLIA-256-CBC", 32, 16, []() -> SymmetricCipher * { return new CBC_Mode<Camellia>::Decryption; }},
	{"CAMELLIA-192-CBC", 24, 16, []() -> SymmetricCipher * { return new CBC_Mode<Camellia>::Decryption; }},
	{"CAMELLIA-128-CBC", 16, 16, []() -> SymmetricCipher * { return new CBC_Mode<Camellia>::Decryption; }},
	{"DES-EDE3-CBC", 24, 8, []() -> SymmetricCipher * { return new CBC_Mode<DES_EDE3>::Decryption; }},
	{"IDEA-CBC", 16, 8, []() -> SymmetricCipher * { return new CBC_Mode<IDEA>::Decryption; }},
	{"DES-CBC", 8, 8, []() -> SymmetricCipher * { return new CBC_Mode<DES>::Decryption; }}};

// Signature changed a bit to match Crypto++. Salt must be PKCS5_SALT_LEN in length.
//  Salt, Data and Count are IN; Key and IV are OUT.
int OPENSSL_EVP_BytesToKey(HashTransformation &hash,
                           const unsigned char *salt, const unsigned char *data, int dlen,
                           unsigned int count, unsigned char *key, unsigned int ksize,
                           unsigned char *iv, unsigned int vsize);

// From OpenSSL, crypto/evp/evp.h.
static const unsigned int OPENSSL_PKCS5_SALT_LEN = 8;

// 64-character line length is required by RFC 1421.
// static const unsigned int RFC1421_LINE_BREAK = 64;
// static const unsigned int OPENSSL_B64_LINE_BREAK = 76;

// From crypto/evp/evp_key.h. Signature changed a bit to match Crypto++.
int OPENSSL_EVP_BytesToKey(HashTransformation &hash,
                           const unsigned char *salt, const unsigned char *data, int dlen,
                           unsigned int count, unsigned char *key, unsigned int ksize,
                           unsigned char *iv, unsigned int vsize)
{
	unsigned int niv, nkey, nhash;
	unsigned int addmd = 0, i;

	nkey = ksize;
	niv = vsize;
	nhash = hash.DigestSize();

	SecByteBlock digest(hash.DigestSize());

	if (data == NULL)
		return (0);

	for (;;)
	{
		hash.Restart();

		if (addmd++)
			hash.Update(digest.data(), digest.size());

		hash.Update(data, dlen);

		if (salt != NULL)
			hash.Update(salt, OPENSSL_PKCS5_SALT_LEN);

		hash.TruncatedFinal(digest.data(), digest.size());

		for (i = 1; i < count; i++)
		{
			hash.Restart();
			hash.Update(digest.data(), digest.size());
			hash.TruncatedFinal(digest.data(), digest.size());
		}

		i = 0;
		if (nkey)
		{
			for (;;)
			{
				if (nkey == 0)
					break;
				if (i == nhash)
					break;
				if (key != NULL)
					*(key++) = digest[i];
				nkey--;
				i++;
			}
		}
		if (niv && (i != nhash))
		{
			for (;;)
			{
				if (niv == 0)
					break;
				if (i == nhash)
					break;
				if (iv != NULL)
					*(iv++) = digest[i];
				niv--;
				i++;
			}
		}
		if ((nkey == 0) && (niv == 0))
			break;
	}

	return ksize;
}

void ssh_agent::add(const std::string &private_key, const std::string &key_comment, std::function<bool(std::string &)> provide_password)
{
	AutoSeededRandomPool prng;
	std::regex rx(
		"^-+BEGIN RSA PRIVATE KEY-+\\n"
		"(?:"
		"((?:^[^:]+:\\s*\\S.+\\n)+)"
		"\\n)?"
		"([ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/\\s]+)=*\\n"
		"-+END RSA PRIVATE KEY-+\n?");

	std::smatch m;

	if (not std::regex_match(private_key, m, rx))
		throw std::runtime_error("Invalid PEM file");

	std::string keystr = m[2].str();
	std::string password;

	std::unique_ptr<SymmetricCipher> cipher;

	if (m[1].matched and provide_password(password))
	{
		// the keystr is probably encrypted
		std::string algo;
		std::string iv;

		std::stringstream s(m[1].str());
		for (;;)
		{
			std::string line;
			getline(s, line);

			if (line.empty())
				break;

			if (ba::starts_with(line, "Proc-Type:") and not ba::ends_with(line, "4,ENCRYPTED"))
			{
				algo.clear();
				break;
			}

			if (ba::starts_with(line, "DEK-Info:"))
			{
				std::string::size_type t = 9;
				while (t < line.length() and line[t] == ' ')
					++t;
				line.erase(0, t);
				t = line.find(',');
				algo = ba::to_upper_copy(line.substr(0, t));
				iv = line.substr(t + 1);
			}
		}

		for (auto c : kKnownCiphers)
		{
			if (c.name != algo)
				continue;

			HexDecoder hex;
			hex.Put((unsigned char *)iv.c_str(), iv.length());
			hex.MessageEnd();

			size_t iv_size = hex.MaxRetrievable();
			if (iv_size > c.iv_size)
				iv_size = c.iv_size;

			SecByteBlock key(c.key_size);
			SecByteBlock iv(iv_size);
			SecByteBlock salt(iv_size);

			hex.Get(iv.data(), iv.size());

			salt = iv;

			CryptoPP::Weak1::MD5 md5;
			(void)OPENSSL_EVP_BytesToKey(md5, iv.data(), (const unsigned char *)password.c_str(), password.length(),
			                             1, key.data(), key.size(), nullptr, 0);

			cipher.reset(c.factory());
			cipher->SetKeyWithIV(key.data(), key.size(), iv.data(), iv.size());

			break;
		}
	}

	std::string key;

	// Base64 decode, place in a ByteQueue
	ByteQueue queue;
	Base64Decoder decoder;

	decoder.Attach(new Redirector(queue));
	decoder.Put((const unsigned char *)keystr.data(), keystr.length());
	decoder.MessageEnd();

	if (cipher)
	{
		ByteQueue temp;
		StreamTransformationFilter filter(*cipher, new Redirector(temp));
		queue.TransferTo(filter);
		filter.MessageEnd();

		queue = temp;
	}

	RSA::PrivateKey rsaPrivate;
	rsaPrivate.BERDecodePrivateKey(queue, false /*paramsPresent*/, queue.MaxRetrievable());

	if (not queue.IsEmpty() or not rsaPrivate.Validate(prng, 3))
		throw std::runtime_error("RSA private key is not valid");

	opacket b;
	b << "ssh-rsa" << rsaPrivate.GetPublicExponent() << rsaPrivate.GetModulus();

	m_private_keys.push_back(ssh_private_key(new ssh_basic_private_key_impl(rsaPrivate, (blob)b, key_comment)));
}

ssh_private_key ssh_agent::get_key(ipacket &b) const
{
	for (auto &key : m_private_keys)
	{
		if ((blob)b == key.get_blob())
			return key;
	}

	throw std::runtime_error("private key not found");
}

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

ssh_agent_channel::ssh_agent_channel(std::shared_ptr<basic_connection> connection)
	: channel(connection)
{
}

ssh_agent_channel::~ssh_agent_channel()
{
}

void ssh_agent_channel::opened()
{
	channel::opened();

	opacket out(msg_channel_open_confirmation);
	out << m_host_channel_id << m_my_channel_id << m_my_window_size << kMaxPacketSize;
	m_connection->async_write(std::move(out));
}

void ssh_agent_channel::receive_data(const char *data, size_t size)
{
	while (size > 0)
	{
		if (m_packet.empty() and size < 4)
		{
			close(); // we have an empty packet and less than 4 bytes...
			break;   // simply fail this agent. I guess this should never happen
		}

		size_t r = m_packet.read(data, size);

		if (m_packet.complete())
		{
			opacket out;
			ssh_agent::instance().process_agent_request(m_packet, out);
			out = (opacket() << out);
			send_data(std::move(out));

			m_packet.clear();
		}

		data += r;
		size -= r;
	}
}

} // namespace pinch


#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

vector<MatrixXd> ukfMed(Vector3d xk, Matrix3d Pk, map<int, vector<double>> zk, Matrix3d Rk)
{
	for(map< int, vector <double> >::iterator im = zk.begin(); im != zk.end(); im++)
	{

		zk[i]

		stringstream ss;

		ss << im->first << ":";

		for(vector<double>::iterator iv = im->second.begin(); iv != im->second.end(); iv++)
			ss << " " << *iv;

		ROS_INFO("%s",ss.str().c_str());
	}
}


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"taskspaceonline2.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include<termios.h>
#include<gazebo_msgs/LinkStates.h>
#include<sensor_msgs/JointState.h>
#include"pidcontroller.h"

ros::Publisher pub1  ;
ros::Publisher pub2  ;
ros::Publisher pub3  ;
ros::Publisher pub4  ;
ros::Publisher pub5  ;
ros::Publisher pub6  ;
ros::Publisher pub7  ;
ros::Publisher pub8  ;
ros::Publisher pub9  ;
ros::Publisher pub10 ;
ros::Publisher pub11 ;
ros::Publisher pub12 ;
ros::Publisher pub13 ;
ros::Publisher pub14 ;
ros::Publisher pub15 ;
ros::Publisher pub16 ;
ros::Publisher pub17 ;
ros::Publisher pub18 ;
ros::Publisher pub19 ;
ros::Publisher pub20 ;
ros::Publisher pub21 ;
ros::Publisher pub22 ;
ros::Publisher pub23 ;
ros::Publisher pub24 ;
ros::Publisher pub25 ;
ros::Publisher pub26 ;
ros::Publisher pub27 ;
ros::Publisher pub28 ;

//ros::Publisher pid1 ;

int getch()
{
  static struct termios oldt, newt;
  tcgetattr( STDIN_FILENO, &oldt);           // save old settings
  newt = oldt;
  newt.c_lflag &= ~(ICANON);                 // disable buffering
  tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

  int c = getchar();  // read character (non-blocking)

  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
  return c;
}

void  SendGazebo(QList<LinkM> links,MatrixXd RollModifieds, double PitchModifieds, double theta_r, double phi_r, double theta_l, double phi_l){
    if(links.count()<28){qDebug()<<"index err";return;}
    std_msgs::Float64 data;

    data.data=links[1].JointAngle;
    pub1.publish(data);
    data.data=links[2].JointAngle+RollModifieds(0,0);
    pub2.publish(data);
    data.data=links[3].JointAngle+PitchModifieds;
    pub3.publish(data);
    data.data=links[4].JointAngle;
    pub4.publish(data);
    data.data=links[5].JointAngle+theta_r;
    pub5.publish(data);
    data.data=links[6].JointAngle+phi_r;
    pub6.publish(data);
    data.data=links[7].JointAngle;
    pub7.publish(data);
    data.data=links[8].JointAngle+RollModifieds(1,0);
    pub8.publish(data);
    data.data=links[9].JointAngle+PitchModifieds;
    pub9.publish(data);
    data.data=links[10].JointAngle;
    pub10.publish(data);
    data.data=links[11].JointAngle+theta_l;
    pub11.publish(data);
    data.data=links[12].JointAngle+phi_l;
    pub12.publish(data);
    data.data=links[13].JointAngle;
    pub13.publish(data);
    data.data=links[14].JointAngle;
    pub14.publish(data);
    data.data=links[15].JointAngle;
    pub15.publish(data);
    data.data=links[16].JointAngle;
    pub16.publish(data);
    data.data=links[17].JointAngle;
    pub17.publish(data);
    data.data=links[18].JointAngle;
    pub18.publish(data);
    data.data=links[19].JointAngle;
    pub19.publish(data);
    data.data=links[20].JointAngle;
    pub20.publish(data);
    data.data=links[21].JointAngle;
    pub21.publish(data);
    data.data=links[22].JointAngle;
    pub22.publish(data);
    data.data=links[23].JointAngle;
    pub23.publish(data);
    data.data=links[24].JointAngle;
    pub24.publish(data);
    data.data=links[25].JointAngle;
    pub25.publish(data);
    data.data=links[26].JointAngle;
    pub26.publish(data);
    data.data=links[27].JointAngle;
    pub27.publish(data);
    data.data=links[28].JointAngle;
    pub28.publish(data);

}

using namespace  std;
using namespace  Eigen;
//data of left foot sensor
int a;
int b;
int c;
int d;

int e;
int f;
int g;
int h;

bool RFT;//True Right Support Phase used in Taskspace online (in Ankle Trajectory)
bool LFT;//True Left Support Phase used in Taskspace online (in Ankle Trajectory)
bool KRtemp;// for online: when true-> current positon is updated _end of step)
bool KLtemp;// for online
bool aState;
bool bState;
bool cState;
bool dState;
bool LeftFootLanded;
bool RightFootLanded;
//bool FullContactDetected;

//angles of ankle adaptation
double teta_motor_L;//pitch
double teta_motor_R;
double phi_motor_L;//roll
double phi_motor_R;

//offsets after adaptation
double Offset_teta_L;
double Offset_teta_R;
double Offset_phi_L;
double Offset_phi_R;

int qc_offset[12];
double roll_absoulte[4];
bool qc_initial_bool;
bool qc_initial_bool_roll;
//
void receiveFootSensor(const std_msgs::Int32MultiArray& msg)
{
    if (msg.data.size()!=8) {
        qDebug("the size of sensor data is in wrong");
        return;
    }

    //ROS_INFO("I heard: [%d  %d %d %d %d  %d %d %d]", (int)msg.data[0],(int)msg.data[1],(int)msg.data[2],(int)msg.data[3],(int)msg.data[4],(int)msg.data[5],(int)msg.data[6],(int)msg.data[7]);
    int temp[8];
    int tempInt[8];

    temp[0]=msg.data[0]-1012;
    temp[1]=-1*(msg.data[1]-924);
    temp[2]=msg.data[2]-3038;
    temp[3]=-1*(msg.data[3]-3098);

    //normalizing data of sensors
    temp[0]=temp[0]*(100.0/(1097-1011));
    temp[1]=temp[1]*(100.0/(925-841));
    temp[2]=temp[2]*(100.0/(3131-3038));
    temp[3]=temp[3]*(100.0/(3098-3001));

    tempInt[0]=temp[0];
    tempInt[1]=temp[1];
    tempInt[2]=temp[2];
    tempInt[3]=temp[3];

    a=tempInt[0];
    b=tempInt[1];
    c=tempInt[2];
    d=tempInt[3];
    //ROS_INFO("I heard a b c d: [%d  %d %d %d]", a,b,c,d);

    temp[4]=msg.data[4]-3042;
    temp[5]=-1*(msg.data[5]-3008);
    temp[6]=msg.data[6]-1133;
    temp[7]=-1*(msg.data[7]-1016);

    //normalizing data of sensors
    temp[4]=temp[4]*(100.0/(3126-3042));
    temp[5]=temp[5]*(100.0/(3008-2914));
    temp[6]=temp[6]*(100.0/(1225-1133));
    temp[7]=temp[7]*(100.0/(1016-920));

    tempInt[4]=temp[4];
    tempInt[5]=temp[5];
    tempInt[6]=temp[6];
    tempInt[7]=temp[7];

    e=tempInt[4];
    f=tempInt[5];
    g=tempInt[6];
    h=tempInt[7];
    // ROS_INFO("I heard e f g h: [%d  %d %d %d]", e,f,g,h);

    //deleting data with negative sign
    if (a<0) {
        a=0;

    }
    if (b<0) {
        b=0;

    }
    if (c<0) {
        c=0;

    }
    if (d<0) {
        d=0;

    }

    if (e<0) {
        e=0;

    }
    if (f<0) {
        f=0;

    }
    if (g<0) {
        g=0;

    }
    if (h<0) {
        h=0;

    }
//     ROS_INFO("I heard a b c d: [%d  %d %d %d]", a,b,c,d);
//     ROS_INFO("I heard e f g h: [%d  %d %d %d]", e,f,g,h);

    // ROS_INFO("I heard: [%d  %d %d %d]", tempInt[0],tempInt[1],tempInt[2],tempInt[3]);
}

void qc_initial(const sensor_msgs::JointState & msg){
    if (qc_initial_bool){
        for (int i = 0; i < 12; ++i) {
            qc_offset[i]=int(msg.position[i+1]);

        }

        qc_initial_bool=false;
        //qc_initial_bool_roll=true;

    ROS_INFO("Offset=%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\nInitialized!",
             qc_offset[0],qc_offset[1],qc_offset[2],qc_offset[3],qc_offset[4],
            qc_offset[5],qc_offset[6],qc_offset[7],qc_offset[8],qc_offset[9],
            qc_offset[10],qc_offset[11]);}
 //getch();
}

void roll_absolute_correction(const sensor_msgs::JointState & msg){
    roll_absoulte[2]=roll_absoulte[0];
    roll_absoulte[3]=roll_absoulte[1];
    roll_absoulte[0]= -msg.position[10];
    roll_absoulte[1]= -msg.position[11];
}

//void roll_qc_init(const sensor_msgs::JointState & msg){
//    if (!qc_initial_bool && qc_initial_bool_roll){

//        qc_offset[9]=int(msg.position[10]);
//        qc_offset[10]=int(msg.position[11]);

//        ROS_INFO("Offset=%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\nInitialized!",
//                 qc_offset[0],qc_offset[1],qc_offset[2],qc_offset[3],qc_offset[4],
//                qc_offset[5],qc_offset[6],qc_offset[7],qc_offset[8],qc_offset[9],
//                qc_offset[10],qc_offset[11]);

//        qc_initial_bool_roll=false;
//        getch();
//    }
//}

//int a,b,c,d,e,f,g,h;

MatrixXd quater2rot(double w,double x,double y, double z){
    MatrixXd R(3,3);
    R<<w*w+x*x-y*y-z*z,2*x*y-2*w*z,2*x*z+2*w*y,
            2*x*y+2*w*z,w*w-x*x+y*y-z*z,2*y*z-2*w*x,
            2*x*z-2*w*y,2*y*z+2*w*x,w*w-x*x-y*y+z*z;
    return R;

}

void ankle_states(const gazebo_msgs::LinkStates& msg){
    double x_left, x_right,y_left, y_right,z_left, z_right;
Vector3d vec_A_E;
Vector3d vec_B_F;
Vector3d vec_C_G;
Vector3d vec_D_H;

vec_A_E<<-.085,
         -.08,
        -.11;
vec_B_F<<.135,
         -.08,
        -.11;
vec_C_G<<.135,
         .08,
        -.11;
vec_D_H<<-.085,
         .08,
        -.11;

    x_left=msg.pose[7].position.x;
    x_right=msg.pose[13].position.x;
    y_left=msg.pose[7].position.y;
    y_right=msg.pose[13].position.y;

    z_left=msg.pose[7].position.z;
    z_right=msg.pose[13].position.z;

    MatrixXd R_left(3,3);
    MatrixXd R_right(3,3);
    R_left=quater2rot(msg.pose[7].orientation.w,msg.pose[7].orientation.x,msg.pose[7].orientation.y,msg.pose[7].orientation.z);
    R_right=quater2rot(msg.pose[13].orientation.w,msg.pose[13].orientation.x,msg.pose[13].orientation.y,msg.pose[13].orientation.z);
Vector3d temp;
temp=R_left*vec_A_E;
//A=temp(2)+z_left;
a=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
temp=R_left*vec_B_F;
//B=temp(2)+z_left;
b=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
temp=R_left*vec_C_G;
//C=temp(2)+z_left;
c=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
temp=R_left*vec_D_H;
//D=temp(2)+z_left;
d=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
temp=R_right*vec_A_E;
//E=temp(2)+z_right;
e=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
temp=R_right*vec_B_F;
//F=temp(2)+z_right;
f=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
temp=R_right*vec_C_G;
//G=temp(2)+z_right;
g=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
temp=R_right*vec_D_H;
//H=temp(2)+z_right;
h=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);

 //ROS_INFO("z_left=%f  A=%d,B=%d,C=%d,D=%d\tz_right=%f  E=%d,F=%d,G=%d,H=%d",z_left,a,b,c,d,z_right,e,f,g,h);

//    ROS_INFO("x_left=%f,x_right=%f,y_left=%f,y_right=%fz_left=%f,z_right=%f",x_left,x_right,y_left,y_right,z_left,z_right);

}

int main(int argc, char **argv)
{
double min_test=0;
double max_test=0;
    vector<double> cntrl(13);
    QCgenerator QC;
    for (int i = 0; i < 12; ++i) {
         qc_offset[i]=0;
         //qc_corretction[i]=0;
    }
    qc_initial_bool=!true;
    qc_initial_bool_roll=false;

    //check _timesteps
    QElapsedTimer timer;
    Robot SURENA;//model of robot & kinematics funcs(IK & FK)
    TaskSpaceOnline2 SURENAOnlineTaskSpace1;
    QList<LinkM> links;
    MatrixXd PoseRoot;//position of pelvis respected to global coordinate
    MatrixXd PoseRFoot;//position of right ankle joint respected to global coordinate
    MatrixXd PoseLFoot;//position of left ankle joint respected to global coordinate
    //double hipRoll=0;
   // double dt;

    //parameters of ankle adaptation
    double k1;
    double k2;
    double k3;
    double k4;

    SURENAOnlineTaskSpace1.RightSupport=true; //not changed with sensor data,
    SURENAOnlineTaskSpace1.LeftSupport=true;

    SURENAOnlineTaskSpace1.RightSensorActive=false; // is set true once when right leg is in swing mode, and immediately false to get out of if statement of bump sensor
    SURENAOnlineTaskSpace1.LeftSensorActive=false;

    SURENAOnlineTaskSpace1.oldLeftFootZ=SURENAOnlineTaskSpace1._lenghtOfAnkle;//expected foot z
    SURENAOnlineTaskSpace1.oldRightFootZ=SURENAOnlineTaskSpace1._lenghtOfAnkle;

    bool aState=false;
    bool bState=false;
    bool cState=false;
    bool dState=false;

    bool eState=false;
    bool fState=false;
    bool gState=false;
    bool hState=false;

    LeftFootLanded=false;//true when landing is detected by sensors
    RightFootLanded=false;
   // FullContactDetected=false;

    SURENAOnlineTaskSpace1.RightFootOrientationAdaptator=false;//adaptation occurs when true
    SURENAOnlineTaskSpace1.LeftFootOrientationAdaptator=false;

    teta_motor_L=0;
    teta_motor_R=0;
    phi_motor_L=0;
    phi_motor_R=0;

    Offset_teta_L=0;
    Offset_teta_R=0;
    Offset_phi_L=0;
    Offset_phi_R=0;

    double footSensorSaturation=75;//if all sensors data are bigger than this amount, this means the foot is landed on the ground
    double footSensorthreshold=4;// will start orientaition correction

    double StartTime=0;//starttime does not mean start time,but means global time
    double  DurationOfStartPhase=6;
    double  DurationOfendPhase=6;
    //SURENAOnlineTaskSpace.GetAccVelPos();
    bool startPhase=true;
    bool endPhase=true;
    bool walk=true;
      MatrixXd RollModified(2,1);//parameters for hip roll angles charge, for keep pelvis straight
     RollModified<<0,0;
     double pitchOffset=SURENAOnlineTaskSpace1.HipPitchModification;
     double PitchModified=0;
    PoseRoot.resize(6,1); //pelvis trajectory from taskspace_online,xyzrpy
    PoseRFoot.resize(6,1);//right ankle joint trajectory from taskspace_online,xyzrpy
    PoseLFoot.resize(6,1);//left ankle joint trajectory from taskspace_online,xyzrpy
    //QList<LinkM> links;
    bool indexLastDS=true;//used in last double support

    //*******************This part of code is for initialization of joints of the robot for walking**********************************
    int count = 0;

    ros::init(argc, argv, "myNode");

    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);

    ros::Publisher  contact_flag  = nh.advertise<std_msgs::Int32MultiArray>("contact_flag_timing",100);
 ros::Subscriber sub = nh.subscribe("/surena/bump_sensor_state", 1000, receiveFootSensor);
 ros::Subscriber qcinit = nh.subscribe("/surena/inc_joint_state", 1000, qc_initial);
ros::Subscriber roll_absolute_sub = nh.subscribe("/surena/abs_joint_state",1000,roll_absolute_correction);
//ros::Subscriber roll_qc_init_sub = nh.subscribe("/surena/abs_joint_num",1000,roll_qc_init);

//    pub1  = nh.advertise<std_msgs::Float64>("rrbot/joint1_position_controller/command",1000);
//    pub2  = nh.advertise<std_msgs::Float64>("rrbot/joint2_position_controller/command",1000);
//    pub3  = nh.advertise<std_msgs::Float64>("rrbot/joint3_position_controller/command",1000);
//    pub4  = nh.advertise<std_msgs::Float64>("rrbot/joint4_position_controller/command",1000);
//    pub5  = nh.advertise<std_msgs::Float64>("rrbot/joint5_position_controller/command",1000);
//    pub6  = nh.advertise<std_msgs::Float64>("rrbot/joint6_position_controller/command",1000);
//    pub7  = nh.advertise<std_msgs::Float64>("rrbot/joint7_position_controller/command",1000);
//    pub8  = nh.advertise<std_msgs::Float64>("rrbot/joint8_position_controller/command",1000);
//    pub9  = nh.advertise<std_msgs::Float64>("rrbot/joint9_position_controller/command",1000);
//    pub10 = nh.advertise<std_msgs::Float64>("rrbot/joint10_position_controller/command",1000);
//    pub11 = nh.advertise<std_msgs::Float64>("rrbot/joint11_position_controller/command",1000);
//    pub12 = nh.advertise<std_msgs::Float64>("rrbot/joint12_position_controller/command",1000);
//    pub13 = nh.advertise<std_msgs::Float64>("rrbot/joint13_position_controller/command",1000);
//    pub14 = nh.advertise<std_msgs::Float64>("rrbot/joint14_position_controller/command",1000);
//    pub15 = nh.advertise<std_msgs::Float64>("rrbot/joint15_position_controller/command",1000);
//    pub16 = nh.advertise<std_msgs::Float64>("rrbot/joint16_position_controller/command",1000);
//    pub17 = nh.advertise<std_msgs::Float64>("rrbot/joint17_position_controller/command",1000);
//    pub18 = nh.advertise<std_msgs::Float64>("rrbot/joint18_position_controller/command",1000);
//    pub19 = nh.advertise<std_msgs::Float64>("rrbot/joint19_position_controller/command",1000);
//    pub20 = nh.advertise<std_msgs::Float64>("rrbot/joint20_position_controller/command",1000);
//    pub21 = nh.advertise<std_msgs::Float64>("rrbot/joint21_position_controller/command",1000);
//    pub22 = nh.advertise<std_msgs::Float64>("rrbot/joint22_position_controller/command",1000);
//    pub23 = nh.advertise<std_msgs::Float64>("rrbot/joint23_position_controller/command",1000);
//    pub24 = nh.advertise<std_msgs::Float64>("rrbot/joint24_position_controller/command",1000);
//    pub25 = nh.advertise<std_msgs::Float64>("rrbot/joint25_position_controller/command",1000);
//    pub26 = nh.advertise<std_msgs::Float64>("rrbot/joint26_position_controller/command",1000);
//    pub27 = nh.advertise<std_msgs::Float64>("rrbot/joint27_position_controller/command",1000);
//    pub28 = nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/command",1000);

    ros::Subscriber ankleStates = nh.subscribe("/gazebo/link_states", 10, ankle_states);

int32_t contact_flag_timing=1000;
int32_t contact_flag_sensor=1000;
int32_t contact_flag_sensor2=1000;

    ros::Rate loop_rate(200);
    std_msgs::Int32MultiArray msg;
std_msgs::Int32MultiArray roll_modif_msg;
    std_msgs::Int32MultiArray msg_contact_flag;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;

    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);

msg_contact_flag.data.clear();
msg_contact_flag.data.push_back(contact_flag_timing);

//msg_flag.data=-msg_flag.data;

    k1=0.000015;
    k2=0.000015;
    k3=0.000015;
    k4=0.000015;

    KRtemp=false;
    KLtemp=false;
    RFT=false;
    LFT=false;
    links=SURENA.GetLinks();
    SURENAOnlineTaskSpace1.StepNumber=1;

    MatrixXd P;
    MatrixXd Pz;

ROS_INFO("press any key to start!");
getch();
ROS_INFO("started!");

bool firstcontact=true;

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

//while (ros::ok()) {
//    if (qc_initial_bool) {
//         ROS_INFO("qc is initializing!");
//            ros::spinOnce();
// continue;
//     }
//    msg.data.clear();
//    for(int  i = 0;i < 12;i++)
//    {
//        msg.data.push_back(qc_offset[i]);
//    }

//    for(int  i = 12;i < 28;i++)
//    {
//        msg.data.push_back(0);
//    }

//    chatter_pub.publish(msg);
//    ros::spinOnce();
//    loop_rate.sleep();
//}
// ROS_INFO("exit");

//exit(0);
// ROS_INFO("exit4");
 //------------------------------------------------------
    while (ros::ok())
    {

       //  for robot test musbe uncommented
       if (qc_initial_bool) {
            ROS_INFO("qc is initializing!");
               ros::spinOnce();
    continue;
        }

//
  //   ROS_INFO("a=%d b=%d c=%d d=%d e=%d f=%d g=%d h=%d",a,b,c,d,e,f,g,h);

       // ROS_INFO("%d\t%f",SURENAOnlineTaskSpace1.localtimingInteger,SURENAOnlineTaskSpace1.localTiming);
        //-------------flag to show expected contact according to timing-------------//
        //-------------sign of flag changes-------------//
        if(SURENAOnlineTaskSpace1.localTiming<.1){contact_flag_timing=-contact_flag_timing;}
       // ROS_INFO("globaltime=%f\tlocaltiming=%f  %d",StartTime,SURENAOnlineTaskSpace1.localTiming,contact_flag_timing);

//ROS_INFO("a=%d,  b=%d,  c=%d,  d=%d,  e=%d,  f=%d,  g=%d,  h=%d",a,b,c,d,e,f,g,h);
        //-------------for detecting the first contact of Left foot with ground-------------//
        //-------------flag to show contact detected by sensors-------------//
        //-------------sign of flag changes-------------//

        if (SURENAOnlineTaskSpace1.LeftSensorActive==true && firstcontact==true &&( (a)>=footSensorSaturation || (b)>=footSensorSaturation || (c)>=footSensorSaturation || (d)>=footSensorSaturation)){
            contact_flag_sensor2=-contact_flag_sensor2;
            firstcontact=false;
            ROS_INFO("shalap [%f  %f] a=%d b=%d c=%d d=%d", SURENAOnlineTaskSpace1.localTiming,SURENAOnlineTaskSpace1.globalTime,a,b,c,d);

        }

        //-------------for detecting the full contact of Left foot with ground-------------//
        //-------------flag to show contact detected by sensors-------------//
        //-------------sign of flag changes-------------//
        if (SURENAOnlineTaskSpace1.LeftSensorActive==true && (a)>=footSensorSaturation && (b)>=footSensorSaturation && (c)>=footSensorSaturation && (d)>=footSensorSaturation){
            contact_flag_sensor=-contact_flag_sensor;
            ROS_INFO("left swing foot landing is successful = [%f  %f] a=%d b=%d c=%d d=%d", SURENAOnlineTaskSpace1.localTiming,SURENAOnlineTaskSpace1.globalTime,a,b,c,d);
            aState=true;
            bState=true;
            cState=true;
            dState=true;
            LeftFootLanded=true;//this variable is used for flag up when the left foot have a full contact with the ground
            SURENAOnlineTaskSpace1.LeftSensorActive=false;
            firstcontact=true;
            SURENAOnlineTaskSpace1.LeftFootOrientationAdaptator=false;
            //do nothing all sensors are on the ground

            Offset_teta_L=teta_motor_L;
            Offset_phi_L=phi_motor_L;

            //exit(0);//will finish the code but during walking is not true
        }

        else if (/*SURENAOnlineTaskSpace1.LeftSensorActive==false*/true) {//---------when the four left foot sensors are not saturated-->>>>during landing------//

            LeftFootLanded=false;
            if ((a)>=footSensorthreshold ){aState=true;}else {aState=false;}
            if ((b)>=footSensorthreshold ){bState=true;}else {bState=false;}
            if ((c)>=footSensorthreshold ){cState=true;}else {cState=false;}
            if ((d)>=footSensorthreshold ){dState=true;}else {dState=false;}

            //-----------------Pitch left ankle motor control---------------//
            if (abs(b-a)>=abs(c-d)) {
                if (abs(a-b)<100) {
                    teta_motor_L=1*teta_motor_L+k1*(SURENAOnlineTaskSpace1.LeftFootOrientationAdaptator==true)*(a-b);
                }
                else {
                    teta_motor_L=teta_motor_L;
                }
            }
            else {
                if (abs(d-c)<100) {
                    teta_motor_L=1*teta_motor_L+k1*(SURENAOnlineTaskSpace1.LeftFootOrientationAdaptator==true)*(d-c);
                }
                else {
                    teta_motor_L=teta_motor_L;
                }
            }

            //----------------Roll left ankle motor control---------------//
            if (abs(c-b)>=abs(d-a)) {
                if (abs(c-b)<100) {
                    phi_motor_L=1*phi_motor_L+k2*(SURENAOnlineTaskSpace1.LeftFootOrientationAdaptator==true)*(c-b);
                }
                else {
                    phi_motor_L=phi_motor_L;
                }
            }
            else {
                if (abs(a-d)<100) {
                    phi_motor_L=1*phi_motor_L+k2*(SURENAOnlineTaskSpace1.LeftFootOrientationAdaptator==true)*(d-a);
                }
                else {
                    phi_motor_L=phi_motor_L;
                }
            }
        }

        //        ROS_INFO("Tc= [%d] ", SURENAOnlineTaskSpace1.Tc);
        //        ROS_INFO("Tds= [%f] ", SURENAOnlineTaskSpace1.TDs);

        //-------------for detecting the first contact of Right foot with ground-------------//
        //-------------flag to show contact detected by sensors-------------//
        //-------------sign of flag changes-------------//
        if (SURENAOnlineTaskSpace1.RightSensorActive==true && firstcontact==true && ( (e)>=footSensorSaturation || (f)>=footSensorSaturation || (g)>=footSensorSaturation || (h)>=footSensorSaturation)){
            contact_flag_sensor2=-contact_flag_sensor2;
            firstcontact=false;
           ROS_INFO("shooloop[%f  %f]  e=%d f=%d g=%d h=%d", SURENAOnlineTaskSpace1.localTiming,SURENAOnlineTaskSpace1.globalTime,e,f,g,h);
        }

        //-------------for detecting the full contact of Right foot with ground-------------//
        if (SURENAOnlineTaskSpace1.RightSensorActive==true && (e)>=footSensorSaturation && (f)>=footSensorSaturation && (g)>=footSensorSaturation && (h)>=footSensorSaturation){
            // qDebug("swing foot landing is successful");
            contact_flag_sensor=-contact_flag_sensor;
            ROS_INFO("Right swing foot landing is successful= [%f  %f]  e=%d f=%d g=%d h=%d", SURENAOnlineTaskSpace1.localTiming,SURENAOnlineTaskSpace1.globalTime,e,f,g,h);
            eState=true;
            fState=true;
            gState=true;
            hState=true;

            RightFootLanded=true;//this variable is used for flag up when the left foot have a full contact with the ground
            SURENAOnlineTaskSpace1.RightSensorActive=false;
            firstcontact=true;
            //do nothing all sensors are on the ground
            SURENAOnlineTaskSpace1.RightFootOrientationAdaptator=false;

            Offset_teta_R=teta_motor_R;
            Offset_phi_R=phi_motor_R;
            //exit(0);//will finish the code but during walking is not true
        }

        else if(/*SURENAOnlineTaskSpace1.RightSensorActive==false*/ true) {//---------when the four left foot sensors are not saturated-->>>>during landing------//

            RightFootLanded=false;
            if ((e)>=footSensorthreshold ){eState=true;}else {eState=false;}
            if ((f)>=footSensorthreshold ){fState=true;}else {fState=false;}
            if ((g)>=footSensorthreshold ){gState=true;}else {gState=false;}
            if ((h)>=footSensorthreshold ){hState=true;}else {hState=false;}

            //-----------------Pitch left ankle motor control---------------//
            if (abs(f-e)>=abs(g-h)) {
                if (abs(e-f)<100) {
                    teta_motor_R=1*teta_motor_R+k3*(SURENAOnlineTaskSpace1.RightFootOrientationAdaptator==true)*(e-f);
                }
                else {
                    teta_motor_R=teta_motor_R;
                }
            }
            else {
                if (abs(h-g)<100) {
                    teta_motor_R=1*teta_motor_R+k3*(SURENAOnlineTaskSpace1.RightFootOrientationAdaptator==true)*(h-g);
                }
                else {
                    teta_motor_R=teta_motor_R;
                }
            }

            //----------------Roll left ankle motor control---------------//
            if (abs(g-f)>=abs(h-e)) {
                if (abs(g-f)<100) {
                    phi_motor_R=1*phi_motor_R+k4*(SURENAOnlineTaskSpace1.RightFootOrientationAdaptator==true)*(g-f);
                }
                else {
                    phi_motor_R=phi_motor_R;
                }
            }
            else {
                if (abs(e-h)<100) {
                    phi_motor_R=1*phi_motor_R+k4*(SURENAOnlineTaskSpace1.RightFootOrientationAdaptator==true)*(h-e);
                }
                else {
                    phi_motor_R=phi_motor_R;
                }
            }
        }

        //------------------------saturation of ankle motors----------------------------//
        if ((abs(phi_motor_L))>0.9) {
            phi_motor_L=0.9;
        }
        if ((abs(teta_motor_L))>0.9) {
            teta_motor_L=0.9;
        }
        if ((abs(phi_motor_R))>0.9) {
            phi_motor_L=0.9;
        }
        if ((abs(teta_motor_R))>0.9) {
            teta_motor_R=0.9;
        }

        // ROS_INFO("I heard data of sensors : [%f %f %f %f]",a,b,c,d);

        //-----------------------------------------------------------------------------------------------------//
        //-----------------start phase--initializing the height of pelvis for walking--------------------------//
        //-----------------------------------------------------------------------------------------------------//
        if (startPhase==true && StartTime<=DurationOfStartPhase) {
            MinimumJerkInterpolation Coef;
            MatrixXd ZPosition(1,2);
           // ZPosition<<SURENAOnlineTaskSpace1.InitialPelvisHeight,0.8600;//0.86 is referencePelvisHeight in task space online
            ZPosition<<SURENAOnlineTaskSpace1.InitialPelvisHeight,SURENAOnlineTaskSpace1.ReferencePelvisHeight;
            MatrixXd ZVelocity(1,2);
            ZVelocity<<0.000,0.000;
            MatrixXd ZAcceleration(1,2);
            ZAcceleration<<0.000,0.000;

            MatrixXd Time(1,2);
            Time<<0,DurationOfStartPhase;
            MatrixXd CoefZStart =Coef.Coefficient(Time,ZPosition,ZVelocity,ZAcceleration);

            double zStart=0;
            double yStart=0;
            double xStart=0;
            StartTime=StartTime+SURENAOnlineTaskSpace1._timeStep;

            MatrixXd outputZStart= SURENAOnlineTaskSpace1.GetAccVelPos(CoefZStart,StartTime,0,5);
            zStart=outputZStart(0,0);

            PoseRoot<<xStart,yStart,zStart,0,0,0;

            PoseRFoot<<0,
                    -0.11500,
                    0.112000,
                    0,
                    0,
                    0;

            PoseLFoot<<0,
                    0.11500,
                    0.11200,
                    0,
                    0,
                    0;

            SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
            SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);

MinimumJerkInterpolation CoefOffline;
                double D_pitch=-1*pitchOffset*(M_PI/180);
                double TstartofPitchModify=DurationOfStartPhase/6;
                double TendofPitchModify=DurationOfStartPhase;
                double D_time=TendofPitchModify-TstartofPitchModify;
            if (StartTime>=TstartofPitchModify && StartTime<=TendofPitchModify){
                    MatrixXd Ct_pitch_st(1,2);
                    Ct_pitch_st<<0 ,D_time;
                    MatrixXd Cp_pitch_st(1,2);
                    Cp_pitch_st<<0, D_pitch;
                    MatrixXd Cv_pitch_st(1,2);
                    Cv_pitch_st<<0 ,0;
                    MatrixXd Ca_pitch_st(1,2);
                    Ca_pitch_st<<0 ,0;
                    MatrixXd C_pitch_st=CoefOffline.Coefficient(Ct_pitch_st,Cp_pitch_st,Cv_pitch_st,Ca_pitch_st);

                                    MatrixXd output=SURENAOnlineTaskSpace1.GetAccVelPos(C_pitch_st,StartTime-(TstartofPitchModify),0,5);
                        PitchModified=output(0,0);

            }

        }

        int NumberOfTimeStep=(SURENAOnlineTaskSpace1.Tc/SURENAOnlineTaskSpace1._timeStep)+1;

        //-----------------------------------------------------------------------------------------------------//
        //------------------------------- main loop of cyclic walking -----------------------------------------//
        //-----------------------------------------------------------------------------------------------------//

        if (StartTime>DurationOfStartPhase && StartTime<(DurationOfStartPhase+SURENAOnlineTaskSpace1.MotionTime)){

            double m1; //Ankle Trajectory Replacement
            double m2;
            double m3;
            double m4;
            double m5;
            double m6;
            double m7;
            double m8;

            StartTime=StartTime+SURENAOnlineTaskSpace1._timeStep;
            //qDebug()<<StartTime;

            if(walk==true){

                if ((SURENAOnlineTaskSpace1.StepNumber==1) && (SURENAOnlineTaskSpace1.localTiming>=SURENAOnlineTaskSpace1.TStart) ) {
                    //ROS_INFO(" Contact detected time: [%f]", SURENAOnlineTaskSpace1.localTiming);
                    SURENAOnlineTaskSpace1.localTiming=SURENAOnlineTaskSpace1._timeStep;//0.001999999999000000;
                    SURENAOnlineTaskSpace1.localtimingInteger=1;
                    SURENAOnlineTaskSpace1.StepNumber=SURENAOnlineTaskSpace1.StepNumber+1;
                    //  cout<<"cooontaaaaaactttt deeeeeteeeecteeeeddddddd="<<SURENAOnlineTaskSpace1.localTiming<<" step number= "<<SURENAOnlineTaskSpace1.StepNumber<<endl;
                    KLtemp=false;
                    SURENAOnlineTaskSpace1.CoeffArrayPelvisZMod();
                    //SURENAOnlineTaskSpace1.OldPelvisZ=SURENAOnlineTaskSpace1.NewPlevisZ;

                  //  if(firststep_sensor_test){
                   // SURENAOnlineTaskSpace1.LeftSensorActive==true; // was uncomment
                  //  firststep_sensor_test=false;}

                }

                else if ((SURENAOnlineTaskSpace1.localtimingInteger>=NumberOfTimeStep) &&   (SURENAOnlineTaskSpace1.StepNumber>1    &&   SURENAOnlineTaskSpace1.StepNumber<(SURENAOnlineTaskSpace1.NStep+2))) {
                    SURENAOnlineTaskSpace1.StepNumber=SURENAOnlineTaskSpace1.StepNumber+1;
                    SURENAOnlineTaskSpace1.CoeffArrayPelvisZMod();
                    SURENAOnlineTaskSpace1.localTiming=SURENAOnlineTaskSpace1._timeStep;//0.001999999999000000;
                    SURENAOnlineTaskSpace1.localtimingInteger=1;

                    if ((SURENAOnlineTaskSpace1.StepNumber%2)==0) {
                        KLtemp=false;
                    }
                    else {
                        KRtemp=false;
                    }
                }
                else if (indexLastDS==true && SURENAOnlineTaskSpace1.localTiming>=SURENAOnlineTaskSpace1.TDs && SURENAOnlineTaskSpace1.StepNumber==SURENAOnlineTaskSpace1.NStep+2) {
                    // ROS_INFO(" Contact detected time: [%f]", SURENAOnlineTaskSpace1.localTiming);
                    SURENAOnlineTaskSpace1.localTiming=0.00200000000000;
                    SURENAOnlineTaskSpace1.localtimingInteger=1;
                    indexLastDS=false;
                    KLtemp=false;

                }

                else if (indexLastDS==false && SURENAOnlineTaskSpace1.localTiming>( 0.5*SURENAOnlineTaskSpace1.TEnd)) {
                    KRtemp=false;
                }

                if (KLtemp==false) {
                    KLtemp=true;
                    SURENAOnlineTaskSpace1.currentLeftFootX2=links[12].PositionInWorldCoordinate(0);
                    SURENAOnlineTaskSpace1.currentLeftFootY2=links[12].PositionInWorldCoordinate(1);
                    SURENAOnlineTaskSpace1.currentLeftFootZ=links[12].PositionInWorldCoordinate(2);
                }

                if (KRtemp==false) {
                    KRtemp=true;
                    SURENAOnlineTaskSpace1.currentRightFootX2=links[6].PositionInWorldCoordinate(0);
                    SURENAOnlineTaskSpace1.currentRightFootY2=links[6].PositionInWorldCoordinate(1);
                    SURENAOnlineTaskSpace1.currentRightFootZ=links[6].PositionInWorldCoordinate(2);
                }

//replace false with following commented for activing sensor
              //  if (  /*(RightFootLanded==true)*/ false &&  ( SURENAOnlineTaskSpace1.StepNumber==1 || SURENAOnlineTaskSpace1.localTiming>(SURENAOnlineTaskSpace1.TDs+SURENAOnlineTaskSpace1.TSS/2)   /*|| SURENAOnlineTaskSpace1.StepNumber==(SURENAOnlineTaskSpace1.NStep+2)*/)) {

                if (  (RightFootLanded==true)  &&  ( SURENAOnlineTaskSpace1.StepNumber==1 || SURENAOnlineTaskSpace1.localTiming>(SURENAOnlineTaskSpace1.TDs+SURENAOnlineTaskSpace1.TSS/2)   /*|| SURENAOnlineTaskSpace1.StepNumber==(SURENAOnlineTaskSpace1.NStep+2)*/)) {

                    SURENAOnlineTaskSpace1.oldRightFootX2= SURENAOnlineTaskSpace1.currentRightFootX2;
                    SURENAOnlineTaskSpace1.oldRightFootY2= SURENAOnlineTaskSpace1.currentRightFootY2;
                    SURENAOnlineTaskSpace1.oldRightFootZ= SURENAOnlineTaskSpace1.currentRightFootZ;

                    SURENAOnlineTaskSpace1.currentRightFootX2=links[6].PositionInWorldCoordinate(0);
                    SURENAOnlineTaskSpace1.currentRightFootY2=links[6].PositionInWorldCoordinate(1);
                    SURENAOnlineTaskSpace1.currentRightFootZ=links[6].PositionInWorldCoordinate(2);
                   // ROS_INFO("Right foot Z height early contact offset: [%f ]", (SURENAOnlineTaskSpace1.currentRightFootZ-SURENAOnlineTaskSpace1._lenghtOfAnkle));
                    // ROS_INFO("Right foot Z height early contact offset: [%f ]", (SURENAOnlineTaskSpace1.currentRightFootX2-SURENAOnlineTaskSpace1._lenghtOfAnkle));
                   //SURENAOnlineTaskSpace1.NewPlevisZ =SURENAOnlineTaskSpace1.OldPelvisZ+(SURENAOnlineTaskSpace1.currentRightFootZ-SURENAOnlineTaskSpace1.OldPelvisZ);
                     SURENAOnlineTaskSpace1.NewPlevisZ =SURENAOnlineTaskSpace1.OldPelvisZ+(SURENAOnlineTaskSpace1.currentRightFootZ-SURENAOnlineTaskSpace1.oldRightFootZ);
                   RFT=true;

                }
                else if (SURENAOnlineTaskSpace1.localTiming<(SURENAOnlineTaskSpace1.TDs+SURENAOnlineTaskSpace1.TSS/2) ) {
                    RFT=false;
                }

//replace false with following commented for activing sensro
              // if ((  /*(LeftFootLanded==true)*/ false && SURENAOnlineTaskSpace1.localTiming>(SURENAOnlineTaskSpace1.TDs+SURENAOnlineTaskSpace1.TSS/2) )) {
                    if ((  (LeftFootLanded==true) && SURENAOnlineTaskSpace1.localTiming>(SURENAOnlineTaskSpace1.TDs+SURENAOnlineTaskSpace1.TSS/2) )) {

                    SURENAOnlineTaskSpace1.oldLeftFootX2= SURENAOnlineTaskSpace1.currentLeftFootX2;
                    SURENAOnlineTaskSpace1.oldLeftFootY2= SURENAOnlineTaskSpace1.currentLeftFootY2;
                    SURENAOnlineTaskSpace1.oldLeftFootZ= SURENAOnlineTaskSpace1.currentLeftFootZ;

                    SURENAOnlineTaskSpace1.currentLeftFootX2=links[12].PositionInWorldCoordinate(0);
                    SURENAOnlineTaskSpace1.currentLeftFootY2=links[12].PositionInWorldCoordinate(1);
                    SURENAOnlineTaskSpace1.currentLeftFootZ=links[12].PositionInWorldCoordinate(2);
                    //SURENAOnlineTaskSpace1.NewPlevisZ =SURENAOnlineTaskSpace1.OldPelvisZ+(SURENAOnlineTaskSpace1.currentLeftFootZ-SURENAOnlineTaskSpace1.OldPelvisZ);
                   // ROS_INFO("left foot Z height early contact offset: [%f ]", (SURENAOnlineTaskSpace1.currentLeftFootZ-SURENAOnlineTaskSpace1._lenghtOfAnkle));
                     SURENAOnlineTaskSpace1.NewPlevisZ =SURENAOnlineTaskSpace1.OldPelvisZ+(SURENAOnlineTaskSpace1.currentLeftFootZ-SURENAOnlineTaskSpace1.oldLeftFootZ);
                    LFT=true;
                }
                else if ( SURENAOnlineTaskSpace1.localTiming<(SURENAOnlineTaskSpace1.TDs+SURENAOnlineTaskSpace1.TSS/2) ) {
                    LFT=false;
                }

                MatrixXd m=SURENAOnlineTaskSpace1.AnkleTrajectory(SURENAOnlineTaskSpace1.globalTime,SURENAOnlineTaskSpace1.StepNumber,SURENAOnlineTaskSpace1.localTiming,RFT,LFT,indexLastDS);
                m1=m(0,0);
                m2=m(1,0);
                m3=m(2,0);
                m4=m(3,0);
                m5=m(4,0);
                m6=m(5,0);
                m7=m(6,0);
                m8=m(7,0);

                Pz=SURENAOnlineTaskSpace1.ModificationOfPelvisHeight(SURENAOnlineTaskSpace1.globalTime,SURENAOnlineTaskSpace1.StepNumber,SURENAOnlineTaskSpace1.localTiming,RFT,LFT,indexLastDS);

                RollModified=SURENAOnlineTaskSpace1.RollAngleModification(SURENAOnlineTaskSpace1.globalTime,SURENAOnlineTaskSpace1.StepNumber,SURENAOnlineTaskSpace1.localTiming,indexLastDS);
                P=SURENAOnlineTaskSpace1.PelvisTrajectory (SURENAOnlineTaskSpace1.globalTime,SURENAOnlineTaskSpace1.StepNumber,SURENAOnlineTaskSpace1.localTiming,indexLastDS);

                SURENAOnlineTaskSpace1.globalTime=SURENAOnlineTaskSpace1.globalTime+SURENAOnlineTaskSpace1._timeStep;
                SURENAOnlineTaskSpace1.localTiming=SURENAOnlineTaskSpace1.localTiming+SURENAOnlineTaskSpace1._timeStep;
                SURENAOnlineTaskSpace1.localtimingInteger= SURENAOnlineTaskSpace1.localtimingInteger+1;

                if (round(SURENAOnlineTaskSpace1.globalTime)<=round(SURENAOnlineTaskSpace1.MotionTime)){

 //if you want to have modification of height of pelvis please active the Pz(0,0) instead of P(2,0)
                    PoseRoot<<P(0,0),
                            P(1,0),
                            Pz(0,0),
                            0,
                            0,
                            0;

                    PoseRFoot<<m5,
                            m6,
                            m7,
                            0,
                            -1*m8*(M_PI/180),
                            0;

                    PoseLFoot<<m1,
                            m2,
                            m3,
                            0,
                            -1*m4*(M_PI/180),
                            0;

                    SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
                    SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);

                    SURENA.ForwardKinematic(1);
                }
            }
        }

        //-----------------------------------------------------------------------------------------------------//
        //------------------- end phase-- finializing height of pelvis to home position -----------------------//
        //-----------------------------------------------------------------------------------------------------//

        if (endPhase==true &&  StartTime>=(DurationOfStartPhase+SURENAOnlineTaskSpace1.MotionTime) && StartTime<=DurationOfendPhase+DurationOfStartPhase+SURENAOnlineTaskSpace1.MotionTime) {

            MinimumJerkInterpolation Coef;
            MatrixXd ZPosition(1,2);
            //ZPosition<<Pz(0,0),Pz(0,0)-SURENAOnlineTaskSpace1.ReferencePelvisHeight+SURENAOnlineTaskSpace1.InitialPelvisHeight;//this one should be edited
            ZPosition<<SURENAOnlineTaskSpace1.ReferencePelvisHeight,SURENAOnlineTaskSpace1.InitialPelvisHeight;//this one should be edited
            MatrixXd ZVelocity(1,2);
            ZVelocity<<0.000,0.000;
            MatrixXd ZAcceleration(1,2);
            ZAcceleration<<0.000,0.000;

            MatrixXd Time(1,2);
            Time<<DurationOfStartPhase+SURENAOnlineTaskSpace1.MotionTime,DurationOfStartPhase+SURENAOnlineTaskSpace1.MotionTime+DurationOfendPhase;
            MatrixXd CoefZStart =Coef.Coefficient(Time,ZPosition,ZVelocity,ZAcceleration);

            double zStart=0;
            double yStart=0;
            double xStart=0;
            StartTime=StartTime+SURENAOnlineTaskSpace1._timeStep;

            MatrixXd outputZStart= SURENAOnlineTaskSpace1.GetAccVelPos(CoefZStart,StartTime,DurationOfStartPhase+SURENAOnlineTaskSpace1.MotionTime,5);
            zStart=outputZStart(0,0);

            PoseRoot<<xStart,yStart,zStart,0,0,0;

            PoseRFoot<<0,
                    -0.11500,
                    SURENAOnlineTaskSpace1.currentRightFootZ,
                    0,
                    0,
                    0;

            PoseLFoot<<0,
                    0.11500,
                    SURENAOnlineTaskSpace1.currentLeftFootZ,
                    0,
                    0,
                    0;

            SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
            SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);

            MinimumJerkInterpolation CoefOffline;
            double D_pitch=-1*pitchOffset*(M_PI/180);
            double TstartofPitchModify=DurationOfendPhase/6+DurationOfStartPhase+SURENAOnlineTaskSpace1.MotionTime;
            double TendofPitchModify=DurationOfendPhase*5/6+DurationOfStartPhase+SURENAOnlineTaskSpace1.MotionTime;
            double D_time=TendofPitchModify-TstartofPitchModify;
            if (StartTime>=TstartofPitchModify && StartTime<=TendofPitchModify){
                MatrixXd Ct_pitch_st(1,2);
                Ct_pitch_st<<-D_time, 0;
                MatrixXd Cp_pitch_st(1,2);
                Cp_pitch_st<<D_pitch, 0;
                MatrixXd Cv_pitch_st(1,2);
                Cv_pitch_st<<0 ,0;
                MatrixXd Ca_pitch_st(1,2);
                Ca_pitch_st<<0 ,0;
                MatrixXd C_pitch_st=CoefOffline.Coefficient(Ct_pitch_st,Cp_pitch_st,Cv_pitch_st,Ca_pitch_st);

                MatrixXd output=SURENAOnlineTaskSpace1.GetAccVelPos(C_pitch_st,StartTime-(TendofPitchModify),-D_time,5);
                PitchModified=output(0,0);

            }

        }

        links = SURENA.GetLinks();
        if(links[5].JointAngle<min_test){min_test=links[5].JointAngle;}
        if(links[5].JointAngle>max_test){max_test=links[5].JointAngle;}
        ROS_INFO("ankl pith min=%f,max=%f",min_test*180/M_PI,max_test*180/M_PI);

        //SURENAOnlineTaskSpace1.RightFootOrientationAdaptator=false;
        //SURENAOnlineTaskSpace1.LeftFootOrientationAdaptator=false;

        //        else {//else  is for situation that sensor is contacting and adapting ground
double ankle_adaptation_switch=0;// 1 for activating adaptation 0 for siktiring adaptation
double k_roll_corr=0;
        cntrl[0]=0.0;
        cntrl[1]=(links[1].JointAngle);
        cntrl[2]=(links[2].JointAngle+k_roll_corr*(links[2].JointAngle-roll_absoulte[0])+1*RollModified(0,0));
        cntrl[3]=links[3].JointAngle+1.25*PitchModified;
        cntrl[4]=links[4].JointAngle;
//        cntrl[5]=links[5].JointAngle+ankle_adaptation_switch*((SURENAOnlineTaskSpace1.RightFootOrientationAdaptator==true)*teta_motor_R+Offset_teta_R);//pitch
//        cntrl[6]=links[6].JointAngle+ankle_adaptation_switch*((SURENAOnlineTaskSpace1.RightFootOrientationAdaptator==true)*phi_motor_R+Offset_phi_R);//roll
        cntrl[5]=links[5].JointAngle+ankle_adaptation_switch*teta_motor_R;//pitch
        cntrl[6]=links[6].JointAngle+ankle_adaptation_switch*(phi_motor_R);//roll

        cntrl[7]=links[7].JointAngle;
        cntrl[8]=links[8].JointAngle+k_roll_corr*(links[8].JointAngle-roll_absoulte[1])+1*RollModified(1,0);
        cntrl[9]=links[9].JointAngle+1.25*PitchModified;
        cntrl[10]=links[10].JointAngle;
//        cntrl[11]=links[11].JointAngle+ankle_adaptation_switch*((SURENAOnlineTaskSpace1.LeftFootOrientationAdaptator==true)*teta_motor_L+Offset_teta_L);
//        cntrl[12]=links[12].JointAngle+ankle_adaptation_switch*((SURENAOnlineTaskSpace1.LeftFootOrientationAdaptator==true)*phi_motor_L+Offset_phi_L);
        cntrl[11]=links[11].JointAngle+ankle_adaptation_switch*teta_motor_L;
        cntrl[12]=links[12].JointAngle+ankle_adaptation_switch*(phi_motor_L);

        //        }
       // ROS_INFO("teta_R=%f,Offset_teta_R=%f,phi_R=%f,Offset_phi_R=%f,teta_L=%f,Offset_teta_L=%f,phi_L=%f,Offset_phi_L=%f",teta_motor_R,Offset_teta_R,phi_motor_R,Offset_phi_R,teta_motor_L,Offset_teta_L,phi_motor_L,Offset_phi_L);
//please uncomment /*SURENAOnlineTaskSpace1.LeftFootOrientationAdaptator==true* for activing sensor and also replace 0 with  1 in above code

        vector<int> qref(12);
        qref=QC.ctrldata2qc(cntrl);

        msg.data.clear();
        bool left_first=true;//right support in first step
        if(left_first){
            for(int  i = 0;i < 12;i++)
            {
                msg.data.push_back(qref[i]+qc_offset[i]);
            }}
        else{
            msg.data.push_back(-qref[5]+qc_offset[0]);
            msg.data.push_back(-qref[4]+qc_offset[1]);
            msg.data.push_back(-qref[6]+qc_offset[2]);
            msg.data.push_back(-qref[7]+qc_offset[3]);
            msg.data.push_back(-qref[1]+qc_offset[4]);
            msg.data.push_back(-qref[0]+qc_offset[5]);
            msg.data.push_back(-qref[2]+qc_offset[6]);
            msg.data.push_back(-qref[3]+qc_offset[7]);
            msg.data.push_back(-qref[11]+qc_offset[8]);
            msg.data.push_back(-qref[10]+qc_offset[9]);
            msg.data.push_back(-qref[9]+qc_offset[10]);
            msg.data.push_back(-qref[8]+qc_offset[11]);
        }

        for(int  i = 12;i < 28;i++)
        {
            msg.data.push_back(0);
        }

       // SendGazebo(links,RollModified,PitchModified,teta_motor_R+0*Offset_teta_R,phi_motor_R+0*Offset_phi_R,teta_motor_L+0*Offset_teta_L,phi_motor_L+0*Offset_phi_L);
        chatter_pub.publish(msg);

        msg_contact_flag.data.clear();
        msg_contact_flag.data.push_back(contact_flag_timing);
        msg_contact_flag.data.push_back(contact_flag_sensor);
        msg_contact_flag.data.push_back(contact_flag_sensor2);

        contact_flag.publish(msg_contact_flag);
        //  ROS_INFO("t={%d} c={%d}",timer.elapsed(),count);
       if(count%20==0){
//           ROS_INFO("right:des=%f,abs=%f,diff=%f (inc=%d),left:des=%f,abs=%f,diff=%f (inc=%d)",links[2].JointAngle,
//                   roll_absoulte[0],links[2].JointAngle-roll_absoulte[0],int((links[2].JointAngle-roll_absoulte[0])*120*2340/2/M_PI),
//                   links[8].JointAngle,roll_absoulte[1],
//                   links[8].JointAngle-roll_absoulte[1],int((links[8].JointAngle-roll_absoulte[1])*120*2340/2/M_PI));
       //ROS_INFO("time=%f\tRollModifiedRight=%f RollModifiedLeft=%f",StartTime,RollModified(0,0)*180/M_PI,RollModified(1,0)*180/M_PI);
//ROS_INFO("rightpitch=%f , leftpitch=%f",cntrl[3],cntrl[9]);
       }
    //    ROS_INFO("q=%d",qref[9]);
        //ROS_INFO("q=%d",qref[9]);
        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }

    return 0;
}


#include "onepass/storage.hpp"

#include <cryptopp/base64.h>

#include <boost/algorithm/string.hpp>
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/insert_linebreaks.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <fstream>
#include <sstream>

#include "cryptopp/cryptlib.h"
#include "cryptopp/files.h"
#include "cryptopp/osrng.h"
#include "cryptopp/salsa.h"
#include "cryptopp/secblock.h"
#include "onepass/database.hpp"

using namespace onepass::storage;

void Storage::read(
    Database &database,
    const std::string password,
    const std::string filename,
    EncodeType encodeType,
    EncryptType encryptType)
{
  if (encryptType == EncryptType::Salsa2)
  {
    CryptoPP::byte digest[CryptoPP::SHA256::DIGESTSIZE];
    CryptoPP::Salsa20::Decryption decryption;
    CryptoPP::SecByteBlock sbb(8);
    memset(sbb, 0x10101110, sbb.size());
    CryptoPP::SHA256().CalculateDigest(
        digest, reinterpret_cast<const CryptoPP::byte *>(password.c_str()), password.length());
    decryption.SetKeyWithIV(digest, CryptoPP::SHA256::DIGESTSIZE, sbb, sbb.size());

    if (encodeType == EncodeType::None)
    {
      std::string decrypted;
      CryptoPP::FileSource database2(
          filename.c_str(), true, new CryptoPP::StreamTransformationFilter(decryption, new CryptoPP::StringSink(decrypted)));
      std::istringstream iss(decrypted);
      boost::archive::text_iarchive iar(iss);
      iar >> database;
    }
    if (encodeType == EncodeType::Url || encodeType == EncodeType::Base64)
    {
      std::ifstream ifs(filename);
      if (ifs.is_open())
      {
        std::stringstream ss;
        ss << ifs.rdbuf();
        ss.str(decode(ss.str(), encodeType));
        std::string decrypted;
        CryptoPP::StringSource stringSource(
            ss.str(), true, new CryptoPP::StreamTransformationFilter(decryption, new CryptoPP::StringSink(decrypted)));
        std::istringstream iss(decrypted);
        boost::archive::text_iarchive iar(iss);
        iar >> database;
      }
    }
  }
  if (encryptType == EncryptType::None)
  {
    std::ifstream ifs(filename);
    if (ifs.is_open())
    {
      std::stringstream ss;
      ss << ifs.rdbuf();
      ss.str(decode(ss.str(), encodeType));
      boost::archive::text_iarchive iar(ss);
      iar >> database;
    }
  }
}

void Storage::save(const Database &database, const std::string filename, EncodeType encodeType, EncryptType encryptType)
{
  if (encryptType == EncryptType::Salsa2)
  {
    CryptoPP::byte digest[CryptoPP::SHA256::DIGESTSIZE];
    CryptoPP::SecByteBlock sbb(8);
    CryptoPP::Salsa20::Encryption encryption;

    memset(sbb, 0x10101110, sbb.size());
    CryptoPP::SHA256().CalculateDigest(
        digest, reinterpret_cast<const CryptoPP::byte *>(database.getPassword().c_str()), database.getPassword().length());
    encryption.SetKeyWithIV(digest, CryptoPP::SHA256::DIGESTSIZE, sbb, sbb.size());

    std::ostringstream oss;
    boost::archive::text_oarchive oar{ oss };
    oar << database;

    if (encodeType == EncodeType::None)
    {
      CryptoPP::StringSource stringSource(
          oss.str(), true, new CryptoPP::StreamTransformationFilter(encryption, new CryptoPP::FileSink(filename.c_str())));
    }
    if (encodeType == EncodeType::Base64 || encodeType == EncodeType::Url)
    {
      std::string encrypted;
      CryptoPP::StringSource stringSource(
          oss.str(), true, new CryptoPP::StreamTransformationFilter(encryption, new CryptoPP::StringSink(encrypted)));

      std::ofstream ofs;
      ofs.open(filename.c_str(), std::ios::out);
      if (ofs.is_open())
      {
        ofs << encode(encrypted, encodeType);
        ofs.close();
      }
    }
  }
  if (encryptType == EncryptType::None)
  {
    std::stringstream ss;
    std::ofstream ofs;
    ofs.open(filename.c_str(), std::ios::out);
    if (ofs.is_open())
    {
      boost::archive::text_oarchive oar{ ss };
      oar << database;
      ofs << encode(ss.str(), encodeType);
      ofs.close();
    }
    else
      throw "Error opening file";
  }
}

bool Storage::doesFileExists(std::string const path)
{
  std::ifstream file(path.c_str());
  return file.good();
}

std::string Storage::encode(const std::string &value, EncodeType encodeType)
{
  switch (encodeType)
  {
    case EncodeType::Url: return encodeUrl64(value);
    case EncodeType::Base64: return encode64(value);
    case EncodeType::None: return value;
    default: return encode64(value);
  }
}

std::string Storage::decode(const std::string &value, EncodeType encodeType)
{
  switch (encodeType)
  {
    case EncodeType::Url: return decodeUrl64(value);
    case EncodeType::Base64: return decode64(value);
    case EncodeType::None: return value;
    default: return decode64(value);
  }
}

std::string Storage::decode64(const std::string &value)
{
  std::string decoded;
  using namespace CryptoPP;
  StringSource ss(value, true, new Base64Decoder(new StringSink(decoded)));
  return decoded;
}

std::string Storage::encode64(const std::string &value)
{
  std::string encoded;
  using namespace CryptoPP;
  StringSource ss(
      reinterpret_cast<const byte *>(value.c_str()), value.size(), true, new Base64Encoder(new StringSink(encoded)));
  return encoded;
}

std::string Storage::decodeUrl64(const std::string &value)
{
  std::string decoded;
  using namespace CryptoPP;
  StringSource ss(value, true, new Base64URLDecoder(new StringSink(decoded)));
  return decoded;
}

std::string Storage::encodeUrl64(const std::string &value)
{
  std::string encoded;
  using namespace CryptoPP;
  StringSource ss(
      reinterpret_cast<const byte *>(value.c_str()), value.size(), true, new Base64URLEncoder(new StringSink(encoded)));
  return encoded;
}


#ifndef IZENELIB_CONCURRENT_CACHE_H
#define IZENELIB_CONCURRENT_CACHE_H

#include "IzeneCacheTraits.h"
#include <stdint.h>
#include <boost/atomic.hpp>
#include <boost/lexical_cast.hpp>
#include <3rdparty/folly/RWSpinLock.h>
#include <util/hashFunction.h>

namespace izenelib { namespace concurrent_cache
{

struct ItemAccessInfo
{
    int64_t last_time;
    boost::atomic<int64_t> get_cnt;
    boost::atomic<int32_t> item_cache_index;
    ItemAccessInfo()
        :last_time(0), get_cnt(0), item_cache_index(-1)
    {
    }
};

template <class KeyType, class ValueType> class CacheItem
{
public:
    typedef std::list<std::pair<KeyType, ValueType> > ContainerT;
    typedef folly::RWTicketSpinLockT<32, true> ItemRWLock;
    ItemRWLock rw_lock;
    ContainerT item_list;
    boost::atomic<int32_t>  access_info_index;
    CacheItem()
        :access_info_index(-1)
    {
    }
};

static const uint32_t MULT_FREE_LIST_NUM = 16;

enum { PRIME_NUM = 28 };

static const int64_t PRIME_LIST[PRIME_NUM] =
{
    53l, 97l, 193l, 389l, 769l,
    1543l, 3079l, 6151l, 12289l, 24593l,
    49157l, 98317l, 196613l, 393241l, 786433l,
    1572869l, 3145739l, 6291469l, 12582917l, 25165843l,
    50331653l, 100663319l, 201326611l, 402653189l, 805306457l,
    1610612741l, 3221225473l, 4294967291l
};

inline int64_t cal_next_prime(int64_t n)
{
    int64_t ret = n;
    if (n > 0)
    {
        const int64_t* first = PRIME_LIST;
        const int64_t* last = PRIME_LIST + PRIME_NUM;
        const int64_t* pos = std::lower_bound(first, last, n);
        ret = ((pos == last) ? *(last - 1) : *pos);
    }
    return ret;
}

template <class KeyType, class ValueType> class ConcurrentCache
{
public:
    typedef CacheItem<KeyType, ValueType> ItemT;

    class spinlock {
        boost::atomic_flag lock_;
    public:
        spinlock()
        {
            lock_.clear();
        }
        inline void lock()
        {
            int cnt = 0;
            while (lock_.test_and_set(boost::memory_order_acquire))
            {
                if (++cnt > 10) sched_yield();
            }
        }
        inline void unlock()
        {
            lock_.clear(boost::memory_order_release);
        }
        inline bool try_lock()
        {
            return !lock_.test_and_set(boost::memory_order_acquire);
        }
    };

    ConcurrentCache(std::size_t cache_size, izenelib::cache::REPLACEMENT_TYPE evit_strategy,
        int32_t wash_out_interval_sec = 60, double wash_out_threshold = 0.1)
    {
        evit_strategy_ = evit_strategy;
        wash_out_interval_sec_ = wash_out_interval_sec;
        wash_out_threshold_ = wash_out_threshold;
        need_exit_ = false;
        wash_out_by_full_ = false;
        total_get_cnt_ = 0;
        total_hit_cnt_ = 0;
        init(cache_size);
    }

    ~ConcurrentCache()
    {
        need_exit_ = true;
        wash_out_cond_.notify_all();
        wash_out_thread_.join();
        free_access_info_list_.clear();
        if (free_list_lock_)
            delete[] free_list_lock_;
        if (item_buffer_)
            delete[] item_buffer_;
        item_buffer_ = NULL;
        if (access_info_list_)
            delete[] access_info_list_;
        access_info_list_ = NULL;
        item_buffer_size_ = 0;
        access_info_list_size_ = 0;
    }

    void init(std::size_t cache_size)
    {
        // keep some free space to reduce collision. Use the prime as bucket size to reduce collision.
        item_buffer_size_ = cal_next_prime(cache_size*4);
        item_buffer_ = new ItemT[item_buffer_size_];
        access_info_list_size_ = cache_size;
        access_info_list_ = new ItemAccessInfo[access_info_list_size_];

        free_list_lock_ = new spinlock[MULT_FREE_LIST_NUM];
        free_access_info_list_.resize(MULT_FREE_LIST_NUM);
        free_size_list_.resize(MULT_FREE_LIST_NUM, 0);
        for (std::size_t i = 0; i < access_info_list_size_; ++i)
        {
            free_access_info_list_[i%MULT_FREE_LIST_NUM].push_back(i);
            ++free_size_list_[i % MULT_FREE_LIST_NUM];
        }

        std::cout << "init hash bucket size : " << item_buffer_size_ << ", access list size: " << access_info_list_size_ << std::endl;
        wash_out_thread_ = boost::thread(boost::bind(&ConcurrentCache::wash_out_bg, this));
    }

    bool insert(const KeyType& key, const ValueType& value, bool overwrite = true)
    {
        std::size_t bucket_index = getBucketIndex(key);
        typename ItemT::ItemRWLock::WriteHolder guard(item_buffer_[bucket_index].rw_lock);
        ItemT& item = item_buffer_[bucket_index];
        if (item.access_info_index == -1)
        {
            assert(item.item_list.empty());
            // first key in this bucket.
            // found a non-used access info for this bucket.
            std::size_t free_list_num = bucket_index % MULT_FREE_LIST_NUM;
            while (true)
            {
                std::size_t free_index = -1;

                free_list_lock_[free_list_num].lock();
                if (!free_access_info_list_[free_list_num].empty())
                {
                    free_index = free_access_info_list_[free_list_num].front();
                    free_access_info_list_[free_list_num].pop_front();
                    --free_size_list_[free_list_num];
                }
                free_list_lock_[free_list_num].unlock();

                if (free_index == (std::size_t)-1)
                    break;
                if (free_index >= access_info_list_size_)
                    continue;
                int32_t nonused = -1;
                if (access_info_list_[free_index].item_cache_index.compare_exchange_weak(nonused, (uint32_t)bucket_index))
                {
                    item.item_list.push_back(std::make_pair(key, value));
                    item.access_info_index = free_index;
                    update_access_info(free_index);
                    return true;
                }
                else
                {
                    std::cerr << "exchange failed for item cache index." << std::endl;
                    break;
                }
            }
            //for (std::size_t i = 0; i < access_info_list_size_; ++i)
            //{
            //    int32_t nonused = -1;
            //    if (access_info_list_[i].item_cache_index.compare_exchange_weak(nonused, (int32_t)bucket_index))
            //    {
            //        item.item_list.push_back(std::make_pair(key, value));
            //        item.access_info_index = i;
            //        update_access_info(i);
            //        return true;
            //    }
            //}
            //std::cerr << "cache is full, need wash out." << std::endl;
            wash_out_by_full_ = true;
            wash_out_cond_.notify_all();
            return false;
        }
        else
        {
            // collision key
            bool is_exist = false;
            for(typename ItemT::ContainerT::iterator it = item.item_list.begin();
                it != item.item_list.end(); ++it)
            {
                if (it->first == key)
                {
                    if (overwrite)
                    {
                        it->second = value;
                    }
                    is_exist = true;
                }
            }
            if (!is_exist)
            {
                item.item_list.push_back(std::make_pair(key, value));
                //if (item.item_list.size() > 5)
                //    std::cerr << "hash collision is heavy : " << item.item_list.size() << std::endl;
            }

            update_access_info(item.access_info_index);
        }
        return true;
    }

    bool get(const KeyType& key, ValueType& value)
    {
        ++total_get_cnt_;
        std::size_t bucket_index = getBucketIndex(key);
        typename ItemT::ItemRWLock::ReadHolder guard(item_buffer_[bucket_index].rw_lock);
        const ItemT& item = item_buffer_[bucket_index];
        if (item.item_list.empty())
        {
            // not found in cache.
            return false;
        }
        else
        {
            // get item and udpate the LRU.
            for(typename ItemT::ContainerT::const_iterator it = item.item_list.begin();
                it != item.item_list.end(); ++it)
            {
                if (it->first == key)
                {
                    value = it->second;
                    update_access_info(item.access_info_index);
                    ++total_hit_cnt_;
                    return true;
                }
            }
        }
        return false;
    }

    void remove(const KeyType& key)
    {
        std::size_t bucket_index = getBucketIndex(key);
        int32_t access_index = -1;
        {
            typename ItemT::ItemRWLock::WriteHolder guard(item_buffer_[bucket_index].rw_lock);
            ItemT& item = item_buffer_[bucket_index];
            for (typename ItemT::ContainerT::iterator it = item.item_list.begin();
                it != item.item_list.end(); ++it)
            {
                if (it->first == key)
                {
                    item.item_list.erase(it);
                    break;
                }
            }
            if (!item.item_list.empty())
                return;
            access_index = item.access_info_index;
            reset_access_info(item.access_info_index);
            item.access_info_index = -1;
        }
        free_access_info_to_list(access_index);
    }

    void clear()
    {
        for(std::size_t i = 0; i < item_buffer_size_; ++i)
        {
            clear_bucket(i);
        }
    }

    bool check_correctness()
    {
        // check free list should be really free and all free pos should be in free list
        for (std::size_t i = 0; i < MULT_FREE_LIST_NUM; ++i)
        {
            free_list_lock_[i].lock();
            std::set<std::size_t> diff_set;
            bool check_ok = true;
            for (std::list<std::size_t>::const_iterator it = free_access_info_list_[i].begin();
                it != free_access_info_list_[i].end(); ++it)
            {
                if (*it >= access_info_list_size_)
                {
                    // free pos should be no larger than size.
                    std::cerr << "free access list position larger than size." << *it << std::endl;
                    check_ok = false;
                    break;
                }
                if (access_info_list_[*it].item_cache_index != -1)
                {
                    std::cerr << "free access list position not really free." << std::endl;
                    check_ok = false;
                    break;
                }
                if (diff_set.find(*it) != diff_set.end())
                {
                    std::cerr << "free access list position duplicate." << std::endl;
                    check_ok = false;
                    break;
                }
                diff_set.insert(*it);
            }
            if (free_access_info_list_[i].size() != free_size_list_[i])
            {
                std::cerr << "free access list size mismatch." << std::endl;
                check_ok = false;
            }

            free_list_lock_[i].unlock();
            if (!check_ok)
                return false;
        }
        //
        // check bucket access info consistent.
        for(std::size_t i = 0; i < item_buffer_size_; ++i)
        {
            typename ItemT::ItemRWLock::ReadHolder guard(item_buffer_[i].rw_lock);
            const ItemT& item = item_buffer_[i];
            if (!item.item_list.empty() && item.access_info_index != -1)
            {
                if (item.access_info_index >= (int32_t)access_info_list_size_)
                {
                    std::cerr << "bucket access info pos out of range." << std::endl;
                    return false;
                }
                if (access_info_list_[item.access_info_index].item_cache_index != (int32_t)i)
                {
                    std::cerr << "bucket access info pos is not consistent. " << i
                        << "-" << item.access_info_index << "-" << access_info_list_[item.access_info_index].item_cache_index << std::endl;
                    return false;
                }
            }

        }
        return true;
    }

    std::string get_useful_info()
    {
        std::string retstr("Concurrent cache statistic:\n");
        retstr += "Current free list size: \n";
        for(std::size_t i = 0; i < MULT_FREE_LIST_NUM; ++i)
        {
            retstr += boost::lexical_cast<std::string>(free_size_list_[i]) + ", ";
        }
        retstr += "\n";

        retstr += "Hit ratio: " + boost::lexical_cast<std::string>((int64_t)total_hit_cnt_)
            + " / " + boost::lexical_cast<std::string>((int64_t)total_get_cnt_);
        return retstr;
    }

private:
    class CmpFunc
    {
    public:
        CmpFunc(const ItemAccessInfo* const access_info, std::size_t access_info_size, izenelib::cache::REPLACEMENT_TYPE evit_strategy)
            : access_info_(access_info), access_info_size_(access_info_size), evit_strategy_(evit_strategy)
        {
        }
        bool is_left_useless(const ItemAccessInfo& left, const ItemAccessInfo& right) const
        {
            if (evit_strategy_ == izenelib::cache::LRU)
            {
                if (left.last_time == right.last_time)
                    return left.get_cnt <= right.get_cnt;
                return left.last_time < right.last_time;
            }
            else if (evit_strategy_ == izenelib::cache::LFU)
            {
                if (left.get_cnt == right.get_cnt)
                    return left.last_time <= right.last_time;
                return left.get_cnt < right.get_cnt;
            }
            else
            {
                // for the frequency used cache item, we add some timestamp to leverage its importance.
                int64_t l = left.last_time + left.get_cnt*60*1000;
                int64_t r = right.last_time + right.get_cnt*60*1000;
                return l <= r;
            }
        }
        bool operator()(std::size_t left, std::size_t right) const
        {
            // return true if left less than right
            return left >= 0 && right >= 0 && left < access_info_size_
                && right < access_info_size_ && access_info_[left].last_time != 0
                && (0 == access_info_[right].last_time
                    || is_left_useless(access_info_[left], access_info_[right]));
        }

    private:
        const ItemAccessInfo* const access_info_;
        const std::size_t access_info_size_;
        const izenelib::cache::REPLACEMENT_TYPE& evit_strategy_;
    };
    std::size_t getBucketIndex(const KeyType& key)
    {
        uint32_t hashkey = hash_func_(key);
        return hashkey % item_buffer_size_;
    }

    void reset_access_info(int32_t access_info_index)
    {
        if (access_info_index >= 0 && access_info_index < (int32_t)access_info_list_size_)
        {
            access_info_list_[access_info_index].last_time = 0;
            access_info_list_[access_info_index].get_cnt = 0;
            access_info_list_[access_info_index].item_cache_index = -1;
        }
    }

    void free_access_info_to_list(int32_t access_info_index)
    {
        if (access_info_index >= 0 && access_info_index < (int32_t)access_info_list_size_)
        {
            free_list_lock_[access_info_index % MULT_FREE_LIST_NUM].lock();
            free_access_info_list_[access_info_index % MULT_FREE_LIST_NUM].push_back(access_info_index);
            ++free_size_list_[access_info_index % MULT_FREE_LIST_NUM];
            free_list_lock_[access_info_index % MULT_FREE_LIST_NUM].unlock();
        }
    }

    void try_clear_bucket(std::size_t bucket_index)
    {
        if (bucket_index >= item_buffer_size_)
            return;
        int32_t access_index = -1;
        {
            if (!item_buffer_[bucket_index].rw_lock.try_lock())
            {
                return;
            }
            ItemT& item = item_buffer_[bucket_index];
            item.item_list.clear();
            access_index = item.access_info_index;
            reset_access_info(item.access_info_index);
            item.access_info_index = -1;
            item_buffer_[bucket_index].rw_lock.unlock();
        }
        free_access_info_to_list(access_index);
    }
    void clear_bucket(std::size_t bucket_index)
    {
        if (bucket_index >= item_buffer_size_)
            return;
        int32_t access_index = -1;
        {
            typename ItemT::ItemRWLock::WriteHolder guard(item_buffer_[bucket_index].rw_lock);
            ItemT& item = item_buffer_[bucket_index];
            item.item_list.clear();
            access_index = item.access_info_index;
            reset_access_info(item.access_info_index);
            item.access_info_index = -1;
        }
        free_access_info_to_list(access_index);
    }

    void update_access_info(int32_t access_info_index)
    {
        if (access_info_index >= 0 &&
            access_info_index < (int32_t)access_info_list_size_)
        {
            ItemAccessInfo& info = access_info_list_[access_info_index];
            struct timespec cur_time;
            clock_gettime(CLOCK_MONOTONIC, &cur_time);
            info.last_time = cur_time.tv_sec * 1000 + cur_time.tv_nsec / 1000000;
            info.get_cnt.fetch_add(1, boost::memory_order_seq_cst);
        }
        else
        {
            std::cerr << "the cache access info data is not ready." << access_info_index << std::endl;
        }
    }

    void wash_out_bg()
    {
        while(true)
        {
            if (need_exit_)
                break;
            boost::unique_lock<boost::mutex> lock(wash_out_mutex_);
            wash_out_cond_.timed_wait(lock, boost::posix_time::seconds(wash_out_interval_sec_));
            if (need_exit_)
                break;
            bool wash_out_many = false;
            if (wash_out_by_full_)
            {
                wash_out_by_full_ = false;
                wash_out_many = true;
            }
            bool is_need_scan = wash_out_many;
            if (!wash_out_many)
            {
                for(std::size_t i = 0; i < MULT_FREE_LIST_NUM; ++i)
                {
                    if (free_size_list_[i] <= wash_out_threshold_ * access_info_list_size_ / MULT_FREE_LIST_NUM)
                    {
                        is_need_scan = true;
                        break;
                    }
                }
            }
            if (!is_need_scan)
            {
                //std::cout << "No need to wash out. " << get_useful_info() << std::endl;
                continue;
            }
            struct timespec start_time;
            clock_gettime(CLOCK_MONOTONIC, &start_time);
            // here, we ignore the updated access info during scan to avoid performance degrade.
            // There may be some inaccurate to evict some useful cache, but I think it will be rare.
            std::vector<int32_t> washheap;
            std::size_t heapsize = access_info_list_size_ * wash_out_threshold_;
            if (heapsize < 1)
                heapsize = 1;
            if (wash_out_many)
                heapsize *= 4;
            washheap.reserve(heapsize);
            std::size_t freesize = 0;
            CmpFunc cmp_func(access_info_list_, access_info_list_size_, evit_strategy_);
            for (std::size_t i = 0; i < access_info_list_size_; ++i)
            {
                if (access_info_list_[i].item_cache_index == -1)
                {
                    freesize++;
                }
                else if (washheap.size() < heapsize || cmp_func(i, washheap.front()))
                {
                    if (washheap.size() < heapsize)
                    {
                        washheap.push_back(i);
                    }
                    else
                    {
                        std::pop_heap(washheap.begin(), washheap.end(), cmp_func);
                        washheap.back() = i;
                    }
                    std::push_heap(washheap.begin(), washheap.end(), cmp_func);
                }
                //std::cout << access_info_list_[i].item_cache_index << "-" << access_info_list_[i].last_time << ", ";
            }
            if (freesize >= heapsize)
            {
                if (!wash_out_many)
                    std::cout << get_useful_info() << std::endl;
                continue;
            }
            struct timespec sort_time;
            clock_gettime(CLOCK_MONOTONIC, &sort_time);

            for (std::size_t i = 0; i < washheap.size(); ++i)
            {
                //std::cout << access_info_list_[washheap[i]].item_cache_index << "-" << access_info_list_[washheap[i]].last_time << ", ";
                try_clear_bucket(access_info_list_[washheap[i]].item_cache_index);
            }

            struct timespec end_time;
            clock_gettime(CLOCK_MONOTONIC, &end_time);
            int64_t wash_cost = (end_time.tv_sec - start_time.tv_sec)*1000 + (end_time.tv_nsec - start_time.tv_nsec)/1000000;
            int64_t sort_cost = (sort_time.tv_sec - start_time.tv_sec)*1000 + (sort_time.tv_nsec - start_time.tv_nsec)/1000000;
            if (wash_cost > 200 || wash_out_many)
            {
                std::cout << "wash out cache item num : " << washheap.size()
                    << ", cost time:" << wash_cost << ", sort time:" << sort_cost << std::endl;
            }
        }
        std::cerr << "cache wash out thread exit." << std::endl;
    }

    ItemT *item_buffer_;
    std::size_t item_buffer_size_;
    ItemAccessInfo*  access_info_list_;  // used for evict strategy.
    std::size_t access_info_list_size_;
    bool wash_out_by_full_;
    bool need_exit_;
    int32_t wash_out_interval_sec_;
    double wash_out_threshold_;
    izenelib::cache::REPLACEMENT_TYPE evit_strategy_;
    boost::mutex wash_out_mutex_;
    boost::condition_variable wash_out_cond_;
    boost::thread wash_out_thread_;
    HashIDTraits<KeyType, uint32_t>  hash_func_;
    std::vector<std::list<std::size_t> >  free_access_info_list_;
    std::vector<std::size_t>  free_size_list_;
    spinlock  *free_list_lock_;
    boost::atomic<int64_t>  total_get_cnt_;
    boost::atomic<int64_t>  total_hit_cnt_;
};

}}

#endif


// Copyright (c) 2012-2018 The Elastos Open Source Project
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#define CATCH_CONFIG_MAIN

#include <catch.hpp>
#include "TestHelper.h"

#include <Common/Log.h>
#include <WalletCore/Mnemonic.h>
#include <Plugin/Transaction/Payload/CRCProposal.h>
#include <boost/filesystem.hpp>
#include <WalletCore/BIP39.h>
#include <WalletCore/HDKeychain.h>
#include <WalletCore/Key.h>

using namespace Elastos::ElaWallet;

static void initCRCProposal(CRCProposal &crcProposal, CRCProposal::Type type) {
	std::string mnemonic = Mnemonic(boost::filesystem::path("Data")).Create("English", Mnemonic::WORDS_12);
	uint512 seed = BIP39::DeriveSeed(mnemonic, "");
	HDSeed hdseed(seed.bytes());
	HDKeychain rootkey(hdseed.getExtendedKey(true));
	HDKeychain masterKey = rootkey.getChild("44'/0'/0'");
	HDKeychain ownerKey = masterKey.getChild("0/0");
	HDKeychain newOwnerKey = masterKey.getChild("0/1");
	HDKeychain secretaryKey = masterKey.getChild("0/2");
	HDKeychain councilMemberKey = masterKey.getChild("0/3");
	uint8_t version = CRCProposalDefaultVersion;

	crcProposal.SetTpye(type);
	crcProposal.SetCategoryData(getRandString(100));
	crcProposal.SetOwnerPublicKey(ownerKey.pubkey().getHex());
	crcProposal.SetDraftHash(getRanduint256());
	std::vector<Budget> budgets;
	for (int i = 0; i < 4; ++i) {
		Budget::Type budgetType = Budget::Type(getRandUInt8() % Budget::maxType);
		Budget budget(budgetType, getRandUInt8(), getRandUInt64());
		budgets.push_back(budget);
	}
	crcProposal.SetBudgets(budgets);
	crcProposal.SetRecipient(Address(Prefix::PrefixStandard, ownerKey.pubkey()));
	crcProposal.SetTargetProposalHash(getRanduint256());
	crcProposal.SetNewRecipient(Address(Prefix::PrefixStandard, newOwnerKey.pubkey()));
	crcProposal.SetNewOwnerPublicKey(newOwnerKey.pubkey());
	crcProposal.SetSecretaryPublicKey(secretaryKey.pubkey());
	crcProposal.SetSecretaryDID(Address(Prefix::PrefixIDChain, secretaryKey.pubkey(), true));
	crcProposal.SetCRCouncilMemberDID(Address(Prefix::PrefixIDChain, councilMemberKey.pubkey(), true));

	Key key;
	uint256 digest;
	bytes_t signature;
	switch (type) {
		case CRCProposal::Type::elip:
		case CRCProposal::Type::normal:
			digest = crcProposal.DigestNormalOwnerUnsigned(version);
			key = ownerKey;
			signature = key.Sign(digest);
			crcProposal.SetSignature(signature);

			digest = crcProposal.DigestNormalCRCouncilMemberUnsigned(version);
			key = councilMemberKey;
			signature = key.Sign(digest);
			crcProposal.SetCRCouncilMemberSignature(signature);
			break;

		case CRCProposal::Type::secretaryGeneralElection:
			digest = crcProposal.DigestSecretaryElectionUnsigned(version);
			key = ownerKey;
			signature = key.Sign(digest);
			crcProposal.SetSignature(signature);

			key = secretaryKey;
			signature = key.Sign(digest);
			crcProposal.SetSecretarySignature(signature);

			digest = crcProposal.DigestSecretaryElectionCRCouncilMemberUnsigned(version);
			key = councilMemberKey;
			signature = key.Sign(digest);
			crcProposal.SetCRCouncilMemberSignature(signature);
			break;

		case CRCProposal::Type::changeProposalOwner:
			digest = crcProposal.DigestChangeOwnerUnsigned(version);
			key = ownerKey;
			signature = key.Sign(digest);
			crcProposal.SetSignature(signature);

			key = newOwnerKey;
			signature = key.Sign(digest);
			crcProposal.SetNewOwnerSignature(signature);

			digest = crcProposal.DigestChangeOwnerCRCouncilMemberUnsigned(version);
			key = councilMemberKey;
			signature = key.Sign(digest);
			crcProposal.SetCRCouncilMemberSignature(signature);
			break;

		case CRCProposal::Type::terminateProposal:
			digest = crcProposal.DigestTerminateProposalOwnerUnsigned(version);
			key = ownerKey;
			signature = key.Sign(digest);
			crcProposal.SetSignature(signature);

			digest = crcProposal.DigestTerminateProposalCRCouncilMemberUnsigned(version);
			key = councilMemberKey;
			signature = key.Sign(digest);
			crcProposal.SetCRCouncilMemberSignature(signature);
			break;

		default:
			break;
	}
}

TEST_CASE("CRCProposal test", "[CRCProposal]") {
	Log::registerMultiLogger();
	uint8_t version = CRCProposalDefaultVersion;
	SECTION("Serialize and Deserialize test") {
		CRCProposal p1, p2;
		std::vector<CRCProposal::Type> types = {
			CRCProposal::normal,
			CRCProposal::secretaryGeneralElection,
			CRCProposal::changeProposalOwner,
			CRCProposal::terminateProposal
		};

		for (size_t i = 0; i < types.size(); ++i) {
			ByteStream byteStream;
			initCRCProposal(p1, types[i]);
			p1.Serialize(byteStream, version);
			REQUIRE(byteStream.GetBytes().size() == p1.EstimateSize(version));
			REQUIRE(p2.Deserialize(byteStream, version));
			REQUIRE(p1 == p2);
		}
	}

	SECTION("ToJson FromJson test") {
		CRCProposal p1, p2;
		std::vector<CRCProposal::Type> types = {
			CRCProposal::normal,
			CRCProposal::secretaryGeneralElection,
			CRCProposal::changeProposalOwner,
			CRCProposal::terminateProposal
		};

		for (size_t i = 0; i < types.size(); ++i) {
			initCRCProposal(p1, types[i]);
			nlohmann::json j = p1.ToJson(version);
			p2.FromJson(j, version);
			REQUIRE(p1 == p2);
		}
	}

}