// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Purpose: Implementation of the HEALPix and rHEALPix projections.
//          For background see <http://code.scenzgrid.org/index.php/p/scenzgrid-py/source/tree/master/docs/rhealpix_dggs.pdf>.
// Authors: Alex Raichev (raichev@cs.auckland.ac.nz)
//          Michael Speth (spethm@landcareresearch.co.nz)
// Notes:   Raichev implemented these projections in Python and
//          Speth translated them into C here.

// Copyright (c) 2001, Thomas Flemming, tf@ttqv.com

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_HEALPIX_HPP
#define BOOST_GEOMETRY_PROJECTIONS_HEALPIX_HPP

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_auth.hpp>
#include <boost/geometry/srs/projections/impl/pj_qsfn.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace healpix
    {

            /* Fuzz to handle rounding errors: */
            static const double epsilon = 1e-15;

            template <typename T>
            struct par_healpix
            {
                int north_square;
                int south_square;
                T qp;
                detail::apa<T> apa;
            };

            template <typename T>
            struct cap_map
            {
                int cn; /* An integer 0--3 indicating the position of the polar cap. */
                T x, y; /* Coordinates of the pole point (point of most extreme latitude on the polar caps). */
                enum region_type {north, south, equatorial} region;
            };
            template <typename T>
            struct point_xy
            {
                T x, y;
            };

            /* IDENT, R1, R2, R3, R1 inverse, R2 inverse, R3 inverse:*/
            static double rot[7][2][2] = {
                /* Identity matrix */
                {{1, 0},{0, 1}},
                /* Matrix for counterclockwise rotation by pi/2: */
                {{ 0,-1},{ 1, 0}},
                /* Matrix for counterclockwise rotation by pi: */
                {{-1, 0},{ 0,-1}},
                /* Matrix for counterclockwise rotation by 3*pi/2:  */
                {{ 0, 1},{-1, 0}},
                {{ 0, 1},{-1, 0}}, // 3*pi/2
                {{-1, 0},{ 0,-1}}, // pi
                {{ 0,-1},{ 1, 0}}  // pi/2
            };

            /**
             * Returns the sign of the double.
             * @param v the parameter whose sign is returned.
             * @return 1 for positive number, -1 for negative, and 0 for zero.
             **/
            template <typename T>
            inline T pj_sign (T const& v)
            {
                return v > 0 ? 1 : (v < 0 ? -1 : 0);
            }
            /**
             * Return the index of the matrix in {{{1, 0},{0, 1}}, {{ 0,-1},{ 1, 0}}, {{-1, 0},{ 0,-1}}, {{ 0, 1},{-1, 0}}, {{ 0, 1},{-1, 0}}, {{-1, 0},{ 0,-1}}, {{ 0,-1},{ 1, 0}}}.
             * @param index ranges from -3 to 3.
             */
            inline int get_rotate_index(int index)
            {
                switch(index) {
                case 0:
                    return 0;
                case 1:
                    return 1;
                case 2:
                    return 2;
                case 3:
                    return 3;
                case -1:
                    return 4;
                case -2:
                    return 5;
                case -3:
                    return 6;
                }
                return 0;
            }
            /**
             * Return 1 if point (testx, testy) lies in the interior of the polygon
             * determined by the vertices in vert, and return 0 otherwise.
             * See http://paulbourke.net/geometry/polygonmesh/ for more details.
             * @param nvert the number of vertices in the polygon.
             * @param vert the (x, y)-coordinates of the polygon's vertices
             **/
            template <typename T>
            inline int pnpoly(int nvert, T vert[][2], T const& testx, T const& testy)
            {
                int i;
                int counter = 0;
                T xinters;
                point_xy<T> p1, p2;

                /* Check for boundrary cases */
                for (i = 0; i < nvert; i++) {
                    if (testx == vert[i][0] && testy == vert[i][1]) {
                        return 1;
                    }
                }

                p1.x = vert[0][0];
                p1.y = vert[0][1];

                for (i = 1; i < nvert; i++) {
                    p2.x = vert[i % nvert][0];
                    p2.y = vert[i % nvert][1];
                    if (testy > (std::min)(p1.y, p2.y)  &&
                        testy <= (std::max)(p1.y, p2.y) &&
                        testx <= (std::max)(p1.x, p2.x) &&
                        p1.y != p2.y)
                    {
                        xinters = (testy-p1.y)*(p2.x-p1.x)/(p2.y-p1.y)+p1.x;
                        if (p1.x == p2.x || testx <= xinters)
                            counter++;
                    }
                    p1 = p2;
                }

                if (counter % 2 == 0) {
                    return 0;
                } else {
                    return 1;
                }
            }
            /**
             * Return 1 if (x, y) lies in (the interior or boundary of) the image of the
             * HEALPix projection (in case proj=0) or in the image the rHEALPix projection
             * (in case proj=1), and return 0 otherwise.
             * @param north_square the position of the north polar square (rHEALPix only)
             * @param south_square the position of the south polar square (rHEALPix only)
             **/
            template <typename T>
            inline int in_image(T const& x, T const& y, int proj, int north_square, int south_square)
            {
                static const T pi = detail::pi<T>();
                static const T half_pi = detail::half_pi<T>();
                static const T fourth_pi = detail::fourth_pi<T>();

                if (proj == 0) {
                    T healpixVertsJit[][2] = {
                        {-pi - epsilon,   fourth_pi},
                        {-3.0*fourth_pi,  half_pi + epsilon},
                        {-half_pi,        fourth_pi + epsilon},
                        {-fourth_pi,      half_pi + epsilon},
                        {0.0,             fourth_pi + epsilon},
                        {fourth_pi,       half_pi + epsilon},
                        {half_pi,         fourth_pi + epsilon},
                        {3.0*fourth_pi,   half_pi + epsilon},
                        {pi + epsilon,    fourth_pi},
                        {pi + epsilon,   -fourth_pi},
                        {3.0*fourth_pi,  -half_pi - epsilon},
                        {half_pi,        -fourth_pi - epsilon},
                        {fourth_pi,      -half_pi - epsilon},
                        {0.0,            -fourth_pi - epsilon},
                        {-fourth_pi,     -half_pi - epsilon},
                        {-half_pi,       -fourth_pi - epsilon},
                        {-3.0*fourth_pi, -half_pi - epsilon},
                        {-pi - epsilon,  -fourth_pi}
                    };
                    return pnpoly((int)sizeof(healpixVertsJit)/
                                  sizeof(healpixVertsJit[0]), healpixVertsJit, x, y);
                } else {
                    T rhealpixVertsJit[][2] = {
                        {-pi - epsilon,                                 fourth_pi + epsilon},
                        {-pi + north_square*half_pi - epsilon,          fourth_pi + epsilon},
                        {-pi + north_square*half_pi - epsilon,          3.0*fourth_pi + epsilon},
                        {-pi + (north_square + 1.0)*half_pi + epsilon,  3.0*fourth_pi + epsilon},
                        {-pi + (north_square + 1.0)*half_pi + epsilon,  fourth_pi + epsilon},
                        {pi + epsilon,                                  fourth_pi + epsilon},
                        {pi + epsilon,                                 -fourth_pi - epsilon},
                        {-pi + (south_square + 1.0)*half_pi + epsilon, -fourth_pi - epsilon},
                        {-pi + (south_square + 1.0)*half_pi + epsilon, -3.0*fourth_pi - epsilon},
                        {-pi + south_square*half_pi - epsilon,         -3.0*fourth_pi - epsilon},
                        {-pi + south_square*half_pi - epsilon,         -fourth_pi - epsilon},
                        {-pi - epsilon,                                -fourth_pi - epsilon}
                    };

                    return pnpoly((int)sizeof(rhealpixVertsJit)/
                                  sizeof(rhealpixVertsJit[0]), rhealpixVertsJit, x, y);
                }
            }
            /**
             * Return the authalic latitude of latitude alpha (if inverse=0) or
             * return the approximate latitude of authalic latitude alpha (if inverse=1).
             * P contains the relavent ellipsoid parameters.
             **/
            template <typename Parameters, typename T>
            inline T auth_lat(const Parameters& par, const par_healpix<T>& proj_parm, T const& alpha, int inverse)
            {
                if (inverse == 0) {
                    /* Authalic latitude. */
                    T q = pj_qsfn(sin(alpha), par.e, 1.0 - par.es);
                    T qp = proj_parm.qp;
                    T ratio = q/qp;

                    if (math::abs(ratio) > 1) {
                        /* Rounding error. */
                        ratio = pj_sign(ratio);
                    }

                    return asin(ratio);
                } else {
                    /* Approximation to inverse authalic latitude. */
                    return pj_authlat(alpha, proj_parm.apa);
                }
            }
            /**
             * Return the HEALPix projection of the longitude-latitude point lp on
             * the unit sphere.
            **/
            template <typename T>
            inline void healpix_sphere(T const& lp_lam, T const& lp_phi, T& xy_x, T& xy_y)
            {
                static const T pi = detail::pi<T>();
                static const T half_pi = detail::half_pi<T>();
                static const T fourth_pi = detail::fourth_pi<T>();

                T lam = lp_lam;
                T phi = lp_phi;
                T phi0 = asin(T(2.0)/T(3.0));

                /* equatorial region */
                if ( fabsl(phi) <= phi0) {
                    xy_x = lam;
                    xy_y = 3.0*pi/8.0*sin(phi);
                } else {
                    T lamc;
                    T sigma = sqrt(3.0*(1 - math::abs(sin(phi))));
                    T cn = floor(2*lam / pi + 2);
                    if (cn >= 4) {
                        cn = 3;
                    }
                    lamc = -3*fourth_pi + half_pi*cn;
                    xy_x = lamc + (lam - lamc)*sigma;
                    xy_y = pj_sign(phi)*fourth_pi*(2 - sigma);
                }
                return;
            }
            /**
             * Return the inverse of healpix_sphere().
            **/
            template <typename T>
            inline void healpix_sphere_inverse(T const& xy_x, T const& xy_y, T& lp_lam, T& lp_phi)
            {
                static const T pi = detail::pi<T>();
                static const T half_pi = detail::half_pi<T>();
                static const T fourth_pi = detail::fourth_pi<T>();

                T x = xy_x;
                T y = xy_y;
                T y0 = fourth_pi;

                /* Equatorial region. */
                if (math::abs(y) <= y0) {
                    lp_lam = x;
                    lp_phi = asin(8.0*y/(3.0*pi));
                } else if (fabsl(y) < half_pi) {
                    T cn = floor(2.0*x/pi + 2.0);
                    T xc, tau;
                    if (cn >= 4) {
                        cn = 3;
                    }
                    xc = -3.0*fourth_pi + (half_pi)*cn;
                    tau = 2.0 - 4.0*fabsl(y)/pi;
                    lp_lam = xc + (x - xc)/tau;
                    lp_phi = pj_sign(y)*asin(1.0 - math::pow(tau, 2)/3.0);
                } else {
                    lp_lam = -1.0*pi;
                    lp_phi = pj_sign(y)*half_pi;
                }
                return;
            }
            /**
             * Return the vector sum a + b, where a and b are 2-dimensional vectors.
             * @param ret holds a + b.
             **/
            template <typename T>
            inline void vector_add(const T a[2], const T b[2], T ret[2])
            {
                int i;
                for(i = 0; i < 2; i++) {
                    ret[i] = a[i] + b[i];
                }
            }
            /**
             * Return the vector difference a - b, where a and b are 2-dimensional vectors.
             * @param ret holds a - b.
             **/
            template <typename T>
            inline void vector_sub(const T a[2], const T b[2], T ret[2])
            {
                int i;
                for(i = 0; i < 2; i++) {
                    ret[i] = a[i] - b[i];
                }
            }
            /**
             * Return the 2 x 1 matrix product a*b, where a is a 2 x 2 matrix and
             * b is a 2 x 1 matrix.
             * @param ret holds a*b.
             **/
            template <typename T1, typename T2>
            inline void dot_product(const T1 a[2][2], const T2 b[2], T2 ret[2])
            {
                int i, j;
                int length = 2;
                for(i = 0; i < length; i++) {
                    ret[i] = 0;
                    for(j = 0; j < length; j++) {
                        ret[i] += a[i][j]*b[j];
                    }
                }
            }
            /**
             * Return the number of the polar cap, the pole point coordinates, and
             * the region that (x, y) lies in.
             * If inverse=0, then assume (x,y) lies in the image of the HEALPix
             * projection of the unit sphere.
             * If inverse=1, then assume (x,y) lies in the image of the
             * (north_square, south_square)-rHEALPix projection of the unit sphere.
             **/
            template <typename T>
            inline cap_map<T> get_cap(T x, T const& y, int north_square, int south_square,
                                     int inverse)
            {
                static const T pi = detail::pi<T>();
                static const T half_pi = detail::half_pi<T>();
                static const T fourth_pi = detail::fourth_pi<T>();

                cap_map<T> capmap;
                T c;
                capmap.x = x;
                capmap.y = y;
                if (inverse == 0) {
                    if (y > fourth_pi) {
                        capmap.region = cap_map<T>::north;
                        c = half_pi;
                    } else if (y < -fourth_pi) {
                        capmap.region = cap_map<T>::south;
                        c = -half_pi;
                    } else {
                        capmap.region = cap_map<T>::equatorial;
                        capmap.cn = 0;
                        return capmap;
                    }
                    /* polar region */
                    if (x < -half_pi) {
                        capmap.cn = 0;
                        capmap.x = (-3.0*fourth_pi);
                        capmap.y = c;
                    } else if (x >= -half_pi && x < 0) {
                        capmap.cn = 1;
                        capmap.x = -fourth_pi;
                        capmap.y = c;
                    } else if (x >= 0 && x < half_pi) {
                        capmap.cn = 2;
                        capmap.x = fourth_pi;
                        capmap.y = c;
                    } else {
                        capmap.cn = 3;
                        capmap.x = 3.0*fourth_pi;
                        capmap.y = c;
                    }
                } else {
                    if (y > fourth_pi) {
                        capmap.region = cap_map<T>::north;
                        capmap.x = (-3.0*fourth_pi + north_square*half_pi);
                        capmap.y = half_pi;
                        x = x - north_square*half_pi;
                    } else if (y < -fourth_pi) {
                        capmap.region = cap_map<T>::south;
                        capmap.x = (-3.0*fourth_pi + south_square*pi/2);
                        capmap.y = -half_pi;
                        x = x - south_square*half_pi;
                    } else {
                        capmap.region = cap_map<T>::equatorial;
                        capmap.cn = 0;
                        return capmap;
                    }
                    /* Polar Region, find the HEALPix polar cap number that
                       x, y moves to when rHEALPix polar square is disassembled. */
                    if (capmap.region == cap_map<T>::north) {
                        if (y >= -x - fourth_pi - epsilon && y < x + 5.0*fourth_pi - epsilon) {
                            capmap.cn = (north_square + 1) % 4;
                        } else if (y > -x -fourth_pi + epsilon && y >= x + 5.0*fourth_pi - epsilon) {
                            capmap.cn = (north_square + 2) % 4;
                        } else if (y <= -x -fourth_pi + epsilon && y > x + 5.0*fourth_pi + epsilon) {
                            capmap.cn = (north_square + 3) % 4;
                        } else {
                            capmap.cn = north_square;
                        }
                    } else if (capmap.region == cap_map<T>::south) {
                        if (y <= x + fourth_pi + epsilon && y > -x - 5.0*fourth_pi + epsilon) {
                            capmap.cn = (south_square + 1) % 4;
                        } else if (y < x + fourth_pi - epsilon && y <= -x - 5.0*fourth_pi + epsilon) {
                            capmap.cn = (south_square + 2) % 4;
                        } else if (y >= x + fourth_pi - epsilon && y < -x - 5.0*fourth_pi - epsilon) {
                            capmap.cn = (south_square + 3) % 4;
                        } else {
                            capmap.cn = south_square;
                        }
                    }
                }
                return capmap;
            }
            /**
             * Rearrange point (x, y) in the HEALPix projection by
             * combining the polar caps into two polar squares.
             * Put the north polar square in position north_square and
             * the south polar square in position south_square.
             * If inverse=1, then uncombine the polar caps.
             * @param north_square integer between 0 and 3.
             * @param south_square integer between 0 and 3.
             **/
            template <typename T>
            inline void combine_caps(T& xy_x, T& xy_y, int north_square, int south_square,
                                     int inverse)
            {
                static const T half_pi = detail::half_pi<T>();
                static const T fourth_pi = detail::fourth_pi<T>();

                T v[2];
                T c[2];
                T vector[2];
                T v_min_c[2];
                T ret_dot[2];
                const double (*tmpRot)[2];
                int pole = 0;

                cap_map<T> capmap = get_cap(xy_x, xy_y, north_square, south_square, inverse);
                if (capmap.region == cap_map<T>::equatorial) {
                    xy_x = capmap.x;
                    xy_y = capmap.y;
                    return;
                }

                v[0] = xy_x; v[1] = xy_y;
                c[0] = capmap.x; c[1] = capmap.y;

                if (inverse == 0) {
                    /* Rotate (xy_x, xy_y) about its polar cap tip and then translate it to
                       north_square or south_square. */

                    if (capmap.region == cap_map<T>::north) {
                        pole = north_square;
                        tmpRot = rot[get_rotate_index(capmap.cn - pole)];
                    } else {
                        pole = south_square;
                        tmpRot = rot[get_rotate_index(-1*(capmap.cn - pole))];
                    }
                } else {
                    /* Inverse function.
                     Unrotate (xy_x, xy_y) and then translate it back. */

                    /* disassemble */
                    if (capmap.region == cap_map<T>::north) {
                        pole = north_square;
                        tmpRot = rot[get_rotate_index(-1*(capmap.cn - pole))];
                    } else {
                        pole = south_square;
                        tmpRot = rot[get_rotate_index(capmap.cn - pole)];
                    }
                }

                vector_sub(v, c, v_min_c);
                dot_product(tmpRot, v_min_c, ret_dot);

                {
                    T a[2];
                    /* Workaround cppcheck git issue */
                    T* pa = a;
                    // TODO: in proj4 5.0.0 this line is used instead
                    //pa[0] = -3.0*fourth_pi + ((inverse == 0) ? 0 : capmap.cn) *half_pi;
                    pa[0] = -3.0*fourth_pi + ((inverse == 0) ? pole : capmap.cn) *half_pi;
                    pa[1] = half_pi;
                    vector_add(ret_dot, a, vector);
                }

                xy_x = vector[0];
                xy_y = vector[1];
            }

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_healpix_ellipsoid
                : public base_t_fi<base_healpix_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_healpix<T> m_proj_parm;

                inline base_healpix_ellipsoid(const Parameters& par)
                    : base_t_fi<base_healpix_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_healpix_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T lp_lat, T& xy_x, T& xy_y) const
                {
                    lp_lat = auth_lat(this->params(), m_proj_parm, lp_lat, 0);
                    return healpix_sphere(lp_lon, lp_lat, xy_x, xy_y);
                }

                // INVERSE(e_healpix_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    /* Check whether (x, y) lies in the HEALPix image. */
                    if (in_image(xy_x, xy_y, 0, 0, 0) == 0) {
                        lp_lon = HUGE_VAL;
                        lp_lat = HUGE_VAL;
                        BOOST_THROW_EXCEPTION( projection_exception(error_invalid_x_or_y) );
                    }
                    healpix_sphere_inverse(xy_x, xy_y, lp_lon, lp_lat);
                    lp_lat = auth_lat(this->params(), m_proj_parm, lp_lat, 1);
                }

                static inline std::string get_name()
                {
                    return "healpix_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_healpix_spheroid
                : public base_t_fi<base_healpix_spheroid<T, Parameters>, T, Parameters>
            {
                par_healpix<T> m_proj_parm;

                inline base_healpix_spheroid(const Parameters& par)
                    : base_t_fi<base_healpix_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_healpix_forward)  sphere
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    return healpix_sphere(lp_lon, lp_lat, xy_x, xy_y);
                }

                // INVERSE(s_healpix_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    /* Check whether (x, y) lies in the HEALPix image */
                    if (in_image(xy_x, xy_y, 0, 0, 0) == 0) {
                        lp_lon = HUGE_VAL;
                        lp_lat = HUGE_VAL;
                        BOOST_THROW_EXCEPTION( projection_exception(error_invalid_x_or_y) );
                    }
                    return healpix_sphere_inverse(xy_x, xy_y, lp_lon, lp_lat);
                }

                static inline std::string get_name()
                {
                    return "healpix_spheroid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_rhealpix_ellipsoid
                : public base_t_fi<base_rhealpix_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_healpix<T> m_proj_parm;

                inline base_rhealpix_ellipsoid(const Parameters& par)
                    : base_t_fi<base_rhealpix_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_rhealpix_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T lp_lat, T& xy_x, T& xy_y) const
                {
                    lp_lat = auth_lat(this->params(), m_proj_parm, lp_lat, 0);
                    healpix_sphere(lp_lon, lp_lat, xy_x, xy_y);
                    combine_caps(xy_x, xy_y, this->m_proj_parm.north_square, this->m_proj_parm.south_square, 0);
                }

                // INVERSE(e_rhealpix_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T xy_x, T xy_y, T& lp_lon, T& lp_lat) const
                {
                    /* Check whether (x, y) lies in the rHEALPix image. */
                    if (in_image(xy_x, xy_y, 1, this->m_proj_parm.north_square, this->m_proj_parm.south_square) == 0) {
                        lp_lon = HUGE_VAL;
                        lp_lat = HUGE_VAL;
                        BOOST_THROW_EXCEPTION( projection_exception(error_invalid_x_or_y) );
                    }
                    combine_caps(xy_x, xy_y, this->m_proj_parm.north_square, this->m_proj_parm.south_square, 1);
                    healpix_sphere_inverse(xy_x, xy_y, lp_lon, lp_lat);
                    lp_lat = auth_lat(this->params(), m_proj_parm, lp_lat, 1);
                }

                static inline std::string get_name()
                {
                    return "rhealpix_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_rhealpix_spheroid
                : public base_t_fi<base_rhealpix_spheroid<T, Parameters>, T, Parameters>
            {
                par_healpix<T> m_proj_parm;

                inline base_rhealpix_spheroid(const Parameters& par)
                    : base_t_fi<base_rhealpix_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_rhealpix_forward)  sphere
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    healpix_sphere(lp_lon, lp_lat, xy_x, xy_y);
                    combine_caps(xy_x, xy_y, this->m_proj_parm.north_square, this->m_proj_parm.south_square, 0);
                }

                // INVERSE(s_rhealpix_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T xy_x, T xy_y, T& lp_lon, T& lp_lat) const
                {
                    /* Check whether (x, y) lies in the rHEALPix image. */
                    if (in_image(xy_x, xy_y, 1, this->m_proj_parm.north_square, this->m_proj_parm.south_square) == 0) {
                        lp_lon = HUGE_VAL;
                        lp_lat = HUGE_VAL;
                        BOOST_THROW_EXCEPTION( projection_exception(error_invalid_x_or_y) );
                    }
                    combine_caps(xy_x, xy_y, this->m_proj_parm.north_square, this->m_proj_parm.south_square, 1);
                    return healpix_sphere_inverse(xy_x, xy_y, lp_lon, lp_lat);
                }

                static inline std::string get_name()
                {
                    return "rhealpix_spheroid";
                }

            };

            // HEALPix
            template <typename Parameters, typename T>
            inline void setup_healpix(Parameters& par, par_healpix<T>& proj_parm)
            {
                if (par.es != 0.0) {
                    proj_parm.apa = pj_authset<T>(par.es); /* For auth_lat(). */
                    proj_parm.qp = pj_qsfn(1.0, par.e, par.one_es); /* For auth_lat(). */
                    par.a = par.a*sqrt(0.5*proj_parm.qp); /* Set par.a to authalic radius. */
                    pj_calc_ellipsoid_params(par, par.a, par.es); /* Ensure we have a consistent parameter set */
                } else {
                }
            }

            // rHEALPix
            template <typename Params, typename Parameters, typename T>
            inline void setup_rhealpix(Params const& params, Parameters& par, par_healpix<T>& proj_parm)
            {
                proj_parm.north_square = pj_get_param_i<srs::spar::north_square>(params, "north_square", srs::dpar::north_square);
                proj_parm.south_square = pj_get_param_i<srs::spar::south_square>(params, "south_square", srs::dpar::south_square);
                /* Check for valid north_square and south_square inputs. */
                if ((proj_parm.north_square < 0) || (proj_parm.north_square > 3)) {
                    BOOST_THROW_EXCEPTION( projection_exception(error_axis) );
                }
                if ((proj_parm.south_square < 0) || (proj_parm.south_square > 3)) {
                    BOOST_THROW_EXCEPTION( projection_exception(error_axis) );
                }
                if (par.es != 0.0) {
                    proj_parm.apa = pj_authset<T>(par.es); /* For auth_lat(). */
                    proj_parm.qp = pj_qsfn(1.0, par.e, par.one_es); /* For auth_lat(). */
                    par.a = par.a*sqrt(0.5*proj_parm.qp); /* Set par.a to authalic radius. */
                    // TODO: why not the same as in healpix?
                    //pj_calc_ellipsoid_params(par, par.a, par.es);
                    par.ra = 1.0/par.a;
                } else {
                }
            }

    }} // namespace detail::healpix
    #endif // doxygen

    /*!
        \brief HEALPix projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_healpix.gif
    */
    template <typename T, typename Parameters>
    struct healpix_ellipsoid : public detail::healpix::base_healpix_ellipsoid<T, Parameters>
    {
        template <typename Params>
        inline healpix_ellipsoid(Params const& , Parameters const& par)
            : detail::healpix::base_healpix_ellipsoid<T, Parameters>(par)
        {
            detail::healpix::setup_healpix(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief HEALPix projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_healpix.gif
    */
    template <typename T, typename Parameters>
    struct healpix_spheroid : public detail::healpix::base_healpix_spheroid<T, Parameters>
    {
        template <typename Params>
        inline healpix_spheroid(Params const& , Parameters const& par)
            : detail::healpix::base_healpix_spheroid<T, Parameters>(par)
        {
            detail::healpix::setup_healpix(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief rHEALPix projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - north_square (integer)
         - south_square (integer)
        \par Example
        \image html ex_rhealpix.gif
    */
    template <typename T, typename Parameters>
    struct rhealpix_ellipsoid : public detail::healpix::base_rhealpix_ellipsoid<T, Parameters>
    {
        template <typename Params>
        inline rhealpix_ellipsoid(Params const& params, Parameters const& par)
            : detail::healpix::base_rhealpix_ellipsoid<T, Parameters>(par)
        {
            detail::healpix::setup_rhealpix(params, this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief rHEALPix projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - north_square (integer)
         - south_square (integer)
        \par Example
        \image html ex_rhealpix.gif
    */
    template <typename T, typename Parameters>
    struct rhealpix_spheroid : public detail::healpix::base_rhealpix_spheroid<T, Parameters>
    {
        template <typename Params>
        inline rhealpix_spheroid(Params const& params, Parameters const& par)
            : detail::healpix::base_rhealpix_spheroid<T, Parameters>(par)
        {
            detail::healpix::setup_rhealpix(params, this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_healpix, healpix_spheroid, healpix_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_rhealpix, rhealpix_spheroid, rhealpix_ellipsoid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI2(healpix_entry, healpix_spheroid, healpix_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI2(rhealpix_entry, rhealpix_spheroid, rhealpix_ellipsoid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(healpix_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(healpix, healpix_entry)
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(rhealpix, rhealpix_entry)
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_HEALPIX_HPP


#ifndef PYTHONIC_INCLUDE_NUMPY_MAXIMUM_HPP
#define PYTHONIC_INCLUDE_NUMPY_MAXIMUM_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/types/numpy_broadcast.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/max.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    template <class T>
    auto max(T const &self, T const &other) ->
        typename std::enable_if<!types::is_complex<T>::value,
                                decltype(boost::simd::max(self, other))>::type
    {
      return boost::simd::max(self, other);
    }
    template <class T0, class T1>
    auto max(T0 const &self, T1 const &other) ->
        typename std::decay<decltype(self < other ? other : self)>::type
    {
      return self < other ? other : self;
    }
  }
#define NUMPY_NARY_FUNC_NAME maximum
#define NUMPY_NARY_FUNC_SYM wrapper::max
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


#include "equality_gadget.hpp"

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/random.hpp>
//#include <gmp.h>
#include <boost/multiprecision/gmp.hpp>
//#include <boost/multiprecision/random.hpp>

using namespace boost::multiprecision;
using namespace boost::random;

//#include <iostream>
//#include <vector>
//#include <cmath>
//#include <ctime>

//#include <stdlib.h>
//#include <fstream>
//#include <chrono>
//#include <cstdint>
//#include <random>

typedef libff::alt_bn128_pp ppT;
typedef libff::Fr<ppT> FieldT;

FieldT sum_t = 0;
FieldT sum_b = FieldT::zero();

struct int256{
	uint64_t value[4];
};

int256 randomNumberGenerator(){
	int256 num;
	srand(time(NULL));
	for(int i=0;i<3;i++){
		num.value[i] = rand() % (uint64_t)(pow(2,64));
	}
	return num;
}

// There are two types of values:
//   _constants_ are values known at circuit generation time; they
//       are global constants known to everyone
//   _variables_ are values that change in each use of the circuit;
//       they have two subtypes:
//
//   _public variables_ are values known to both the prover
//       and verifier but change in each use of the circuit
//   _private variables_ are values known only to the prover
//       and change in each use of the circuit

// The elliptic curve we're operating on must have a _modulus_ that is
// the same as the _order_ of the underlying SNARK curve (BN128, MNT4,
// etc.).  So we need to be able to specify a suitable curve and
// generators for each such underlying SNARK curve.
template<typename FieldT>
struct curveParams {
    // Some generators
    static FieldT Gx, Gy, Hx, Hy, Cx, Cy, Ax, Ay;
};

typedef libff::Fr<libff::alt_bn128_pp> BN128Fr;
typedef curveParams<BN128Fr> BN128Params;
typedef libff::Fr<libff::mnt4_pp> MNT4Fr;
typedef curveParams<MNT4Fr> MNT4Params;
typedef libff::Fr<libff::mnt6_pp> MNT6Fr;
typedef curveParams<MNT6Fr> MNT6Params;

void init_curveparams(void) {
    // BN128 has order 21888242871839275222246405745257275088548364400416034343698204186575808495617.
    // The curve we use has that number as a modulus, equation
    // y^2 = x^3 - 3*x + 7950939520449436327800262930799465135910802758673292356620796789196167463969,
    // order 21888242871839275222246405745257275088760161411100494528458776273921456643749,
    // and twist order 21888242871839275222246405745257275088336567389731574158937632099230160347487
    BN128Params::Gx = BN128Fr(0);
    BN128Params::Gy = BN128Fr("11977228949870389393715360594190192321220966033310912010610740966317727761886");
    BN128Params::Hx = BN128Fr(1);
    BN128Params::Hy = BN128Fr("21803877843449984883423225223478944275188924769286999517937427649571474907279");
    BN128Params::Cx = BN128Fr(2);
    BN128Params::Cy = BN128Fr("4950745124018817972378217179409499695353526031437053848725554590521829916331");
    BN128Params::Ax = BN128Fr(4);
    BN128Params::Ay = BN128Fr("1929778687269876629657252589535788315400602403700102541701561325064015752665");

    // MNT4 has order 475922286169261325753349249653048451545124878552823515553267735739164647307408490559963137.
    // The curve we use has that number as a modulus, equation
    // y^2 = x^3 - 3*x + 231167148323223259519222248276530122498019837271767399092881541755570759528915690054257617,
    // order 475922286169261325753349249653048451545124877609388602970058907680650183700694415633043899,
    // and twist order 475922286169261325753349249653048451545124879496258428136476563797679110914122565486882377
    MNT4Params::Gx = MNT4Fr(0);
    MNT4Params::Gy = MNT4Fr("69340010096176642671075936244233205591761175107929619077175443746098492155210682688004000");
    MNT4Params::Hx = MNT4Fr(4);
    MNT4Params::Hy = MNT4Fr("89962085395108430328776481330922276788164520703635405311225917405228387147951802989614963");
    MNT4Params::Cx = MNT4Fr(5);
    MNT4Params::Cy = MNT4Fr("52902001285898935334481582927659505082867000922458881015269230130767369971501119682509581");
    MNT4Params::Ax = MNT4Fr(13);
    MNT4Params::Ay = MNT4Fr("121053423448209007180763047755032137130187089528003831161099799540651189694573076331882906");

    // MNT6 has order 475922286169261325753349249653048451545124879242694725395555128576210262817955800483758081
    // The curve we use has that number as a modulus, equation
    // y^2 = x^3 - 3*x + 24546313041565681523715355676371506472020535518551005057500340479469011985449670363024622,
    // order 475922286169261325753349249653048451545124878803858277348714592806990498327174348276061263,
    // and twist order 475922286169261325753349249653048451545124879681531173442395664345430027308737252691454901
    MNT6Params::Gx = MNT6Fr(6);
    MNT6Params::Gy = MNT6Fr("24197108752891306593933912637919640614809244712814357996916386860820196450211056738894088");
    MNT6Params::Hx = MNT6Fr(7);
    MNT6Params::Hy = MNT6Fr("38986684752414230937697051240187730249331222579878762386361563720275249449300503095108315");
    MNT6Params::Cx = MNT6Fr(10);
    MNT6Params::Cy = MNT6Fr("16456076723096839034614236624058053946787958080849874304391400047777491942015349039526487");
    MNT6Params::Ax = MNT6Fr(15);
    MNT6Params::Ay = MNT6Fr("217167731603808417993030053532106278784760282438477394477321645018696010454906317296597425");
}

// These need to be here for the linker to work
template<> BN128Fr BN128Params::Gx = 0;
template<> BN128Fr BN128Params::Gy = 0;
template<> BN128Fr BN128Params::Hx = 0;
template<> BN128Fr BN128Params::Hy = 0;
template<> BN128Fr BN128Params::Cx = 0;
template<> BN128Fr BN128Params::Cy = 0;
template<> BN128Fr BN128Params::Ax = 0;
template<> BN128Fr BN128Params::Ay = 0;
template<> MNT4Fr MNT4Params::Gx = 0;
template<> MNT4Fr MNT4Params::Gy = 0;
template<> MNT4Fr MNT4Params::Hx = 0;
template<> MNT4Fr MNT4Params::Hy = 0;
template<> MNT4Fr MNT4Params::Cx = 0;
template<> MNT4Fr MNT4Params::Cy = 0;
template<> MNT4Fr MNT4Params::Ax = 0;
template<> MNT4Fr MNT4Params::Ay = 0;
template<> MNT6Fr MNT6Params::Gx = 0;
template<> MNT6Fr MNT6Params::Gy = 0;
template<> MNT6Fr MNT6Params::Hx = 0;
template<> MNT6Fr MNT6Params::Hy = 0;
template<> MNT6Fr MNT6Params::Cx = 0;
template<> MNT6Fr MNT6Params::Cy = 0;
template<> MNT6Fr MNT6Params::Ax = 0;
template<> MNT6Fr MNT6Params::Ay = 0;

// Double a constant EC point (inx,iny) to yield (outx,outy).  The input
// point must not be the point at infinity.
template<typename FieldT>
static void ec_double_point(FieldT &outx, FieldT &outy,
    const FieldT &inx, const FieldT &iny)
{
    FieldT xsq = inx.squared();
    FieldT lambda = (xsq * 3 - 3) * (iny * 2).inverse();
    outx = lambda.squared() - inx * 2;
    outy = lambda * (inx - outx) - iny;
}

// Add constant EC points (inx, iny) and (addx, addy) to yield (outx, outy).
// inx and addx must not be equal.
template<typename FieldT>
static void ec_add_points(FieldT &outx, FieldT &outy,
    const FieldT &inx, const FieldT &iny,
    const FieldT &addx, const FieldT &addy)
{
    FieldT lambda = (addy - iny) * (addx - inx).inverse();
    outx = lambda.squared() - (addx + inx);
    outy = lambda * (inx - outx) - iny;
}

// Double the variable EC point (inx,iny) to yield (outx,outy).  The
// input point must not be the point at infinity.
template<typename FieldT>
class ec_double_gadget : public gadget<FieldT> {
private:
  pb_variable<FieldT> lambda, inxsq;
public:
  const pb_variable<FieldT> outx, outy, inx, iny;

  ec_double_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_linear_combination<FieldT> &inx,
              const pb_linear_combination<FieldT> &iny) :
    gadget<FieldT>(pb, "ec_double_gadget"), outx(outx), outy(outy),
        inx(inx), iny(iny)
  {
    // Allocate variables to protoboard
    // The strings (like "x") are only for debugging purposes

    lambda.allocate(this->pb, "lambda");
    inxsq.allocate(this->pb, "inxsq");
  }

  void generate_r1cs_constraints()
  {
    // inxsq = inx * inx
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(inx, inx, inxsq));

    // 2 * iny * lambda = 3 * inxsq - 3  (a = -3 on our curve)
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(2 * iny, lambda, 3 * inxsq - 3));

    // outx = lambda^2 - 2 * inx
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(lambda, lambda, outx + 2 * inx));

    // outy = lambda * (inx - outx) - iny
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(lambda, inx - outx, outy + iny));

  }

  void generate_r1cs_witness()
  {
    this->pb.val(inxsq) = this->pb.lc_val(inx) * this->pb.lc_val(inx);
    this->pb.val(lambda) = (this->pb.val(inxsq) * 3 - 3) * (this->pb.lc_val(iny) * 2).inverse();
    this->pb.val(outx) = this->pb.val(lambda).squared() - this->pb.lc_val(inx) * 2;
    this->pb.val(outy) = this->pb.val(lambda) * (this->pb.lc_val(inx) - this->pb.val(outx)) - this->pb.lc_val(iny);
  }
};

// Add the variable EC point (addx,addy) to the variable EC point
// (inx,iny) to yield (outx,outy).  The input point must not be the
// point at infinity.
template<typename FieldT>
class ec_add_gadget : public gadget<FieldT> {
private:
  pb_variable<FieldT> lambda;
public:
  const pb_variable<FieldT> outx, outy;
  const pb_linear_combination<FieldT> inx, iny, addx, addy;

  ec_add_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_linear_combination<FieldT> &inx,
              const pb_linear_combination<FieldT> &iny,
              const pb_linear_combination<FieldT> &addx,
              const pb_linear_combination<FieldT> &addy) :
    gadget<FieldT>(pb, "ec_add_gadget"),
    outx(outx), outy(outy), inx(inx), iny(iny), addx(addx), addy(addy)
  {
    // Allocate variables to protoboard
    // The strings (like "x") are only for debugging purposes

    lambda.allocate(this->pb, "lambda");
  }

  void generate_r1cs_constraints()
  {
    // (addx - inx) * lambda = addy - iny
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(addx - inx, lambda, addy - iny));

    // outx = lambda^2 - (addx + inx)
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(lambda, lambda, outx + addx + inx));

    // outy = lambda * (inx - outx) - iny
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(lambda, inx - outx, outy + iny));

  }

  void generate_r1cs_witness()
  {
    this->pb.val(lambda) = (this->pb.lc_val(addy) - this->pb.lc_val(iny)) * (this->pb.lc_val(addx) - this->pb.lc_val(inx)).inverse();
    this->pb.val(outx) = this->pb.val(lambda).squared() - (this->pb.lc_val(addx) + this->pb.lc_val(inx));
    this->pb.val(outy) = this->pb.val(lambda) * (this->pb.lc_val(inx) - this->pb.val(outx)) - this->pb.lc_val(iny);
  }
};

// Add the variable EC point P to the constant EC point (inx,iny) to
// yield (outx,outy).  The input point must not be the point at
// infinity.
template<typename FieldT>
class ec_constant_add_gadget : public gadget<FieldT> {
private:
  pb_variable<FieldT> lambda;
public:
  const pb_variable<FieldT> outx, outy;
  const pb_linear_combination<FieldT> inx, iny;
  const FieldT Px, Py;

  ec_constant_add_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_linear_combination<FieldT> &inx,
              const pb_linear_combination<FieldT> &iny,
              const FieldT &Px, const FieldT &Py) :
    gadget<FieldT>(pb, "ec_constant_add_gadget"),
    outx(outx), outy(outy), inx(inx), iny(iny), Px(Px), Py(Py)
  {
    // Allocate variables to protoboard
    // The strings (like "x") are only for debugging purposes

    lambda.allocate(this->pb, "lambda");
  }

  void generate_r1cs_constraints()
  {
    // (Px - inx) * lambda = Py - iny
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(Px - inx, lambda, Py - iny));

    // outx = lambda^2 - (Px + inx)
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(lambda, lambda, outx + Px + inx));

    // outy = lambda * (inx - outx) - iny
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(lambda, inx - outx, outy + iny));

  }

  void generate_r1cs_witness()
  {
    this->pb.val(lambda) = (Py - this->pb.lc_val(iny)) * (Px - this->pb.lc_val(inx)).inverse();
    this->pb.val(outx) = this->pb.val(lambda).squared() - (Px + this->pb.lc_val(inx));
    this->pb.val(outy) = this->pb.val(lambda) * (this->pb.lc_val(inx) - this->pb.val(outx)) - this->pb.lc_val(iny);
  }
};

// Add the constant EC point P0 or the constant EC point P1 to the
// variable EC point (inx,iny) to yield (outx,outy).  The input point
// must not be the point at infinity.  The input bit choice controls
// which addition is done.
template<typename FieldT>
class ec_2_constant_add_gadget : public gadget<FieldT> {
private:
  pb_linear_combination<FieldT> addx, addy;
  std::vector<ec_add_gadget<FieldT> > adder;
public:
  const pb_variable<FieldT> outx, outy;
  const pb_linear_combination<FieldT> inx, iny;
  const pb_variable<FieldT> choice;
  const FieldT P0x, P0y, P1x, P1y;

  ec_2_constant_add_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_linear_combination<FieldT> &inx,
              const pb_linear_combination<FieldT> &iny,
              const pb_variable<FieldT> &choice,
              const FieldT &P0x, const FieldT &P0y,
              const FieldT &P1x, const FieldT &P1y) :
    gadget<FieldT>(pb, "ec_2_constant_add_gadget"),
    outx(outx), outy(outy), inx(inx), iny(iny), choice(choice),
    P0x(P0x), P0y(P0y), P1x(P1x), P1y(P1y)
  {
    // Allocate variables to protoboard

    addx.assign(pb, choice * (P1x-P0x) + P0x);
    addy.assign(pb, choice * (P1y-P0y) + P0y);
    adder.emplace_back(this->pb, outx, outy, inx, iny, addx, addy);
  }

  void generate_r1cs_constraints()
  {
    adder[0].generate_r1cs_constraints();
  }

  void generate_r1cs_witness()
  {
    addx.evaluate(this->pb);
    addy.evaluate(this->pb);
    adder[0].generate_r1cs_witness();
  }
};

// Add the constant EC point P0 or the variable EC point P1 to the
// variable EC point (inx,iny) to yield (outx,outy).  The input point
// must not be the point at infinity.  The input bit choice controls
// which addition is done.
template<typename FieldT>
class ec_2_1constant_add_gadget : public gadget<FieldT> {
private:
  pb_variable<FieldT> addx, addy;
  std::vector<ec_add_gadget<FieldT> > adder;
public:
  const pb_variable<FieldT> outx, outy;
  const pb_linear_combination<FieldT> inx, iny;
  const pb_variable<FieldT> choice;
  const FieldT P0x, P0y;
  const pb_variable<FieldT> P1x, P1y;

  ec_2_1constant_add_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_linear_combination<FieldT> &inx,
              const pb_linear_combination<FieldT> &iny,
              const pb_variable<FieldT> &choice,
              const FieldT &P0x,
              const FieldT &P0y,
              const pb_variable<FieldT> &P1x,
              const pb_variable<FieldT> &P1y) :
    gadget<FieldT>(pb, "ec_2_1constant_add_gadget"),
    outx(outx), outy(outy), inx(inx), iny(iny), choice(choice),
    P0x(P0x), P0y(P0y), P1x(P1x), P1y(P1y)
  {
    // Allocate variables to protoboard

    addx.allocate(this->pb, "addx");
    addy.allocate(this->pb, "addy");
    adder.emplace_back(this->pb, outx, outy, inx, iny, addx, addy);
  }

  void generate_r1cs_constraints()
  {
    // Set (addx,addy) = choice ? (P0x, P0y) : (P1x, P1y)
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(P1x - P0x, choice, addx - P0x));
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(P1y - P0y, choice, addy - P0y));
    adder[0].generate_r1cs_constraints();
  }

  void generate_r1cs_witness()
  {
    bool choiceb = this->pb.val(choice) != FieldT(0);
    this->pb.val(addx) = choiceb ? this->pb.val(P1x) : P0x;
    this->pb.val(addy) = choiceb ? this->pb.val(P1y) : P0y;
    adder[0].generate_r1cs_witness();
  }
};

// Add the variable EC point P0 or the variable EC point P1 to the
// variable EC point (inx,iny) to yield (outx,outy).  The input point
// must not be the point at infinity.  The input bit choice controls
// which addition is done.
template<typename FieldT>
class ec_2_add_gadget : public gadget<FieldT> {
private:
  pb_variable<FieldT> addx, addy;
  std::vector<ec_add_gadget<FieldT> > adder;
public:
  const pb_variable<FieldT> outx, outy;
  const pb_linear_combination<FieldT> inx, iny;
  const pb_variable<FieldT> choice;
  const pb_variable<FieldT> P0x, P0y, P1x, P1y;

  ec_2_add_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_linear_combination<FieldT> &inx,
              const pb_linear_combination<FieldT> &iny,
              const pb_variable<FieldT> &choice,
              const pb_variable<FieldT> &P0x,
              const pb_variable<FieldT> &P0y,
              const pb_variable<FieldT> &P1x,
              const pb_variable<FieldT> &P1y) :
    gadget<FieldT>(pb, "ec_2_add_gadget"),
    outx(outx), outy(outy), inx(inx), iny(iny), choice(choice),
    P0x(P0x), P0y(P0y), P1x(P1x), P1y(P1y)
  {
    // Allocate variables to protoboard

    addx.allocate(this->pb, "addx");
    addy.allocate(this->pb, "addy");
    adder.emplace_back(this->pb, outx, outy, inx, iny, addx, addy);
  }

  void generate_r1cs_constraints()
  {
    // Set (addx,addy) = choice ? (P0x, P0y) : (P1x, P1y)
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(P1x - P0x, choice, addx - P0x));
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(P1y - P0y, choice, addy - P0y));
    adder[0].generate_r1cs_constraints();
  }

  void generate_r1cs_witness()
  {
    bool choiceb = this->pb.val(choice) != FieldT(0);
    this->pb.val(addx) = choiceb ? this->pb.val(P1x) : this->pb.val(P0x);
    this->pb.val(addy) = choiceb ? this->pb.val(P1y) : this->pb.val(P0y);
    adder[0].generate_r1cs_witness();
  }
};

// Add one of the four constant EC points to the variable EC point
// (inx,iny) to yield (outx,outy).  The input point must not be the
// point at infinity.  The input bits choice0 and choice1 control which
// addition is done (P{2*choice1+choice0} is added).
template<typename FieldT>
class ec_4_constant_add_gadget : public gadget<FieldT> {
private:
  pb_variable<FieldT> both;
  pb_linear_combination<FieldT> addx, addy;
  std::vector<ec_add_gadget<FieldT> > adder;
public:
  const pb_variable<FieldT> outx, outy;
  const pb_linear_combination<FieldT> inx, iny;
  const pb_variable<FieldT> choice0, choice1;
  const FieldT P0x, P0y, P1x, P1y, P2x, P2y, P3x, P3y;

  ec_4_constant_add_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_linear_combination<FieldT> &inx,
              const pb_linear_combination<FieldT> &iny,
              const pb_variable<FieldT> &choice0,
              const pb_variable<FieldT> &choice1,
              const FieldT &P0x, const FieldT &P0y,
              const FieldT &P1x, const FieldT &P1y,
              const FieldT &P2x, const FieldT &P2y,
              const FieldT &P3x, const FieldT &P3y) :
    gadget<FieldT>(pb, "ec_4_constant_add_gadget"),
    outx(outx), outy(outy), inx(inx), iny(iny),
    choice0(choice0), choice1(choice1),
    P0x(P0x), P0y(P0y), P1x(P1x), P1y(P1y),
    P2x(P2x), P2y(P2y), P3x(P3x), P3y(P3y)
  {
    // Allocate variables to protoboard

    both.allocate(this->pb, "both");
    addx.assign(this->pb, both * (P3x - P2x - P1x + P0x) + choice1 * (P2x - P0x) + choice0 * (P1x - P0x) + P0x);
    addy.assign(this->pb, both * (P3y - P2y - P1y + P0y) + choice1 * (P2y - P0y) + choice0 * (P1y - P0y) + P0y);
    adder.emplace_back(this->pb, outx, outy, inx, iny, addx, addy);
  }

  void generate_r1cs_constraints()
  {
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(choice0, choice1, both));
    adder[0].generate_r1cs_constraints();
  }

  void generate_r1cs_witness()
  {
    bool c0 = this->pb.val(choice0) != FieldT(0);
    bool c1 = this->pb.val(choice1) != FieldT(0);
    this->pb.val(both) = c0 && c1;
    addx.evaluate(this->pb);
    addy.evaluate(this->pb);
    adder[0].generate_r1cs_witness();
  }
};

// Compute A + s*P as (outx, outy) for an accumulator A, a given
// constant point P, and s given as a bit vector.  The _caller_ is
// responsible for proving that the elements of svec are bits.  The
// (constant) accumulator excess (AXS) will be updated; when all the
// computations are complete, AXS should be subtracted from the
// accumulator A.
template<typename FieldT>
class ec_constant_scalarmul_vec_accum_gadget : public gadget<FieldT> {
private:
  pb_variable_array<FieldT> accumx, accumy;
  std::vector<ec_4_constant_add_gadget<FieldT> > fouradders;
  std::vector<ec_2_constant_add_gadget<FieldT> > twoadders;
public:
  const pb_variable<FieldT> outx, outy;
  const pb_variable<FieldT> Ax, Ay;
  const pb_variable_array<FieldT> svec;
  const FieldT Px, Py;

  // Strategy: We compute (as compile-time constants) (powers of 2)
  // times P.  Based on each bit of s, we add one of the constant points
  // C or (2^i * P) + C to the accumulator, and regardless of s, add C
  // to the excess.

  ec_constant_scalarmul_vec_accum_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
           //   const pb_variable<FieldT> &Ax,
           //   const pb_variable<FieldT> &Ay,
              const pb_variable_array<FieldT> &svec,
              const FieldT &Px, const FieldT &Py):
            //  FieldT &AXSx, FieldT &AXSy) :
    gadget<FieldT>(pb, "ec_constant_scalarmul_vec_accum_gadget"),
    outx(outx), outy(outy), svec(svec), Px(Px), Py(Py)
  {
    size_t numbits = svec.size();
    // See loop comments below: if numbits is odd, we need (numbits-1)/2
    // slots.  If numbits is even, we need (numbits-2)/2 slots.  So
    // with integer truncated division, (numbits-1)/2 will be correct
    // in both cases.  (Well, if numbits is 0 for some reason, we also want
    // to get 0.)
    size_t accumslots = 0;
    if (numbits > 0) {
        accumslots = (numbits-1)/2;
    }
    accumx.allocate(this->pb, accumslots, "accumx");
    accumy.allocate(this->pb, accumslots, "accumy");

    FieldT twoiPx = Px, twoiPy = Py;
    size_t i = 0, accnext = 0;

    while(i < numbits) {
        // Invariant: twoiP = 2^i * P
        // Invariant: i is even and accnext = i/2

        if (i == numbits-1) {
            FieldT twoiPCx, twoiPCy;
            ec_add_points(twoiPCx, twoiPCy, twoiPx, twoiPy,
                curveParams<FieldT>::Cx, curveParams<FieldT>::Cy);

            twoadders.emplace_back(this->pb,
                outx, outy,
                (i == 0 ? 0 : accumx[accnext-1]),
                (i == 0 ? 0 : accumy[accnext-1]),
                svec[i],
                curveParams<FieldT>::Cx, curveParams<FieldT>::Cy,
                twoiPCx, twoiPCy);

            // This makes i odd, but also exits the loop with
            // i = numbits and accnext = (numbits-1)/2
            i += 1;
        } else {
            // Do two bits at a time

            // We need to compute 2^i * a * P + C for a = 1,2,3
            FieldT twoi2Px, twoi2Py;
            FieldT twoi1PCx, twoi1PCy, twoi2PCx, twoi2PCy, twoi3PCx, twoi3PCy;

            ec_add_points(twoi1PCx, twoi1PCy, twoiPx, twoiPy,
                    curveParams<FieldT>::Cx, curveParams<FieldT>::Cy);
            ec_double_point(twoi2Px, twoi2Py, twoiPx, twoiPy);
            ec_add_points(twoi2PCx, twoi2PCy, twoi2Px, twoi2Py,
                    curveParams<FieldT>::Cx, curveParams<FieldT>::Cy);
            ec_add_points(twoi3PCx, twoi3PCy, twoi2Px, twoi2Py,
                    twoi1PCx, twoi1PCy);

            fouradders.emplace_back(this->pb,
                (i == numbits-2 ? outx : accumx[accnext]),
                (i == numbits-2 ? outy : accumy[accnext]),
                (i == 0 ? 0 : accumx[accnext-1]),
                (i == 0 ? 0 : accumy[accnext-1]),
                svec[i], svec[i+1],
                curveParams<FieldT>::Cx, curveParams<FieldT>::Cy,
                twoi1PCx, twoi1PCy,
                twoi2PCx, twoi2PCy, twoi3PCx, twoi3PCy);

            // If i == numbits-2, we write directly to out and not accum above, and
            // exit the loop with i even and i == numbits and accnext = (numbits-2)/2
            if (i < numbits - 2) {
                accnext += 1;
            }
            i += 2;
            ec_double_point(twoiPx, twoiPy, twoi2Px, twoi2Py);
        }

      //  FieldT newAXSx, newAXSy;
      //  ec_add_points(newAXSx, newAXSy, AXSx, AXSy,
      //          curveParams<FieldT>::Cx, curveParams<FieldT>::Cy);
      //  AXSx = newAXSx;
      //  AXSy = newAXSy;
    }
  }

  void generate_r1cs_constraints()
  {
    for (auto&& gadget : fouradders) {
        gadget.generate_r1cs_constraints();
    }
    for (auto&& gadget : twoadders) {
        gadget.generate_r1cs_constraints();
    }
  }

  void generate_r1cs_witness()
  {
    for (auto&& gadget : fouradders) {
        gadget.generate_r1cs_witness();
    }
    for (auto&& gadget : twoadders) {
        gadget.generate_r1cs_witness();
    }
  }
};

// Compute A + s*P as (outx, outy) for an accumulator A, a given
// constant point P, and s given as a field element.  The (constant)
// accumulator excess (AXS) will be updated; when all the computations
// are complete, AXS should be subtracted from the accumulator A.
template<typename FieldT>
class ec_constant_scalarmul_accum_gadget : public gadget<FieldT> {
private:
  pb_variable_array<FieldT> svec;
  std::vector<packing_gadget<FieldT> > packers;
  std::vector<ec_constant_scalarmul_vec_accum_gadget<FieldT> > vecgadget;

public:
  const pb_variable<FieldT> outx, outy;
  const pb_variable<FieldT> Ax, Ay;
  const pb_variable<FieldT> s;
  const FieldT Px, Py;
  const int numbits;

  ec_constant_scalarmul_accum_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
            //  const pb_variable<FieldT> &Ax,
            //  const pb_variable<FieldT> &Ay,
              const pb_variable<FieldT> &s,
              const int &numbits,
              const FieldT &Px, const FieldT &Py,
              FieldT &AXSx, FieldT &AXSy) :
    gadget<FieldT>(pb, "ec_constant_scalarmul_accum_gadget"),
    outx(outx), outy(outy), s(s), numbits(numbits), Px(Px), Py(Py)
  {
    // Allocate variables to protoboard
    // The strings (like "x") are only for debugging purposes

    //size_t numbits = FieldT::num_bits;
    svec.allocate(this->pb, numbits, "svec");
    packers.emplace_back(this->pb, svec, s);
    vecgadget.emplace_back(this->pb, outx, outy, svec, Px, Py, AXSx, AXSy);
  }

  void generate_r1cs_constraints()
  {
    packers[0].generate_r1cs_constraints(true);
    vecgadget[0].generate_r1cs_constraints();
  }

  void generate_r1cs_witness()
  {
    packers[0].generate_r1cs_witness_from_packed();
    vecgadget[0].generate_r1cs_witness();
  }
};

// Compute s*P as (outx, outy) for a given constant point P, and s given
// as a bit vector.  The _caller_ is responsible for proving that the
// elements of svec are bits.
template<typename FieldT>
class ec_constant_scalarmul_vec_gadget : public gadget<FieldT> {
private:
  FieldT AXSx, AXSy;
  pb_variable<FieldT> accinx, acciny, accoutx, accouty;
  std::vector<ec_constant_scalarmul_vec_accum_gadget<FieldT> > scalarmuls;
  std::vector<ec_constant_add_gadget<FieldT> > adders;
public:
  const pb_variable<FieldT> outx, outy;
  const pb_variable_array<FieldT> svec;
  const FieldT Px, Py;

  ec_constant_scalarmul_vec_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_variable_array<FieldT> &svec,
              const FieldT &Px, const FieldT &Py) :
    gadget<FieldT>(pb, "ec_constant_scalarmul_vec_gadget"),
    outx(outx), outy(outy), svec(svec), Px(Px), Py(Py)
  {
    AXSx = curveParams<FieldT>::Ax;
    AXSy = curveParams<FieldT>::Ay;
    accinx.allocate(this->pb, "accinx");
    acciny.allocate(this->pb, "acciny");
    accoutx.allocate(this->pb, "accoutx");
    accouty.allocate(this->pb, "accouty");

    scalarmuls.emplace_back(pb, outx, outy, svec, Px, Py);
   // adders.emplace_back(pb, outx, outy, accoutx, accouty, AXSx, -AXSy);
  }

  void generate_r1cs_constraints()
  {
   // this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(accinx, 1, curveParams<FieldT>::Ax));
   // this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(acciny, 1, curveParams<FieldT>::Ay));
    scalarmuls[0].generate_r1cs_constraints();
   // adders[0].generate_r1cs_constraints();
  }

  void generate_r1cs_witness()
  {
  //  this->pb.val(accinx) = curveParams<FieldT>::Ax;
  //  this->pb.val(acciny) = curveParams<FieldT>::Ay;
    scalarmuls[0].generate_r1cs_witness();
  //  adders[0].generate_r1cs_witness();
  }
};

// Compute s*P as (outx, outy) for a given constant point P, and s given
// as a field element.
template<typename FieldT>
class ec_constant_scalarmul_gadget : public gadget<FieldT> {
private:
  pb_variable_array<FieldT> svec;
  std::vector<packing_gadget<FieldT> > packers;
  std::vector<ec_constant_scalarmul_vec_gadget<FieldT> > vecgadget;

public:
  const pb_variable<FieldT> outx, outy;
  const pb_variable<FieldT> s;
  const FieldT Px, Py;
  const int numbits;

  ec_constant_scalarmul_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_variable<FieldT> &s,
              const int &numbits,
              const FieldT &Px, const FieldT &Py) :
    gadget<FieldT>(pb, "ec_constant_scalarmul_gadget"),
    outx(outx), outy(outy), s(s), numbits(numbits), Px(Px), Py(Py)
  {
    // Allocate variables to protoboard
    // The strings (like "x") are only for debugging purposes

    //size_t numbits = FieldT::num_bits;
    svec.allocate(this->pb, numbits, "svec");
    packers.emplace_back(this->pb, svec, s);
    vecgadget.emplace_back(this->pb, outx, outy, svec, Px, Py);
  }

  void generate_r1cs_constraints()
  {
    packers[0].generate_r1cs_constraints(true);
    vecgadget[0].generate_r1cs_constraints();
  }

  void generate_r1cs_witness()
  {
    packers[0].generate_r1cs_witness_from_packed();
    vecgadget[0].generate_r1cs_witness();
  }
};

template<typename FieldT>
class ec_scalarmul_gadget;

// Compute A + s*P as (outx, outy) for an accumulator A, a precomputed
// addition table Ptable for a variable point P, and s given as a bit
// vector.  The _caller_ is responsible for proving that the elements of
// svec are bits.  The (constant) accumulator excess (AXS) will be
// updated; when all the computations are complete, AXS should be
// subtracted from the accumulator A.  The addition table is a variable
// array of length 2*numbits (where numbits is the length of svec) such
// that Ptable[2*i] and Ptable[2*i+1] are the (x,y) coordinates of
// 2^i * P + C.  Set Ptable_set_constraints to true (exactly once
// in the event the same Ptable is reused in the same circuit) if
// the Ptable is part of the private input.  Set Ptable_fill_values
// to true exactly once per Ptable (again, in case it it reused in the
// same circuit).
template<typename FieldT>
class ec_scalarmul_vec_accum_gadget : public gadget<FieldT> {
private:
  pb_variable_array<FieldT> accumx, accumy;
  pb_variable_array<FieldT> twoiPx, twoiPy;
  std::vector<ec_constant_add_gadget<FieldT> > cadders;
  std::vector<ec_add_gadget<FieldT> > adders;
  std::vector<ec_2_1constant_add_gadget<FieldT> > twoadders;
public:
  const pb_variable<FieldT> outx, outy;
  const pb_variable<FieldT> Ax, Ay;
  const pb_variable_array<FieldT> svec;
  const pb_variable<FieldT> Px, Py;
  const pb_variable_array<FieldT> Ptable;
  bool Ptable_set_constraints, Ptable_fill_values;

  ec_scalarmul_vec_accum_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_variable<FieldT> &Ax,
              const pb_variable<FieldT> &Ay,
              const pb_variable_array<FieldT> &svec,
              const pb_variable<FieldT> &Px,
              const pb_variable<FieldT> &Py,
              const pb_variable_array<FieldT> &Ptable,
              bool Ptable_set_constraints,
              bool Ptable_fill_values,
              FieldT &AXSx, FieldT &AXSy) :
    gadget<FieldT>(pb, "ec_scalarmul_vec_accum_gadget"),
    outx(outx), outy(outy), Ax(Ax), Ay(Ay), svec(svec),
    Px(Px), Py(Py), Ptable(Ptable),
    Ptable_set_constraints(Ptable_set_constraints),
    Ptable_fill_values(Ptable_fill_values)
  {
    size_t numbits = svec.size();
    assert(Ptable.size() == 2*numbits);

    if (Ptable_set_constraints) {
        // Create the adders to fill the Ptable with the correct values.
        // Ptable[2*i] and Ptable[2*i+1] are the (x,y) coordinates of
        // 2^i * P + C.
        if (numbits > 0) {
            // Add P and C to get Ptable[0,1] = P+C
            cadders.emplace_back(this->pb, Ptable[0], Ptable[1],
                    Px, Py, curveParams<FieldT>::Cx, curveParams<FieldT>::Cy);
        }
        if (numbits > 1) {
            // Add P and P+C to get Ptable[2,3] = 2*P+C
            adders.emplace_back(this->pb, Ptable[2], Ptable[3],
                    Px, Py, Ptable[0], Ptable[1]);
        }
        if (numbits > 2) {
            twoiPx.allocate(this->pb, numbits-2, "twoiPx");
            twoiPy.allocate(this->pb, numbits-2, "twoiPy");
        }
        for (size_t i = 2; i < numbits; ++i) {
            // Invariant: twoiP[i] = 2^{i+1} * P

            // Compute 2^{i-1}*P = (2^{i-1}*P + C) - C
            cadders.emplace_back(this->pb,
                    twoiPx[i-2], twoiPy[i-2],
                    Ptable[2*(i-1)], Ptable[2*(i-1)+1],
                    curveParams<FieldT>::Cx, -curveParams<FieldT>::Cy);

            // Compute 2^{i}*P + C = (2^{i-1}*P + C) + (2^{i-1}*P)
            adders.emplace_back(this->pb,
                    Ptable[2*i], Ptable[2*i+1],
                    Ptable[2*i-2], Ptable[2*i-1],
                    twoiPx[i-2], twoiPy[i-2]);
        }
    }

    accumx.allocate(this->pb, numbits-1, "accumx");
    accumy.allocate(this->pb, numbits-1, "accumy");

    for (size_t i = 0; i < numbits; ++i) {
        twoadders.emplace_back(this->pb,
            (i == numbits-1 ? outx : accumx[i]),
            (i == numbits-1 ? outy : accumy[i]),
            (i == 0 ? Ax : accumx[i-1]),
            (i == 0 ? Ay : accumy[i-1]),
            svec[i], curveParams<FieldT>::Cx, curveParams<FieldT>::Cy, Ptable[2*i], Ptable[2*i+1]);

        FieldT newAXSx, newAXSy;
        ec_add_points(newAXSx, newAXSy, AXSx, AXSy, curveParams<FieldT>::Cx, curveParams<FieldT>::Cy);
        AXSx = newAXSx;
        AXSy = newAXSy;
    }
  }

  void generate_r1cs_constraints()
  {
    if (Ptable_set_constraints) {
        for (auto&& gadget : cadders) {
            gadget.generate_r1cs_constraints();
        }
        for (auto&& gadget : adders) {
            gadget.generate_r1cs_constraints();
        }
    }
    for (auto&& gadget : twoadders) {
        gadget.generate_r1cs_constraints();
    }
  }

  void generate_r1cs_witness()
  {
    if (Ptable_set_constraints) {
        // We also have to satisfy the constraints we set
        size_t numbits = Ptable.size() / 2;

        if (numbits > 0) {
            cadders[0].generate_r1cs_witness();
        }
        if (numbits > 1) {
            adders[0].generate_r1cs_witness();
        }
        for (size_t i = 2; i < numbits; ++i) {
            cadders[i-1].generate_r1cs_witness();
            adders[i-1].generate_r1cs_witness();
        }
    } else if (Ptable_fill_values) {
        // We can just compute the Ptable values manually
        ec_scalarmul_gadget<FieldT>::compute_Ptable(this->pb, Ptable, Px, Py);
    }
    for (auto&& gadget : twoadders) {
        gadget.generate_r1cs_witness();
    }
  }
};

// Compute A + s*P as (outx, outy) for an accumulator A, a precomputed
// addition table Ptable for a variable point P, and s given as a field
// element.  The _caller_ is responsible for proving that the elements
// of svec are bits.  The (constant) accumulator excess (AXS) will be
// updated; when all the computations are complete, AXS should be
// subtracted from the accumulator A.  The addition table is a variable
// array of length 2*numbits (where numbits is the length of the FieldT
// size) such that Ptable[2*i] and Ptable[2*i+1] are the (x,y)
// coordinates of 2^i * P + C.  Set Ptable_set_constraints to true
// (exactly once in the event the same Ptable is reused in the same
// circuit) if the Ptable is part of the private input.  Set
// Ptable_fill_values to true exactly once per Ptable (again, in case it
// it reused in the same circuit).
template<typename FieldT>
class ec_scalarmul_accum_gadget : public gadget<FieldT> {
private:
  pb_variable_array<FieldT> svec;
  std::vector<packing_gadget<FieldT> > packers;
  std::vector<ec_scalarmul_vec_accum_gadget<FieldT> > vecgadget;

public:
  const pb_variable<FieldT> outx, outy;
  const pb_variable<FieldT> Ax, Ay;
  const pb_variable<FieldT> s;
  const pb_variable<FieldT> Px, Py;
  const pb_variable_array<FieldT> Ptable;
  bool Ptable_set_constraints, Ptable_fill_values;

  ec_scalarmul_accum_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_variable<FieldT> &Ax,
              const pb_variable<FieldT> &Ay,
              const pb_variable<FieldT> &s,
              const pb_variable<FieldT> &Px,
              const pb_variable<FieldT> &Py,
              const pb_variable_array<FieldT> &Ptable,
              bool Ptable_set_constraints,
              bool Ptable_fill_values,
              FieldT &AXSx, FieldT &AXSy) :
    gadget<FieldT>(pb, "ec_scalarmul_accum_gadget"),
    outx(outx), outy(outy), Ax(Ax), Ay(Ay), s(s),
    Px(Px), Py(Py), Ptable(Ptable),
    Ptable_set_constraints(Ptable_set_constraints),
    Ptable_fill_values(Ptable_fill_values)
  {
    // Allocate variables to protoboard
    // The strings (like "x") are only for debugging purposes

    size_t numbits = FieldT::num_bits;
    svec.allocate(this->pb, numbits, "svec");
    packers.emplace_back(this->pb, svec, s);
    vecgadget.emplace_back(this->pb, outx, outy, Ax, Ay, svec,
        Px, Py, Ptable, Ptable_set_constraints, Ptable_fill_values,
        AXSx, AXSy);
  }

  void generate_r1cs_constraints()
  {
    packers[0].generate_r1cs_constraints(true);
    vecgadget[0].generate_r1cs_constraints();
  }

  void generate_r1cs_witness()
  {
    packers[0].generate_r1cs_witness_from_packed();
    vecgadget[0].generate_r1cs_witness();
  }
};

// Compute s*P as (outx, outy) for a precomputed addition table Ptable
// for a variable point P, and s given as a bit vector.  The _caller_ is
// responsible for proving that the elements of svec are bits.
// The addition table is a variable array of length 2*numbits (where
// numbits is the length of svec) such that Ptable[2*i] and
// Ptable[2*i+1] are the (x,y) coordinates of 2^i * P + C.  Set
// Ptable_set_constraints to true (exactly once in the event the same
// Ptable is reused in the same circuit) if the Ptable is part of the
// private input.  Set Ptable_fill_values to true exactly once per
// Ptable (again, in case it it reused in the same circuit).
template<typename FieldT>
class ec_scalarmul_vec_gadget : public gadget<FieldT> {
private:
  FieldT AXSx, AXSy;
  pb_variable<FieldT> accinx, acciny, accoutx, accouty;
  std::vector<ec_scalarmul_vec_accum_gadget<FieldT> > scalarmuls;
  std::vector<ec_constant_add_gadget<FieldT> > adders;
public:
  const pb_variable<FieldT> outx, outy;
  const pb_variable_array<FieldT> svec;
  const pb_variable<FieldT> Px, Py;
  const pb_variable_array<FieldT> Ptable;
  bool Ptable_set_constraints, Ptable_fill_values;

  ec_scalarmul_vec_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_variable_array<FieldT> &svec,
              const pb_variable<FieldT> &Px,
              const pb_variable<FieldT> &Py,
              const pb_variable_array<FieldT> &Ptable,
              bool Ptable_set_constraints,
              bool Ptable_fill_values) :
    gadget<FieldT>(pb, "ec_scalarmul_vec_gadget"),
    outx(outx), outy(outy), svec(svec),
    Px(Px), Py(Py), Ptable(Ptable),
    Ptable_set_constraints(Ptable_set_constraints),
    Ptable_fill_values(Ptable_fill_values)
  {
    AXSx = curveParams<FieldT>::Ax;
    AXSy = curveParams<FieldT>::Ay;
    accinx.allocate(this->pb, "accinx");
    acciny.allocate(this->pb, "acciny");
    accoutx.allocate(this->pb, "accoutx");
    accouty.allocate(this->pb, "accouty");

    scalarmuls.emplace_back(pb, accoutx, accouty, accinx, acciny, svec,
        Px, Py, Ptable, Ptable_set_constraints, Ptable_fill_values,
        AXSx, AXSy);
    adders.emplace_back(pb, outx, outy, accoutx, accouty, AXSx, -AXSy);
  }

  void generate_r1cs_constraints()
  {
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(accinx, 1, curveParams<FieldT>::Ax));
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(acciny, 1, curveParams<FieldT>::Ay));
    scalarmuls[0].generate_r1cs_constraints();
    adders[0].generate_r1cs_constraints();
  }

  void generate_r1cs_witness()
  {
    this->pb.val(accinx) = curveParams<FieldT>::Ax;
    this->pb.val(acciny) = curveParams<FieldT>::Ay;
    scalarmuls[0].generate_r1cs_witness();
    adders[0].generate_r1cs_witness();
  }
};

// Compute s*P as (outx, outy) for a precomputed addition table Ptable
// for a variable point P, and s given as a field element.  The addition
// table is a variable array of length 2*numbits (where numbits is the
// length of the FieldT size) such that Ptable[2*i] and Ptable[2*i+1]
// are the (x,y) coordinates of 2^i * P + C.  Set Ptable_set_constraints
// to true (exactly once in the event the same Ptable is reused in the
// same circuit) if the Ptable is part of the private input.  Set
// Ptable_fill_values to true exactly once per Ptable (again, in case it
// it reused in the same circuit).
template<typename FieldT>
class ec_scalarmul_gadget : public gadget<FieldT> {
private:
  pb_variable_array<FieldT> svec;
  std::vector<packing_gadget<FieldT> > packers;
  std::vector<ec_scalarmul_vec_gadget<FieldT> > vecgadget;

public:
  const pb_variable<FieldT> outx, outy;
  const pb_variable<FieldT> s;
  const pb_variable<FieldT> Px, Py;
  const pb_variable_array<FieldT> Ptable;
  bool Ptable_set_constraints, Ptable_fill_values;

  ec_scalarmul_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_variable<FieldT> &s,
              const pb_variable<FieldT> &Px,
              const pb_variable<FieldT> &Py,
              const pb_variable_array<FieldT> &Ptable,
              bool Ptable_set_constraints,
              bool Ptable_fill_values) :
    gadget<FieldT>(pb, "ec_scalarmul_gadget"),
    outx(outx), outy(outy), s(s),
    Px(Px), Py(Py), Ptable(Ptable),
    Ptable_set_constraints(Ptable_set_constraints),
    Ptable_fill_values(Ptable_fill_values)
  {
    // Allocate variables to protoboard
    // The strings (like "x") are only for debugging purposes

    size_t numbits = FieldT::num_bits;
    svec.allocate(this->pb, numbits, "svec");
    packers.emplace_back(this->pb, svec, s);
    vecgadget.emplace_back(this->pb, outx, outy, svec,
        Px, Py, Ptable, Ptable_set_constraints, Ptable_fill_values);
  }

  void generate_r1cs_constraints()
  {
    packers[0].generate_r1cs_constraints(true);
    vecgadget[0].generate_r1cs_constraints();
  }

  void generate_r1cs_witness()
  {
    packers[0].generate_r1cs_witness_from_packed();
    vecgadget[0].generate_r1cs_witness();
  }

  // Compute the addition table.  The addition table is a variable array
  // of length 2*numbits such that Ptable[2*i] and Ptable[2*i+1] are the
  // (x,y) coordinates of 2^i * P + C.
  static void compute_Ptable(protoboard<FieldT> &pb,
                const pb_variable_array<FieldT> &Ptable,
                const pb_variable<FieldT> &Px,
                const pb_variable<FieldT> &Py)
  {
    assert(Ptable.size() % 2 == 0);
    size_t numbits = Ptable.size() / 2;

    FieldT twoiPx = pb.val(Px);
    FieldT twoiPy = pb.val(Py);

    for (size_t i = 0; i < numbits; ++i) {
        // Invariant: (twoiPx, twoiPy) = 2^i * P

        // Compute 2^i * P + C
        FieldT twoiPCx, twoiPCy;
        ec_add_points(twoiPCx, twoiPCy, twoiPx, twoiPy,
                curveParams<FieldT>::Cx, curveParams<FieldT>::Cy);
        pb.val(Ptable[2*i]) = twoiPCx;
        pb.val(Ptable[2*i+1]) = twoiPCy;

        // Compute 2^{i+1} * P
        FieldT twoi1Px, twoi1Py;
        ec_double_point(twoi1Px, twoi1Py, twoiPx, twoiPy);
        twoiPx = twoi1Px;
        twoiPy = twoi1Py;
    }
  }
};

// Compute s*G as (outx, outy), given s as field elements.
template<typename FieldT>
class scalarmul_gadget : public gadget<FieldT> {
private:
  std::vector<ec_constant_scalarmul_gadget<FieldT> > compute_PK;

public:
  const pb_variable<FieldT> outx, outy, s;
  const int numbits;

  scalarmul_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_variable<FieldT> &s,
              const int &numbits) :
    gadget<FieldT>(pb, "ec_pedersen_gadget"),
    outx(outx), outy(outy), s(s), numbits(numbits)
  {
	  compute_PK.emplace_back(this->pb, outx, outy, s, numbits,
            curveParams<FieldT>::Gx, curveParams<FieldT>::Gy);
  }

   void generate_r1cs_constraints()
    {
       compute_PK[0].generate_r1cs_constraints();
    }

   void generate_r1cs_witness()
    {
		//this->pb.val(s) = FieldT::random_element();
		compute_PK[0].generate_r1cs_witness();
	}
};

// Compute a*G + b*H as (outx, outy), given a and b as field elements.
template<typename FieldT>
class ec_pedersen_gadget : public gadget<FieldT> {
private:
  pb_variable<FieldT> accinx, acciny, accmidx, accmidy, accoutx, accouty;
  std::vector<ec_constant_scalarmul_gadget<FieldT> > mulgadgets;
  std::vector<ec_add_gadget<FieldT> > addgadget;

public:
  const pb_variable<FieldT> outx, outy, a, b;
  const int n1, n2;

  ec_pedersen_gadget(protoboard<FieldT> &pb,
              const pb_variable<FieldT> &outx,
              const pb_variable<FieldT> &outy,
              const pb_variable<FieldT> &a,
              const pb_variable<FieldT> &b,
              const int &n1, const int &n2) :
    gadget<FieldT>(pb, "ec_pedersen_gadget"),
    outx(outx), outy(outy), a(a), b(b), n1(n1), n2(n2)
  {
    // Allocate variables to protoboard
    // The strings (like "x") are only for debugging purposes

    accinx.allocate(this->pb, "accinx");
    acciny.allocate(this->pb, "acciny");
    accmidx.allocate(this->pb, "accmidx");
    accmidy.allocate(this->pb, "accmidy");
    accoutx.allocate(this->pb, "accoutx");
    accouty.allocate(this->pb, "accouty");

    // Initialize the accumulator
    FieldT AXSx = curveParams<FieldT>::Ax;
    FieldT AXSy = curveParams<FieldT>::Ay;
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(accinx, 1, curveParams<FieldT>::Ax));
    this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(acciny, 1, curveParams<FieldT>::Ay));

    // Initialize the gadgets
    mulgadgets.emplace_back(this->pb, accmidx, accmidy, a, n1,
            curveParams<FieldT>::Gx, curveParams<FieldT>::Gy);
    mulgadgets.emplace_back(this->pb, accoutx, accouty, b, n2,
            curveParams<FieldT>::Hx, curveParams<FieldT>::Hy);
    // Subtract the accumulator excess to get the result
    addgadget.emplace_back(this->pb, outx, outy, accoutx, accouty, accmidx, accmidy);
  }

  void generate_r1cs_constraints()
  {
    mulgadgets[0].generate_r1cs_constraints();
    mulgadgets[1].generate_r1cs_constraints();
    addgadget[0].generate_r1cs_constraints();
  }

  void generate_r1cs_witness()
  {
    this->pb.val(accinx) = curveParams<FieldT>::Ax;
    this->pb.val(acciny) = curveParams<FieldT>::Ay;
    mulgadgets[0].generate_r1cs_witness();
    mulgadgets[1].generate_r1cs_witness();
    addgadget[0].generate_r1cs_witness();
  }
};

template<typename FieldT>
class POA_gadget : public gadget<FieldT> {
public:
  pb_variable<FieldT>  PKx, PKy;     // Public key calculated from private key x
  pb_variable<FieldT> b;             // b = s * bal
  pb_variable<FieldT> Ax, Ay, Bx, By;

public:
  pb_variable<FieldT> s, cmx, cmy;   // output s and pedersen commitment
  pb_variable<FieldT> x, Yx, Yy;     // Private and public keys
  pb_variable<FieldT> bal, t;        // balance and blinding factor
  //const FieldT Gx, Gy, Hx, Hy;
  const int numbits, n1, n2;

  std::vector<scalarmul_gadget<FieldT> > compute_PK;
  std::vector<equality_if_gadget<FieldT> > compute_s;
  std::vector<ec_pedersen_gadget<FieldT> > compute_cm;

  POA_gadget(protoboard<FieldT> &pb,
			 const pb_variable<FieldT> x,
			 const int numbits,
			 const int n1,
			 const int n2,
			 const pb_variable<FieldT> s,
			 const pb_variable<FieldT> cmx,
			 const pb_variable<FieldT> cmy):
		gadget<FieldT>(pb, "POA_gadget"), x(x), numbits(numbits), n1(n1), n2(n2), s(s), cmx(cmx), cmy(cmy)
		{
			// Initialize the gadget for calculating public key from private key
			PKx.allocate(pb, "PKx");
			PKy.allocate(pb, "PKy");
			compute_PK.emplace_back(this->pb, PKx, PKy, x, numbits);

			// Equality gadget
			Yx.allocate(pb, "Yx");
			Yy.allocate(pb, "Yy");
			compute_s.emplace_back(this->pb, PKx, PKy, Yx, Yy, s);

			// Pedersen commitment gadget
			bal.allocate(pb, "bal");
			t.allocate(pb, "t");
			b.allocate(pb, "b");
			compute_cm.emplace_back(this->pb, cmx, cmy, b, t, n1, n2);
		}

	void generate_r1cs_constraints(){
		compute_PK[0].generate_r1cs_constraints();

		compute_s[0].generate_r1cs_constraints();

		this->pb.add_r1cs_constraint(r1cs_constraint<FieldT>(s, bal, b));
		compute_cm[0].generate_r1cs_constraints();
	}

	void generate_r1cs_witness() {
		compute_PK[0].generate_r1cs_witness();

		this->pb.val(Yx) = this->pb.val(PKx);
		this->pb.val(Yy) = this->pb.val(PKy);
		compute_s[0].generate_r1cs_witness();

		//this->pb.val(bal) = FieldT::random_element();
		this->pb.val(bal) = rand() % uint64_t(pow(2,51));
		//this->pb.val(bal) = ui(mt);

		//uint256_t temp = rand() % uint256_t(pow(2,256));
		this->pb.val(t) = rand() % uint64_t(pow(2,256));
		//this->pb.val(t) = FieldT::random_element();
		//this-pb.val(t) = ui1(mt);
		this->pb.val(b) = this->pb.val(s) * this->pb.val(bal);
		sum_b += this->pb.val(b);
		//sum_b = long(sum_b) % 21888242871839275222246405745257275088760161411100494528458776273921456643749;
		sum_t += this->pb.val(t);
		//sum_t = sum_t % FieldT("21888242871839275222246405745257275088760161411100494528458776273921456643749");
		compute_cm[0].generate_r1cs_witness();
	}
};


#include "builders/generators/CylinderGenerator.hpp"
#include "builders/generators/IcoSphereGenerator.hpp"
#include "builders/generators/LSystemGenerator.hpp"
#include "entities/Node.hpp"
#include "lsys/LSystemParser.hpp"

#include <boost/test/unit_test.hpp>
#include <fstream>

#include "config.hpp"
#include "test_utils/DependencyProvider.hpp"
#include "test_utils/ElementUtils.hpp"

using namespace utymap;
using namespace utymap::builders;
using namespace utymap::entities;
using namespace utymap::heightmap;
using namespace utymap::mapcss;
using namespace utymap::math;
using namespace utymap::index;
using namespace utymap::utils;
using namespace utymap::tests;

namespace {
const std::string stylesheet =
    "node|z1[natural=tree] {"
        "lsys: tree;"
        "lsys-size: 1m;"
        "lsys-colors: gray,yellow;"
        "lsys-texture-indices: 0,0;"
        "lsys-texture-types: background,grass;"
        "lsys-texture-scales: 50,200;"
        "}";

struct Builders_Generators_GeneratorFixture {
  Builders_Generators_GeneratorFixture() :
      dependencyProvider(),
      mesh(""),
      gradient(),
      textureRegion(),
      style(dependencyProvider.getStyleProvider(stylesheet)
                ->forElement(ElementUtils::createElement<Node>(*dependencyProvider.getStringTable(),
                                                               0,
                                                               {{"natural", "tree"}}), 1)),
      builderContext(
          QuadKey(1, 35205, 21489),
          *dependencyProvider.getStyleProvider(stylesheet),
          *dependencyProvider.getStringTable(),
          *dependencyProvider.getMeshPool(),
          *dependencyProvider.getElevationProvider(),
          [](const Mesh &) {},
          [](const Element &) {},
          dependencyProvider.getCancellationToken()),
      meshContext(mesh, style, gradient, textureRegion) {
  }

  DependencyProvider dependencyProvider;
  Mesh mesh;
  ColorGradient gradient;
  TextureRegion textureRegion;
  Style style;
  BuilderContext builderContext;
  MeshContext meshContext;
};
}

BOOST_FIXTURE_TEST_SUITE(Builders_Generators_Generator, Builders_Generators_GeneratorFixture)

BOOST_AUTO_TEST_CASE(GivenIcoSphereGeneratorWithSimpleData_WhenGenerate_ThenCanGenerateMesh) {
  IcoSphereGenerator(builderContext, meshContext)
      .setCenter(Vector3(0, 0, 0))
      .setSize(Vector3(10, 10, 10))
      .setRecursionLevel(2)
      .isSemiSphere(false)
      .setColorNoiseFreq(0.1)
      .setVertexNoiseFreq(0.5)
      .generate();

  BOOST_CHECK_GT(mesh.vertices.size(), 0);
  BOOST_CHECK_GT(mesh.triangles.size(), 0);
  BOOST_CHECK_GT(mesh.colors.size(), 0);
}

BOOST_AUTO_TEST_CASE(GivenCylinderGeneratorWithSimpleData_WhenGenerate_ThenCanGenerateMesh) {
  CylinderGenerator(builderContext, meshContext)
      .setCenter(Vector3(0, 0, 0))
      .setMaxSegmentHeight(5)
      .setRadialSegments(7)
      .setSize(Vector3(0.5, 10, 0.5))
      .setColorNoiseFreq(0.1)
      .generate();

  BOOST_CHECK_GT(mesh.vertices.size(), 0);
  BOOST_CHECK_GT(mesh.triangles.size(), 0);
  BOOST_CHECK_GT(mesh.colors.size(), 0);
}

BOOST_AUTO_TEST_CASE(GivenLSystemGeneratorWithSimpleData_WhenGenerate_ThenCanGenerateMesh) {
  GeoCoordinate center(52.53178, 13.38750);
  std::ifstream file(TEST_MAPCSS_PATH "tree.lsys");
  auto lsystem = utymap::lsys::LSystemParser().parse(file);

  LSystemGenerator::generate(builderContext, style, lsystem, mesh, center, 0);

  BOOST_CHECK_GT(mesh.vertices.size(), 0);
  BOOST_CHECK_GT(mesh.triangles.size(), 0);
  BOOST_CHECK_GT(mesh.colors.size(), 0);
}

BOOST_AUTO_TEST_SUITE_END()


// MIT License

// Copyright (c) 2020 Joseph Auckley, Matthew O'Kelly, Aman Sinha, Hongrui Zheng

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "pose_2d.hpp"
#include "scan_simulator_2d.hpp"
#include "car_state.hpp"
#include "car_params.hpp"
#include "car_odom.hpp"
#include "car_obs.hpp"
#include "ackermann_kinematics.hpp"
#include "ks_kinematics.hpp"
#include "st_kinematics.hpp"

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <iostream>
#include <vector>
// #include <chrono> // do we need to keep timestamp for each car?
#include <math.h>

using namespace racecar_simulator;

class RaceCar {
public:
    RaceCar(double time_step, double mu, double h_cg, double l_r, double cs_f, double cs_r, double I_z, double mass, bool is_ego);
    virtual ~RaceCar();
    void set_map(std::vector<double> &map, int map_height, int map_width, double map_resolution, double origin_x, double origin_y, double free_threshold);
    void reset();
    void reset_bypose(Pose2D pose);
    CarObs update_scan();
    void update_pose();
    void update_op_poses(const std::vector<Pose2D> &op_poses);
    void set_velocity(double vel);
    void set_steering_angle(double ang);
    Pose2D get_pose();
    double get_accel();
    double get_steer_vel();
    void update_params(double mu, double h_cg, double l_r, double cs_f, double cs_r, double I_z, double mass);
private:
    // objects
    CarState state;
    CarParams params;
    CarOdom odom;
    Pose2D pose;
    ScanSimulator2D scan_simulator;

    // flags
    bool map_exists;
    bool ego;

    // car params
    double scan_distance_to_base_link;
    double car_width, car_length;
    double max_speed, max_steering_angle;
    double max_accel, max_decel, max_steering_vel;
    double accel, steer_angle_vel;
    double width;

    // sim param
    double delt_t;

    // scan params
    std::vector<double> cosines;
    double scan_fov, scan_ang_incr;
    std::vector<double> scan_angles;

    // current scan
    std::vector<double> current_scan;

    // collision flag
    bool in_collision = false;

    // which beam in laser in collision
    double collision_angle;

    // ttc lower bound
    double ttc_threshold;

    // map params
    double map_free_threshold;

    // PI
    const double PI = 3.141592653;

    // distaces form lidar to edge of car
    std::vector<double> car_distances;

    // knowledge of other agents index doesn't matter?
    std::vector<Pose2D> opponent_poses;

    // steering delay
    int steering_delay_buffer_length;
    std::vector<double> steer_buffer;

    Eigen::Matrix4d get_transformation_matrix(const Pose2D &pose);
    Pose2D transform_between_frames(const Pose2D &p1, const Pose2D &p2);
    void ray_cast_opponents(std::vector<double> &scan, const Pose2D &scan_pose);
    void check_ttc();
    void set_accel(double acceleration);
    void set_steering_angle_vel(double steer_vel);
    double compute_steer_vel(double desired_angle);
    void compute_accel(double desired_velocity);
    double get_range(const Pose2D &pose, double beam_theta, Eigen::Vector2d line_segment_a, Eigen::Vector2d line_segment_b);
    bool are_collinear(Eigen::Vector2d pt_a, Eigen::Vector2d pt_b, Eigen::Vector2d pt_c);
    double cross(Eigen::Vector2d v1, Eigen::Vector2d v2);
};