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

/* Author: Mark Moll */

#include "ompl/base/spaces/ReedsSheppStateSpace.h"
#include "ompl/base/SpaceInformation.h"
#include "ompl/util/Exception.h"
#include <queue>
#include <boost/math/constants/constants.hpp>

using namespace ompl::base;

namespace
{
    // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.

    const double pi = boost::math::constants::pi<double>();
    const double twopi = 2. * pi;
    const double RS_EPS = 1e-6;
    const double ZERO = 10*std::numeric_limits<double>::epsilon();

    inline double mod2pi(double x)
    {
        double v = fmod(x, twopi);
        if (v < -pi)
            v += twopi;
        else
            if (v > pi)
                v -= twopi;
        return v;
    }
    inline void polar(double x, double y, double &r, double &theta)
    {
        r = sqrt(x*x + y*y);
        theta = atan2(y, x);
    }
    inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
    {
        double delta = mod2pi(u-v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
        double t1 = atan2(eta*A - xi*B, xi*A + eta*B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
        tau = (t2<0) ? mod2pi(t1+pi) : mod2pi(t1);
        omega = mod2pi(tau - u + v - phi) ;
    }

    // formula 8.1 in Reeds-Shepp paper
    inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
    {
        polar(x - sin(phi), y - 1. + cos(phi), u, t);
        if (t >= -ZERO)
        {
            v = mod2pi(phi - t);
            if (v >= -ZERO)
            {
                assert(fabs(u*cos(t) + sin(phi) - x) < RS_EPS);
                assert(fabs(u*sin(t) - cos(phi) + 1 - y) < RS_EPS);
                assert(fabs(mod2pi(t+v - phi)) < RS_EPS);
                return true;
            }
        }
        return false;
    }
    // formula 8.2
    inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
    {
        double t1, u1;
        polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
        u1 = u1*u1;
        if (u1 >= 4.)
        {
            double theta;
            u = sqrt(u1 - 4.);
            theta = atan2(2., u);
            t = mod2pi(t1 + theta);
            v = mod2pi(t - phi);
            assert(fabs(2*sin(t) + u*cos(t) - sin(phi) - x) < RS_EPS);
            assert(fabs(-2*cos(t) + u*sin(t) + cos(phi) + 1 - y) < RS_EPS);
            assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
            return t>=-ZERO && v>=-ZERO;
        }
        return false;
    }
    void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length(), L;
        if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
            Lmin = L;
        }
        if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
            Lmin = L;
        }
        if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
            Lmin = L;
        }
        if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
            Lmin = L;
        }
        if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
            Lmin = L;
        }
        if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
            Lmin = L;
        }
        if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
            Lmin = L;
        }
        if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
    }
    // formula 8.3 / 8.4  *** TYPO IN PAPER ***
    inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
        polar(xi, eta, u1, theta);
        if (u1 <= 4.)
        {
            u = -2.*asin(.25 * u1);
            t = mod2pi(theta + .5 * u + pi);
            v = mod2pi(phi - t + u);
            assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
            assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
            assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
            return t>=-ZERO && u<=ZERO;
        }
        return false;
    }
    void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length(), L;
        if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
            Lmin = L;
        }
        if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
            Lmin = L;
        }
        if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
            Lmin = L;
        }
        if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
            Lmin = L;
        }

        // backwards
        double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
        if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
            Lmin = L;
        }
        if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
            Lmin = L;
        }
        if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
            Lmin = L;
        }
        if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
    }
    // formula 8.7
    inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
        if (rho <= 1.)
        {
            u = acos(rho);
            tauOmega(u, -u, xi, eta, phi, t, v);
            assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
            assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
            assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
            return t>=-ZERO && v<=ZERO;
        }
        return false;
    }
    // formula 8.8
    inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
        if (rho>=0 && rho<=1)
        {
            u = -acos(rho);
            if (u >= -.5 * pi)
            {
                tauOmega(u, u, xi, eta, phi, t, v);
                assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
                assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
                assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
                return t>=-ZERO && v>=-ZERO;
            }
        }
        return false;
    }
    void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length(), L;
        if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
            Lmin = L;
        }
        if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
            Lmin = L;
        }
        if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
            Lmin = L;
        }
        if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
            Lmin = L;
        }

        if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
            Lmin = L;
        }
        if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
            Lmin = L;
        }
        if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
            Lmin = L;
        }
        if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
    }
    // formula 8.9
    inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
        polar(xi, eta, rho, theta);
        if (rho >= 2.)
        {
            double r = sqrt(rho*rho - 4.);
            u = 2. - r;
            t = mod2pi(theta + atan2(r, -2.));
            v = mod2pi(phi - .5*pi - t);
            assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
            assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
            assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
            return t>=-ZERO && u<=ZERO && v<=ZERO;
        }
        return false;
    }
    // formula 8.10
    inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
        polar(-eta, xi, rho, theta);
        if (rho >= 2.)
        {
            t = theta;
            u = 2. - rho;
            v = mod2pi(t + .5*pi - phi);
            assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
            assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
            assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
            return t>=-ZERO && u<=ZERO && v<=ZERO;
        }
        return false;
    }
    void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length() - .5*pi, L;
        if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
            Lmin = L;
        }
        if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
            Lmin = L;
        }
        if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
            Lmin = L;
        }
        if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
            Lmin = L;
        }

        if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
            Lmin = L;
        }
        if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
            Lmin = L;
        }
        if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
            Lmin = L;
        }
        if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
            Lmin = L;
        }

        // backwards
        double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
        if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
            Lmin = L;
        }
        if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
            Lmin = L;
        }
        if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
            Lmin = L;
        }
        if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
            Lmin = L;
        }

        if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
            Lmin = L;
        }
        if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
            Lmin = L;
        }
        if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
            Lmin = L;
        }
        if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
    }
    // formula 8.11 *** TYPO IN PAPER ***
    inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
    {
        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
        polar(xi, eta, rho, theta);
        if (rho >= 2.)
        {
            u = 4. - sqrt(rho*rho - 4.);
            if (u <= ZERO)
            {
                t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
                v = mod2pi(t - phi);
                assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
                assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
                assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
                return t>=-ZERO && v>=-ZERO;
            }
        }
        return false;
    }
    void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
    {
        double t, u, v, Lmin = path.length() - pi, L;
        if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
            Lmin = L;
        }
        if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
            Lmin = L;
        }
        if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
        {
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
            Lmin = L;
        }
        if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
            path = ReedsSheppStateSpace::ReedsSheppPath(
                ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5*pi, -u, .5*pi, -v);
    }

    ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
    {
        ReedsSheppStateSpace::ReedsSheppPath path;
        CSC(x, y, phi, path);
        CCC(x, y, phi, path);
        CCCC(x, y, phi, path);
        CCSC(x, y, phi, path);
        CCSCC(x, y, phi, path);
        return path;
    }
}

const ompl::base::ReedsSheppStateSpace::ReedsSheppPathSegmentType
ompl::base::ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
    { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP },             // 0
    { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP },            // 1
    { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP },           // 2
    { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP },           // 3
    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP },        // 4
    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP },       // 5
    { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },        // 6
    { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },       // 7
    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP },       // 8
    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP },        // 9
    { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },       // 10
    { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },        // 11
    { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },         // 12
    { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },         // 13
    { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },          // 14
    { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },        // 15
    { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT },      // 16
    { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT }       // 17
};

ompl::base::ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
    double t, double u, double v, double w, double x)
    : type_(type)
{
    length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
    totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
}

double ompl::base::ReedsSheppStateSpace::distance(const State *state1, const State *state2) const
{
    return rho_ * reedsShepp(state1, state2).length();
}

void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
{
    bool firstTime = true;
    ReedsSheppPath path;
    interpolate(from, to, t, firstTime, path, state);
}

void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t,
    bool &firstTime, ReedsSheppPath &path, State *state) const
{
    if (firstTime)
    {
        if (t>=1.)
        {
            if (to != state)
                copyState(state, to);
            return;
        }
        if (t<=0.)
        {
            if (from != state)
                copyState(state, from);
            return;
        }
        path = reedsShepp(from, to);
        firstTime = false;
    }
    interpolate(from, path, t, state);
}

void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const ReedsSheppPath &path, double t, State *state) const
{
    StateType *s = allocState()->as<StateType>();
    double seg = t * path.length(), phi, v;

    s->setXY(0., 0.);
    s->setYaw(from->as<StateType>()->getYaw());
    for (unsigned int i=0; i<5 && seg>0; ++i)
    {
        if (path.length_[i]<0)
        {
            v = std::max(-seg, path.length_[i]);
            seg += v;
        }
        else
        {
            v = std::min(seg, path.length_[i]);
            seg -= v;
        }
        phi = s->getYaw();
        switch(path.type_[i])
        {
            case RS_LEFT:
                s->setXY(s->getX() + sin(phi+v) - sin(phi), s->getY() - cos(phi+v) + cos(phi));
                s->setYaw(phi+v);
                break;
            case RS_RIGHT:
                s->setXY(s->getX() - sin(phi-v) + sin(phi), s->getY() + cos(phi-v) - cos(phi));
                s->setYaw(phi-v);
                break;
            case RS_STRAIGHT:
                s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
                break;
            case RS_NOP:
                break;
        }
    }
    state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
    state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
    getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
    state->as<StateType>()->setYaw(s->getYaw());
    freeState(s);
}

ompl::base::ReedsSheppStateSpace::ReedsSheppPath ompl::base::ReedsSheppStateSpace::reedsShepp(const State *state1, const State *state2) const
{
    const StateType *s1 = static_cast<const StateType*>(state1);
    const StateType *s2 = static_cast<const StateType*>(state2);
    double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
    double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
    double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1);
    double x = c*dx + s*dy, y = -s*dx + c*dy, phi = th2 - th1;
    return ::reedsShepp(x/rho_, y/rho_, phi);
}

void ompl::base::ReedsSheppMotionValidator::defaultSettings()
{
    stateSpace_ = dynamic_cast<ReedsSheppStateSpace*>(si_->getStateSpace().get());
    if (!stateSpace_)
        throw Exception("No state space for motion validator");
}

bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
{
    /* assume motion starts in a valid configuration so s1 is valid */

    bool result = true, firstTime = true;
    ReedsSheppStateSpace::ReedsSheppPath path;
    int nd = stateSpace_->validSegmentCount(s1, s2);

    if (nd > 1)
    {
        /* temporary storage for the checked state */
        State *test = si_->allocState();

        for (int j = 1 ; j < nd ; ++j)
        {
            stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
            if (!si_->isValid(test))
            {
                lastValid.second = (double)(j - 1) / (double)nd;
                if (lastValid.first)
                    stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
                result = false;
                break;
            }
        }
        si_->freeState(test);
    }

    if (result)
        if (!si_->isValid(s2))
        {
            lastValid.second = (double)(nd - 1) / (double)nd;
            if (lastValid.first)
                stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
            result = false;
        }

    if (result)
        valid_++;
    else
        invalid_++;

    return result;
}

bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2) const
{
    /* assume motion starts in a valid configuration so s1 is valid */
    if (!si_->isValid(s2))
        return false;

    bool result = true, firstTime = true;
    ReedsSheppStateSpace::ReedsSheppPath path;
    int nd = stateSpace_->validSegmentCount(s1, s2);

    /* initialize the queue of test positions */
    std::queue< std::pair<int, int> > pos;
    if (nd >= 2)
    {
        pos.push(std::make_pair(1, nd - 1));

        /* temporary storage for the checked state */
        State *test = si_->allocState();

        /* repeatedly subdivide the path segment in the middle (and check the middle) */
        while (!pos.empty())
        {
            std::pair<int, int> x = pos.front();

            int mid = (x.first + x.second) / 2;
            stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);

            if (!si_->isValid(test))
            {
                result = false;
                break;
            }

            pos.pop();

            if (x.first < mid)
                pos.push(std::make_pair(x.first, mid - 1));
            if (x.second > mid)
                pos.push(std::make_pair(mid + 1, x.second));
        }

        si_->freeState(test);
    }

    if (result)
        valid_++;
    else
        invalid_++;

    return result;
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/third.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/negate.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/twotomnmbo_3.hpp>
#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/function/is_less.hpp>
#endif

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/logical_or.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(cbrt_
                          , (typename A0,typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::single_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(const A0& a0 ) const BOOST_NOEXCEPT
      {
        A0 z =  bs::abs(a0);
        using int_type =  bd::as_integer_t<A0, signed>;
  #ifndef BOOST_SIMD_NO_DENORMALS
        auto denormal = is_less(z, Smallestposval<A0>());
        z = if_else(denormal, z*Twotonmb<A0>(), z);
        A0 f = if_else(denormal, Twotomnmbo_3<A0>(), One<A0>());
  #endif
        const A0 CBRT2  = Constant< A0, 0x3fa14518> ();
        const A0 CBRT4  = Constant< A0, 0x3fcb2ff5> ();
        const A0 CBRT2I = Constant< A0, 0x3f4b2ff5> ();
        const A0 CBRT4I = Constant< A0, 0x3f214518> ();
        int_type e;
        A0 x; std::tie(x, e) = fast_(frexp)(z);
        x = horn <A0,
          0x3ece0609,
          0x3f91eb77,
          0xbf745265,
          0x3f0bf0fe,
          0xbe09e49a
          > (x);
        auto flag = is_gez(e);
        int_type e1 =  bs::abs(e);
        int_type rem = e1;
        e1 /= Three<int_type>();
        rem -= e1*Three<int_type>();
        e = negate(e1, e);
        const A0 cbrt2 = if_else(flag, CBRT2, CBRT2I);
        const A0 cbrt4 = if_else(flag, CBRT4, CBRT4I);
        A0 fact = if_else(is_equal(rem, One<int_type>()), cbrt2, One<A0>());
        fact = if_else(is_equal(rem, Two<int_type>()), cbrt4, fact);
        x = fast_(ldexp)(x*fact, e);
        x -= (x-z/sqr(x))*Third<A0>();
  #ifndef BOOST_SIMD_NO_DENORMALS
        x = bitwise_or(x, bitofsign(a0))*f;
  #else
        x = bitwise_or(x, bitofsign(a0));
  #endif
  #ifndef BOOST_SIMD_NO_INFINITIES
        return if_else(logical_or(is_eqz(a0),is_inf(a0)), a0, x);
  #else
        return if_else(is_eqz(a0), a0, x);
  #endif
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF(cbrt_
                          , (typename A0,typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::double_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(const A0& a0 ) const BOOST_NOEXCEPT
      {
        using int_type =  bd::as_integer_t<A0, signed>;
        A0 z =  bs::abs(a0);
     #ifndef BOOST_SIMD_NO_DENORMALS
        auto denormal = is_less(z, Smallestposval<A0>());
        z = if_else(denormal, z*Twotonmb<A0>(), z);
        A0 f = if_else(denormal, Twotomnmbo_3<A0>(), One<A0>());
     #endif
        const A0 CBRT2  = Constant< A0, 0x3ff428a2f98d728bll> ();
        const A0 CBRT4  = Constant< A0, 0x3ff965fea53d6e3dll> ();
        const A0 CBRT2I = Constant< A0, 0x3fe965fea53d6e3dll> ();
        const A0 CBRT4I = Constant< A0, 0x3fe428a2f98d728bll> ();
        int_type e;
        A0 x;
        std::tie(x, e) = fast_(frexp)(z);
        x = horn <A0,
          0x3fd9c0c12122a4fell,
          0x3ff23d6ee505873all,
          0xbfee8a4ca3ba37b8ll,
          0x3fe17e1fc7e59d58ll,
          0xbfc13c93386fdff6ll >  (x);
        auto flag = is_gez(e);
        int_type e1 =  bs::abs(e);
        int_type rem = e1;
        e1 /= Three<int_type>();
        rem -= e1*Three<int_type>();
        e =  negate(e1, e);
        const A0 cbrt2 = if_else(flag, CBRT2, CBRT2I);
        const A0 cbrt4 = if_else(flag, CBRT4, CBRT4I);
        A0 fact = if_else(is_equal(rem, One<int_type>()), cbrt2, One<A0>());
        fact = if_else(is_equal(rem, Two<int_type>()), cbrt4, fact);
        x = fast_(ldexp)(x*fact, e);
        x -= (x-z/sqr(x))*Third<A0>();
        x -= (x-z/sqr(x))*Third<A0>(); //two newton passes
      #ifndef BOOST_SIMD_NO_DENORMALS
        x = bitwise_or(x, bitofsign(a0))*f;
      #else
        x = bitwise_or(x, bitofsign(a0));
      #endif
      #ifndef BOOST_SIMD_NO_INFINITIES
        return if_else(logical_or(is_eqz(a0),is_inf(a0)), a0, x);
      #else
        return if_else(is_eqz(a0), a0, x);
      #endif
    }
   };

}

} }

#endif


#define DEBUG 1
/**
 * File    : C.cpp
 * Author  : Kazune Takahashi
 * Created : 12/29/2019, 11:12:55 AM
 * Powered by Visual Studio Code
 */
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <vector>
#include <string>
#include <complex>
#include <tuple>
#include <queue>
#include <stack>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <bitset>
#include <functional>
#include <random>
#include <chrono>
#include <cctype>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
// ----- boost -----
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using boost::rational;
using namespace std;
using ll = long long;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{x % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
ll gcd(ll x, ll y) { return y ? gcd(y, x % y) : x; }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}
// ----- main() -----

class Domino
{
  using board = vector<string>;
  using point = tuple<int, int>;
  vector<board> seed;

  int N;
  board answer;

public:
  Domino(int N) : N{N}
  {
    init_seed();
    stringstream SS;
    for (auto i = 0; i < N; i++)
    {
      SS << '.';
    }
    answer = board(N, SS.str());
  }

  void flush()
  {
    if (N == 2)
    {
      cout << "-1" << endl;
      return;
    }
    make_ans();
    assert(static_cast<int>(answer.size()) == N);
    for (auto i = 0; i < N; i++)
    {
      assert(static_cast<int>(answer[i].size()) == N);
      cout << answer[i] << endl;
    }
  }

  void init_seed()
  {
    seed.resize(8);
    seed[3] = {
        "aa.",
        "..a",
        "..a"};
    seed[4] = {
        "aabc",
        "ddbc",
        "bcaa",
        "bcdd"};
    seed[5] = {
        "aabba",
        "bcc.a",
        "b..cb",
        "a..cb",
        "abbaa"};
    seed[6] = {
        "aabc..",
        "ddbc..",
        "..aabc",
        "..ddbc",
        "bc..aa",
        "bc..dd"};
    seed[7] = {
        "aabbcc.",
        "dd.dd.a",
        "..d..da",
        "..d..db",
        "dd.dd.b",
        "..d..dc",
        "..d..dc"};
  }

private:
  void make_ans()
  {
    if (N == 3)
    {
      answer = seed[3];
      return;
    }
    int S{N % 4 + 4};
    int x{(N - S) / 4};
    for (auto k = 0; k < x; k++)
    {
      for (auto i = k * 4; i < (k + 1) * 4; i++)
      {
        for (auto j = k * 4; j < (k + 1) * 4; j++)
        {
          answer[i][j] = seed[4][i % 4][j % 4];
        }
      }
    }
    for (auto i = x * 4; i < N; i++)
    {
      for (auto j = x * 4; j < N; j++)
      {
        answer[i][j] = seed[S][i - x * 4][j - x * 4];
      }
    }
  }
};

int main()
{
  int N;
  cin >> N;
  Domino domino{N};
  domino.flush();
}


#include <boost/foreach.hpp>
#include "duffell.hpp"

Duffell::Duffell
(const double alpha,
 const int iter):
  alpha_(alpha),
  iter_(iter) {}

namespace {
  vector<Vector2D> extract_velocity
  (const vector<ComputationalCell>& cells,size_t N)
  {
    vector<Vector2D> res(N);
    for(size_t i=0;i<res.size();++i)
      res.at(i) = cells.at(i).velocity;
    return res;
  }

  Vector2D neighbor_average
  (const Tessellation& tess,
   const vector<Vector2D>& previous,
   const size_t i)
  {
    Vector2D res(0,0);
    double circumference = 0;
    const vector<int> edge_indices =
      tess.GetCellEdges(static_cast<int>(i));
    BOOST_FOREACH(int index, edge_indices){
      const Edge edge = tess.GetEdge(index);
      const int other =
	edge.neighbors.first +
	edge.neighbors.second -
	static_cast<int>(i);
      const double edge_length =
	abs(edge.vertices.first-
	    edge.vertices.second);
      circumference += edge_length;
      if(other<tess.GetPointNo())
	res += edge_length*
	  previous.at(static_cast<size_t>(other));
    }
    return (1.0/circumference)*res;
  }

  vector<Vector2D> smooth
  (const Tessellation& tess,
   const vector<Vector2D>& previous,
   const double alpha)
  {
    vector<Vector2D> res(previous.size());
    for(size_t i=0;i<res.size();++i)
      res.at(i) = previous.at(i)*(1-alpha)+
	alpha*neighbor_average(tess,previous,i);
    return res;
  }
}

vector<Vector2D> Duffell::operator()
(const Tessellation& tess,
 const vector<ComputationalCell>& cells,
 double /*time*/,TracerStickerNames const& /*tracerstickernames*/) const
{
  vector<Vector2D> res = extract_velocity(cells,static_cast<size_t>(tess.GetPointNo()));
  for(int i=0;i<iter_;++i)
    res = smooth(tess,res,alpha_);
  return res;
}


// ***********************************************************************
// Code Created by James Michael Armstrong (https://github.com/BlazesRus)
// Latest Code Release at https://github.com/BlazesRus/BlazesRusSharedCode
// ***********************************************************************
#pragma once

#ifdef BlazesSharedCode_LocalLayout
#ifndef DLL_API
#ifdef UsingBlazesSharedCodeDLL
#define DLL_API __declspec(dllimport)
#elif defined(BLAZESSharedCode_LIBRARY)
#define DLL_API __declspec(dllexport)
#else
#define DLL_API
#endif
#endif
#else
#include "..\DLLAPI.h"
#endif

#include "MediumDec.hpp"
#include <boost/multiprecision/cpp_int.hpp>

namespace BlazesRusCode
{
	using UInt128 = boost::multiprecision::uint128_t;
	//using Int128Dec = boost::multiprecision::;
	/*
	MediumDec ToMediumDec(Int128Dec value)
	{

	}

	Int128Dec ToInt128Dec(MediumDec value)
	{

	}
	*/
}


#pragma once

#include "../Univariate.hpp"
#include "../Variable.hpp"
#include "../utils.hpp"

#include "Hamiltonian.hpp"
#include "Operator.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>

#include <memory>

namespace yavque
{

class HamEvol final : public Operator, public Univariate
{
private:
	bool conjugate_ = false;
	std::shared_ptr<const detail::HamiltonianImpl> ham_;

	void dagger_in_place_impl() override { conjugate_ = !conjugate_; }

	HamEvol(HamEvol&&) = default;
	HamEvol(const HamEvol&) = default;

public:
	explicit HamEvol(const Hamiltonian& ham)
		: Operator(ham.dim(), "HamEvol of " + ham.name()), ham_{ham.get_impl()}
	{
	}

	explicit HamEvol(const Hamiltonian& ham, Variable var)
		: Operator(ham.dim(), "HamEvol of " + ham.name()),
		  Univariate(std::move(var)), ham_{ham.get_impl()}
	{
	}

	HamEvol& operator=(const HamEvol&) = delete;
	HamEvol& operator=(HamEvol&&) = delete;
	~HamEvol() override = default;

	[[nodiscard]] Hamiltonian hamiltonian() const { return Hamiltonian(ham_); }

	[[nodiscard]] std::unique_ptr<Operator> clone() const override
	{
		auto p = std::unique_ptr<HamEvol>{new HamEvol(*this)};
		p->change_variable(Variable{var_.value()});
		return p;
	}

	[[nodiscard]] std::unique_ptr<Operator> log_deriv() const override
	{
		constexpr std::complex<double> I(0., 1.0);
		std::string op_name = std::string("derivative of ") + name(); // change to fmt
		cx_double constant = conjugate_ ? I : -I;
		return std::make_unique<Hamiltonian>(ham_, op_name, constant);
	}

	[[nodiscard]] Eigen::VectorXcd apply_right(const Eigen::VectorXcd& st) const override
	{
		constexpr std::complex<double> I(0., 1.0);
		Eigen::VectorXcd res = ham_->evecs().adjoint() * st;
		double t = conjugate_ ? -var_.value() : var_.value();
		res.array() *= exp(-I * ham_->evals().array() * t);
		return ham_->evecs() * res;
	}

	[[nodiscard]] bool can_merge(const Operator& rhs) const override
	{
		if(const auto* p = dynamic_cast<const HamEvol*>(&rhs))
		{
			if(ham_ == p->ham_)
			{
				return true;
			}
		}
		return false;
	}

	[[nodiscard]] std::string desc() const override
	{
		std::ostringstream ss;
		ss << "[" << name() << ", variable name: " << var_.name() << ", "
		   << "variable value: " << var_.value() << "]";
		return ss.str();
	}
};
} // namespace yavque


/*
 * LiquidSampler.cpp
 *
 *  Created on: Apr 18, 2012
 *      Author: selman.joe@gmail.com
 */

#include <cstdlib>
#include <vector>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/bernoulli_distribution.hpp>

#include "LiquidSampler.h"
#include "../SpanInterval.h"
#include "../SISet.h"

std::vector<SpanInterval> LiquidSampler::operator ()(const SpanInterval& si, double p, boost::mt19937& rng) const {
    std::vector<SpanInterval> sampled;

    if (p <= 0.0) return sampled;
    if (p >= 1.0) {
        sampled.push_back(si);
        return sampled;
    }

    if (si.isEmpty()) return sampled;

    // loop over intervals ranging from the width of si, down to 1
    std::list<Interval> toSample;
    toSample.push_front(Interval(si.start().start(), si.finish().finish()));
    do {
        Interval curInterval = toSample.front();
        toSample.pop_front();
        //bool sampleCurInterval = ((double)rand() / (double)RAND_MAX) <= p;
        boost::bernoulli_distribution<double> flip(p);
        bool sampleCurInterval = flip(rng);
        if (sampleCurInterval) {
            sampled.push_back(SpanInterval(curInterval));
            // now make sure none of our intervals overlap the sampled interval
            for (std::list<Interval>::iterator it = toSample.begin();
                    it != toSample.end();) {
                if (intersection(curInterval, *it)) {
                    std::vector<Interval> subtracted = it->subtract(curInterval);
                    // put what we just processed in there and erase it
                    for (std::vector<Interval>::const_iterator it2 = subtracted.begin();
                            it2 != subtracted.end();
                            it2++) {
                        toSample.insert(it, *it2);
                    }
                    it = toSample.erase(it);
                } else {
                    it++;
                }
            }
        } else {
            // didn't sample, so lets put the smaller size of intervals next in to sample
            if (curInterval.size() != 1) {
                Interval left = Interval(curInterval.start(), curInterval.finish()-1);
                Interval right = Interval(curInterval.start()+1, curInterval.finish());
                toSample.push_front(right);
                toSample.push_front(left);
            }
        }
    } while (!toSample.empty());

    return sampled;
}


/*
 * Eigen - an OCaml interface to C++ Eigen library
 * Copyright (c) 2016-2020 Liang Wang <liang.wang@cl.cam.ac.uk>
 */

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include "eigen_spmat.h"

using namespace Eigen;

#include "eigen_spmat_s.cpp"
#include "eigen_spmat_d.cpp"
#include "eigen_spmat_c.cpp"
#include "eigen_spmat_z.cpp"


/**
 *  测试在 KITTI 数据集上的 PR 曲线
 * 以 位姿 ground truth 作为判断是否在同一地点的标准
 */

#include <iostream>
#include <vector>
#include <fstream>
#include <chrono>
#include <iomanip> // for 'setw'
// DBoW3
#include "DBoW3.h" // defines OrbVocabulary and OrbDatabase
// OpenCV
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/features2d.hpp>
#include "opencv2/imgproc/imgproc.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "DescManip.h"

using namespace std;

typedef Eigen::Matrix<double, 3, 3> Mat33;
typedef Eigen::Matrix<double, 3, 1> Vec3;

void ExtractFeaturesFromOneImage(const cv::Mat &imgGray, cv::Ptr<cv::ORB> &orb,
    cv::Mat &vfeaturesInOneImg);

void LoadImages(const string &strPathToSequence,
    vector<string> &vstrImageFilenames, vector<double> &vTimestamps);

void LoadGroundtruthPose(const string &strPose,
    std::vector<Mat33, Eigen::aligned_allocator<Mat33> > &vPoses_R,
    std::vector<Vec3, Eigen::aligned_allocator<Vec3> > &vPoses_t);

std::pair<double, double> VectorMeanAndVariance(const std::vector<double> v);

bool PoseIsVeryNear(std::vector<Mat33, Eigen::aligned_allocator<Mat33> > &vPoses_R,
    std::vector<Vec3, Eigen::aligned_allocator<Vec3> > &vPoses_t,
    int current_id, int loop_id);

bool PoseIsAcceptablyNear(std::vector<Mat33, Eigen::aligned_allocator<Mat33> > &vPoses_R,
    std::vector<Vec3, Eigen::aligned_allocator<Vec3> > &vPoses_t,
    int current_id, int loop_id);

// ----------------------------------------------------------------------------------------------------------------------------------------------------------
int main(int argc, char **argv){
    if(argc != 5){
        std::cerr << std::endl <<
            "Usage: ./mytest/test1   PATH_TO_SEQUENCE  PATH_TO_VOCABULARY PATH_TO_GROUNDTRUTH_POSE  PATH_TO_RESULT"
            << std::endl;
        return 1;
    }

    // ---------------------------------- Load the images' path -----------------------------------------
    std::string strPathToSequence = argv[1];
    std::vector<string> vstrImageFilenames;
    std::vector<double> vTimestamps;
    LoadImages(strPathToSequence,  vstrImageFilenames, vTimestamps);

    // ---------------------------------- Load the groud_truth file -----------------------------------------
    std::string strPathToPose = argv[3];
    std::vector<Mat33, Eigen::aligned_allocator<Mat33>> vPoses_R;
    std::vector<Vec3, Eigen::aligned_allocator<Vec3>> vPoses_t;
    LoadGroundtruthPose(strPathToPose,  vPoses_R, vPoses_t);

    // ---------------------------------- check if the vocabulary file exists or not----------------------------------
    std::string strPathToVocabulary = argv[2];

    fstream vocFile;
    vocFile.open(strPathToVocabulary);
    if(!vocFile){
        std::cerr << "error: cannot open the vocabulary file " << strPathToVocabulary << endl;
        return -1;
    }
    vocFile.close();

    // ---------------------------------- create the file to store test results ----------------------------------
    ofstream f;
    std::string filename = argv[4];
    f.open(filename.c_str());
    f << fixed;

    // ----------------------------------------- 一些数据初始化 ----------------------------------------------
    int totalNumGroundTruthP = 0;
    // 存储不同 threshold 下的结果
    std::vector<double> vThreshold;
    std::vector<int> vTotalNumTP;
    std::vector<int> vTotalNumFP;
    std::vector<int> vTotalNumFN;
    for(double thres = 0.10; thres > -1e-6; thres -= 0.002){
        vThreshold.push_back(thres);
        vTotalNumTP.push_back(0);
        vTotalNumFP.push_back(0);
        vTotalNumFN.push_back(0);
    }

    // ---------------------------------- load & create KF Database ----------------------------------
    std::cout << "load the vocabulary ..." << std::endl;
    DBoW3::Vocabulary voc(strPathToVocabulary);
    std::cout << "has done." << endl;

    // 记录 存储在 Database 中的 frame 对应的 frame id
    std::vector<unsigned long> vFrameIdInDatabase;

    std::cout << "create the database ..." << std::endl;
    DBoW3::Database db(voc, true, 1);
    cv::Ptr<cv::ORB> orb = cv::ORB::create();
    for(size_t i = 0,  nImgs = vstrImageFilenames.size(); i < nImgs; i+=10)
    {
        cv::Mat originalImg = cv::imread(vstrImageFilenames[i], CV_LOAD_IMAGE_UNCHANGED);
        cv::Mat imgGray;
        cv::cvtColor(originalImg, imgGray, cv::COLOR_BGR2GRAY);

        cv::Mat vfeaturesInOneImg;
        ExtractFeaturesFromOneImage(imgGray, orb, vfeaturesInOneImg);
        DBoW3::BowVector bowVector;
        voc.transform(vfeaturesInOneImg, bowVector);

        db.add(bowVector);
        vFrameIdInDatabase.push_back(i);
    }
    std::cout << "has done." << endl;

    // ---------------------------------- query the specified image in the database & record the time cost ----------------------------------
    std::cout << "start test: querying ... " << endl;

    for(size_t nImgId = 0,  nImgs = vstrImageFilenames.size(); nImgId < nImgs; nImgId++){
        int current_id = nImgId;
        cv::Mat originalImg = cv::imread(vstrImageFilenames[nImgId], CV_LOAD_IMAGE_UNCHANGED);
        cv::Mat imgGray;
        cv::cvtColor(originalImg, imgGray, cv::COLOR_BGR2GRAY);

        // ------------------   根据 groundtruth 位姿找出数据集中总共存在的正确匹配的个数
        int numGroundTruthP = 0;
        for(auto &db_id: vFrameIdInDatabase){

            if( std::abs((int)current_id - (int)db_id) < 100){ // 时间上临近的就不算 LOOP 了
                continue;
            }
            bool bGroundTruthCorrect = PoseIsVeryNear(vPoses_R, vPoses_t, current_id, db_id);

            if (bGroundTruthCorrect){
                std::cout << std::setprecision(3) << "GroundTruth for Image "
                    << current_id << ":  db id: " << db_id  << std::endl;
                numGroundTruthP++;
            }
        }
        totalNumGroundTruthP += numGroundTruthP;

        // DBoW3 查询
        cv::Mat vfeaturesInOneImg;
        ExtractFeaturesFromOneImage(imgGray, orb, vfeaturesInOneImg);
        DBoW3::BowVector bowVector;
        voc.transform(vfeaturesInOneImg, bowVector);

        DBoW3::QueryResults queryResult;
        db.query(bowVector, queryResult, vFrameIdInDatabase.size(), -1); // max_result=1, max_id=-1 unlimited

        for(size_t index = 0; index < vThreshold.size(); index++){
            int numTP = 0;
            int numFP = 0;
            for(auto qit = queryResult.begin(); qit != queryResult.end(); qit++){
                int loop_id =  vFrameIdInDatabase[qit->Id];
                if(qit->Score < vThreshold[index]){
                    continue;
                }
                if( std::abs((int)current_id - (int)loop_id) < 100){ // 时间上临近的就不算 LOOP 了
                    continue;
                }
                bool bCorrect = PoseIsVeryNear(vPoses_R, vPoses_t, current_id, loop_id);
                bool bAcceptable = PoseIsAcceptablyNear(vPoses_R, vPoses_t, current_id, loop_id);

                 if (bCorrect){
                    numTP++;
                } else if ( ! bAcceptable) {
                    numFP++;
                }
                std::cout << std::setprecision(3) << "Searching for Image " << current_id << ":  loop id: " << loop_id
                        << ", score: " << qit->Score  << ", bAcceptable: " << bAcceptable << std::endl;
            }

            vTotalNumTP[index] += numTP;
            vTotalNumFP[index] += numFP;
        }

    }

   // 结果输出 保存至 txt 文件
    for(size_t i = 0; i < vThreshold.size(); i++){
        vTotalNumFN[i] = totalNumGroundTruthP - vTotalNumTP[i];

        double precision  = ((double)vTotalNumTP[i]) / (double)(vTotalNumTP[i] + vTotalNumFP[i]);
        double recall = (double)vTotalNumTP[i] / (double)(vTotalNumTP[i] + vTotalNumFN[i]);

        std::cout << "threshold:" << vThreshold[i] <<", precision: " << precision << ", recall: " << recall << std::endl;
        f << std::setprecision(6) << vThreshold[i] << " " << precision << " " << recall << std::endl;
    }

    f.close();
    std::cout << "result has been saved to  "  << filename << "." << std::endl;

    return 0;
}

// --------------------------------------------------------------------------------------------------------------------------
std::pair<double, double> VectorMeanAndVariance(const std::vector<double> v){
    double sum = std::accumulate(std::begin(v), std::end(v), 0.0);
    double m =  sum / v.size();
    double accum = 0.0;
    std::for_each (std::begin(v), std::end(v), [&](const double d) {
        accum += (d - m) * (d - m);
    });
    double stdev = sqrt(accum / (v.size()-1));

    std::pair<double, double> pairMeanAndVariance;
    pairMeanAndVariance.first = m;
    pairMeanAndVariance.second = stdev;

    return pairMeanAndVariance;
}

// --------------------------------------------------------------------------------------------------------------------------
void ExtractFeaturesFromOneImage(const cv::Mat &imgGray, cv::Ptr<cv::ORB> &orb,
    cv::Mat &descriptors){
     // extract the KeyPoints and corresponding descripors
    cv::Mat mask;
    std::vector<cv::KeyPoint> keypoints;
    orb->detectAndCompute(imgGray, mask, keypoints, descriptors);
}

// --------------------------------------------------------------------------------------------------------------------------
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
    ifstream fTimes;
    string strPathTimeFile = strPathToSequence + "/times.txt";
    fTimes.open(strPathTimeFile.c_str());
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            double t;
            ss >> t;
            vTimestamps.push_back(t);
        }
    }
    fTimes.close();

    string strPrefixLeft = strPathToSequence + "/image_2/";  // 使用 rgb 图

    const int nTimes = vTimestamps.size();
    vstrImageFilenames.resize(nTimes);

    for(int i=0; i<nTimes; i++)
    {
        stringstream ss;
        ss << setfill('0') << setw(6) << i;
        vstrImageFilenames[i] = strPrefixLeft + ss.str() + ".png";
    }
}

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

void LoadGroundtruthPose(const string &strPose,
    std::vector<Mat33, Eigen::aligned_allocator<Mat33> > &vPoses_R,
    std::vector<Vec3, Eigen::aligned_allocator<Vec3> > &vPoses_t){

    vPoses_R.clear();
    vPoses_t.clear();

    ifstream fPoses;
    std::string strPathPoseFile = strPose;
    fPoses.open(strPathPoseFile.c_str());
    while(!fPoses.eof())
    {
        double R0, R1, R2, R3, R4, R5, R6, R7, R8;
        double t0, t1, t2;
        fPoses >> R0 >> R1 >> R2 >> t0
                      >> R3 >> R4 >> R5 >> t1
                      >> R6 >> R7 >> R8 >> t2;

        Mat33 R;
        R << R0, R1, R2,
                  R3, R4, R5,
                  R6, R7, R8;
        Vec3 t;
        t << t0, t1, t2;

        vPoses_R.push_back(R);
        vPoses_t.push_back(t);
    }

    fPoses.close();
}

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

bool PoseIsVeryNear(std::vector<Mat33, Eigen::aligned_allocator<Mat33> > &vPoses_R,
    std::vector<Vec3, Eigen::aligned_allocator<Vec3> > &vPoses_t,
    int current_id, int loop_id){

    Eigen::AngleAxisd rotation_vector;
    rotation_vector.fromRotationMatrix(vPoses_R[current_id].inverse() * vPoses_R[loop_id]);

    bool bCorrect = ((vPoses_t[current_id] - vPoses_t[loop_id]).norm() < 5)
                                        && ((std::abs(rotation_vector.angle()) < 3.14 / 12));

    return bCorrect;
}

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

bool PoseIsAcceptablyNear(std::vector<Mat33, Eigen::aligned_allocator<Mat33> > &vPoses_R,
    std::vector<Vec3, Eigen::aligned_allocator<Vec3> > &vPoses_t,
    int current_id, int loop_id){

    Eigen::AngleAxisd rotation_vector;
    rotation_vector.fromRotationMatrix(vPoses_R[current_id].inverse() * vPoses_R[loop_id]);

    bool bCorrect = ((vPoses_t[current_id] - vPoses_t[loop_id]).norm() < 20)
                                        && ((std::abs(rotation_vector.angle()) < 3.14 / 6));

    return bCorrect;
}

// --------------------------------------------------------------------------------------------------------------------------
// void SaveTestResult(const string &filename, std::vector<double> &vTimeExtractFeatures,
//     std::vector<double> &vTimeExtractBoW, std::vector<double> &vTimeAddToDatabase,
//     std::vector<double> &vTimeQuerying){

//     ofstream f;
//     f.open(filename.c_str());
//     f << fixed;

//     for(size_t i = 0, n = vTimeExtractFeatures.size(); i < n; i++){
//          f <<  setprecision(6)

//     }

// }


/*
 * Copyright (c) 2015-2021 Agalmic Ventures LLC (www.agalmicventures.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.
 */

#include <boost/test/unit_test.hpp>

#include "Werk/Math/SummaryStatistics.hpp"
#include "Werk/Math/WeightedSummaryStatistics.hpp"

BOOST_AUTO_TEST_SUITE(WeightedSummaryStatisticsTest)

BOOST_AUTO_TEST_CASE(TestEmpty)
{
	werk::WeightedSummaryStatistics s;
	BOOST_REQUIRE_EQUAL(s.count(), 0);
	BOOST_REQUIRE_EQUAL(s.weightSum(), 0.0);
	BOOST_REQUIRE_EQUAL(s.sum(), 0.0);
}

BOOST_AUTO_TEST_CASE(TestBasicUnweighted)
{
	werk::WeightedSummaryStatistics s;
	s.sample(5.0, 1.0);
	s.sample(1.0, 1.0);

	BOOST_REQUIRE_EQUAL(s.count(), 2);
	BOOST_REQUIRE_EQUAL(s.weightSum(), 2.0);
	BOOST_REQUIRE_EQUAL(s.sum(), 6.0);
	BOOST_REQUIRE_EQUAL(s.average(), 3.0);
	BOOST_REQUIRE_EQUAL(s.variance(), 4.0);
	BOOST_REQUIRE_EQUAL(s.stddev(), 2.0);

	s.reset();
	BOOST_REQUIRE_EQUAL(s.count(), 0);
}

BOOST_AUTO_TEST_CASE(TestBasicEqualWeights)
{
	werk::WeightedSummaryStatistics s;
	s.sample(5.0, 2.0);
	s.sample(1.0, 2.0);

	BOOST_REQUIRE_EQUAL(s.count(), 2);
	BOOST_REQUIRE_EQUAL(s.weightSum(), 4.0);
	BOOST_REQUIRE_EQUAL(s.sum(), 12.0);
	BOOST_REQUIRE_EQUAL(s.average(), 3.0);
	BOOST_REQUIRE_EQUAL(s.variance(), 4.0);
	BOOST_REQUIRE_EQUAL(s.stddev(), 2.0);
}

BOOST_AUTO_TEST_CASE(TestBasicUnequalWeights)
{
	werk::WeightedSummaryStatistics s;
	s.sample(5.0, 1.0);
	s.sample(1.0, 3.0);

	BOOST_REQUIRE_EQUAL(s.count(), 2);
	BOOST_REQUIRE_EQUAL(s.weightSum(), 4.0);
	BOOST_REQUIRE_EQUAL(s.sum(), 8.0);
	BOOST_REQUIRE_EQUAL(s.average(), 2.0);
	BOOST_REQUIRE_EQUAL(s.variance(), 3.0);
}

BOOST_AUTO_TEST_CASE(TestAdvancedEqualWeights)
{
	werk::SummaryStatistics<double> s1;
	werk::WeightedSummaryStatistics s2;

	for (double i = 0.0; i < 1000.0; ++i) {
		s1.sample(i);
		s2.sample(i, 1.0);

		BOOST_REQUIRE_EQUAL(s1.count(), s2.count());
		BOOST_REQUIRE_EQUAL(s1.sum(), s2.sum());
		BOOST_REQUIRE_EQUAL(s2.weightSum(), i + 1.0);
		BOOST_REQUIRE_EQUAL(s1.average(), s2.average());
		BOOST_REQUIRE_EQUAL(s1.variance(), s2.variance());
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include "VDBVolume.h"

// OpenVDB
#include <openvdb/Types.h>
#include <openvdb/math/DDA.h>
#include <openvdb/math/Ray.h>
#include <openvdb/openvdb.h>

#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <functional>
#include <iostream>
#include <memory>
#include <vector>

namespace {

float ComputeSDF(const Eigen::Vector3d& origin,
                 const Eigen::Vector3d& point,
                 const Eigen::Vector3d& voxel_center) {
    const Eigen::Vector3d v_voxel_origin = voxel_center - origin;
    const Eigen::Vector3d v_point_voxel = point - voxel_center;
    const double dist = v_point_voxel.norm();
    const double proj = v_voxel_origin.dot(v_point_voxel);
    const double sign = proj / std::abs(proj);
    return static_cast<float>(sign * dist);
}

Eigen::Vector3d GetVoxelCenter(const openvdb::Coord& voxel, const openvdb::math::Transform& xform) {
    const float voxel_size = xform.voxelSize()[0];
    openvdb::math::Vec3d v_wf = xform.indexToWorld(voxel) + voxel_size / 2.0;
    return Eigen::Vector3d(v_wf.x(), v_wf.y(), v_wf.z());
}

}  // namespace

namespace vdbfusion {

VDBVolume::VDBVolume(float voxel_size, float sdf_trunc, bool space_carving /* = false*/)
    : voxel_size_(voxel_size), sdf_trunc_(sdf_trunc), space_carving_(space_carving) {
    tsdf_ = openvdb::FloatGrid::create(sdf_trunc_);
    tsdf_->setName("D(x): signed distance grid");
    tsdf_->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size_));
    tsdf_->setGridClass(openvdb::GRID_LEVEL_SET);

    weights_ = openvdb::FloatGrid::create(0.0f);
    weights_->setName("W(x): weights grid");
    weights_->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size_));
    weights_->setGridClass(openvdb::GRID_UNKNOWN);
}

void VDBVolume::UpdateTSDF(const float& sdf,
                           const openvdb::Coord& voxel,
                           const std::function<float(float)>& weighting_function) {
    using AccessorRW = openvdb::tree::ValueAccessorRW<openvdb::FloatTree>;
    if (sdf > -sdf_trunc_) {
        AccessorRW tsdf_acc = AccessorRW(tsdf_->tree());
        AccessorRW weights_acc = AccessorRW(weights_->tree());
        const float tsdf = std::min(sdf_trunc_, sdf);
        const float weight = weighting_function(sdf);
        const float last_weight = weights_acc.getValue(voxel);
        const float last_tsdf = tsdf_acc.getValue(voxel);
        const float new_weight = weight + last_weight;
        const float new_tsdf = (last_tsdf * last_weight + tsdf * weight) / (new_weight);
        tsdf_acc.setValue(voxel, new_tsdf);
        weights_acc.setValue(voxel, new_weight);
    }
}

void VDBVolume::Integrate(openvdb::FloatGrid::Ptr grid,
                          const std::function<float(float)>& weighting_function) {
    for (auto iter = grid->cbeginValueOn(); iter.test(); ++iter) {
        const auto& sdf = iter.getValue();
        const auto& voxel = iter.getCoord();
        this->UpdateTSDF(sdf, voxel, weighting_function);
    }
}

void VDBVolume::Integrate(const std::vector<Eigen::Vector3d>& points,
                          const Eigen::Vector3d& origin,
                          const std::function<float(float)>& weighting_function) {
    if (points.empty()) {
        std::cerr << "PointCloud provided is empty\n";
        return;
    }

    // Get some variables that are common to all rays
    const openvdb::math::Transform& xform = tsdf_->transform();
    const openvdb::Vec3R eye(origin.x(), origin.y(), origin.z());

    // Get the "unsafe" version of the grid acessors
    auto tsdf_acc = tsdf_->getUnsafeAccessor();
    auto weights_acc = weights_->getUnsafeAccessor();

    // Launch an for_each execution, use std::execution::par to parallelize this region
    std::for_each(points.cbegin(), points.cend(), [&](const auto& point) {
        // Get the direction from the sensor origin to the point and normalize it
        const Eigen::Vector3d direction = point - origin;
        openvdb::Vec3R dir(direction.x(), direction.y(), direction.z());
        dir.normalize();

        // Truncate the Ray before and after the source unless space_carving_ is specified.
        const auto depth = static_cast<float>(direction.norm());
        const float t0 = space_carving_ ? 0.0f : depth - sdf_trunc_;
        const float t1 = depth + sdf_trunc_;

        // Create one DDA per ray(per thread), the ray must operate on voxel grid coordinates.
        const auto ray = openvdb::math::Ray<float>(eye, dir, t0, t1).worldToIndex(*tsdf_);
        openvdb::math::DDA<decltype(ray)> dda(ray);
        do {
            const auto voxel = dda.voxel();
            const auto voxel_center = GetVoxelCenter(voxel, xform);
            const auto sdf = ComputeSDF(origin, point, voxel_center);
            if (sdf > -sdf_trunc_) {
                const float tsdf = std::min(sdf_trunc_, sdf);
                const float weight = weighting_function(sdf);
                const float last_weight = weights_acc.getValue(voxel);
                const float last_tsdf = tsdf_acc.getValue(voxel);
                const float new_weight = weight + last_weight;
                const float new_tsdf = (last_tsdf * last_weight + tsdf * weight) / (new_weight);
                tsdf_acc.setValue(voxel, new_tsdf);
                weights_acc.setValue(voxel, new_weight);
            }
        } while (dda.step());
    });
}

openvdb::FloatGrid::Ptr VDBVolume::Prune(float min_weight) const {
    const auto weights = weights_->tree();
    const auto tsdf = tsdf_->tree();
    const auto background = sdf_trunc_;
    openvdb::FloatGrid::Ptr clean_tsdf = openvdb::FloatGrid::create(sdf_trunc_);
    clean_tsdf->setName("D(x): Pruned signed distance grid");
    clean_tsdf->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size_));
    clean_tsdf->setGridClass(openvdb::GRID_LEVEL_SET);
    clean_tsdf->tree().combine2Extended(tsdf, weights, [=](openvdb::CombineArgs<float>& args) {
        if (args.aIsActive() && args.b() > min_weight) {
            args.setResult(args.a());
            args.setResultIsActive(true);
        } else {
            args.setResult(background);
            args.setResultIsActive(false);
        }
    });
    return clean_tsdf;
}
}  // namespace vdbfusion


/*
 * FeatureChannelExtractor.cpp
 *
 *  Created on: Sep 7, 2011
 *      Author: Matthias Dantone
 */

#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#include <boost/thread.hpp>
#include <boost/foreach.hpp>
#include <glog/logging.h>

#include "hog_extractor.h"
#include "feature_channel_factory.hpp"

#include "cpp/fashion/skin/SkinSegmentation.h"
#include "cpp/utils/thread_pool.hpp"
#include "cpp/utils/timing.hpp"
#include "cpp/vision/min_max_filter.hpp"

using namespace cv;
using namespace std;

namespace vision {
namespace features {
namespace feature_channels {

FeatureChannelFactory::FeatureChannelFactory() {
  init_gabor_kernels();
}

void FeatureChannelFactory::extractChannel(int type, bool useIntegral,
    const Mat& src,
    const Mat& src_color,
    vector<Mat>& channels) const {

  vector<Mat> tmp;
  if (type == FC_GRAY) {
    extract_fc_gray(src, src_color, tmp);

  }else if (type == FC_NORM) {
    extract_fc_norm(src, src_color, tmp);

  }else if (type == FC_GABOR) {
    extract_fc_gabor(src, src_color, tmp);

  } else if (type == FC_SOBEL){
    extract_fc_sobel(src, src_color, tmp);

  } else if (type == FC_MIN_MAX){
    extract_fc_mim_max(src, src_color, tmp);

  } else if (type == FC_CANNY) {
    extract_fc_canny(src, src_color, tmp);

  } else if (type == FC_HOG) {
    extract_fc_hog(src, src_color, tmp);

  } else if( type == FC_SKIN) {
    extract_fc_skin(src, src_color, tmp);

  }else if( type == FC_LBP){
    extract_fc_lbp(src, src_color, tmp);

  }else if(type == FC_SURF){
    extract_fc_surf(src, src_color, tmp);

  }else if(type == FC_T2SURF) {
		extract_fc_t2surf(src, src_color, tmp);

	}else if(type == FC_LAB){
    extract_fc_lab(src, src_color, tmp);
  }else if(type == FC_COLOR_HOG){
    extract_fc_color_hog(src, src_color, tmp);
  }else{
    LOG(ERROR)<< "unkown feature channel" << std::endl;
    CHECK(false);
  }

  if(useIntegral) {
    for(unsigned int i = 0; i < tmp.size(); i++) {
      cv::Mat int_out;
      cv::integral(tmp[i], int_out, CV_32F);
      channels.push_back(int_out);
    }
  }else{
    channels.insert(channels.end(), tmp.begin(), tmp.end());
  }

}

void FeatureChannelFactory::extract_fc_gray( const Mat& src,
      const Mat& src_color,
      vector<Mat>& channels) const {
  channels.push_back(src);
}

void FeatureChannelFactory::extract_fc_norm( const Mat& src,
      const Mat& src_color,
      vector<Mat>& channels) const {
  cv::Mat normal;
  cv::equalizeHist(src, normal);
  channels.push_back(normal);
}

void FeatureChannelFactory::extract_fc_gabor( const Mat& src,
  const Mat& src_color,
  vector<Mat>& channels) const {
  //check if kernels are initzialized
  int old_size = channels.size();
  bool multithreaded = true;
  if (multithreaded) {
    channels.resize(channels.size() + reals.size());
    int num_treads = boost::thread::hardware_concurrency();
    boost::thread_pool::executor e(num_treads);
    for (unsigned int i = 0; i < reals.size(); i++) {
      e.submit(boost::bind(&FeatureChannelFactory::gabor_transform,
          this, src, &channels[old_size + i],  i, old_size));
    }
    e.join_all();
  } else {
    for (unsigned int i = 0; i < reals.size(); i++) {
      cv::Mat final;
      cv::Mat r_mat;
      cv::Mat i_mat;
      cv::filter2D(src, r_mat, CV_32F, reals[i]);
      cv::filter2D(src, i_mat, CV_32F, imags[i]);
      cv::pow(r_mat, 2, r_mat);
      cv::pow(i_mat, 2, i_mat);
      cv::add(i_mat, r_mat, final);
      cv::pow(final, 0.5, final);
      cv::normalize(final, final, 0, 1, CV_MINMAX, CV_32F);

      final.convertTo(final, CV_8UC1, 255);
      channels.push_back(final);

    }
  }
}

void FeatureChannelFactory::extract_fc_sobel( const Mat& src,
      const Mat& src_color,
      vector<Mat>& channels) const {
  Mat sob_x(src.size(), CV_8U);
  Mat sob_y(src.size(), CV_8U);

  Sobel(src, sob_x, CV_8U, 0, 1);
  Sobel(src, sob_y, CV_8U, 1, 0);

  channels.push_back(sob_x);
  channels.push_back(sob_y);
}

void FeatureChannelFactory::extract_fc_mim_max( const Mat& src,
      const Mat& src_color,
      vector<Mat>& channels) const {
  cv::Mat kernel(cv::Size(3, 3), CV_8UC1);
  kernel.setTo(cv::Scalar(1));
  cv::Mat img_min(src.size(), CV_8U);
  cv::Mat img_max(src.size(), CV_8U);

  cv::erode(src, img_min, kernel);
  cv::dilate(src, img_max, kernel);

  channels.push_back(img_min);
  channels.push_back(img_max);
}

void FeatureChannelFactory::extract_fc_canny( const Mat& src,
      const Mat& src_color,
      vector<Mat>& channels) const {
  cv::Mat cannyImg;
  cv::Canny(src, cannyImg, -1, 5);
  channels.push_back(cannyImg);
}

void FeatureChannelFactory::extract_fc_hog( const Mat& src,
      const Mat& src_color,
      vector<Mat>& channels) const {
  HOGExtractor hog_ex;
  CHECK(src_color.data != NULL);
  hog_ex.extractFeatureChannels15( src_color, channels );
}

void FeatureChannelFactory::extract_fc_skin( const Mat& src,
      const Mat& src_color,
      vector<Mat>& channels) const {

  fashion::SkinSegmentation skin_seg;
  cv::Mat skin_probability_map;
  cv::Mat skin_class_map;
  CHECK(src_color.data != NULL);
  skin_seg.createSkinProbabilityMap(src_color, &skin_probability_map );
  skin_seg.createSkinPixelMap(src_color, &skin_class_map);
  channels.push_back(skin_probability_map);
  channels.push_back(skin_class_map);
}

void FeatureChannelFactory::extract_fc_lbp( const Mat& src,
      const Mat& src_color,
      vector<Mat>& channels) const {
  cv::Mat src_blur = src;
//  cv::blur(src_blur, src_blur, cv::Size(3,3));

  std::vector<int> step_widths;
  step_widths.push_back(1);
  step_widths.push_back(2);
  step_widths.push_back(3);
  step_widths.push_back(4);
  step_widths.push_back(5);
  step_widths.push_back(6);
  BOOST_FOREACH(int step_width, step_widths) {
    cv::Mat out = cv::Mat(src_blur.size(), CV_8UC1);
    for (int x = step_width; x < src_blur.cols-step_width; x++) {
      for (int y = step_width; y < src_blur.rows-step_width; y++) {
        uchar feature = vision::features::get_lbp_code(y,x, src_blur,step_width);
        out.at<unsigned char>(y,x) = feature;
      }
    }
    channels.push_back(out);
  }
}

void FeatureChannelFactory::extract_fc_surf( const Mat& src,
      const Mat& src_color,
      vector<Mat>& channels) const {
  Mat dx, dx_min;
  Sobel(src,dx,CV_8U,1,0,3);
  Sobel(src,dx_min,CV_8U,1,0,3,-1);

  Mat dy, dy_min;
  Sobel(src,dy,CV_8U,0,1,3);
  Sobel(src,dy_min,CV_8U,0,1,3,-1);

  channels.push_back(dx);
  channels.push_back(dx_min);
  channels.push_back(dy);
  channels.push_back(dy_min);
}

void FeatureChannelFactory::extract_fc_t2surf( const Mat& src,
      const Mat& src_color,
      vector<Mat>& channels) const {
	// Extract standard SURF.
	extract_fc_surf(src, src_color, channels);
	for (int c = 0; c < channels.size(); ++c) {
		channels[c].convertTo(channels[c], CV_32F);
		channels[c] *= 1.0 / 255;
	}

	const Mat& dx = channels[0];
	const Mat& dx_min = channels[1];
	const Mat& dy = channels[2];
	const Mat& dy_min = channels[3];

	Mat dx2, dx_min2, dy2, dy_min2;
	cv::pow(dx, 2, dx2);
	cv::pow(dx_min, 2, dx_min2);
	cv::pow(dy, 2, dy2);
	cv::pow(dy_min, 2, dy_min2);

	Mat dx45;
	cv::add(dx2, dy2, dx45);
	cv::sqrt(dx45, dx45);

	Mat dx45_min;
	cv::add(dx_min2, dy_min2, dx45_min);
	cv::sqrt(dx45_min, dx45_min);

	Mat dy45;
	cv::subtract(dx2, dy2, dy45);
	cv::sqrt(dy45, dy45);

	Mat dy45_min;
	cv::subtract(dx_min2, dy_min2, dy45_min);
	cv::sqrt(dy45_min, dy45_min);

	channels.push_back(dx45);
	channels.push_back(dx45_min);
	channels.push_back(dy45);
	channels.push_back(dy45_min);

	for (int c = 0; c < channels.size(); ++c) {
		channels[c] *= 255;
		channels[c].convertTo(channels[c], CV_8U);
	}
}

void FeatureChannelFactory::extract_fc_lab( const cv::Mat& src,
    const cv::Mat& src_color,
    std::vector<cv::Mat>& channels) const {

  Mat imgRGB(src_color.size(), CV_8UC3);
  cvtColor( src_color, imgRGB, CV_RGB2Lab  );
  cv::split(imgRGB, channels);
  channels.resize(6);

  for(int c=0; c < 3; c++) {
    channels[c+3].create(src_color.rows,src_color.cols, CV_8U);
    vision::MinMaxFilter::minfilt(channels[c], channels[c+3], 5);
  }
  for(int c=0; c < 3; c++) {
    vision::MinMaxFilter::maxfilt(channels[c], 5);
  }
}

void FeatureChannelFactory::extract_fc_color_hog( const cv::Mat& src,
    const cv::Mat& src_color,
    std::vector<cv::Mat>& channels, bool use_max_filter) const {

  std::vector<cv::Mat> channels_tmp;
  Mat imgRGB(src_color.size(), CV_8UC3);
//  cvtColor( src_color, imgRGB, CV_RGB2Lab  );
  cv::split(src_color, channels_tmp);

  for(int i=0; i < channels_tmp.size(); i++) {
    cv::equalizeHist(channels_tmp[i], channels_tmp[i]);

    std::vector<cv::Mat> channels_hog;
    HOGExtractor hog_ex;
    hog_ex.extractFeatureChannels9(channels_tmp[i],channels_hog, use_max_filter);

    channels.push_back(channels_tmp[i]);
    for(int j=0; j < channels_hog.size(); j++) {
      channels.push_back(channels_hog[j]);
    }

  }
}

void FeatureChannelFactory::gabor_transform(const cv::Mat& src, cv::Mat* dst,
      int index, int old_size) const {
    cv::Mat final;
    cv::Mat r_mat;
    cv::Mat i_mat;
    cv::filter2D(src, r_mat, CV_32F, reals[index]);
    cv::filter2D(src, i_mat, CV_32F, imags[index]);
    cv::pow(r_mat, 2, r_mat);
    cv::pow(i_mat, 2, i_mat);
    cv::add(i_mat, r_mat, final);
    cv::pow(final, 0.5, final);
    cv::normalize(final, final, 0, 1, CV_MINMAX);

    final.convertTo(final, CV_8UC1, 255);
    *dst = final;
}

void FeatureChannelFactory::init_gabor_kernels() {

  //create kernels
  int NuMin = 0;
  int NuMax = 4;
  int MuMin = 0;
  int MuMax = 7;
  double sigma = 1. / 2.0 * CV_PI;
  double dF = sqrt(2.0);

  int iMu = 0;
  int iNu = 0;

  for (iNu = NuMin; iNu <= NuMax; iNu++)
    for (iMu = MuMin; iMu < MuMax; iMu++)
      createKernel(iMu, iNu, sigma, dF);

};

void FeatureChannelFactory::createKernel(int iMu, int iNu, double sigma, double dF) {
  //Initilise the parameters
  double F = dF;
  double k = (CV_PI / 2) / pow(F, (double) iNu);
  double phi = CV_PI * iMu / 8;

  double width = round((sigma / k) * 6 + 1);
  if (fmod(width, 2.0) == 0.0)
    width++;

  //create kernel
  cv::Mat m_real = cv::Mat(width, width, CV_32FC1);
  cv::Mat m_imag = cv::Mat(width, width, CV_32FC1);

  int x, y;
  double dReal;
  double dImag;
  double dTemp1, dTemp2, dTemp3;

  int off_set = (width - 1) / 2;
  for (int i = 0; i < width; i++) {
    for (int j = 0; j < width; j++) {
      x = i - off_set;
      y = j - off_set;
      dTemp1 = (pow(k, 2) / pow(sigma, 2)) * exp(-(pow((double) x, 2) + pow((double) y, 2)) * pow(k, 2) / (2 * pow(sigma, 2)));
      dTemp2 = cos(k * cos(phi) * x + k * sin(phi) * y) - exp(-(pow(sigma, 2) / 2));
      dTemp3 = sin(k * cos(phi) * x + k * sin(phi) * y);
      dReal = dTemp1 * dTemp2;
      dImag = dTemp1 * dTemp3;
      m_real.at<float>(j, i) = dReal;
      m_imag.at<float>(j, i) = dImag;
    }
  }

  reals.push_back(m_real);
  imags.push_back(m_imag);
};

} //namespace vision
} //namespace features
} //namespace feature_channels


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * @file    testDecisionTree.cpp
 * @brief    Develop DecisionTree
 * @author  Frank Dellaert
 * @author  Can Erdogan
 * @date    Jan 30, 2012
 */

#include <boost/timer.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/Signature.h>

//#define DT_DEBUG_MEMORY
//#define DT_NO_PRUNING
#define DISABLE_DOT
#include <gtsam/discrete/DecisionTree-inl.h>
using namespace std;
using namespace gtsam;

template<typename T>
void dot(const T&f, const string& filename) {
#ifndef DISABLE_DOT
  f.dot(filename);
#endif
}

#define DOT(x)(dot(x,#x))

struct Crazy { int a; double b; };
typedef DecisionTree<string,Crazy> CrazyDecisionTree; // check that DecisionTree is actually generic (as it pretends to be)

// traits
namespace gtsam {
template<> struct traits<CrazyDecisionTree> : public Testable<CrazyDecisionTree> {};
}

/* ******************************************************************************** */
// Test string labels and int range
/* ******************************************************************************** */

typedef DecisionTree<string, int> DT;

// traits
namespace gtsam {
template<> struct traits<DT> : public Testable<DT> {};
}

struct Ring {
  static inline int zero() {
    return 0;
  }
  static inline int one() {
    return 1;
  }
  static inline int add(const int& a, const int& b) {
    return a + b;
  }
  static inline int mul(const int& a, const int& b) {
    return a * b;
  }
};

/* ******************************************************************************** */
// test DT
TEST(DT, example)
{
  // Create labels
  string A("A"), B("B"), C("C");

  // create a value
  Assignment<string> x00, x01, x10, x11;
  x00[A] = 0, x00[B] = 0;
  x01[A] = 0, x01[B] = 1;
  x10[A] = 1, x10[B] = 0;
  x11[A] = 1, x11[B] = 1;

  // A
  DT a(A, 0, 5);
  LONGS_EQUAL(0,a(x00))
  LONGS_EQUAL(5,a(x10))
  DOT(a);

  // pruned
  DT p(A, 2, 2);
  LONGS_EQUAL(2,p(x00))
  LONGS_EQUAL(2,p(x10))
  DOT(p);

  // \neg B
  DT notb(B, 5, 0);
  LONGS_EQUAL(5,notb(x00))
  LONGS_EQUAL(5,notb(x10))
  DOT(notb);

  // apply, two nodes, in natural order
  DT anotb = apply(a, notb, &Ring::mul);
  LONGS_EQUAL(0,anotb(x00))
  LONGS_EQUAL(0,anotb(x01))
  LONGS_EQUAL(25,anotb(x10))
  LONGS_EQUAL(0,anotb(x11))
  DOT(anotb);

  // check pruning
  DT pnotb = apply(p, notb, &Ring::mul);
  LONGS_EQUAL(10,pnotb(x00))
  LONGS_EQUAL( 0,pnotb(x01))
  LONGS_EQUAL(10,pnotb(x10))
  LONGS_EQUAL( 0,pnotb(x11))
  DOT(pnotb);

  // check pruning
  DT zeros = apply(DT(A, 0, 0), notb, &Ring::mul);
  LONGS_EQUAL(0,zeros(x00))
  LONGS_EQUAL(0,zeros(x01))
  LONGS_EQUAL(0,zeros(x10))
  LONGS_EQUAL(0,zeros(x11))
  DOT(zeros);

  // apply, two nodes, in switched order
  DT notba = apply(a, notb, &Ring::mul);
  LONGS_EQUAL(0,notba(x00))
  LONGS_EQUAL(0,notba(x01))
  LONGS_EQUAL(25,notba(x10))
  LONGS_EQUAL(0,notba(x11))
  DOT(notba);

  // Test choose 0
  DT actual0 = notba.choose(A, 0);
  EXPECT(assert_equal(DT(0.0), actual0));
  DOT(actual0);

  // Test choose 1
  DT actual1 = notba.choose(A, 1);
  EXPECT(assert_equal(DT(B, 25, 0), actual1));
  DOT(actual1);

  // apply, two nodes at same level
  DT a_and_a = apply(a, a, &Ring::mul);
  LONGS_EQUAL(0,a_and_a(x00))
  LONGS_EQUAL(0,a_and_a(x01))
  LONGS_EQUAL(25,a_and_a(x10))
  LONGS_EQUAL(25,a_and_a(x11))
  DOT(a_and_a);

  // create a function on C
  DT c(C, 0, 5);

  // and a model assigning stuff to C
  Assignment<string> x101;
  x101[A] = 1, x101[B] = 0, x101[C] = 1;

  // mul notba with C
  DT notbac = apply(notba, c, &Ring::mul);
  LONGS_EQUAL(125,notbac(x101))
  DOT(notbac);

  // mul now in different order
  DT acnotb = apply(apply(a, c, &Ring::mul), notb, &Ring::mul);
  LONGS_EQUAL(125,acnotb(x101))
  DOT(acnotb);
}

/* ******************************************************************************** */
// test Conversion
enum Label {
  U, V, X, Y, Z
};
typedef DecisionTree<Label, bool> BDT;
bool convert(const int& y) {
  return y != 0;
}

TEST(DT, conversion)
{
  // Create labels
  string A("A"), B("B");

  // apply, two nodes, in natural order
  DT f1 = apply(DT(A, 0, 5), DT(B, 5, 0), &Ring::mul);

  // convert
  map<string, Label> ordering;
  ordering[A] = X;
  ordering[B] = Y;
  boost::function<bool(const int&)> op = convert;
  BDT f2(f1, ordering, op);
  //  f1.print("f1");
  //  f2.print("f2");

  // create a value
  Assignment<Label> x00, x01, x10, x11;
  x00[X] = 0, x00[Y] = 0;
  x01[X] = 0, x01[Y] = 1;
  x10[X] = 1, x10[Y] = 0;
  x11[X] = 1, x11[Y] = 1;
  EXPECT(!f2(x00));
  EXPECT(!f2(x01));
  EXPECT(f2(x10));
  EXPECT(!f2(x11));
}

/* ******************************************************************************** */
// test Compose expansion
TEST(DT, Compose)
{
  // Create labels
  string A("A"), B("B"), C("C");

  // Put two stumps on A together
  DT f1(B, DT(A, 0, 1), DT(A, 2, 3));

  // Create from string
  vector<DT::LabelC> keys;
  keys += DT::LabelC(A,2), DT::LabelC(B,2);
  DT f2(keys, "0 2 1 3");
  EXPECT(assert_equal(f2, f1, 1e-9));

  // Put this AB tree together with another one
  DT f3(keys, "4 6 5 7");
  DT f4(C, f1, f3);
  DOT(f4);

  // a bigger tree
  keys += DT::LabelC(C,2);
  DT f5(keys, "0 4 2 6 1 5 3 7");
  EXPECT(assert_equal(f5, f4, 1e-9));
  DOT(f5);
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


/**
 * @file max_coverage_example.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-03-13
 */
//! [Max Coverage Example]
#include <iostream>
#include <vector>
#include <iterator>

#include <boost/range/irange.hpp>

#include "paal/greedy/set_cover/maximum_coverage.hpp"

int main() {
    std::vector<std::vector<int>> set_to_elements = {
        { 1, 2 },
        { 3, 4, 5, 6 },
        { 7, 8, 9, 10, 11, 12, 13, 0 },
        { 1, 3, 5, 7, 9, 11, 13 },
        { 2, 4, 6, 8, 10, 12, 0 }
    };
    const int NUMBER_OF_SETS_TO_SELECT = 2;
    auto sets = boost::irange(0, 5);
    using SetIterator = decltype(sets)::iterator;
    std::vector<int> result;
    auto element_index = [](int el){return el;};
    auto covered = paal::greedy::maximum_coverage(
        sets,
        [&](int set){return set_to_elements[set];},
        back_inserter(result),
        element_index,
        NUMBER_OF_SETS_TO_SELECT);
    std::cout << "Covered: " << covered << std::endl;
}
//! [Max Coverage Example]


// Copyright (c) 2020 The Navcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "blsct/bulletproofs.h"
#include "test/test_navcoin.h"

#include <map>

#include <boost/test/unit_test.hpp>
#include "boost/assign.hpp"

BOOST_FIXTURE_TEST_SUITE(bulletproofsrangeproof, BasicTestingSetup)

bool TestRange(std::vector<Scalar> values, bls::G1Element nonce)
{
    std::vector<Scalar> gamma;
    std::vector<bls::G1Element> nonces;

    for (unsigned int i = 0; i < values.size(); i++)
    {
        gamma.push_back(HashG1Element(nonce, 100+i));
        nonces.push_back(nonce);
    }

    BulletproofsRangeproof bprp;
    bprp.Prove(values, nonce, {1, 2, 3, 4});

    std::vector<std::pair<int, BulletproofsRangeproof>> proofs;
    proofs.push_back(std::make_pair(0,bprp));

    std::vector<RangeproofEncodedData> data;

    bool ret = VerifyBulletproof(proofs, data, nonces);

    if (!ret)
        return ret;

    if (values.size() == 1)
    {
        for (unsigned int i = 0; i < values.size(); i++)
        {
            if(data[i].amount != values[i].GetInt64())
                return false;
            if(!(data[i].gamma == gamma[i]))
                return false;
            if(data[i].message[0] != 1)
                return false;
            if(data[i].message[1] != 2)
                return false;
            if(data[i].message[2] != 3)
                return false;
            if(data[i].message[3] != 4)
                return false;
        }
    }

    return true;
}

bool TestRangeBatch(std::vector<Scalar> values, bls::G1Element nonce)
{
    std::vector<std::pair<int, BulletproofsRangeproof>> proofs;
    std::vector<bls::G1Element> nonces;
    std::vector<Scalar> gamma;
    Scalar mask = HashG1Element(nonce, 100);

    for (unsigned int i = 0; i < values.size(); i++)
    {
        std::vector<Scalar> v;
        v.push_back(values[i]);

        gamma.push_back(mask);

        BulletproofsRangeproof bprp;
        bprp.Prove(v, nonce, {1,2,3,4});

        nonces.push_back(nonce);

        proofs.push_back(std::make_pair(i, bprp));
    }

    std::vector<RangeproofEncodedData> data;

    bool ret = VerifyBulletproof(proofs, data, nonces);

    if (!ret)
        return ret;

    for (unsigned int i = 0; i < values.size(); i++)
    {
        if(data[i].amount != values[i].GetInt64())
            return false;
        if(!(data[i].gamma == gamma[i]))
            return false;
        if(data[i].message[0] != 1)
            return false;
        if(data[i].message[1] != 2)
            return false;
        if(data[i].message[2] != 3)
            return false;
        if(data[i].message[3] != 4)
            return false;
    }

    return true;
}

BOOST_AUTO_TEST_CASE(RangeProofTest)
{
    std::vector<Scalar> vInRange;
    std::vector<Scalar> vOutOfRange;

    bls::G1Element nonce = bls::G1Element::Infinity();

    // Admited range is (0, 2**64)
    Scalar one;
    one = 1;
    Scalar two;
    two = 2;
    Scalar twoPow64;
    twoPow64.SetPow2(64);
    Scalar bnLowerBound = 0;
    Scalar bnUpperBound = twoPow64-one;

    vInRange.push_back(bnLowerBound);
    vInRange.push_back(bnLowerBound+one);
    vInRange.push_back(bnUpperBound-one);
    vInRange.push_back(bnUpperBound);
    vOutOfRange.push_back(bnUpperBound+one);
    vOutOfRange.push_back(bnUpperBound+one+one);
    vOutOfRange.push_back(bnUpperBound*two);
    vOutOfRange.push_back(one.Negate());

    for (Scalar v: vInRange)
    {
        std::vector<Scalar> values;
        bn_print(v.bn);
        values.push_back(v);
        BOOST_CHECK(TestRange(values, nonce));
    }

    for (Scalar v: vOutOfRange)
    {
        std::vector<Scalar> values;
        values.push_back(v);
        BOOST_CHECK(!TestRange(values, nonce));
    }

    BOOST_CHECK(TestRangeBatch(vInRange, nonce));
    BOOST_CHECK(TestRange(vInRange, nonce));
    BOOST_CHECK(!TestRangeBatch(vOutOfRange, nonce));
    BOOST_CHECK(!TestRange(vOutOfRange, nonce));
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/cotpi.hpp>
#include <boost/simd/function/fast.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>

STF_CASE_TPL (" cotpi",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cotpi;

  using r_t = decltype(cotpi(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(cotpi(-bs::Zero<T>()), -bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::One<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Zero<T>()), bs::Inf<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(cotpi(-bs::Quarter<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Half<T>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Mhalf<T>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cotpi(bs::Quarter<T>()), bs::One<r_t>(), 0.5);
}
STF_CASE_TPL (" cotpi fast_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cotpi;

  using r_t = decltype(cotpi(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::fast_(cotpi)(-bs::Zero<T>()), -bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cotpi)(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cotpi)(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cotpi)(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cotpi)(bs::One<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cotpi)(bs::Zero<T>()), bs::Inf<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(bs::fast_(cotpi)(-bs::Quarter<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cotpi)(bs::Half<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cotpi)(bs::Mhalf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::fast_(cotpi)(bs::Quarter<T>()), bs::One<r_t>(), 0.5);
}


/*
 * This file is part of the Interpolated Polyline (https://github.com/fzi-forschungszentrum-informatik/P3IV),
 * copyright by FZI Forschungszentrum Informatik, licensed under the BSD-3 license (see LICENSE file in main directory)
 */

#pragma once
#include <iostream>
#include <Eigen/Core>
#include "sequence_distribution.hpp"

namespace util_probability {

template <typename T>
struct TruncatedUnivariateNormalDistributionSequence : public UnivariateNormalDistributionSequence<T> {

    TruncatedUnivariateNormalDistributionSequence() = default;

    using Mean = Eigen::Matrix<T, Eigen::Dynamic, 1>;
    using Variance = Eigen::Matrix<T, Eigen::Dynamic, 1>;
    using Truncation = Eigen::Matrix<T, Eigen::Dynamic, 1>;

    // todo: check to pass (const std::vector<T> mean_ ...)
    TruncatedUnivariateNormalDistributionSequence(std::vector<T> mean_,
                                                  std::vector<T> variance_,
                                                  std::vector<T> upper_bound_,
                                                  std::vector<T> lower_bound_) {

        assert(mean_.size() == variance_.size());

        this->_mean = Eigen::Map<Mean>(mean_.data(), mean_.size(), 1);
        this->_covariance = Eigen::Map<Variance>(variance_.data(), variance_.size(), 1);
        this->_upper_truncation = Eigen::Map<Truncation>(upper_bound_.data(), upper_bound_.size(), 1);
        this->_lower_truncation = Eigen::Map<Truncation>(lower_bound_.data(), lower_bound_.size(), 1);
    }

    // todo: replace with range()
    std::vector<T> upperBound(const T sigma) const {

        std::vector<double> upper_bound;
        upper_bound.resize(this->_mean.size());

        for (size_t i = 0; i < upper_bound.size(); i++) {
            upper_bound[i] = std::min(_upper_truncation(i, 0), this->_mean(i, 0) + this->_covariance(i, 0) * sigma);
        }
        return upper_bound;
    }

    std::vector<T> lowerBound(const T sigma) const {

        std::vector<double> lower_bound;
        lower_bound.resize(this->_mean.size());

        for (size_t i = 0; i < lower_bound.size(); i++) {
            lower_bound[i] = std::max(_lower_truncation(i, 0), this->_mean(i, 0) - this->_covariance(i, 0) * sigma);
        }
        return lower_bound;
    }

protected:
    Truncation _upper_truncation;
    Truncation _lower_truncation;
};

} // namespace util_probability