/* methods.cpp */

#include "linalg.h"
#include "methods.h"
#include "structs.h"

#include <Python.h>
#include <math.h>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/QR>

extern "C"
{
    void _ETS_IK(PyObject *ets, int n, double *q, double *Tep, double *ret)
    {
        // double E;
        double *Te = (double *)PyMem_RawCalloc(16, sizeof(double));
        double *e = (double *)PyMem_RawCalloc(6, sizeof(double));

        double *a = (double *)PyMem_RawCalloc(6, sizeof(double));
        // a[0] = 1.0;
        // a[1] = 4.0;
        // a[2] = 2.0;
        // a[3] = 5.0;
        // a[4] = 3.0;
        // a[5] = 6.0;
        a[0] = 1.0;
        a[1] = 2.0;
        a[2] = 3.0;
        a[3] = 4.0;
        a[4] = 5.0;
        a[5] = 6.0;
        double *b = (double *)PyMem_RawCalloc(12, sizeof(double));
        b[0] = 11.0;
        b[1] = 15.0;
        b[2] = 19.0;
        b[3] = 12.0;
        b[4] = 16.0;
        b[5] = 20.0;
        b[6] = 13.0;
        b[7] = 17.0;
        b[8] = 21.0;
        b[9] = 14.0;
        b[10] = 18.0;
        b[11] = 22.0;
        // b[0] = 11.0;
        // b[1] = 12.0;
        // b[2] = 13.0;
        // b[3] = 14.0;
        // b[4] = 15.0;
        // b[5] = 16.0;
        // b[6] = 17.0;
        // b[7] = 18.0;
        // b[8] = 19.0;
        // b[9] = 20.0;
        // b[10] = 21.0;
        // b[11] = 22.0;

        // double *U = (double *)PyMem_RawCalloc(16, sizeof(double));
        // double *invU = (double *)PyMem_RawCalloc(16, sizeof(double));
        // double *temp = (double *)PyMem_RawCalloc(16, sizeof(double));
        // double *ret = (double *)PyMem_RawCalloc(16, sizeof(double));
        // Py_ssize_t m;
        int arrived = 0, iter = 0;

        while (arrived == 0 && iter < 500)
        {
            // Current pose Te
            // _ETS_fkine(ets, q, (double *)NULL, NULL, Te);

            // Angle axis error e
            _angle_axis(Te, Tep, e);

            // Squared error E
            // E = 0.5 * e @ We @ e
            // E = 0.5 * (e[0] * e[0] + e[1] * e[1] + e[2] * e[2] + e[3] * e[3] + e[4] * e[4] + e[5] * e[5]);
        }

        // _ETS_fkine(ets, q, (double *)NULL, NULL, Te);

        // for (int i = 0; i < 2; i++)
        // {
        //     for (int j = 0; j < 4; j++)
        //     {
        //         ret[i * 4 + j] = 0.0;
        //     }
        // }

        // _mult_T(2, 3, 0, a, 3, 4, 0, b, ret);
        // _mult_T(3, 2, 1, a, 3, 4, 0, b, ret);
        // _mult_T(3, 2, 1, a, 4, 3, 1, b, ret);
        // _mult_T(2, 3, 0, a, 4, 3, 1, b, ret);

        // int j = 0;
    }

    void _ETS_hessian(int n, MapMatrixJc &J, MapMatrixHr &H)
    {
        for (int j = 0; j < n; j++)
        {
            for (int i = j; i < n; i++)
            {
                H.block<3, 1>(j * 6, i) = J.block<3, 1>(3, j).cross(J.block<3, 1>(0, i));
                H.block<3, 1>(j * 6 + 3, i) = J.block<3, 1>(3, j).cross(J.block<3, 1>(3, i));

                if (i != j)
                {
                    H.block<3, 1>(i * 6, j) = H.block<3, 1>(j * 6, i);
                    H.block<3, 1>(i * 6 + 3, j) = Eigen::Vector3d::Zero();
                }
            }
        }
    }

    void _ETS_jacob0(ETS *ets, double *q, double *tool, MapMatrixJc &eJ)
    {
        // ET *et;
        // double T[16];
        // MapMatrix4dc eT(T);
        // Matrix4dc U;
        // Matrix4dc invU;
        // Matrix4dc temp;
        // Matrix4dc ret;

        // int j = 0;

        // U = Eigen::Matrix4d::Identity();

        // // Get the forward  kinematics into T
        // _ETS_fkine(ets, q, (double *)NULL, tool, eT);

        // for (int i = 0; i < ets->m; i++)
        // {
        //     et = ets->ets[i];

        //     if (et->isjoint)
        //     {
        //         _ET_T(et, &ret(0), q[et->jindex]);
        //         temp = U * ret;
        //         U = temp;

        //         if (i == ets->m - 1 && tool != NULL)
        //         {
        //             MapMatrix4dc e_tool(tool);
        //             temp = U * e_tool;
        //             U = temp;
        //         }

        //         _inv(&U(0), &invU(0));
        //         temp = invU * eT;

        //         if (et->axis == 0)
        //         {
        //             eJ(Eigen::seq(0, 2), j) = U(Eigen::seq(0, 2), 2) * temp(1, 3) - U(Eigen::seq(0, 2), 1) * temp(2, 3);

        //             eJ(Eigen::seq(3, 5), j) = U(Eigen::seq(0, 2), 0);
        //         }
        //         else if (et->axis == 1)
        //         {
        //             eJ(Eigen::seq(0, 2), j) = U(Eigen::seq(0, 2), 0) * temp(2, 3) - U(Eigen::seq(0, 2), 2) * temp(0, 3);
        //             eJ(Eigen::seq(3, 5), j) = U(Eigen::seq(0, 2), 1);
        //         }
        //         else if (et->axis == 2)
        //         {
        //             eJ(Eigen::seq(0, 2), j) = U(Eigen::seq(0, 2), 1) * temp(0, 3) - U(Eigen::seq(0, 2), 0) * temp(1, 3);
        //             eJ(Eigen::seq(3, 5), j) = U(Eigen::seq(0, 2), 2);
        //         }
        //         else if (et->axis == 3)
        //         {
        //             eJ(Eigen::seq(0, 2), j) = U(Eigen::seq(0, 2), 0);
        //             eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
        //         }
        //         else if (et->axis == 4)
        //         {
        //             eJ(Eigen::seq(0, 2), j) = U(Eigen::seq(0, 2), 1);
        //             eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
        //         }
        //         else if (et->axis == 5)
        //         {
        //             eJ(Eigen::seq(0, 2), j) = U(Eigen::seq(0, 2), 2);
        //             eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
        //         }
        //         j++;
        //     }
        //     else
        //     {
        //         _ET_T(et, &ret(0), q[et->jindex]);
        //         temp = U * ret;
        //         U = temp;
        //     }
        // }

        ET *et;
        Eigen::Matrix<double, 6, Eigen::Dynamic> tJ(6, ets->n);
        double T[16];
        MapMatrix4dc eT(T);
        Matrix4dc U = Eigen::Matrix4d::Identity();
        Matrix4dc invU;
        Matrix4dc temp;
        Matrix4dc ret;
        int j = ets->n - 1;

        if (tool != NULL)
        {
            Matrix4dc e_tool(tool);
            temp = e_tool * U;
            U = temp;
        }

        for (int i = ets->m - 1; i >= 0; i--)
        {
            et = ets->ets[i];

            if (et->isjoint)
            {
                if (et->axis == 0)
                {
                    tJ(Eigen::seq(0, 2), j) = U(2, Eigen::seq(0, 2)) * U(1, 3) - U(1, Eigen::seq(0, 2)) * U(2, 3);
                    tJ(Eigen::seq(3, 5), j) = U(0, Eigen::seq(0, 2));
                }
                else if (et->axis == 1)
                {
                    tJ(Eigen::seq(0, 2), j) = U(0, Eigen::seq(0, 2)) * U(2, 3) - U(2, Eigen::seq(0, 2)) * U(0, 3);
                    tJ(Eigen::seq(3, 5), j) = U(1, Eigen::seq(0, 2));
                }
                else if (et->axis == 2)
                {
                    tJ(Eigen::seq(0, 2), j) = U(1, Eigen::seq(0, 2)) * U(0, 3) - U(0, Eigen::seq(0, 2)) * U(1, 3);
                    tJ(Eigen::seq(3, 5), j) = U(2, Eigen::seq(0, 2));
                }
                else if (et->axis == 3)
                {
                    tJ(Eigen::seq(0, 2), j) = U(0, Eigen::seq(0, 2));
                    tJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
                }
                else if (et->axis == 4)
                {
                    tJ(Eigen::seq(0, 2), j) = U(1, Eigen::seq(0, 2));
                    tJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
                }
                else if (et->axis == 5)
                {
                    tJ(Eigen::seq(0, 2), j) = U(2, Eigen::seq(0, 2));
                    tJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
                }

                _ET_T(et, &ret(0), q[et->jindex]);
                temp = ret * U;
                U = temp;
                j--;
            }
            else
            {
                _ET_T(et, &ret(0), q[et->jindex]);
                temp = ret * U;
                U = temp;
            }
        }

        Eigen::Matrix<double, 6, 6> ev;
        ev.topLeftCorner<3, 3>() = U.topLeftCorner<3, 3>();
        ev.topRightCorner<3, 3>() = Eigen::Matrix3d::Zero();
        ev.bottomLeftCorner<3, 3>() = Eigen::Matrix3d::Zero();
        ev.bottomRightCorner<3, 3>() = U.topLeftCorner<3, 3>();
        eJ = ev * tJ;
    }

    void _ETS_jacobe(ETS *ets, double *q, double *tool, MapMatrixJc &eJ)
    {
        ET *et;
        double T[16];
        MapMatrix4dc eT(T);
        Matrix4dc U = Eigen::Matrix4d::Identity();
        Matrix4dc invU;
        Matrix4dc temp;
        Matrix4dc ret;
        int j = ets->n - 1;

        if (tool != NULL)
        {
            Matrix4dc e_tool(tool);
            temp = e_tool * U;
            U = temp;
        }

        for (int i = ets->m - 1; i >= 0; i--)
        {
            et = ets->ets[i];

            if (et->isjoint)
            {
                if (et->axis == 0)
                {
                    eJ(Eigen::seq(0, 2), j) = U(2, Eigen::seq(0, 2)) * U(1, 3) - U(1, Eigen::seq(0, 2)) * U(2, 3);
                    eJ(Eigen::seq(3, 5), j) = U(0, Eigen::seq(0, 2));
                }
                else if (et->axis == 1)
                {
                    eJ(Eigen::seq(0, 2), j) = U(0, Eigen::seq(0, 2)) * U(2, 3) - U(2, Eigen::seq(0, 2)) * U(0, 3);
                    eJ(Eigen::seq(3, 5), j) = U(1, Eigen::seq(0, 2));
                }
                else if (et->axis == 2)
                {
                    eJ(Eigen::seq(0, 2), j) = U(1, Eigen::seq(0, 2)) * U(0, 3) - U(0, Eigen::seq(0, 2)) * U(1, 3);
                    eJ(Eigen::seq(3, 5), j) = U(2, Eigen::seq(0, 2));
                }
                else if (et->axis == 3)
                {
                    eJ(Eigen::seq(0, 2), j) = U(0, Eigen::seq(0, 2));
                    eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
                }
                else if (et->axis == 4)
                {
                    eJ(Eigen::seq(0, 2), j) = U(1, Eigen::seq(0, 2));
                    eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
                }
                else if (et->axis == 5)
                {
                    eJ(Eigen::seq(0, 2), j) = U(2, Eigen::seq(0, 2));
                    eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
                }

                _ET_T(et, &ret(0), q[et->jindex]);
                temp = ret * U;
                U = temp;
                j--;
            }
            else
            {
                _ET_T(et, &ret(0), q[et->jindex]);
                temp = ret * U;
                U = temp;
            }
        }
    }

    void _ETS_fkine(ETS *ets, double *q, double *base, double *tool, MapMatrix4dc &e_ret)
    {
        ET *et;
        Matrix4dc temp;
        Matrix4dc current;

        if (base != NULL)
        {
            MapMatrix4dc e_base(base);
            current = e_base;
        }
        else
        {
            current = Eigen::Matrix4d::Identity();
        }

        for (int i = 0; i < ets->m; i++)
        {
            et = ets->ets[i];

            _ET_T(et, &e_ret(0), q[et->jindex]);
            temp = current * e_ret;
            current = temp;
        }

        if (tool != NULL)
        {
            MapMatrix4dc e_tool(tool);
            e_ret = current * e_tool;
        }
        else
        {
            e_ret = current;
        }
    }

    void _ET_T(ET *et, double *ret, double eta)
    {
        // Check if static and return static transform
        if (!et->isjoint)
        {
            _copy(et->T, ret);
            return;
        }

        if (et->isflip)
        {
            eta = -eta;
        }

        // Calculate ET trasform based on eta
        et->op(ret, eta);
    }

} /* extern "C" */


////////////////////////////////////////////////////////////////////////////////
// Copyright 2019 FZI Research Center for Information Technology
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived from this
// software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
////////////////////////////////////////////////////////////////////////////////

//-----------------------------------------------------------------------------
/*!\file    DampedLeastSquaresSolver.cpp
 *
 * \author  Stefan Scherzinger <scherzin@fzi.de>
 * \date    2020/03/27
 *
 */
//-----------------------------------------------------------------------------

// this package
#include <cartesian_controller_base/DampedLeastSquaresSolver.h>

// Pluginlib
#include <pluginlib/class_list_macros.h>

// other
#include <boost/algorithm/clamp.hpp>

/**
 * \class cartesian_controller_base::DampedLeastSquaresSolver
 *
 * Users may explicitly specify this solver with \a "damped_least_squares" as \a
 * ik_solver in their controllers.yaml configuration file for each controller:
 *
 * \code{.yaml}
 * <name_of_your_controller>:
 *     type: "<type_of_your_controller>"
 *     ik_solver: "damped_least_squares"
 *     ...
 *
 *     solver:
 *         ...
 *         damped_least_squares:
 *             alpha: 0.5
 * \endcode
 *
 */
PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::DampedLeastSquaresSolver, cartesian_controller_base::IKSolver)

namespace cartesian_controller_base{

  DampedLeastSquaresSolver::DampedLeastSquaresSolver()
    : m_alpha(0.01)
  {
  }

  DampedLeastSquaresSolver::~DampedLeastSquaresSolver(){}

  trajectory_msgs::JointTrajectoryPoint DampedLeastSquaresSolver::getJointControlCmds(
        ros::Duration period,
        const ctrl::Vector6D& net_force)
  {
    // Compute joint jacobian
    m_jnt_jacobian_solver->JntToJac(m_current_positions,m_jnt_jacobian);

    // Compute joint velocities according to:
    // \f$ \dot{q} = ( J^T J + \alpha^2 I )^{-1} J^T f \f$
    ctrl::MatrixND identity;
    identity.setIdentity(m_number_joints, m_number_joints);

    m_current_velocities.data =
      (m_jnt_jacobian.data.transpose() * m_jnt_jacobian.data
       + m_alpha * m_alpha * identity).inverse() * m_jnt_jacobian.data.transpose() * net_force;

    // Integrate once, starting with zero motion
    m_current_positions.data = m_last_positions.data + 0.5 * m_current_velocities.data * period.toSec();

    // Make sure positions stay in allowed margins
    applyJointLimits();

    // Apply results
    trajectory_msgs::JointTrajectoryPoint control_cmd;
    for (int i = 0; i < m_number_joints; ++i)
    {
      control_cmd.positions.push_back(m_current_positions(i));
      control_cmd.velocities.push_back(m_current_velocities(i));

      // Accelerations should be left empty. Those values will be interpreted
      // by most hardware joint drivers as max. tolerated values. As a
      // consequence, the robot will move very slowly.
    }
    control_cmd.time_from_start = period; // valid for this duration

    return control_cmd;
  }

  bool DampedLeastSquaresSolver::init(ros::NodeHandle& nh,
                                      const KDL::Chain& chain,
                                      const KDL::JntArray& upper_pos_limits,
                                      const KDL::JntArray& lower_pos_limits)
  {
    IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);

    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
    m_jnt_jacobian.resize(m_number_joints);

    // Connect dynamic reconfigure and overwrite the default values with values
    // on the parameter server. This is done automatically if parameters with
    // the according names exist.
    m_callback_type = boost::bind(
        &DampedLeastSquaresSolver::dynamicReconfigureCallback, this, _1, _2);

    m_dyn_conf_server.reset(
        new dynamic_reconfigure::Server<IKConfig>(
          ros::NodeHandle(nh.getNamespace() + "/solver/damped_least_squares")));
    m_dyn_conf_server->setCallback(m_callback_type);
    return true;
  }

    void DampedLeastSquaresSolver::dynamicReconfigureCallback(IKConfig& config, uint32_t level)
    {
      m_alpha = config.alpha;
    }

} // namespace


//
//  main.cpp
//  sthlm
//
//  Created by Jilin Zhang on 2/14/17.
//  Copyright © 2017 Jilin Zhang. All rights reserved.
//

#include <iostream>
#include <fstream>
#include <string>
#include "unordered_map"
#include <sstream>
#include <vector>
#include <stdlib.h>
#include <math.h>
#include <iomanip>
#include <algorithm>
#include <boost/program_options.hpp>
//#include "omp.h"
#include "kseq.h"
#include <zlib.h>

KSEQ_INIT(gzFile,gzread);

//
struct Unitscore{
    long count;
    double discore;
    double monoscore;
};

std::unordered_map<std::string, Unitscore > kstore;

//g++ -std=c++11 main.cpp -o rmap
//g++ -std=c++11 main.cpp -o rmap -lboost_program_options -lz
/*
 basemap[] works by storing a very small array that maps a base to
 its complement, by dereferencing the array with the ASCII char's
 decimal value as the index
 (int) 'A' = 65;
 (int) 'C' = 67;
 (int) 'G' = 71;
 (int) 'T' = 84;
 (int) 'a' = 97;
 (int) 'c' = 99;
 (int) 'g' = 103;
 (int) 't' = 116;
 (int) 'N' = 78;
 (int) 'U' = 85;
 (int) 'u' = 117;
 for example: basemap['A'] => basemap[65] => 'T' etc.
 */
static const char basemap[255] =
{
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /*   0 -   9 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /*  10 -  19 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /*  20 -  29 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /*  30 -  39 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /*  40 -  49 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /*  50 -  59 */
    '\0', '\0', '\0', '\0', '\0',  'U', '\0',  'G', '\0', '\0', /*  60 -  69 */
    '\0',  'C', '\0', '\0', '\0', '\0', '\0', '\0',  'N', '\0', /*  70 -  79 */
    '\0', '\0', '\0', '\0',  'A',  'A', '\0', '\0', '\0', '\0', /*  80 -  89 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0',  'u', '\0',  'g', /*  90 -  99 */
    '\0', '\0', '\0',  'c', '\0', '\0', '\0', '\0', '\0', '\0', /* 100 - 109 */
    '\0', '\0', '\0', '\0', '\0', '\0',  'a',  'a', '\0', '\0', /* 110 - 119 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 120 - 129 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 130 - 139 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 140 - 149 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 150 - 159 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 160 - 169 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 170 - 179 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 180 - 189 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 190 - 199 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 200 - 209 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 210 - 219 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 220 - 229 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 230 - 239 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 240 - 249 */
    '\0', '\0', '\0', '\0', '\0'                                /* 250 - 254 */
};

void fetch_matrix(std::unordered_map<std::string, std::vector<double> > *map, std::string str, int ind, double count);
void show_matrix(std::unordered_map<std::string, std::vector<double> > *x);
void kmerizing (std::string *id, std::string* seq,  int kmer, int gap, std::string* name, std::unordered_map<std::string, std::vector<double> > *map);
double monoscoring(std::string subseq, std::unordered_map<std::string, std::vector<double> > *map, int gap_len=0, int gap_pos=0);
double discoring(std::string subseq, std::unordered_map<std::string, std::vector<double> > *map, int gap_len=0, int gap_pos=0);
inline double singleStringScore(double x, std::string* s, long p, std::unordered_map<std::string, std::vector<double> > *m);
inline double pairStringScore(double x, std::string* s, long sp, std::unordered_map<std::string,std::vector<double> > *m);
void Matrixfile(const char*x, std::unordered_map<std::string, std::vector<double> > *y, double pseudo_count);
int listParser(const char* list_name, std::string* names, int* length, std::unordered_map<std::string, std::vector<double> >* map,double pcount);
std::string seq_revcomp(std::string str);
void GenomeMap(std::string *id, std::string *seq, long monoLen, std::string *mname, std::unordered_map<std::string, std::vector<double> >* map );
void hashOut();
void unCenterCorr();

template<class T>
std::ostream& operator<<(std::ostream& os, const std::vector<T>& v){
         copy(v.begin(), v.end(), std::ostream_iterator<T>(os, " "));
         return os;
}

int main(int argc,   char * argv[]) {

    std::unordered_map<std::string, std::vector<double> > matrixmap[100];
    int motif_len[100];
    std::string names_motif[100];
    double pseudo_count=0.00001;
    int motif_counts=0;
    char list_name[100],fasta_name[100];
    int kmer_length, gap_length;
    bool kmerflag=false ,outflag=false,corrflag=false, revcomflag=false;

    boost::program_options::options_description desc("Allowed options:");
    desc.add_options()
    ("help", "produce help message")
    ("fasta", boost::program_options::value<std::string>(), "input sequence file")
    ("list", boost::program_options::value<std::string> (), "specify a list of motifs")
    ("motif", boost::program_options::value<std::string>(), "specify one motif file")
    ("kmer,k", boost::program_options::value<int>(&kmer_length)->default_value(8), "specify the kmer length")
    ("gap,g", boost::program_options::value<int>(&gap_length)->default_value(0), "specify the gap length")
    ("kmerflag,f", boost::program_options::bool_switch(&kmerflag), "flag to switch to the kmer counting-score procedure")
    ("revcomp,r",boost::program_options::bool_switch(&revcomflag),"reverse compliment sequence")
    ("output,o",boost::program_options::bool_switch(&outflag),"output the kmer count-score")
    ("corr,c",boost::program_options::bool_switch(&corrflag),"output the correlation score (un centered cosine)")
    ;
    boost::program_options::variables_map vm;
    boost::program_options::positional_options_description p;
    p.add("fasta",-1);
    boost::program_options::store(boost::program_options::command_line_parser(argc,argv).options(desc).positional(p).run(), vm);
    boost::program_options::notify(vm);

    if (vm.count("list"))
    {
        const char* temx=vm["list"].as<std::string >().c_str();
        motif_counts= listParser(temx,names_motif, motif_len, matrixmap, pseudo_count);
    }
    if (vm.count("motif")) {
        const char* temname =vm["motif"].as<std::string>().c_str();
        motif_counts=1;
        strcpy(list_name,temname);
        names_motif[0]=(std::string) list_name;
        Matrixfile(temname, matrixmap, pseudo_count);
    }
    if (vm.count("fasta"))
    {
        const char * fastaname=vm["fasta"].as<std::string>().c_str();
        motif_len[0]=matrixmap[0].at("A").size();
        strcpy(fasta_name, fastaname );
    }
    if (vm.count("help")) {
        std::cout << "Usage:" << argv[0] << " [options]\n";
        std::cout << desc;
        return 0;
    }

// Load fasta sequence one by one;
    gzFile fp;
    kseq_t *seq;
    int l;
    fp=gzopen(fasta_name, "r");
    seq=kseq_init(fp);
    while((l=kseq_read(seq)) >=0){
        std::string seqnametmp, seqtmp;
        seqnametmp= &seq->name.s[0];
        seqtmp= &seq->seq.s[0];
        if(revcomflag)
            seqtmp=seq_revcomp(seqtmp);

        for(int i=0;i < motif_counts ; i++){
            if(kmerflag)
                kmerizing(&seqnametmp, &seqtmp,kmer_length,gap_length,&names_motif[i], &matrixmap[i]);
            else
                GenomeMap(&seqnametmp, &seqtmp,motif_len[i],&names_motif[i], &matrixmap[i]);
        }
    }
    kseq_destroy(seq);
    gzclose(fp);
    if(kmerflag && outflag)
        hashOut();
    if(kmerflag && corrflag)
        unCenterCorr();

    return 0;
}

//output the kmer count-score table
void hashOut(){
    for(auto it=kstore.begin(); it !=kstore.end(); it++)
        std::cout<<it->first << "\t" << it->second.count<< "\t" << it->second.discore <<std::endl;
//        std::cout<<it->first << "\t" << it->second.count<< "\t" << it->second.discore << "\t" << it->second.monoscore <<std::endl;

}

// function to calculate the uncentred the correlation, cosine
void unCenterCorr(){
    double sumx,sumy, sumz, sumxz, sumxy;
    for(auto it=kstore.begin(); it !=kstore.end(); it++){
        sumx+=it->second.count * it->second.count;
        sumy+=it->second.discore * it->second.discore ;
        sumz+=it->second.monoscore * it->second.monoscore  ;
        sumxy+= it->second.count * it->second.discore;
        sumxz+= it->second.count * it->second.monoscore;
    }
//    std::cout << sumxy/sqrt(sumx *sumy)<< "\t" << sumxz/sqrt(sumx *sumz) << std::endl;
    std::cout << sumxy/sqrt(sumx *sumy)<< std::endl;
}

// function to score all the locus in the sequence
void GenomeMap(std::string *id, std::string *seq, long monoLen, std::string *mname, std::unordered_map<std::string, std::vector<double> >* map ){
    unsigned long slen =seq->size();

    std::transform(seq->begin(), seq->end(), seq->begin(),::toupper);
    std::replace(seq->begin(),seq->end(),'T','U');

    for(long i=0; i < slen - monoLen +1 ; i++){
        std::string subseq=seq->substr(i,monoLen);
        if(subseq.find("N") != std::string::npos)
            continue;
        double mscore=0,dscore=0,rc_mscore=0,rc_dscore=0;
        std::string rc_subseq=seq_revcomp(subseq);

        mscore=singleStringScore(mscore, &subseq, 0, map);
        dscore= pairStringScore(dscore, &subseq,0, map);
        rc_mscore=singleStringScore(rc_mscore, &rc_subseq, 0,map);
        rc_dscore=pairStringScore(rc_dscore,&rc_subseq,0,map);
        std::cout << *id << "\t"<< i << "\t" << std::setprecision(5) <<mscore << std::fixed <<":"  << std::setprecision(5) <<dscore <<std::fixed << "\t+\t" << *mname <<std::endl;
        std::cout << *id << "\t"<< i << "\t" << std::setprecision(5) <<rc_mscore <<std::fixed << ":"  << std::setprecision(5) <<rc_dscore << std::fixed <<"\t-\t" <<*mname <<std::endl;
    }
}

// read the motif list
int listParser(const char* list_name, std::string* names, int* length, std::unordered_map<std::string, std::vector<double> >* map,double pcount){
    std::fstream motif_list;
    char motif_file_name[100];
    motif_list.open(list_name);
    int motif_line=0;

    while( motif_list.getline( motif_file_name,100) ){
        Matrixfile(motif_file_name, &map[motif_line],  pcount);
//        std::cout << motif_file_name << std::endl;
        length[motif_line]=(int) (map[motif_line])["A"].size();
        names[motif_line]=(std::string) motif_file_name;
        ++motif_line;
    }
//    std::cout << motif_line << " motifs loaded \n"<<std::endl;
    return motif_line;
}

//process the matrix input
void Matrixfile(const char* x, std::unordered_map<std::string, std::vector<double> > *y, double pseudo_count){
    std::fstream file;
    file.open(x);

    while(!file.is_open()){
        std::cout << "Something wrong with the filename" << std::endl;
        std::exit(EXIT_FAILURE);
    }
    int line_count=0;
    std::string iss;
    while(getline(file, iss)){
        line_count++;
        if(line_count <=2){
            continue;
        }
        else if(line_count >=3  and line_count <=18){
            fetch_matrix(y, iss, 1, pseudo_count);
        }
        else if(line_count >=19 && line_count<=22){
            fetch_matrix(y, iss, 2, pseudo_count);
        }
    }
}

//show the matrix and output or the screen
void show_matrix(std::unordered_map<std::string, std::vector<double> > *x){
    for(auto it=(*x).begin(); it !=(*x).end(); it++){
        std::cout << it->first << "\t";
        std::vector<double> tem= it->second;
        for( int i=0; i< tem.size(); ++i)
            std::cout <<  "\t" <<  tem[i];
        std::cout << std::endl;
    }
}
std::string seq_revcomp(std::string str){
    std::string rc_seq;
    for(int i=str.size()-1; i>=0; --i){
        rc_seq += (char) basemap[(int)str[i]];
    }
    return rc_seq;
}

//store the matrix into map table
void fetch_matrix(std::unordered_map<std::string, std::vector<double> > *map, std::string str, int ind, double count){
    std::vector<std::string> matrix;
    std::stringstream ms(str);
    std::string units;
    while(ms >> units){
        matrix.push_back(units);
    }
    unsigned long matrix_length_raw=matrix.size();
    std::string base_name= matrix.back();
    long matrix_length_new;
    if(ind ==1){
        base_name=base_name.substr(0,2);
        matrix_length_new=matrix_length_raw-2;
    }
    else if(ind==2){
        base_name=base_name.substr(14,1);
        matrix_length_new=matrix_length_raw-1;
    }
    double new_matrix[matrix_length_new];
    std::vector<double> hallo (new_matrix,new_matrix+ matrix_length_new);
    for(int i=0; i< matrix_length_new; i++){
        std::string::size_type sz;
        hallo[i]= std::stod(matrix[i], &sz);
        if(ind ==1)
            hallo[i] = std::log2((hallo[i]+ count)/(0.0625 + count));
        else
            hallo[i] = std::log2((hallo[i]+ count)/(0.25+ count));
    }
    map->insert({base_name,hallo});
//    return map;
}

//function to generate the kmer count-score table
void kmerizing (std::string* id, std::string* seq,  int kmer, int gap, std::string* name, std::unordered_map<std::string, std::vector<double> > *map){
    std::transform(seq->begin(), seq->end(), seq->begin(),::toupper);
    std::replace(seq->begin(),seq->end(),'T','U');
    unsigned long seq_length=seq->size();
    int raw_kseq_len=kmer+gap;

    for(unsigned long i=0; i < seq_length-raw_kseq_len +1; i++){
        std::string subseq=seq->substr(i,raw_kseq_len);
        if(subseq.find("N") != std::string::npos)
            continue;

        if(gap >0){
            for (int ii=1; ii <kmer ;ii++){
                double mscore,dscore;
                mscore=monoscoring(subseq, map, gap,ii);
                dscore=discoring(subseq, map, gap,ii);
                std::string tem=subseq;
                tem.replace(ii,gap, gap, '-' );

                if(kstore.find(tem) != kstore.end()){
                    kstore[tem].count +=1;
                }
                else
                    kstore.insert({tem,{1,dscore,mscore}});
            }
        }
        else{
            double mscore,dscore,rc_mscore, rc_dscore;
            //         std::string rc_subseq=seq_revcomp(subseq);
            mscore=monoscoring(subseq, map, 0,0);
            dscore=discoring(subseq, map, 0,0);
            std::string tem=subseq;

            if(kstore.find(tem) != kstore.end()){
                kstore[tem].count +=1;
            }
            else
                kstore.insert({tem,{1,dscore,mscore}});
        }

//        double mscore,dscore,rc_mscore,rc_dscore;
//        std::string rc_subseq=seq_revcomp(subseq);
//        mscore=monoscoring(subseq, map, 0,0);
//        dscore=discoring(subseq, map, 0,0);
//
//        rc_mscore=monoscoring(rc_subseq, map, 0,0);
//        rc_dscore=discoring(rc_subseq, map, 0,0);
//        std::cout << *id << "\t"<< i << "\t" << std::setprecision(5) <<mscore << std::fixed <<":"  << std::setprecision(5) <<dscore <<std::fixed << "\t+\t" << *name <<std::endl;
//        std::cout << *id << "\t"<< i << "\t" << std::setprecision(5) <<rc_mscore <<std::fixed << ":"  << std::setprecision(5) <<rc_dscore << std::fixed <<"\t-\t" <<*name <<std::endl;
//
    }
}

// The scoring function using the linear model only, which has considered all the conditions
double monoscoring(std::string subseq, std::unordered_map<std::string, std::vector<double> > *map, int gap_len, int gap_pos){
    long monoLen=(*map)["A"].size();
    if(gap_len >0)
        subseq.replace(gap_pos,gap_len, gap_len, '-' );

    long kseq_prime_len= subseq.size();
    long cycle=kseq_prime_len+ monoLen;
    std::string valid_seq;
    double maximum_score=-1000;
    for(int p=0; p < cycle -1 ; p++){
        long pos;
        double score=0;
        if(p <kseq_prime_len-1){
            if(monoLen >= kseq_prime_len){
                valid_seq=subseq.substr(kseq_prime_len-(p+1),p+1);
                pos=0;
            }
            else if(monoLen < kseq_prime_len){
                pos=0;
                if(p <monoLen -1)
                    valid_seq=subseq.substr(kseq_prime_len-(p+1));
                else
                    valid_seq=subseq.substr(kseq_prime_len-(p+1),monoLen);
            }
        }
        else if(p >=kseq_prime_len -1){
            if(monoLen >=kseq_prime_len){
                pos=p+1-kseq_prime_len;
                if(p <monoLen-1)
                    valid_seq=subseq;
                else
                    valid_seq=subseq.substr(0,kseq_prime_len+monoLen-p-1);
                }
            else{
                valid_seq=subseq.substr(0,kseq_prime_len-(p-monoLen) -1);
                pos=p+1-kseq_prime_len;
            }
        }
        score= singleStringScore(score, &valid_seq, pos, map);
        maximum_score=(score>maximum_score)?score:maximum_score;
    }
    return maximum_score ;
}
// The scoring function for the dependency matrix
double discoring(std::string subseq, std::unordered_map<std::string, std::vector<double> > *map, int gap_len, int gap_pos){
    if(gap_len >0)
        subseq.replace(gap_pos,gap_len,gap_len,'-');
    long monoLen=(*map)["A"].size();
    long kseq_prime_len=subseq.size();
    double maximum_score=-1000;
    if(monoLen %2 ==0){ //matrix length is even
        for(int p=0; p < monoLen + kseq_prime_len -1; p++){
            std::string subs;
            double score=0;
            long pos;
            if( p< monoLen/2){
                if( p < kseq_prime_len-1){
                    subs=subseq.substr(kseq_prime_len-1-p,p+1); //for kmers longer than the 1/2 matrix length
                    pos=0;
                }
                else if(p >=kseq_prime_len-1){  //for kmers shorter than the 1/2 marix length
                    subs=subseq;
                    pos=p-kseq_prime_len+1;
                }
                score= singleStringScore(score, &subs, pos, map);
                maximum_score=(score > maximum_score)?score:maximum_score;
            }
            else if(p >= monoLen/2   && (p - monoLen/2 < kseq_prime_len -1)){
                std::string left_seq; // leftover from the kmer which has been cut for dinucleotide matrix
                long single_pos;
                if(kseq_prime_len <= monoLen && kseq_prime_len >= 0.5 * monoLen){
                    if(2*(p-(monoLen/2 -1)) <=kseq_prime_len){
                        subs=subseq.substr(kseq_prime_len-2*(p-(monoLen/2 -1)),2*(p-(monoLen/2 -1)));
                        single_pos=p+1-subs.size();
                        if(p+1 >= kseq_prime_len){
                            pos=p+1-kseq_prime_len;
                            left_seq=subseq.substr(0,kseq_prime_len-2*(p-(monoLen/2-1))); //if($kseq_prime_len-2*($p-($mono_len/2 -1)) > 0);
                        }
                        else{
                            pos=0;
                            left_seq=subseq.substr(kseq_prime_len-(p+1),p+1-2*(p-(monoLen/2-1)));
                        }
                    }
                    else{
                        subs=subseq.substr(0,2*(kseq_prime_len-(p+1-(monoLen-1)/2 )+1) );
                        pos=p-kseq_prime_len+1+subs.size();
                        single_pos=p+1-kseq_prime_len;
                        if(p >= monoLen  ){
                            std::string left_seq_pre=subseq.substr(2*(kseq_prime_len-(p-(monoLen/2-1))));
                            left_seq=left_seq_pre.substr(0,kseq_prime_len-(p+1-monoLen)-subs.size());
                        }
                        else
                            left_seq=subseq.substr(2*(kseq_prime_len-(p-(monoLen/2 -1))));
                    }
//                    std::cout << subseq<< "\t" << subs << "\t" << single_pos << "\t" << pos << std::endl;
                }
                else if(kseq_prime_len > monoLen){
                    std::string left_seq_pre;
                    if(p < monoLen-1){
                        subs=subseq.substr(kseq_prime_len-2*(p-(monoLen/2 -1)),2*(p-(monoLen/2 -1)));
                        if(2*(p-(monoLen/2 -1)) <kseq_prime_len)
                            left_seq_pre =subseq.substr(0,kseq_prime_len-2*(p-(monoLen/2 -1)));
                        left_seq=left_seq_pre.substr(kseq_prime_len - (p+1));
                        pos=0;
                        single_pos=p+1-subs.size();
                    }
                    else if( p >= monoLen-1 && p <= kseq_prime_len-1){
                        subs=subseq.substr(kseq_prime_len-(p+1),monoLen);
                        pos=0;
                        single_pos=0;

                    }
                    else if(p > kseq_prime_len -1){
                        subs=subseq.substr(0,2*(kseq_prime_len-(p-(monoLen/2 -1))));
                        left_seq_pre=subseq.substr(2*(kseq_prime_len-(p-(monoLen/2 -1))));
                        left_seq=left_seq_pre.substr(0, left_seq_pre.size()-(p+1-monoLen));
                        pos=p-kseq_prime_len+1+subs.size();
                        single_pos=p+1-kseq_prime_len;
                    }
//                    std::cout << subseq<< "\t" << subs << "\t" << single_pos << "\t" << pos << std::endl;
                }
                else if(kseq_prime_len <  0.5 * monoLen){
                    if(2*(p-(monoLen/2 -1)) <=kseq_prime_len){
                        subs=subseq.substr(kseq_prime_len-2*(p-(monoLen/2 -1)),2*(p-(monoLen/2 -1)));
                        pos=p+1-kseq_prime_len;
                        left_seq=subseq.substr(0,kseq_prime_len-2*(p-(monoLen/2 -1)));
                        single_pos=p+1-subs.size();

                    }
                    else{
                        subs=subseq.substr(0,2*(kseq_prime_len-(p-(monoLen/2 -1))));
                        pos=p-kseq_prime_len+1+subs.size();
                        left_seq=subseq.substr(2*(kseq_prime_len-(p-(monoLen/2 -1))));
                        single_pos=p+1-kseq_prime_len;
                    }
                }

                score=pairStringScore(score,&subs,single_pos,map);
//                std::cout << subseq<< "\t" << subs << "\t" << single_pos << "\t" << pos << std::endl;

                if(!left_seq.empty())
                    score= singleStringScore(score, &left_seq, pos, map);
                maximum_score=(score > maximum_score)?score:maximum_score;
            }
            else if(p >= monoLen/2 + kseq_prime_len -1){
                if(p <monoLen){
                    subs=subseq;
                    pos=p+1-kseq_prime_len;
                }
                else if(p >= monoLen){
                    subs=subseq.substr(0,kseq_prime_len-(p-monoLen)-1);
                    pos=p+1-kseq_prime_len;
                }
                score= singleStringScore(score, &subs, pos, map);
                maximum_score=(score > maximum_score)?score:maximum_score;
            }
        }
    }
    else{
        for(int p=0; p < monoLen + kseq_prime_len -1; p++){
            std::string subs;
            double score=0;
            long pos;
            if( p < (monoLen-1)/2){
                if(p < kseq_prime_len-1){
                    subs=subseq.substr(kseq_prime_len-1-p,p+1); // for kmers longer than the 1/2 matrix length
                    pos=0;
                }
                else if(p >=kseq_prime_len-1){  //for kmers shorter than the 1/2marix length
                    subs=subseq;
                    pos=p-kseq_prime_len+1;
                }
                score= singleStringScore(score, &subs, pos, map);
                maximum_score=(score > maximum_score)?score:maximum_score;
            }
            else if(p >= (monoLen-1)/2  && (p - (monoLen-1)/2 <= kseq_prime_len -1)){
                std::string left_seq;
                long single_pos;
                if(kseq_prime_len <= monoLen && kseq_prime_len >= 0.5 * (monoLen-1)){
                    if(2*(p-(monoLen-1)/2 ) <=kseq_prime_len-1){
                        subs=subseq.substr(kseq_prime_len-2*(p+1-(monoLen-1)/2)+1,2*(p+1-(monoLen-1)/2)-1);
                        single_pos=p+1-subs.size();
                        pos=0;
                        if(p+1 >= kseq_prime_len){
                            pos=p+1-kseq_prime_len;
                            left_seq=subseq.substr(0,kseq_prime_len-2*(p-(monoLen-1)/2)-1); //if($kseq_prime_len-2*($    p-($mono_len/2 -1)) > 0);
                        }
                        else{
                            pos=0;
                            left_seq=subseq.substr(kseq_prime_len-(p+1),p+1-2*(p-(monoLen-1)/2)-1);
                        }
                    }
                    else{
                        subs=subseq.substr(0,2*(kseq_prime_len-(p+1-(monoLen-1)/2 ))+1);
                        pos= p-kseq_prime_len+1+subs.size();
                        single_pos=p+1-kseq_prime_len;
                        if(p >= monoLen){
                            std::string left_seq_pre=subseq.substr(2*(kseq_prime_len-(p+1-(monoLen-1)/2))+1);
                            left_seq=left_seq_pre.substr(0,kseq_prime_len-(p+1-monoLen)-subs.size());
                        }
                        else{
                            left_seq=subseq.substr(2*(kseq_prime_len-(p+1-(monoLen-1)/2))+1);
                        }
                    }
                }
                else if( kseq_prime_len > monoLen){
                    std::string left_seq_pre;
                    if(p < monoLen-1){
                        subs=subseq.substr(kseq_prime_len-2*(p+1-(monoLen-1)/2)+1,2*(p+1-(monoLen-1)/2)+1);
                        if(2*(p-(monoLen/2 -1)) < kseq_prime_len)
                            left_seq_pre=subseq.substr(0,kseq_prime_len-2*(p+1-(monoLen-1)/2)+1);
                        left_seq=left_seq_pre.substr(kseq_prime_len - (p+1));
                        pos=0;
                        single_pos=p+1-subs.size();
                        }
                    else if( p >= monoLen-1 && p <= kseq_prime_len-1){
                        subs=subseq.substr(kseq_prime_len-(p+1),monoLen);
                        pos=0;
                        single_pos=0;
                        }
                    else if(p > kseq_prime_len -1){
                        subs=subseq.substr(0,2*(kseq_prime_len-(p+1-(monoLen-1)/2 ))+1);
                        left_seq_pre=subseq.substr(2*(kseq_prime_len-(p+1-(monoLen-1)/2 ))+1);
                        left_seq=left_seq_pre.substr(0, left_seq_pre.size()-(p+1-monoLen));
                        pos=p-kseq_prime_len+1+subs.size();
                        single_pos=p+1-kseq_prime_len;
                    }
                }
                else if(kseq_prime_len <  0.5 * (monoLen-1)){
                    if(2*(p-(monoLen-1)/2 ) <=kseq_prime_len-1){
                        subs=subseq.substr(kseq_prime_len-2*(p+1-(monoLen-1)/2)+1,2*(p+1-(monoLen-1)/2)+1);
                        pos=p+1-kseq_prime_len;
                        left_seq=subseq.substr(0,kseq_prime_len-2*(p-(monoLen-1)/2)-1);
                        single_pos=p+1-subs.size();
                    }
                    else{
                        subs=subseq.substr(0,2*(kseq_prime_len-(p+1-(monoLen-1)/2))+1);
                        pos= p-kseq_prime_len+1+ subs.size();
                        left_seq=subseq.substr(2*(kseq_prime_len-(p+1-(monoLen-1)/2))+1);
                        single_pos=p+1-kseq_prime_len;
                    }
                }
                score=pairStringScore(score,&subs,single_pos,map);
                if(!left_seq.empty())
                    score= singleStringScore(score, &left_seq, pos, map);
                maximum_score=(score > maximum_score)?score:maximum_score;
            }
            else if(p >= (monoLen-1)/2 +kseq_prime_len -1){
                if(p <monoLen){
                    subs=subseq;
                    pos=p+1-kseq_prime_len;
                    }
                else if(p >= monoLen){
                    subs=subseq.substr(0,kseq_prime_len-(p-monoLen)-1);
                    pos=p+1-kseq_prime_len;
                }
                score= singleStringScore(score, &subs, pos, map);
                maximum_score=(score > maximum_score )?score:maximum_score;
            }
        }
    }
    return maximum_score;
}
//sub-routines for the dangling sequence
inline double singleStringScore(double x, std::string* s, long p, std::unordered_map<std::string, std::vector<double> > *m){
    for(int j=0; j<s->size(); j++){
        std::string letter=s->substr(j,1);
        if(letter == "-")
            continue;
        x+= (*m)[letter][p+j];
    }
    return x;
}
//sub-routines for the paired sequence
inline double pairStringScore(double x, std::string * s, long sp, std::unordered_map<std::string,std::vector<double> > *m){
    long subs_len=s->size();
    long diLen=(*m)["AA"].size();
    int half_subs_len;
    if(subs_len %2==0)
        half_subs_len=0.5* subs_len ;
    else
        half_subs_len=0.5* (subs_len-1) ;

    for (int j=0; j <= half_subs_len -1 ; j++){
        std::string letterA=s->substr(j,1);
        std::string letterB=s->substr(subs_len-j-1,1);
        std::string letter;

        if(letterA =="-" && letterB=="-")
            continue;
        else if(letterA =="-" ||letterB=="-"){
            long spos;
            if(letterB=="-"){
                spos=sp +j ;
                letter=letterA;
            }//#letterA is not gap so use the postion from the subs' beginning
            else if(letterA=="-"){
                spos=sp+(subs_len-1)-j; //#B is not gap, so from the end of subs
                letter=letterB;
            }
            x+=(*m)[letter][spos];
        }
        else{
            letter=letterA+letterB;
//            std::cout << *s << "\t" << letter  << "\t" << diLen-half_subs_len+j<< std::endl;
            x+=(*m)[letter][diLen-half_subs_len+j];
        }
    }
    return x;
}


/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef EXTENDEDBIDOMAINTISSUE_HPP_
#define EXTENDEDBIDOMAINTISSUE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include <vector>
#include "UblasMatrixInclude.hpp"

#include "AbstractStimulusFunction.hpp"
#include "AbstractStimulusFactory.hpp"
#include "AbstractConductivityTensors.hpp"

#include "AbstractCardiacTissue.hpp"

/**
 * Class that provides functionalities to specify a tissue within the context of the extended bidomain framework.
 *
 * The extended bidomain equations are of the form:
 *
 *  - div ( sigma_i1 grad Phi_1 ) + Am1*Cm1*d(phi_1)/dt - Am2*Cm2*d(phi_e)/dt + Am1*I_ion1 - Am1*I_stim1 + Amgap*G_gap*(phi_1 - phi_2)
 *  - div ( sigma_i2 grad Phi_2 ) + Am2*Cm2*d(phi_2)/dt - Am2*Cm2*d(phi_e)/dt + Am2*I_ion2 - Am2*I_stim2 - Amgap*G_gap*(phi_1 - phi_2)
 *   div ( sigma_e grad Phi_e ) + div ( sigma_i1 grad Phi_1 ) + div ( sigma_i2 grad Phi_2 )  = I_stim
 *
 *   The unknowns are:
 *
 *   - Phi_1 (intracellular potential of the first cell).
 *   - Phi_2 (intracellular potential of the second cell).
 *   - Phi_e (extracellular potential).
 *
 *   Am1, Am2 and Amgap are surface-to-volume ratios for first cell, second cell and gap junction. User can set their values.
 *   Cm1 and cm2 are capaciatnce values of first and second cell respectively
 *   sigma_i1 and sigma_i2 are intracellular conductivity tensors of first and second cell respectively
 *   sigma_e is the conductivity tensor for the extracellular space
 *   G_gap is the conductance (in ms/cm2) of the gap junction channel.
 *
 *
 *
 */
template <unsigned SPACE_DIM>
class ExtendedBidomainTissue : public virtual AbstractCardiacTissue<SPACE_DIM>
{
private:
    friend class TestExtendedBidomainTissue; // for testing.

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the member variables.
     *
     * @param archive
     * @param version
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractCardiacTissue<SPACE_DIM> >(*this);
        // Conductivity tensors are dealt with by HeartConfig, and the caches get regenerated.

        archive & mAmFirstCell;
        archive & mAmSecondCell;
        archive & mAmGap;
        archive & mCmFirstCell;
        archive & mCmSecondCell;
        archive & mGGap;
        archive & mUserSuppliedExtracellularStimulus;
    }

    /** Intracellular conductivity tensors for the second cell.*/
    AbstractConductivityTensors<SPACE_DIM, SPACE_DIM> *mpIntracellularConductivityTensorsSecondCell;

    /**
     * Stores the values of the conductivities for the second cell. Accessible via get and set methods. The problem class will set it
     * This variable is a convenient interface for other classes. It is used to fill in mpIntracellularConductivityTensorsSecondCell.
     */
    c_vector<double, SPACE_DIM>  mIntracellularConductivitiesSecondCell;

    /** Extracellular conductivity tensors. */
    AbstractConductivityTensors<SPACE_DIM, SPACE_DIM> *mpExtracellularConductivityTensors;

    /**
     *  Cache containing all the stimulus currents for each node,
     *  replicated over all processes.
     */
    ReplicatableVector mExtracellularStimulusCacheReplicated;

    /**
     *  Cache containing all the stimulus currents for each node,
     *  replicated over all processes.
     */
    ReplicatableVector mGgapCacheReplicated;

    /**
     *  Cache containing all the ionic currents for each node for the seconed cell,
     *  replicated over all processes.
     */
    ReplicatableVector mIionicCacheReplicatedSecondCell;

    /**
     *  Cache containing all the stimulus currents for each node for the second cell,
     *  replicated over all processes.
     */
    ReplicatableVector mIntracellularStimulusCacheReplicatedSecondCell;

    /** The vector of cells (the second one). Distributed. */
    std::vector< AbstractCardiacCellInterface* > mCellsDistributedSecondCell;

    /** The vector of stimuli for the extracellular stimulus. Distributed. */
    std::vector<boost::shared_ptr<AbstractStimulusFunction> > mExtracellularStimuliDistributed;

    /** The vector of gap junction conductances. Distributed*/
    std::vector<double> mGgapDistributed;

    /**the Am for the first cell, set by the problem class and picked up by the assembler*/
    double mAmFirstCell;
    /**the Am for the second cell, set by the problem class and picked up by the assembler*/
    double mAmSecondCell;
    /**the Am for the gap junction, set by the problem class and picked up by the assembler*/
    double mAmGap;
    /**the Cm for the first cell, set by the problem class and picked up by the assembler*/
    double mCmFirstCell;
    /**the Cm for the second cell, set by the problem class and picked up by the assembler*/
    double mCmSecondCell;
    /**the conductance of the gap junction, in mS/cm2. Set by the problem class and picked up by the assembler*/
    double mGGap;

    /**
     * Whether the extracellular stimulus that is passed in was supplied by the user or not
     * (it could be the default zero implementation). Initialise to false (user did not pass in anything).
     */
    bool mUserSuppliedExtracellularStimulus;

    /**
     * Convenience method for extracellular conductivity tensors creation
     */
    void CreateExtracellularConductivityTensors();

    /**
     * The parent class AbstractCardiacTissue has a method UpdateCaches that updates some caches of general use.
     * This method updates more caches that are specific to extended bidomain problems, namely:
     *
     * - Iionic and intracellular stimulus for the second cell
     * - Extracellular stimulus
     * - Gap junction conductivities (Ggap)
     *
     * It is typically called right after the UpdateCaches method in the parent class.
     *
     * @param globalIndex  global index of the entry to update
     * @param localIndex  local index of the entry to update
     * @param nextTime  the next PDE time point, at which to evaluate the stimulus current
     */
    void UpdateAdditionalCaches(unsigned globalIndex, unsigned localIndex, double nextTime);

    /**
     * The parent class AbstractCardiacTissue has a method ReplicateCaches that replicates some caches of general use.
     * This method replicates more caches that are specific to extended bidomain problems, namely:
     *
     * - Iionic and intracellular stimulus for the second cell
     * - Extracellular stimulus
     * - Gap junction conductivities (Ggap)
     *
     * It is typically called right after the ReplicateCaches method in the parent class.
     */
    void ReplicateAdditionalCaches();

    /** vector of regions for Ggap heterogeneities*/
    std::vector<boost::shared_ptr<AbstractChasteRegion<SPACE_DIM> > > mGgapHeterogeneityRegions;
    /**values of heterogeneous Ggaps corresponding to mGgapHeterogeneityRegions. This has the same size as mGgapHeterogeneityRegions*/
    std::vector<double> mGgapValues;

public:

    /**
     * Constructor sets up extracellular conductivity tensors.
     * @param pCellFactory factory to pass on to the base class constructor
     * @param pCellFactorySecondCell factory to pass on to the base class constructor for the second cell
     * @param pExtracellularStimulusFactory factory for creating extracellular stimuli
     */
    ExtendedBidomainTissue(AbstractCardiacCellFactory<SPACE_DIM>* pCellFactory, AbstractCardiacCellFactory<SPACE_DIM>* pCellFactorySecondCell, AbstractStimulusFactory<SPACE_DIM>* pExtracellularStimulusFactory);

    /**
     *  Archiving constructor
     * @param rCellsDistributed  local cell models (recovered from archive)
     * @param rSecondCellsDistributed  local cell models for second cells (recovered from archive)
     * @param rExtraStimuliDistributed local extracellular stimuli (recovered from archive)
     * @param rGgapsDistributed distributed Ggaps (recovered from archive)
     * @param pMesh  a pointer to the AbstractTetrahedral mesh (recovered from archive).
     * @param intracellularConductivitiesSecondCell a vector with the orthotropic conductivities for the second cell (this is needed because the second cell values may not be taken from HeartConfig as the the ones for the first cell are).
     */
    ExtendedBidomainTissue(std::vector<AbstractCardiacCellInterface*> & rCellsDistributed,
                           std::vector<AbstractCardiacCellInterface*> & rSecondCellsDistributed,
                           std::vector<boost::shared_ptr<AbstractStimulusFunction> > & rExtraStimuliDistributed,
                           std::vector<double>& rGgapsDistributed,
                           AbstractTetrahedralMesh<SPACE_DIM,SPACE_DIM>* pMesh,
                           c_vector<double, SPACE_DIM>  intracellularConductivitiesSecondCell);

    /**
     * Destructor
     */
    virtual ~ExtendedBidomainTissue();

    /**
     * Sets the value of the conductivities for the second cell.
     *
     * @param conductivities the conductivities to be set.
     */
    void SetIntracellularConductivitiesSecondCell(c_vector<double, SPACE_DIM> conductivities);

    /**
     * @return a pointer to the second cell
     *
     * @param globalIndex the global index in the mesh
     */
    AbstractCardiacCellInterface* GetCardiacSecondCell( unsigned globalIndex );

    /**
     * @return a pointer to the extracellular stimulus. Useful for testing
     *
     * @param globalIndex the global index in the mesh
     */
    boost::shared_ptr<AbstractStimulusFunction> GetExtracellularStimulus( unsigned globalIndex );

    /**
     *  @return a reference to the vector of distributed cells (second cell). Needed for archiving.
     */
    const std::vector<AbstractCardiacCellInterface*>& rGetSecondCellsDistributed() const;

    /**
     *  @return a reference to the vector of distributed values of Ggaps. Needed for archiving.
     */
    const std::vector<double>& rGetGapsDistributed() const;

    /**
     *  @return a reference to the vector of distributed extracellular stimuli. Needed for archiving.
     */
    const std::vector<boost::shared_ptr<AbstractStimulusFunction> >& rGetExtracellularStimulusDistributed() const;

    /**
     * @return the intracellular conductivities of the second cell
     */
    c_vector<double, SPACE_DIM> GetIntracellularConductivitiesSecondCell() const;

    /**
     * Integrate the cell ODEs and update ionic current etc for each of the
     * cells, between the two times provided. This is a re-implementation from the version in the base class.
     *
     * @param existingSolution  the current voltage solution vector
     * @param time  the current simulation time
     * @param nextTime  when to simulate the cells until
     * @param updateVoltage (unused here)
     */
    virtual void SolveCellSystems(Vec existingSolution, double time, double nextTime, bool updateVoltage = false);

    /**
     * Convenience method for intracellular conductivity tensors creation for the second cell
     */
    void CreateIntracellularConductivityTensorSecondCell();

    /**
     *  Set the values of mCellHeterogeneityRegions and mGgapValues for the heterogeneities of Ggap.
     *
     *  @param rGgapHeterogeneityRegions a vector of (pointers to) heterogeneity regions for gap junctions
     *  @param rGgapValues a vector (of the same size as rGgapHeterogeneityRegions) with the respective values of Ggap for every region.
     */
    void SetGgapHeterogeneities ( std::vector<boost::shared_ptr<AbstractChasteRegion<SPACE_DIM> > > & rGgapHeterogeneityRegions, std::vector<double> rGgapValues);

    /**
     * Create the pattern of Ggap across the mesh based upon mCellHeterogeneityRegions, mGgapValues and mGgap. This will fill in mGgapDistributed.
     * It will set mGgap everywhere except in  the areas mCellHeterogeneityRegions[i] where it will put mGgapValues[i] instead.
     * If mCellHeterogeneityRegions (and mGgapValues) are empty, mGgap will be set everywhere.
     */
    void CreateGGapConductivities();

    /**
     * @return the extracellular conductivity tensor for the given element
     * @param elementIndex  index of the element of interest
     */
     const c_matrix<double, SPACE_DIM, SPACE_DIM>& rGetExtracellularConductivityTensor(unsigned elementIndex);

     /**
      * @return the intracellular conductivity tensor for the given element for tehs econd cell
      * @param elementIndex  index of the element of interest
      */
      const c_matrix<double, SPACE_DIM, SPACE_DIM>& rGetIntracellularConductivityTensorSecondCell(unsigned elementIndex);

     /** @return the entire ionic current cache for the second cell*/
     ReplicatableVector& rGetIionicCacheReplicatedSecondCell();

     /** @return the entire stimulus current cache for the second cell*/
     ReplicatableVector& rGetIntracellularStimulusCacheReplicatedSecondCell();

     /** @return the extracellular stimulus*/
     ReplicatableVector& rGetExtracellularStimulusCacheReplicated();

     /** @return the values of ggap*/
     ReplicatableVector& rGetGgapCacheReplicated();

     /**
      * @return Am for the first cell
      */
     double GetAmFirstCell();

     /**
      * @return Am for the second cell
      */
     double GetAmSecondCell();

     /**
      * @return Am for the gap junction
      */
     double GetAmGap();

     /**
      * @return Cm for the first cell
      */
     double GetCmFirstCell();

     /**
      * @return Cm for the second cell
      */
     double GetCmSecondCell();

     /**
      *  @return the conducatnce of the gap junction (mGGap)
      */
     double GetGGap();

     /**
      * @param value Am for the first cell
      */
     void SetAmFirstCell(double value);

     /**
      * @param value Am for the second cell
      */
     void SetAmSecondCell(double value);

     /**
      * @param value Am for the gap junction
      */
     void SetAmGap(double value);

     /**
      * @param value Cm for the first cell
      */
     void SetCmFirstCell(double value);

     /**
      * @param value Cm for the first cell
      */
     void SetCmSecondCell(double value);

     /**
      * @param value conductance, in mS of the gap junction
      */
     void SetGGap(double value);

     /**
      * This method gives access to the member variable mUserSuppliedExtracellularStimulus,
      * which is false by default but turned true if the user supplies an extracellular stimulus
      * in any form.
      *
      * @return true if the user supplied an extracellular stimulus.
      */
     bool HasTheUserSuppliedExtracellularStimulus();

     /**
      * This method allows modifications of the  mUserSuppliedExtracellularStimulus flag (false by default).
      * Other classes (e.g., Problem classes) can use this method to tell the Tissue that the user
      * specified an extracellular stimulus.
      *
      * @param flag ; true if you want to tell the Tissue object that the user supplied an extracellular stimulus explicitly
      */
     void SetUserSuppliedExtracellularStimulus(bool flag);

     /**
      * This method is the equivalent of SaveCardiacCells in the abstract class but save both cells of the extended bidomain tissue
      *
      * @param archive  the master archive; cells will actually be written to the process-specific archive.
      * @param version
      */
     template<class Archive>
     void SaveExtendedBidomainCells(Archive & archive, const unsigned int version) const
     {
         Archive& r_archive = *ProcessSpecificArchive<Archive>::Get();
         const std::vector<AbstractCardiacCellInterface*> & r_cells_distributed = this->rGetCellsDistributed();
         const std::vector<AbstractCardiacCellInterface*> & r_cells_distributed_second_cell = rGetSecondCellsDistributed();
         const std::vector<double> & r_ggaps_distributed = rGetGapsDistributed();

         r_archive & this->mpDistributedVectorFactory; // Needed when loading
         const unsigned num_cells = r_cells_distributed.size();
         r_archive & num_cells;
         for (unsigned i=0; i<num_cells; i++)
         {
             AbstractDynamicallyLoadableEntity* p_entity = dynamic_cast<AbstractDynamicallyLoadableEntity*>(r_cells_distributed[i]);
             bool is_dynamic = (p_entity != NULL);
             r_archive & is_dynamic;
             if (is_dynamic)
             {
 #ifdef CHASTE_CAN_CHECKPOINT_DLLS
                 ///\todo Dynamically loaded cell models aren't saved to archive in extended Bidomain
                 NEVER_REACHED;
                 //r_archive & p_entity->GetLoader()->GetLoadableModulePath();
 #else
                 // We should have thrown an exception before this point
                 NEVER_REACHED;
 #endif // CHASTE_CAN_CHECKPOINT_DLLS
             }
             r_archive & r_cells_distributed[i];
             r_archive & r_cells_distributed_second_cell[i];
             r_archive & r_ggaps_distributed[i];
         }
     }

     /**
      * Load our tissue from an archive. This is the equivalent of LoadCardiacCells in the abstract class.
      * it loads the two cells instead of only one.
      *
      * Handles the checkpoint migration case, deleting loaded cells immediately if they are
      * not local to this process.
      *
      * @param archive  the process-specific archive to load from
      * @param version  archive version
      * @param rCells  vector to fill in with pointers to local cells
      * @param rSecondCells vector to fill in with pointers to the second cells
      * @param rGgaps vector of values of gap junctions
      * @param pMesh  the mesh, so we can get at the node permutation, if any
      */
     template<class Archive>
     static void LoadExtendedBidomainCells(Archive & archive, const unsigned int version,
                                  std::vector<AbstractCardiacCellInterface*>& rCells,
                                  std::vector<AbstractCardiacCellInterface*>& rSecondCells,
                                  std::vector<double>& rGgaps,
                                  AbstractTetrahedralMesh<SPACE_DIM,SPACE_DIM>* pMesh)
     {
         assert(pMesh!=NULL);
         DistributedVectorFactory* p_factory;
         archive & p_factory;
         unsigned num_cells;
         archive & num_cells;
         rCells.resize(p_factory->GetLocalOwnership());
         rSecondCells.resize(p_factory->GetLocalOwnership());
         rGgaps.resize(p_factory->GetLocalOwnership());
 #ifndef NDEBUG
         // Paranoia
         assert(rCells.size() == rSecondCells.size());
         for (unsigned i=0; i<rCells.size(); i++)
         {
             assert(rCells[i] == NULL);
             assert(rSecondCells[i] == NULL);
         }
 #endif

         // We don't store a cell index in the archive, so need to work out what global
         // index this tissue starts up.  If we're migrating (so have an
         // original factory) we use the original low index; otherwise we use the current
         // low index.
         unsigned index_low = p_factory->GetOriginalFactory() ? p_factory->GetOriginalFactory()->GetLow() : p_factory->GetLow();

         for (unsigned local_index=0; local_index<num_cells; local_index++)
         {
             unsigned global_index = index_low + local_index;
             unsigned new_local_index = global_index - p_factory->GetLow();
             bool local = p_factory->IsGlobalIndexLocal(global_index);

             bool is_dynamic;
             archive & is_dynamic;

             if (is_dynamic)
             {
 #ifdef CHASTE_CAN_CHECKPOINT_DLLS
                 ///\todo Dynamically loaded cell models aren't loaded from archive in extended Bidomain
                 NEVER_REACHED;
                 // Ensure the shared object file for this cell model is loaded.
                 // We need to do this here, rather than in the class' serialization code,
                 // because that code won't be available until this is done...
//                 std::string shared_object_path;
//                 archive & shared_object_path;
//                 DynamicModelLoaderRegistry::Instance()->GetLoader(shared_object_path);
 #else
                 // Could only happen on Mac OS X, and will probably be trapped earlier.
                 NEVER_REACHED;
 #endif // CHASTE_CAN_CHECKPOINT_DLLS
             }

             AbstractCardiacCellInterface* p_cell;
             AbstractCardiacCellInterface* p_second_cell;
             double g_gap;
             archive & p_cell;
             archive & p_second_cell;
             archive & g_gap;
             if (local)
             {
                 rCells[new_local_index] = p_cell; // Add to local cells
                 rSecondCells[new_local_index] = p_second_cell;
                 rGgaps[new_local_index] = g_gap;
             }
             else
             {
                 //not sure how to cover this, we are already looping over local cells...
                 NEVER_REACHED;
                // Non-local real cell, so free the memory.
                // delete p_cell;
                // delete p_second_cell;
             }
         }
     }

     /**
      * This method is the equivalent of SaveCardiacCells but Saves the extracellular stimulus instead
      *
      * @param archive  the master archive; cells will actually be written to the process-specific archive.
      * @param version
      */
     template<class Archive>
     void SaveExtracellularStimulus(Archive & archive, const unsigned int version) const
     {
         Archive& r_archive = *ProcessSpecificArchive<Archive>::Get();
         const std::vector<boost::shared_ptr<AbstractStimulusFunction> > & r_stimulus_distributed = rGetExtracellularStimulusDistributed();
         r_archive & this->mpDistributedVectorFactory; // Needed when loading
         const unsigned num_cells = r_stimulus_distributed.size();
         r_archive & num_cells;
         for (unsigned i=0; i<num_cells; i++)
         {
             r_archive & r_stimulus_distributed[i];
         }
     }

     /**
      * This method is the equivalent of LoadCardiacCells but Load the extracellular stimulus instead
      *
      * @param archive  the master archive; cells will actually be written to the process-specific archive.
      * @param version
      * @param rStimuli the extracellular stimuli (will be filled from the archive).
      * @param pMesh the mesh (needed to work out number of nodes). Here it is assumed we have already unarchived the mesh somewhere and the pointer passed in is not NULL.
      */
     template<class Archive>
     void LoadExtracellularStimulus(Archive & archive, const unsigned int version,
                                               std::vector<boost::shared_ptr<AbstractStimulusFunction> >& rStimuli,
                                               AbstractTetrahedralMesh<SPACE_DIM,SPACE_DIM>* pMesh)
     {

        DistributedVectorFactory* p_factory;
        archive & p_factory;
        unsigned num_cells;
        archive & num_cells;
        rStimuli.resize(p_factory->GetLocalOwnership());
#ifndef NDEBUG
          // Paranoia
          for (unsigned i=0; i<rStimuli.size(); i++)
          {
              assert(rStimuli[i] == NULL);
          }
#endif

        // We don't store a cell index in the archive, so need to work out what global
        // index this tissue starts up.  If we're migrating (so have an
        // original factory) we use the original low index; otherwise we use the current
        // low index.
        unsigned index_low = p_factory->GetOriginalFactory() ? p_factory->GetOriginalFactory()->GetLow() : p_factory->GetLow();

        assert(pMesh!=NULL);
        //unsigned num_cells = pMesh->GetNumNodes();
        for (unsigned local_index=0; local_index<num_cells; local_index++)
        {
          unsigned global_index = index_low + local_index;

          unsigned new_local_index = global_index - p_factory->GetLow();
          bool local = p_factory->IsGlobalIndexLocal(global_index);

          boost::shared_ptr<AbstractStimulusFunction> p_stim;
          archive & p_stim;//get from archive

          if (local)
          {
              rStimuli[new_local_index] = p_stim; // Add stimulus to local cells
          }
          //otherwise we should delete, but I think shared pointers delete themselves?
        }
    }
};

 // Declare identifier for the serializer
 #include "SerializationExportWrapper.hpp"
 EXPORT_TEMPLATE_CLASS_SAME_DIMS(ExtendedBidomainTissue)

 namespace boost
 {
 namespace serialization
 {

 template<class Archive, unsigned SPACE_DIM>
 inline void save_construct_data(
     Archive & ar, const ExtendedBidomainTissue<SPACE_DIM> * t, const unsigned int file_version)
 {
     //archive the conductivity tensor of the second cell (which may not be dealt with by heartconfig)
     c_vector<double, SPACE_DIM>  intracellular_conductivities_second_cell = t->GetIntracellularConductivitiesSecondCell();
     //note that simple: ar & intracellular_conductivities_second_cell may not be liked by some boost versions
     for (unsigned i = 0; i < SPACE_DIM; i++)
     {
         ar & intracellular_conductivities_second_cell(i);
     }

     const AbstractTetrahedralMesh<SPACE_DIM,SPACE_DIM>* p_mesh = t->pGetMesh();
     ar & p_mesh;

     // Don't use the std::vector serialization for cardiac cells, so that we can load them
     // more cleverly when migrating checkpoints.
     t->SaveExtendedBidomainCells(ar, file_version);
     t->SaveExtracellularStimulus(ar, file_version);

     // Creation of conductivity tensors are called by constructor and uses HeartConfig. So make sure that it is
     // archived too (needs doing before construction so appears here instead of usual archive location).
     HeartConfig* p_config = HeartConfig::Instance();
     ar & *p_config;
     ar & p_config;
 }

 /**
  * Allow us to not need a default constructor, by specifying how Boost should
  * instantiate an instance (using existing constructor)
  */
 template<class Archive, unsigned SPACE_DIM>
 inline void load_construct_data(
     Archive & ar, ExtendedBidomainTissue<SPACE_DIM> * t, const unsigned int file_version)
 {
     //Load conductivities of the conductivity of the second cell.
     c_vector<double, SPACE_DIM>  intra_cond_second_cell;
     //note that simple: ar & intra_cond_second_cell may not be liked by some boost versions
     for (unsigned i = 0; i < SPACE_DIM; i++)
     {
         double cond;
         ar & cond;
         intra_cond_second_cell(i) = cond;
     }

     std::vector<AbstractCardiacCellInterface*> cells_distributed;
     std::vector<AbstractCardiacCellInterface*> cells_distributed_second_cell;
     std::vector<boost::shared_ptr<AbstractStimulusFunction> > extra_stim;
     std::vector<double> g_gaps;
     AbstractTetrahedralMesh<SPACE_DIM,SPACE_DIM>* p_mesh;
     ar & p_mesh;

     // Load only the cells we actually own
     t->LoadExtendedBidomainCells(
             *ProcessSpecificArchive<Archive>::Get(), file_version, cells_distributed, cells_distributed_second_cell, g_gaps, p_mesh);

     t->LoadExtracellularStimulus(
             *ProcessSpecificArchive<Archive>::Get(), file_version, extra_stim, p_mesh);

     // CreateIntracellularConductivityTensor() is called by AbstractCardiacTissue constructor and uses HeartConfig.
     // (as does CreateExtracellularConductivityTensor). So make sure that it is
     // archived too (needs doing before construction so appears here instead of usual archive location).
     HeartConfig* p_config = HeartConfig::Instance();
     ar & *p_config;
     ar & p_config;

     ::new(t)ExtendedBidomainTissue<SPACE_DIM>(cells_distributed, cells_distributed_second_cell, extra_stim, g_gaps, p_mesh, intra_cond_second_cell);
 }
 }
 } // namespace ...

#endif /*EXTENDEDBIDOMAINTISSUE_HPP_*/


/**************************************************************************\
 *
 *  This file is part of the Coin 3D visualization library.
 *  Copyright (C) by Kongsberg Oil & Gas Technologies.
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  ("GPL") version 2 as published by the Free Software Foundation.
 *  See the file LICENSE.GPL at the root directory of this source
 *  distribution for additional information about the GNU GPL.
 *
 *  For using Coin with software that can not be combined with the GNU
 *  GPL, and for taking advantage of the additional benefits of our
 *  support services, please contact Kongsberg Oil & Gas Technologies
 *  about acquiring a Coin Professional Edition License.
 *
 *  See http://www.coin3d.org/ for more information.
 *
 *  Kongsberg Oil & Gas Technologies, Bygdoy Alle 5, 0257 Oslo, NORWAY.
 *  http://www.sim.no/  sales@sim.no  coin-support@coin3d.org
 *
\**************************************************************************/

#include <Inventor/engines/SoHeightMapToNormalMap.h>

#include <boost/scoped_array.hpp>

#include <Inventor/SbVec3f.h>
#include <Inventor/SbImage.h>
#include "engines/SoSubEngineP.h"

/*!
  \class SoHeightMapToNormalMap SoHeightMapToNormalMap.h Inventor/engines/SoHeightMapToNormalMap.h
  \brief Engine for computing a normal map from a height map.

  This engine will create a normal map texture from a height map texture.
  You can use it in an Inventor file like this:

  \code
  Texture2 {
    image = HeightMapToNormalMap {
      sourceImage = Texture2 { filename "HeightMap.jpg" } . image
    } . image
  }
  \endcode

  Be aware that the field connections will remain active, so both
  Texture2 nodes and the HeightMapToNormalMap engine will be kept resident
  in memory (unless you intervene manually and detach the engine) even
  though only the "outer" Texture2 node is needed. This can give quite
  a big memory use overhead.

  \ingroup engines
  \COIN_CLASS_EXTENSION
  \since Coin 3.0
*/

/*!
  \enum SoHeightMapToNormalMap::NormalMapFormat
  Enumeration of available normal map formats.
*/

/*!
  \var SoHeightMapToNormalMap::NormalMapFormat SoHeightMapToNormalMap::INT8
  Encode the normals as a 3 component byte texture.
  This is the only option for now, as long as float textures are not conveniently
  supported in Coin.
*/

/*!
  \var SoMFEnum SoHeightMapToNormalMap::format
  This setting decides what kind of normal map is generated.  For now, only the
  INT8 format is available, and it is the default value.
*/

SO_ENGINE_SOURCE(SoHeightMapToNormalMap);

/*!
  Class initializer.
*/
void
SoHeightMapToNormalMap::initClass(void)
{
  SO_ENGINE_INTERNAL_INIT_CLASS(SoHeightMapToNormalMap);
}

/*!
  Constructor.
*/
SoHeightMapToNormalMap::SoHeightMapToNormalMap(void)
{
  SO_ENGINE_INTERNAL_CONSTRUCTOR(SoHeightMapToNormalMap);

  SO_ENGINE_ADD_INPUT(format, (INT8));

  SO_ENGINE_DEFINE_ENUM_VALUE(NormalMapFormat, INT8);
  SO_ENGINE_SET_SF_ENUM_TYPE(format, NormalMapFormat);
}

/*!
  Static function for computing a normal map from a height map.
  This function can be used directly without any engine instantiation.
*/
void
SoHeightMapToNormalMap::convert(const unsigned char * srcptr, SbVec2s size, int nc, SbImage & dst_out)
{
  float dx, dy;
  int width = size[0];
  int height = size[1];
  boost::scoped_array<unsigned char> dstarray(new unsigned char[width*height*3]);
  unsigned char * dstptr = dstarray.get();
  unsigned char red;
  SbVec3f n;

#define GET_PIXEL_RED(x_, y_) \
  srcptr[(y_)*width*nc + (x_)*nc]

  for (int y = 0; y < height; y++) {
    for (int x = 0; x < width; x++) {
      // do Y Sobel filter
      red = GET_PIXEL_RED((x-1+width) % width, (y+1) % height);
      dy  = static_cast<float>(red) / 255.0f * -1.0f;

      red = GET_PIXEL_RED(x % width, (y+1) % height);
      dy += static_cast<float>(red) / 255.0f * -2.0f;

      red = GET_PIXEL_RED((x+1) % width, (y+1) % height);
      dy += static_cast<float>(red) / 255.0f * -1.0f;

      red = GET_PIXEL_RED((x-1+width) % width, (y-1+height) % height);
      dy += static_cast<float>(red) / 255.0f *  1.0f;

      red = GET_PIXEL_RED(x % width, (y-1+height) % height);
      dy += static_cast<float>(red) / 255.0f *  2.0f;

      red = GET_PIXEL_RED((x+1) % width, (y-1+height) % height);
      dy += static_cast<float>(red) / 255.0f *  1.0f;

      // Do X Sobel filter
      red = GET_PIXEL_RED((x-1+width) % width, (y-1+height) % height);
      dx  = static_cast<float>(red) / 255.0f * -1.0f;

      red = GET_PIXEL_RED((x-1+width) % width, y % height);
      dx += static_cast<float>(red) / 255.0f * -2.0f;

      red = GET_PIXEL_RED((x-1+width) % width, (y+1) % height);
      dx += static_cast<float>(red) / 255.0f * -1.0f;

      red = GET_PIXEL_RED((x+1) % width, (y-1+height) % height);
      dx += static_cast<float>(red) / 255.0f *  1.0f;

      red = GET_PIXEL_RED((x+1) % width, y % height);
      dx += static_cast<float>(red) / 255.0f *  2.0f;

      red = GET_PIXEL_RED((x+1) % width, (y+1) % height);
      dx += static_cast<float>(red) / 255.0f *  1.0f;

      n[0] = -dx;
      n[1] = -dy;
      n[2] = 1.0f;
      (void) n.normalize();

      *dstptr++ = static_cast<unsigned char>(SbMin((n[0]+1.0f) * 128.0f, 255.0f));
      *dstptr++ = static_cast<unsigned char>(SbMin((n[1]+1.0f) * 128.0f, 255.0f));
      *dstptr++ = static_cast<unsigned char>(SbMin((n[2]+1.0f) * 128.0f, 255.0f));
    }
  }
#undef GET_PIXEL_RED
  dst_out.setValue(size, 3, dstarray.get());
}

void
SoHeightMapToNormalMap::inputChanged(SoField * which)
{
  // in case we need to override later
  inherited::inputChanged(which);
}

void
SoHeightMapToNormalMap::evaluate(void)
{
  SbVec2s size;
  int nc;
  const unsigned char * ptr =
    static_cast<const unsigned char *>(sourceImage.getValue(size, nc));

  SbImage targetimg;
  SoHeightMapToNormalMap::convert(ptr, size, nc, targetimg);

  ptr = static_cast<const unsigned char *>(targetimg.getValue(size, nc));
  SO_ENGINE_OUTPUT(image, SoSFImage, setValue(size, nc, ptr));
}


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/restrict.hpp>

#include "dbglog/dbglog.hpp"

#include "utility/expect.hpp"
#include "utility/binaryio.hpp"

#include "math/math.hpp"
#include "math/geometry.hpp"
#include "math/transform.hpp"

#include "imgproc/scanconversion.hpp"

#include "half/half.hpp"

#include "../storage/error.hpp"

#include "mesh.hpp"
#include "meshio.hpp"
#include "multifile.hpp"
#include "math.hpp"
#include "tileindex.hpp"

namespace fs = boost::filesystem;
namespace bio = boost::iostreams;
namespace bin = utility::binaryio;

namespace half = half_float::detail;

namespace vtslibs { namespace vts {

namespace {

/** Geo coordinates to coverage mask mapping.
 * NB: result is in pixel system: pixel centers have integral indices
 */
math::Matrix4 geo2mask(const math::Extents2 &extents
                              , const math::Size2 &gridSize)
{
    math::Matrix4 trafo(boost::numeric::ublas::identity_matrix<double>(4));

    auto es(size(extents));

    // scales
    math::Size2f scale(gridSize.width / es.width
                       , gridSize.height / es.height);

    // scale to grid
    trafo(0, 0) = scale.width;
    trafo(1, 1) = -scale.height;

    // move to origin
    trafo(0, 3) = -extents.ll(0) * scale.width;
    trafo(1, 3) = extents.ur(1) * scale.height;

    return trafo;
}

void updateCoverage(Mesh::CoverageMask &cm, const SubMesh &sm
                    , const math::Extents2 &sdsExtents
                    , std::uint8_t smIndex)
{
    const auto rasterSize(cm.size());
    auto trafo(geo2mask(sdsExtents, rasterSize));

    std::vector<imgproc::Scanline> scanlines;
    cv::Point3f tri[3];
    for (const auto &face : sm.faces) {
        for (int i : { 0, 1, 2 }) {
            auto p(transform(trafo, sm.vertices[face[i]]));
            tri[i].x = p(0); tri[i].y = p(1); tri[i].z = p(2);
        }

        scanlines.clear();
        imgproc::scanConvertTriangle(tri, 0, rasterSize.height, scanlines);

        for (const auto &sl : scanlines) {
            imgproc::processScanline
                (sl, 0, rasterSize.width, [&](int x, int y, float)
            {
                cm.set(x, y, smIndex + 1);
            });
        }
    }
}

} // namespace

void updateCoverage(Mesh &mesh, const SubMesh &sm
                    , const math::Extents2 &sdsExtents
                    , std::uint8_t smIndex)
{
    updateCoverage(mesh.coverageMask, sm, sdsExtents, smIndex);
}

void generateCoverage(Mesh &mesh, const math::Extents2 &sdsExtents)
{
    mesh.createCoverage(false);

    std::uint8_t smIndex(0);
    for (const auto &sm : mesh) {
        updateCoverage(mesh, sm, sdsExtents, smIndex++);
    }
}

void generateMeshMask(MeshMask &mask, const Mesh &mesh
                      , const math::Extents2 &sdsExtents)
{
    mask.createCoverage(false);
    mask.surfaceReferences.clear();

    std::uint8_t smIndex(0);
    for (const auto &sm : mesh) {
        updateCoverage(mask.coverageMask, sm, sdsExtents, smIndex++);
        mask.surfaceReferences.push_back(sm.surfaceReference);
    }
}

} } // namespace vtslibs::vts


#include <boost/algorithm/hex.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <nil/crypto3/zk/components/blueprint.hpp>
#include <nil/crypto3/zk/components/blueprint_variable.hpp>

#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark.hpp>
#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark/marshalling.hpp>

#include <nil/crypto3/zk/snark/algorithms/generate.hpp>
#include <nil/crypto3/zk/snark/algorithms/verify.hpp>
#include <nil/crypto3/zk/snark/algorithms/prove.hpp>

#include "./comparerLogic.h"

using namespace std;

using namespace nil::crypto3::zk::components;
using namespace nil::crypto3::zk::snark;

ushort INVALID_PROOF_RETURN_CODE = 200;

std::string convert_byteblob_to_hex_string(std::vector<std::uint8_t> blob) {
    // convert byte_blob to hex string and print it to output
    std::string hex;
    hex.reserve(blob.size() * 2);
    boost::algorithm::hex(blob.begin(), blob.end(), back_inserter(hex));
    return hex;
}

void save_byteblob(std::vector<std::uint8_t> byteblob, boost::filesystem::path fname) {
    boost::filesystem::ofstream out(fname);
    for (const auto &v : byteblob) {
        out << v;
    }
    out.close();
}

std::vector<std::uint8_t> load_byteblob(boost::filesystem::path fname) {
    boost::filesystem::ifstream stream(fname, std::ios::in | std::ios::binary);
    std::vector<std::uint8_t> contents((std::istreambuf_iterator<char>(stream)), std::istreambuf_iterator<char>());
    if (contents.size() == 0) {
        throw std::ios_base::failure("Empty file");
    }
    return contents;
}

// proving key

void save_proving_key(scheme_type::proving_key_type pk, boost::filesystem::path fname) {
    std::vector<std::uint8_t> byteblob = nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(pk);
    save_byteblob(byteblob, fname);
}

scheme_type::proving_key_type load_proving_key(boost::filesystem::path fname) {
    std::vector<std::uint8_t> byteblob = load_byteblob(fname);
    nil::marshalling::status_type processingStatus = nil::marshalling::status_type::success;
    return nil::marshalling::verifier_input_deserializer_tvm<scheme_type>::proving_key_process(
        byteblob.cbegin(),
        byteblob.cend(),
        processingStatus);
}

// verification key

void save_verification_key(scheme_type::verification_key_type vk, boost::filesystem::path fname) {
    std::vector<std::uint8_t> byteblob = nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(vk);
    save_byteblob(byteblob, fname);
}

scheme_type::verification_key_type load_verification_key(boost::filesystem::path fname) {
    std::vector<std::uint8_t> byteblob = load_byteblob(fname);
    nil::marshalling::status_type processingStatus = nil::marshalling::status_type::success;
    return nil::marshalling::verifier_input_deserializer_tvm<scheme_type>::verification_key_process(
        byteblob.cbegin(),
        byteblob.cend(),
        processingStatus);
}

// proof

void save_proof(scheme_type::proof_type proof, boost::filesystem::path fname) {
    std::vector<std::uint8_t> byteblob = nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(proof);
    save_byteblob(byteblob, fname);
}

scheme_type::proof_type load_proof(boost::filesystem::path fname) {
    std::vector<std::uint8_t> byteblob = load_byteblob(fname);
    nil::marshalling::status_type processingStatus = nil::marshalling::status_type::success;
    return nil::marshalling::verifier_input_deserializer_tvm<scheme_type>::proof_process(
        byteblob.cbegin(),
        byteblob.cend(),
        processingStatus);
}

//  primary input

void save_primary_input(zk::snark::r1cs_primary_input<field_type> primary_input, boost::filesystem::path fname) {
    std::vector<std::uint8_t> byteblob = nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(primary_input);
    save_byteblob(byteblob, fname);
}

zk::snark::r1cs_primary_input<field_type> load_primary_input(boost::filesystem::path fname) {
    std::vector<std::uint8_t> byteblob = load_byteblob(fname);
    nil::marshalling::status_type processingStatus = nil::marshalling::status_type::success;
    return nil::marshalling::verifier_input_deserializer_tvm<scheme_type>::primary_input_process(
        byteblob.cbegin(),
        byteblob.cend(),
        processingStatus);
}

int setup_keys(boost::filesystem::path pk_path, boost::filesystem::path vk_path) {

    blueprint<field_type> bp;
    ComparerLogic comparerLogic(bp);
    comparerLogic.generate_r1cs_constraints(bp);

    cout << "Blueprint size: " << bp.num_variables() << endl;
    cout << "Generating constraint system..." << endl;
    const r1cs_constraint_system<field_type> constraint_system = bp.get_constraint_system();
    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;

    cout << "Generating keypair..." << endl;
    scheme_type::keypair_type keypair = generate<scheme_type>(constraint_system);

    cout << "Saving proving key to a file " << pk_path<< endl;
    save_proving_key(keypair.first, pk_path);

    cout << "Saving verification key to a file " << vk_path << endl;
    save_verification_key(keypair.second, vk_path);

    return 0;
}

int create_proof(boost::filesystem::path pk_path, boost::filesystem::path proof_path, boost::filesystem::path pi_path, int minYear, int maxYear, int year) {

    cout << "Loading proving key from a file " << pk_path << endl;
    typename scheme_type::proving_key_type pk = load_proving_key(pk_path);

    blueprint<field_type> bp;
    ComparerLogic comparerLogic(bp);

    cout << "Generating constraint system..." << endl;
    comparerLogic.generate_r1cs_constraints(bp);

    cout << "Generating witness..." << endl;
    comparerLogic.generate_r1cs_witness(bp, minYear, maxYear, year);

    cout << "Blueprint is satisfied: " << bp.is_satisfied() << endl;

    if (!bp.is_satisfied()) {
        return INVALID_PROOF_RETURN_CODE;
    }

    cout << "Generating proof..." << endl;
    const scheme_type::proof_type proof = prove<scheme_type>(pk, bp.primary_input(), bp.auxiliary_input());

    cout << "Saving proof to file " << proof_path << endl;
    save_proof(proof, proof_path);

    cout << "Saving primary input to file " << pi_path << endl;
    save_primary_input(bp.primary_input(), pi_path);
    return 0;
}

int verify_proof(boost::filesystem::path proof_path, boost::filesystem::path vk_path, boost::filesystem::path pi_path) {

    cout << "Loading proof from a file " << proof_path << endl;
    typename scheme_type::proof_type proof = load_proof(proof_path);

    cout << "Loading primary input from a file " << pi_path << endl;
    r1cs_primary_input<field_type> input = load_primary_input(pi_path);

    cout << "Loading verification key from a file " << vk_path << endl;
    typename scheme_type::verification_key_type vk = load_verification_key(vk_path);

    // verify
    using basic_proof_system = r1cs_gg_ppzksnark<curve_type>;
    const bool verified = verify<basic_proof_system>(vk, input, proof);
    cout << "Verification status: " << verified << endl;

    return verified ? 0 : INVALID_PROOF_RETURN_CODE;
}

int main(int argc, char *argv[]) {
    int maxYear, minYear, year;
    boost::filesystem::path pk_path, vk_path, proof_path, pi_path;
    // bool hexFlag;

    boost::program_options::options_description options("CLI Proof Generator");
    options.add_options()
    // ("hex,h", boost::program_options::bool_switch(&hexFlag), "print only hex proof to output")
    ("minYear,minYear", boost::program_options::value<int>(&minYear)->default_value(0))
    ("maxYear,maxYear", boost::program_options::value<int>(&maxYear)->default_value(100))
    ("year,year", boost::program_options::value<int>(&year)->default_value(18))
    ("proving-key-path,pk", boost::program_options::value<boost::filesystem::path>(&pk_path)->default_value("proving.key"))
    ("verification-key-path,vk", boost::program_options::value<boost::filesystem::path>(&vk_path)->default_value("verification.key"))
    ("proof-path,p", boost::program_options::value<boost::filesystem::path>(&proof_path)->default_value("proof"))
    ("primary-input-path,pi", boost::program_options::value<boost::filesystem::path>(&pi_path)->default_value("primary.input"));

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(options).run(), vm);
    boost::program_options::notify(vm);

    cout << setprecision(16);

    if (!argv[1]) {
        cout << "Please select a command: [setup/prove/verify]" << endl;
        return 0;
    }
    else if (string(argv[1]) == "setup") {
        // Generate proving.key & verification.key
        return setup_keys(pk_path, vk_path);
    } else if (string(argv[1]) == "prove") {
        return create_proof(pk_path, proof_path, pi_path, minYear, maxYear, year);
    } else if (string(argv[1]) == "verify") {
        return verify_proof(proof_path, pi_path, vk_path);
    }

    return 0;
}