//
// Copyright (c) 2017 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#include "hpp/bezier-com-traj/solver/solver-abstract.hpp"
#ifdef USE_GLPK_SOLVER
#include <hpp/bezier-com-traj/solver/glpk-wrapper.hpp>
#include <glpk.h>
#endif
#include <hpp/bezier-com-traj/solver/eiquadprog-fast.hpp>

#include <Eigen/Sparse>
#include <stdexcept>

namespace solvers {

template <typename Derived>
inline bool is_nan(const Eigen::MatrixBase<Derived>& x) {
  bool isnan = !((x.array() == x.array()).all());
  return isnan;
}

typedef Eigen::SparseMatrix<double> SpMat;
typedef Eigen::SparseVector<double> SpVec;
typedef Eigen::SparseVector<int> SpVeci;

namespace {
void addConstraintMinBoundQuadProg(solvers::Cref_vectorX minBounds, std::pair<MatrixXd, VectorXd>& data) {
  if (minBounds.size() == 0) return;
  MatrixXd& res = data.first;
  VectorXd& resv = data.second;
  MatrixXd D(res.rows() + res.cols(), res.cols());
  VectorXd d(resv.rows() + res.cols());
  D.block(0, 0, res.rows(), res.cols()) = res;
  D.block(res.rows(), 0, res.cols(), res.cols()) = (-1.) * MatrixXd::Identity(res.cols(), res.cols());
  d.head(resv.size()) = resv;
  d.tail(res.cols()) = -minBounds;
  data.first = D;
  data.second = d;
}

void addConstraintMaxBoundQuadProg(solvers::Cref_vectorX maxBounds, std::pair<MatrixXd, VectorXd>& data) {
  if (maxBounds.size() == 0) return;
  MatrixXd& res = data.first;
  VectorXd& resv = data.second;
  MatrixXd D(res.rows() + res.cols() - 3, res.cols());
  VectorXd d(resv.rows() + res.cols());
  D.block(0, 0, res.rows(), res.cols()) = res;
  D.block(res.rows(), 0, res.cols(), res.cols()) = MatrixXd::Identity(res.cols(), res.cols());
  d.head(resv.size()) = resv;
  d.tail(res.cols()) = maxBounds;
  data.first = D;
  data.second = d;
}

std::pair<MatrixXd, VectorXd> addBoundaryConstraintsQuadProg(solvers::Cref_vectorX minBounds,
                                                             solvers::Cref_vectorX maxBounds, const MatrixXd& CI,
                                                             const VectorXd& ci0) {
  std::pair<MatrixXd, VectorXd> data;
  data.first = CI;
  data.second = ci0;
  addConstraintMinBoundQuadProg(minBounds, data);
  addConstraintMaxBoundQuadProg(maxBounds, data);
  return data;
}
}  // namespace

ResultData solve(const MatrixXd& A, const VectorXd& b, const MatrixXd& D, const VectorXd& d, const MatrixXd& Hess,
                 const VectorXd& g, const VectorXd& initGuess, solvers::Cref_vectorX minBounds,
                 solvers::Cref_vectorX maxBounds, const SolverType solver) {
  assert(!(is_nan(A)));
  assert(!(is_nan(b)));
  assert(!(is_nan(D)));
  assert(!(is_nan(d)));
  assert(!(is_nan(initGuess)));
  ResultData res;
  res.x = initGuess;
  switch (solver) {
    /*
     * solves the problem
     * min. x' Hess x + 2 g0' x
     * s.t. CE x + ce0 = 0
     *      CI x + ci0 >= 0
     * Thus CI = -A; ci0 = b
     * CI = D; ce0 = -d
     */
    case SOLVER_QUADPROG:
      // case SOLVER_QUADPROG_SPARSE:
      {
        assert(!(is_nan(Hess)));
        std::pair<MatrixXd, VectorXd> CIp = addBoundaryConstraintsQuadProg(minBounds, maxBounds, A, b);
        VectorXd ce0 = -d;
        tsid::solvers::EiquadprogFast QPsolver = tsid::solvers::EiquadprogFast();
        tsid::solvers::EiquadprogFast_status status;
        // if(solver == SOLVER_QUADPROG)
        status = QPsolver.solve_quadprog(Hess, g, D, ce0, -CIp.first, CIp.second, res.x);
        /* else
         {
             SpMat Hsp = Hess.sparseView();
             status = QPsolver.solve_quadprog_sparse(Hsp,g,D,ce0,CI,b,res.x);
         }*/
        res.success_ = (status == tsid::solvers::EIQUADPROG_FAST_OPTIMAL);
        if (res.success_) res.cost_ = QPsolver.getObjValue();
        return res;
      }
#ifdef USE_GLPK_SOLVER
    case SOLVER_GLPK: {
      res.success_ = (solvers::solveglpk(g, D, d, A, b, minBounds, maxBounds, res.x, res.cost_) == GLP_OPT);
      return res;
    }
#endif
    default:
      throw std::runtime_error("Unknown solver type in solver-asbtract");
  }
}

} /* namespace solvers */


#ifndef _DLIB_FDETECT_AND_LANDMARK_HPP_
#define _DLIB_FDETECT_AND_LANDMARK_HPP_
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_io.h>
#include <dlib/opencv.h>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>

#include <iostream>
#include <fstream>
#include <string>

bool debug;

// Apply affine transform calculated using srcTri and dstTri to src
void applyAffineTransform(cv::Mat &warpImage, cv::Mat &src, std::vector<cv::Point2f> &srcTri, std::vector<cv::Point2f> &dstTri)
{
    // Given a pair of triangles, find the affine transform.
    cv::Mat warpMat = getAffineTransform( srcTri, dstTri );

    // Apply the Affine Transform just found to the src image
    cv::warpAffine( src, warpImage, warpMat, warpImage.size(), cv::INTER_LINEAR, cv::BORDER_REFLECT_101);
}

// Calculate Delaunay triangles for set of points
// Returns the vector of indices of 3 points for each triangle
static void calculateDelaunayTriangles(cv::Rect rect, std::vector<cv::Point2f> &points, std::vector< std::vector<int> > &delaunayTri){

    // Create an instance of Subdiv2D
    cv::Subdiv2D subdiv(rect);

    // Insert points into subdiv
    for( std::vector<cv::Point2f>::iterator it = points.begin(); it != points.end(); it++)
        subdiv.insert(*it);

    std::vector<cv::Vec6f> triangleList;
    subdiv.getTriangleList(triangleList);
    std::vector<cv::Point2f> pt(3);
    std::vector<int> ind(3);

    for( size_t i = 0; i < triangleList.size(); i++ )
    {
        cv::Vec6f t = triangleList[i];
        pt[0] = cv::Point2f(t[0], t[1]);
        pt[1] = cv::Point2f(t[2], t[3]);
        pt[2] = cv::Point2f(t[4], t[5]);

        if ( rect.contains(pt[0]) && rect.contains(pt[1]) && rect.contains(pt[2])){
            for(int j = 0; j < 3; j++)
                for(size_t k = 0; k < points.size(); k++)
                    if(abs(pt[j].x - points[k].x) < 1.0 && abs(pt[j].y - points[k].y) < 1)
                        ind[j] = k;

            delaunayTri.push_back(ind);
        }
    }
}

// Warps and alpha blends triangular regions from img1 and img2 to img
void warpTriangle(cv::Mat &img1, cv::Mat &img2, std::vector<cv::Point2f> &t1, std::vector<cv::Point2f> &t2)
{

    cv::Rect r1 = boundingRect(t1);
    cv::Rect r2 = boundingRect(t2);

    // Offset points by left top corner of the respective rectangles
    std::vector<cv::Point2f> t1Rect, t2Rect;
    std::vector<cv::Point> t2RectInt;
    for(int i = 0; i < 3; i++)
    {

        t1Rect.push_back( cv::Point2f( t1[i].x - r1.x, t1[i].y -  r1.y) );
        t2Rect.push_back( cv::Point2f( t2[i].x - r2.x, t2[i].y - r2.y) );
        t2RectInt.push_back( cv::Point(t2[i].x - r2.x, t2[i].y - r2.y) ); // for fillConvexPoly

    }

    // Get mask by filling triangle
    cv::Mat mask = cv::Mat::zeros(r2.height, r2.width, CV_32FC3);
    fillConvexPoly(mask, t2RectInt, cv::Scalar(1.0, 1.0, 1.0), 16, 0);

    // Apply warpImage to small rectangular patches
    cv::Mat img1Rect;
    img1(r1).copyTo(img1Rect);

    cv::Mat img2Rect = cv::Mat::zeros(r2.height, r2.width, img1Rect.type());

    applyAffineTransform(img2Rect, img1Rect, t1Rect, t2Rect);

    cv::multiply(img2Rect,mask, img2Rect);
    cv::multiply(img2(r2), cv::Scalar(1.0,1.0,1.0) - mask, img2(r2));
    img2(r2) = img2(r2) + img2Rect;

}

cv::Mat swap(cv::Mat img1, cv::Mat img2, std::vector<cv::Point2f> points1, std::vector<cv::Point2f> points2) {
    cv::Mat img1Warped = img2.clone();

    //convert Mat to float data type
    img1.convertTo(img1, CV_32F);
    img1Warped.convertTo(img1Warped, CV_32F);

    // Find convex hull
    std::vector<cv::Point2f> hull1;
    std::vector<cv::Point2f> hull2;
    std::vector<int> hullIndex;

    cv::convexHull(points2, hullIndex, false, false);

    for(int i = 0; i < hullIndex.size(); i++)
    {
        hull1.push_back(points1[hullIndex[i]]);
        hull2.push_back(points2[hullIndex[i]]);
    }

    // Find delaunay triangulation for points on the convex hull
    std::vector< std::vector<int> > dt;
    cv::Rect rect(0, 0, img1Warped.cols, img1Warped.rows);
    calculateDelaunayTriangles(rect, hull2, dt);

    // Apply affine transformation to Delaunay triangles
    for(size_t i = 0; i < dt.size(); i++)
    {
        std::vector<cv::Point2f> t1, t2;
        // Get points for img1, img2 corresponding to the triangles
        for(size_t j = 0; j < 3; j++)
        {
            t1.push_back(hull1[dt[i][j]]);
            t2.push_back(hull2[dt[i][j]]);
        }

        warpTriangle(img1, img1Warped, t1, t2);

    }

    // Calculate mask
    std::vector<cv::Point> hull8U;
    for(int i = 0; i < hull2.size(); i++)
    {
        cv::Point pt(hull2[i].x, hull2[i].y);
        hull8U.push_back(pt);
    }

    cv::Mat mask = cv::Mat::zeros(img2.rows, img2.cols, img2.depth());
    cv::fillConvexPoly(mask,&hull8U[0], hull8U.size(), cv::Scalar(255,255,255));

    // Clone seamlessly.
    cv::Rect r = boundingRect(hull2);
    cv::Point center = (r.tl() + r.br()) / 2;

    cv::Mat output;
    img1Warped.convertTo(img1Warped, CV_8UC3);
    cv::seamlessClone(img1Warped,img2, mask, center, output, cv::NORMAL_CLONE);
    return output;
}

std::vector<std::vector<cv::Point2f>> landmark_detection(dlib::cv_image<dlib::rgb_pixel> dlibIm, std::vector<dlib::rectangle> rects, dlib::shape_predictor sp) {
    std::vector<std::vector<cv::Point2f>> both_points;
    for (unsigned long j = 0; j < 2; ++j)
    {
        dlib::full_object_detection shape = sp(dlibIm, rects[j]);
        if (shape.num_parts() == 68){
            std::vector<cv::Point2f> points;
            for(int i = 0; i < 68; ++i){
                points.push_back(cv::Point2f((float)shape.part(i)(0),(float)shape.part(i)(1)));
                // std::cout << i << " " << shape.part(i)(0) << " " << shape.part(i)(1) << std::endl;
            }
            both_points.push_back(points);
        }
    }

    return both_points;
}

std::tuple<dlib::cv_image<dlib::rgb_pixel>,std::vector<dlib::rectangle>> detectFaceDlibHog(dlib::frontal_face_detector hogFaceDetector, cv::Mat& frameDlibHog, int inHeight=300, int inWidth=0)
{

    int frameHeight = frameDlibHog.rows;
    dlib::cv_image<dlib::rgb_pixel> dlibIm(frameDlibHog);

    // Detect faces in the image
    std::vector<dlib::rectangle> faceRects = hogFaceDetector(dlibIm);
    if(debug){
        for ( size_t i = 0; i < faceRects.size(); i++ )
        {
            int x1 = (int)(faceRects[i].left());
            int y1 = (int)(faceRects[i].top());
            int x2 = (int)(faceRects[i].right());
            int y2 = (int)(faceRects[i].bottom());
            cv::rectangle(frameDlibHog, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0,255,0), (int)(frameHeight/150.0), 4);
        }
    }
    return std::make_tuple(dlibIm,faceRects);
}

#endif


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

#include "key.h"

#include "base58.h"
#include "script/script.h"
#include "uint256.h"
#include "util.h"
#include "utilstrencodings.h"
#include "test/test_bitcoin.h"
#include "crypto/aes.h"

#include <string>
#include <vector>

#include <boost/test/unit_test.hpp>

using namespace std;

/*
static const string strSecret1     ("6uGFQ4DSW7zh1viHZi6iiVT17CncvoaV4MHvGvJKPDaLCdymj87");
static const string strSecret2     ("6vVo7sPkeLTwVdAntrv4Gbnsyr75H8ChD3P5iyHziwaqe8mCYR5");
static const string strSecret1C    ("T3gJYmBuZXsdd65E7NQF88ZmUP2MaUanqnZg9GFS94W7kND4Ebjq");
static const string strSecret2C    ("T986ZKRRdnuuXLeDZuKBRrZW1ujotAncU9WTrFU1n7vMgRW75ZtF");
static const CBitcoinAddress addr1 ("LiUo6Zn39joYJBzPUhssbDwAywhjFcoHE3");
static const CBitcoinAddress addr2 ("LZJvLSP5SGKcFS13MHgdrVhpFUbEMB5XVC");
static const CBitcoinAddress addr1C("Lh2G82Bi33RNuzz4UfSMZbh54jnWHVnmw8");
static const CBitcoinAddress addr2C("LWegHWHB5rmaF5rgWYt1YN3StapRdnGJfU");

static const string strAddressBad("Lbi6bpMhSwp2CXkivEeUK9wzyQEFzHDfSr");

#ifdef KEY_TESTS_DUMPINFO
void dumpKeyInfo(uint256 privkey)
{
    CKey key;
    key.resize(32);
    memcpy(&secret[0], &privkey, 32);
    vector<unsigned char> sec;
    sec.resize(32);
    memcpy(&sec[0], &secret[0], 32);
    printf("  * secret (hex): %s\n", HexStr(sec).c_str());

    for (int nCompressed=0; nCompressed<2; nCompressed++)
    {
        bool fCompressed = nCompressed == 1;
        printf("  * %s:\n", fCompressed ? "compressed" : "uncompressed");
        CBitcoinSecret bsecret;
        bsecret.SetSecret(secret, fCompressed);
        printf("    * secret (base58): %s\n", bsecret.ToString().c_str());
        CKey key;
        key.SetSecret(secret, fCompressed);
        vector<unsigned char> vchPubKey = key.GetPubKey();
        printf("    * pubkey (hex): %s\n", HexStr(vchPubKey).c_str());
        printf("    * address (base58): %s\n", CBitcoinAddress(vchPubKey).ToString().c_str());
    }
}
#endif
*/

BOOST_FIXTURE_TEST_SUITE(enc_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(enc_test1)
{
	CKey secret_a, secret_b;
	secret_a.MakeNewKey(false);
	secret_b.MakeNewKey(false);
	CPubKey pubkey_a = secret_a.GetPubKey();
	CPubKey pubkey_b = secret_b.GetPubKey();
	std::string pubkey_a_str = HexStr(pubkey_a.begin(), pubkey_a.end());
	std::string pubkey_b_str = HexStr(pubkey_b.begin(), pubkey_b.end());
	printf("pubkeys\n%s\n%s\n", pubkey_a_str.c_str(), pubkey_b_str.c_str());

	std::vector<unsigned char> result_a, result_b;
	secret_a.GetECDHSecret(pubkey_b, result_a);
	secret_b.GetECDHSecret(pubkey_a, result_b);
	std::string result_a_str = HexStr(result_a.begin(), result_a.end());
	std::string result_b_str = HexStr(result_b.begin(), result_b.end());
	printf("ecdh secret\n%s\n%s\n", result_a_str.c_str(), result_b_str.c_str());

	AES256Encrypt enc(&result_a[0]);
	std::vector<unsigned char> plain = {'t','e','s','t',0}, cipher, result_plain;
	plain.resize(16, 0);
	cipher.resize(16, 0);
	result_plain.resize(16, 0);
	enc.Encrypt(&cipher[0], &plain[0]);
	AES256Decrypt dec(&result_b[0]);
	dec.Decrypt(&result_plain[0], &cipher[0]);
    std::string cipher_str = HexStr(cipher.begin(), cipher.end());
	printf("encrypt\n%s\n%s\n%s\n", &plain[0], cipher_str.c_str() ,&result_plain[0]);

    unsigned char chIV[16], chIV2[16];
    unsigned char chKey[32];
    uint256 hash_a = pubkey_a.GetHash();
    memcpy(chIV, &hash_a, 16);
    memcpy(chIV2, &hash_a, 16);
    memcpy(chKey, &result_a[0], 32);
    AES256CBCEncrypt enc2(chKey, chIV, true);
    unsigned char data_str[50] = "Testing testings gaga tits make me happy";
    std::vector<unsigned char> plain2, cipher2, result_plain2;
    plain2.resize(50 ,0);
    memcpy(&plain2[0], data_str, strlen((const char*)data_str));
    cipher2.resize(plain2.size() + AES_BLOCKSIZE);
    int len = enc2.Encrypt(&plain2[0], plain2.size(), &cipher2[0]);
    cipher2.resize(len);
    memcpy(chIV, &hash_a, 16);
    memcpy(chKey, &result_b[0], 32);
    AES256CBCDecrypt dec2(chKey, chIV2, true);
    result_plain2.resize(cipher2.size());
    len = dec2.Decrypt(&cipher2[0], cipher2.size(), &result_plain2[0]);
    std::string cipher_str2 = HexStr(cipher2.begin(), cipher2.end());
    printf("encrypt 2\n%s\n%s\n%s\n", &plain2[0], cipher_str2.c_str(), &result_plain2[0]);

/*
    CBitcoinSecret bsecret1, bsecret2, bsecret1C, bsecret2C, baddress1;
    BOOST_CHECK( bsecret1.SetString (strSecret1));
    BOOST_CHECK( bsecret2.SetString (strSecret2));
    BOOST_CHECK( bsecret1C.SetString(strSecret1C));
    BOOST_CHECK( bsecret2C.SetString(strSecret2C));
    BOOST_CHECK(!baddress1.SetString(strAddressBad));

    CKey key1  = bsecret1.GetKey();
    BOOST_CHECK(key1.IsCompressed() == false);
    CKey key2  = bsecret2.GetKey();
    BOOST_CHECK(key2.IsCompressed() == false);
    CKey key1C = bsecret1C.GetKey();
    BOOST_CHECK(key1C.IsCompressed() == true);
    CKey key2C = bsecret2C.GetKey();
    BOOST_CHECK(key2C.IsCompressed() == true);

    CPubKey pubkey1  = key1. GetPubKey();
    CPubKey pubkey2  = key2. GetPubKey();
    CPubKey pubkey1C = key1C.GetPubKey();
    CPubKey pubkey2C = key2C.GetPubKey();

    BOOST_CHECK(key1.VerifyPubKey(pubkey1));
    BOOST_CHECK(!key1.VerifyPubKey(pubkey1C));
    BOOST_CHECK(!key1.VerifyPubKey(pubkey2));
    BOOST_CHECK(!key1.VerifyPubKey(pubkey2C));

    BOOST_CHECK(!key1C.VerifyPubKey(pubkey1));
    BOOST_CHECK(key1C.VerifyPubKey(pubkey1C));
    BOOST_CHECK(!key1C.VerifyPubKey(pubkey2));
    BOOST_CHECK(!key1C.VerifyPubKey(pubkey2C));

    BOOST_CHECK(!key2.VerifyPubKey(pubkey1));
    BOOST_CHECK(!key2.VerifyPubKey(pubkey1C));
    BOOST_CHECK(key2.VerifyPubKey(pubkey2));
    BOOST_CHECK(!key2.VerifyPubKey(pubkey2C));

    BOOST_CHECK(!key2C.VerifyPubKey(pubkey1));
    BOOST_CHECK(!key2C.VerifyPubKey(pubkey1C));
    BOOST_CHECK(!key2C.VerifyPubKey(pubkey2));
    BOOST_CHECK(key2C.VerifyPubKey(pubkey2C));

    BOOST_CHECK(addr1.Get()  == CTxDestination(pubkey1.GetID()));
    BOOST_CHECK(addr2.Get()  == CTxDestination(pubkey2.GetID()));
    BOOST_CHECK(addr1C.Get() == CTxDestination(pubkey1C.GetID()));
    BOOST_CHECK(addr2C.Get() == CTxDestination(pubkey2C.GetID()));

    for (int n=0; n<16; n++)
    {
        string strMsg = strprintf("Very secret message %i: 11", n);
        uint256 hashMsg = Hash(strMsg.begin(), strMsg.end());

        // normal signatures

        vector<unsigned char> sign1, sign2, sign1C, sign2C;

        BOOST_CHECK(key1.Sign (hashMsg, sign1));
        BOOST_CHECK(key2.Sign (hashMsg, sign2));
        BOOST_CHECK(key1C.Sign(hashMsg, sign1C));
        BOOST_CHECK(key2C.Sign(hashMsg, sign2C));

        BOOST_CHECK( pubkey1.Verify(hashMsg, sign1));
        BOOST_CHECK(!pubkey1.Verify(hashMsg, sign2));
        BOOST_CHECK( pubkey1.Verify(hashMsg, sign1C));
        BOOST_CHECK(!pubkey1.Verify(hashMsg, sign2C));

        BOOST_CHECK(!pubkey2.Verify(hashMsg, sign1));
        BOOST_CHECK( pubkey2.Verify(hashMsg, sign2));
        BOOST_CHECK(!pubkey2.Verify(hashMsg, sign1C));
        BOOST_CHECK( pubkey2.Verify(hashMsg, sign2C));

        BOOST_CHECK( pubkey1C.Verify(hashMsg, sign1));
        BOOST_CHECK(!pubkey1C.Verify(hashMsg, sign2));
        BOOST_CHECK( pubkey1C.Verify(hashMsg, sign1C));
        BOOST_CHECK(!pubkey1C.Verify(hashMsg, sign2C));

        BOOST_CHECK(!pubkey2C.Verify(hashMsg, sign1));
        BOOST_CHECK( pubkey2C.Verify(hashMsg, sign2));
        BOOST_CHECK(!pubkey2C.Verify(hashMsg, sign1C));
        BOOST_CHECK( pubkey2C.Verify(hashMsg, sign2C));

        // compact signatures (with key recovery)

        vector<unsigned char> csign1, csign2, csign1C, csign2C;

        BOOST_CHECK(key1.SignCompact (hashMsg, csign1));
        BOOST_CHECK(key2.SignCompact (hashMsg, csign2));
        BOOST_CHECK(key1C.SignCompact(hashMsg, csign1C));
        BOOST_CHECK(key2C.SignCompact(hashMsg, csign2C));

        CPubKey rkey1, rkey2, rkey1C, rkey2C;

        BOOST_CHECK(rkey1.RecoverCompact (hashMsg, csign1));
        BOOST_CHECK(rkey2.RecoverCompact (hashMsg, csign2));
        BOOST_CHECK(rkey1C.RecoverCompact(hashMsg, csign1C));
        BOOST_CHECK(rkey2C.RecoverCompact(hashMsg, csign2C));

        BOOST_CHECK(rkey1  == pubkey1);
        BOOST_CHECK(rkey2  == pubkey2);
        BOOST_CHECK(rkey1C == pubkey1C);
        BOOST_CHECK(rkey2C == pubkey2C);
    }

    // test deterministic signing

    std::vector<unsigned char> detsig, detsigc;
    string strMsg = "Very deterministic message";
    uint256 hashMsg = Hash(strMsg.begin(), strMsg.end());
    BOOST_CHECK(key1.Sign(hashMsg, detsig));
    BOOST_CHECK(key1C.Sign(hashMsg, detsigc));
    BOOST_CHECK(detsig == detsigc);
    BOOST_CHECK(detsig == ParseHex("304402205dbbddda71772d95ce91cd2d14b592cfbc1dd0aabd6a394b6c2d377bbe59d31d022014ddda21494a4e221f0824f0b8b924c43fa43c0ad57dccdaa11f81a6bd4582f6"));
    BOOST_CHECK(key2.Sign(hashMsg, detsig));
    BOOST_CHECK(key2C.Sign(hashMsg, detsigc));
    BOOST_CHECK(detsig == detsigc);
    BOOST_CHECK(detsig == ParseHex("3044022052d8a32079c11e79db95af63bb9600c5b04f21a9ca33dc129c2bfa8ac9dc1cd5022061d8ae5e0f6c1a16bde3719c64c2fd70e404b6428ab9a69566962e8771b5944d"));
    BOOST_CHECK(key1.SignCompact(hashMsg, detsig));
    BOOST_CHECK(key1C.SignCompact(hashMsg, detsigc));
    BOOST_CHECK(detsig == ParseHex("1c5dbbddda71772d95ce91cd2d14b592cfbc1dd0aabd6a394b6c2d377bbe59d31d14ddda21494a4e221f0824f0b8b924c43fa43c0ad57dccdaa11f81a6bd4582f6"));
    BOOST_CHECK(detsigc == ParseHex("205dbbddda71772d95ce91cd2d14b592cfbc1dd0aabd6a394b6c2d377bbe59d31d14ddda21494a4e221f0824f0b8b924c43fa43c0ad57dccdaa11f81a6bd4582f6"));
    BOOST_CHECK(key2.SignCompact(hashMsg, detsig));
    BOOST_CHECK(key2C.SignCompact(hashMsg, detsigc));
    BOOST_CHECK(detsig == ParseHex("1c52d8a32079c11e79db95af63bb9600c5b04f21a9ca33dc129c2bfa8ac9dc1cd561d8ae5e0f6c1a16bde3719c64c2fd70e404b6428ab9a69566962e8771b5944d"));
    BOOST_CHECK(detsigc == ParseHex("2052d8a32079c11e79db95af63bb9600c5b04f21a9ca33dc129c2bfa8ac9dc1cd561d8ae5e0f6c1a16bde3719c64c2fd70e404b6428ab9a69566962e8771b5944d"));
*/
}

BOOST_AUTO_TEST_SUITE_END()


/*
* TS Elements
* Copyright 2015-2018 M. Newhouse
* Released under the MIT license.
*/

#include "handling_v2.hpp"
#include "car.hpp"
#include "world.hpp"

#include "resources/terrain_definition.hpp"

#include "utility/transform.hpp"
#include "utility/interpolate.hpp"
#include "utility/math_utilities.hpp"

#include <boost/container/small_vector.hpp>

#include <array>
#include <algorithm>

namespace ts
{
  namespace world
  {
    HandlingState update_car_state(Car& car, const World& world, double frame_duration)
    {
      auto& handling = car.handling();

      using controls::Control;
      auto half_wheelbase = handling.wheelbase_length * 0.5;

      std::array<Vector2d, 2> front_wheel_positions =
      { {
        { 0.0, -half_wheelbase + handling.wheelbase_offset }
        } };

      std::array<Vector2d, 2> rear_wheel_positions =
      { {
        { 0.0, half_wheelbase + handling.wheelbase_offset }
        } };

      if (handling.num_front_wheels >= 2)
      {
        front_wheel_positions =
        { {
          { -handling.front_axle_width * 0.5, -half_wheelbase + handling.wheelbase_offset },
          { handling.front_axle_width * 0.5, -half_wheelbase + handling.wheelbase_offset },
          } };
      }

      if (handling.num_rear_wheels >= 2)
      {
        rear_wheel_positions =
        { {
          { -handling.rear_axle_width * 0.5, half_wheelbase + handling.wheelbase_offset },
          { handling.rear_axle_width * 0.5, half_wheelbase + handling.wheelbase_offset }
        } };
      }

      auto transform = make_transformation(car.rotation());
      auto inv_transform = make_transformation(-car.rotation());

      auto handling_state = car.handling_state();
      handling_state.wheel_states.clear();

      auto local_velocity = transform_point(car.velocity(), inv_transform);
      auto local_heading = normalize(local_velocity);
      auto speed = magnitude(local_velocity);
      auto angular_velocity = car.angular_velocity();
      auto mass = car.mass();
      auto center_of_mass = car.center_of_mass();

      auto inv_moment = 1.0 / car.moment_of_inertia();

      auto num_wheels = handling.num_front_wheels + handling.num_rear_wheels;
      auto inv_num_wheels = 1.0 / num_wheels;
      auto wheel_traction_limit = handling.traction_limit * inv_num_wheels;

      auto total_downforce = local_heading.y * local_heading.y * speed * speed * handling.downforce_coefficient;
      if (local_heading.y > 0.0) total_downforce = 0.0;

      auto weight_distribution = clamp(center_of_mass.y / (handling.wheelbase_length * 0.5) + 0.5, 0.25, 0.75);

      auto rear_downforce = total_downforce * (0.5 + handling.downforce_balance);
      auto front_downforce = total_downforce * (0.5 - handling.downforce_balance);

      auto inv_num_front_wheels = 1.0 / handling.num_front_wheels;
      auto inv_num_rear_wheels = 1.0 / handling.num_rear_wheels;

      auto front_base_load = wheel_traction_limit * 2.0 * (1.0 - weight_distribution);
      auto rear_base_load = wheel_traction_limit * 2.0 * weight_distribution;

      auto front_load_transfer = handling_state.net_force.y * handling.load_transfer * 0.5;
      auto rear_load_transfer = -handling_state.net_force.y * handling.load_transfer * 0.5;

      auto front_traction_limit = std::max(front_base_load + (front_load_transfer + front_downforce) * inv_num_front_wheels, 0.0);
      auto rear_traction_limit = std::max(rear_base_load + (rear_load_transfer + rear_downforce) * inv_num_rear_wheels, 0.0);

      auto throttle_rate = car.control_state(Control::Throttle) / 255.0;
      auto braking_rate = car.control_state(Control::Brake) / 255.0;
      auto turning_left_rate = car.control_state(Control::Left) / 255.0;
      auto turning_right_rate = car.control_state(Control::Right) / 255.0;
      auto turning_rate = -turning_left_rate + turning_right_rate;
      auto net_throttle = throttle_rate - braking_rate;

      auto inv_frame_duration = 1.0 / frame_duration;

      auto front_steering = 0.5 - handling.steering_balance;
      auto rear_steering = 0.5 + handling.steering_balance;
      auto steering_multiplier = 1.0 / std::max(front_steering, rear_steering);
      front_steering *= steering_multiplier;
      rear_steering *= steering_multiplier;

      auto front_braking = 0.5 - handling.brake_balance;
      auto rear_braking = 0.5 + handling.brake_balance;
      auto braking_multiplier = 1.0 / std::max(front_steering, rear_steering);
      front_braking *= braking_multiplier;
      rear_braking *= braking_multiplier;

      auto front_acceleration = 0.0;
      auto rear_acceleration = 0.0;
      if (handling.front_driven && handling.rear_driven)
      {
        front_acceleration = 0.5 * inv_num_front_wheels;
        rear_acceleration = 0.5 * inv_num_rear_wheels;
      }

      else if (handling.front_driven)
      {
        front_acceleration = inv_num_front_wheels;
      }

      else if (handling.rear_driven)
      {
        rear_acceleration = inv_num_rear_wheels;
      }

      auto num_gears = static_cast<int>(handling.gear_ratios.size());
      if (handling_state.current_gear >= 0 && (speed <= 0.0001 || local_heading.y > 0.7) && net_throttle < 0.0)
      {
        handling_state.current_gear = -1;
      }

      if (handling_state.current_gear <= 0 && (speed <= 0.0001 || local_heading.y < -0.7) && net_throttle > 0.0)
      {
        auto gear = 0;

        while (gear < num_gears && speed * handling.gear_ratios[gear] >= handling.max_engine_revs * 0.8)
        {
          ++gear;
        }

        handling_state.current_gear = gear + 1;
      }

      if (handling_state.gear_shift_state != 0)
      {
        auto new_gear = handling_state.current_gear;
        if (handling_state.gear_shift_state > 0) ++new_gear;
        else --new_gear;

        auto old_ratio = handling.gear_ratios[handling_state.current_gear - 1];
        auto new_ratio = handling.gear_ratios[new_gear - 1];

        auto frame_progress = 1.0 / handling.gear_shift_duration;
        auto progress = 1.0 - (std::abs(handling_state.gear_shift_state) * frame_progress);

        auto current_ratio = interpolate_linearly(old_ratio, new_ratio, progress);
        auto updated_ratio = interpolate_linearly(old_ratio, new_ratio, progress + frame_progress);

        handling_state.engine_rev_speed /= current_ratio;
        handling_state.engine_rev_speed *= updated_ratio;

        if (handling_state.gear_shift_state > 0) --handling_state.gear_shift_state;
        else ++handling_state.gear_shift_state;

        if (handling_state.gear_shift_state == 0)
        {
          handling_state.current_gear = new_gear;
        }
      }

      else if (handling_state.engine_rev_speed >= 0.99)
      {
        if (handling_state.current_gear >= 1 && handling_state.current_gear < num_gears)
        {
          handling_state.gear_shift_state = handling.gear_shift_duration;
        }
      }

      else if (handling_state.engine_rev_speed < 0.8 && handling_state.current_gear > 1)
      {
        auto prev_gear = handling_state.current_gear - 1;
        auto new_revs = handling_state.engine_rev_speed;
        new_revs /= handling.gear_ratios[handling_state.current_gear - 1];
        new_revs *= handling.gear_ratios[prev_gear - 1];
        if (new_revs < 0.8)
        {
          handling_state.gear_shift_state = -handling.gear_shift_duration;
        }
      }

      auto gear_ratio = 1.0;
      if (handling_state.current_gear < 0)
      {
        net_throttle = -net_throttle;
        gear_ratio = -handling.reverse_gear_ratio;
      }

      else if (handling_state.current_gear == 0)
      {
        gear_ratio = 0.0;
      }

      else if (handling_state.current_gear - 1 < num_gears)
      {
        gear_ratio = handling.gear_ratios[handling_state.current_gear - 1];
      }

      if (handling_state.gear_shift_state == 0)
      {
        handling_state.engine_rev_speed = std::min(speed * std::abs(gear_ratio) / handling.max_engine_revs, 1.05);
      }

      else
      {
        gear_ratio = 0.0;
      }

      auto throttle_factor = 1.0;
      if (handling_state.engine_rev_speed > 1.0)
      {
        throttle_factor = 0.0;
      }

      auto acceleration_force_1d = std::max(net_throttle, 0.0) * handling.max_acceleration_force * gear_ratio * throttle_factor;
      auto braking_force_1d = std::max(-net_throttle, 0.0) * handling.max_braking_force;

      struct WheelState
      {
        Vector2d pos;
        Vector2d velocity;
        Vector2d bias;
        Vector2d wheel_facing;
        double heading_angle;
        double traction_limit;
        double vertical_load;
        double acceleration;
        double braking;
        double cornering;
        double max_steering_angle;
        double acceleration_force;
        double braking_force;
        double cornering_force;
        double slide_ratio;

        resources::TerrainDefinition terrain;
      };

      auto throttle_bias = 1.0;
      auto brake_bias = 1.0;
      auto cornering_bias = 1.0;
      auto antislide_bias = 1.0;
      auto longitudinal_bias = net_throttle >= 0.0 ? throttle_bias : brake_bias;

      auto cornering_bias_2d = normalize(make_vector2(cornering_bias, longitudinal_bias));
      auto antislide_bias_2d = normalize(make_vector2(cornering_bias, longitudinal_bias));

      boost::container::small_vector<WheelState, 4> wheel_states;
      for (auto p : front_wheel_positions)
      {
        WheelState ws{};
        ws.pos = p;
        ws.traction_limit = front_traction_limit;
        ws.acceleration = front_acceleration;
        ws.braking = front_braking;
        ws.cornering = handling.cornering;
        ws.max_steering_angle = front_steering * degrees(handling.max_steering_angle).radians();
        ws.bias = cornering_bias_2d;
        wheel_states.push_back(ws);
      }

      for (auto p : rear_wheel_positions)
      {
        WheelState ws{};
        ws.pos = p;
        ws.traction_limit = rear_traction_limit;
        ws.acceleration = rear_acceleration;
        ws.braking = rear_braking;
        ws.cornering = handling.cornering;
        ws.max_steering_angle = -rear_steering * degrees(handling.max_steering_angle).radians();
        ws.bias = antislide_bias_2d;
        wheel_states.push_back(ws);
      }

      auto max_cornering_multiplier = speed * mass * inv_num_wheels * inv_frame_duration;
      auto is_turning = std::abs(turning_rate) >= 0.0001;

      auto adjusted_steering_rate = 0.0;

      for (auto& ws : wheel_states)
      {
        auto global_pos = car.position() + transform_point(ws.pos, transform);

        ws.vertical_load = ws.traction_limit;
        ws.terrain = world.terrain_at(global_pos, car.z_level());
        ws.velocity = local_velocity + angular_velocity * make_vector2(-ws.pos.y, ws.pos.x);
        ws.heading_angle = std::atan2(ws.velocity.x, -ws.velocity.y);

        if (is_turning && std::abs(ws.max_steering_angle) >= 0.00001)
        {
          auto max_angle = turning_rate < 0.0 ? -ws.max_steering_angle : ws.max_steering_angle;
          auto desired_angle = degrees(handling.non_slide_angle);
          if (max_angle < 0.0) desired_angle = -desired_angle;

          auto steering_angle = radians(ws.heading_angle) + desired_angle;
          if (std::abs(steering_angle.degrees()) >= 90.0)
          {
            steering_angle -= degrees(180.0);
            steering_angle.normalize();
          }

          auto steering_rate = std::min(steering_angle.radians() / max_angle, 1.0);
          if (steering_rate > adjusted_steering_rate)
          {
            adjusted_steering_rate = steering_rate;
          }
        }
      }

      turning_rate *= adjusted_steering_rate;

      auto pedal_adjustment = 1.0;
      for (auto& ws : wheel_states)
      {
        ws.wheel_facing = make_vector2(0.0, -1.0);
        auto steering_angle = radians(turning_rate * ws.max_steering_angle);
        if (std::abs(steering_angle.radians()) >= 0.00001)
        {
          ws.wheel_facing = transform_point(ws.wheel_facing, steering_angle);
        }

        auto slip_angle = radians(ws.heading_angle) - steering_angle;
        if (std::abs(slip_angle.degrees()) >= 90.0)
        {
          slip_angle -= degrees(180.0);
          slip_angle.normalize();
        }

        auto slip_degrees = std::abs(slip_angle.degrees());

        ws.slide_ratio = 1.0;
        auto cornering_ratio = 1.0;
        if (slip_degrees < handling.full_slide_angle)
        {
          if (slip_degrees < handling.non_slide_angle)
          {
            //cornering_ratio = slip_degrees / handling.non_slide_angle;
            ws.slide_ratio = 0.0;
          }

          else
          {
            ws.slide_ratio = (slip_degrees - handling.non_slide_angle) /
              (handling.full_slide_angle - handling.non_slide_angle);
            ws.slide_ratio *= ws.slide_ratio;
          }
        }

        ws.traction_limit *= interpolate_linearly(ws.terrain.traction,
                                                  handling.sliding_grip * ws.terrain.sliding_traction,
                                                  ws.slide_ratio);

        auto facing = ws.wheel_facing;
        if (ws.velocity.y > 0.0) facing = -facing;

        auto max_cornering = std::abs(slip_angle.radians()) * max_cornering_multiplier;
        ws.cornering_force = std::min(cornering_ratio * ws.cornering * ws.traction_limit, max_cornering);

        auto braking_factor = facing.y * facing.y;
        ws.acceleration_force = ws.acceleration * acceleration_force_1d;
        ws.braking_force = ws.braking * braking_force_1d * braking_factor;

        auto longitudinal_force = (std::abs(ws.acceleration_force) + ws.braking_force) * pedal_adjustment;
        auto applied_force = make_vector2(ws.cornering_force, longitudinal_force);

        if (magnitude_squared(applied_force) > ws.traction_limit * ws.traction_limit)
        {
          auto reduce_x = applied_force.x > ws.bias.x * ws.traction_limit;
          auto reduce_y = applied_force.y > ws.bias.y * ws.traction_limit;

          if (reduce_y)
          {
            if (reduce_x)
            {
              pedal_adjustment = (ws.traction_limit * ws.bias.y) /
                (std::abs(ws.acceleration_force) + ws.braking_force);
            }

            else
            {
              auto rem = ws.traction_limit * ws.traction_limit - ws.cornering_force * ws.cornering_force;
              pedal_adjustment = std::sqrt(rem) / (std::abs(ws.acceleration_force) + ws.braking_force);
            }
          }
        }
      }

      handling_state.wheel_states.clear();
      auto net_force = make_vector2(0.0, 0.0);
      for (auto& ws : wheel_states)
      {
        {
          auto longitudinal_force = (std::abs(ws.acceleration_force) + ws.braking_force) * pedal_adjustment;
          auto rem = std::max(ws.traction_limit * ws.traction_limit - longitudinal_force * longitudinal_force, 0.0);

          if (rem < ws.cornering_force * ws.cornering_force)
          {
            ws.cornering_force = std::sqrt(rem);
          }
        }

        auto force = make_vector2(0.0, 0.0);

        auto wheel_heading = normalize(ws.velocity);
        auto target_heading = ws.wheel_facing;
        if (ws.velocity.y > 0.0) target_heading = -target_heading;
        auto slide_direction = normalize(target_heading - wheel_heading);

        auto terrain_cornering_multiplier = ws.terrain.cornering;
        if (&ws - wheel_states.data() >= handling.num_front_wheels)
        {
          terrain_cornering_multiplier = ws.terrain.antislide;
        }

        force += ws.acceleration_force * pedal_adjustment * ws.wheel_facing * ws.terrain.acceleration;
        force += ws.braking_force * pedal_adjustment * -target_heading * ws.terrain.braking;
        force += ws.cornering_force * slide_direction * terrain_cornering_multiplier;

        auto rolling_resistance = (1.0 - ws.slide_ratio) * handling.rolling_drag_coefficient *
          ws.vertical_load * ws.terrain.rolling_resistance;

        force += rolling_resistance * -wheel_heading;

        force += ws.terrain.roughness * mass * -ws.velocity * inv_num_wheels;

        car.apply_force(force, ws.pos);

        net_force += force;

        HandlingState::WheelState stored_info;
        stored_info.pos = car.position() + transform_point(ws.pos, transform);
        stored_info.slide_ratio = ws.slide_ratio;
        stored_info.terrain_color = ws.terrain.color;
        stored_info.terrain_roughness = ws.terrain.roughness;
        stored_info.speed = dot_product(wheel_heading, ws.velocity);
        handling_state.wheel_states.push_back(stored_info);
      }

      auto drag = speed * -local_velocity * handling.drag_coefficient;
      car.apply_force(drag, center_of_mass);
      net_force += drag;

      auto torque_effect = car.applied_torque() * inv_moment * mass;
      auto min_lateral_force = net_force.x + -wheel_states.front().pos.y * torque_effect;
      auto max_lateral_force = min_lateral_force;
      for (std::uint32_t idx = 1; idx < wheel_states.size(); ++idx)
      {
        auto lateral_force = net_force.x + -wheel_states[idx].pos.y * torque_effect;
        if (lateral_force < min_lateral_force)
        {
          min_lateral_force = lateral_force;
        }

        if (lateral_force > max_lateral_force)
        {
          max_lateral_force = lateral_force;
        }
      }

      if (std::signbit(min_lateral_force) == std::signbit(max_lateral_force))
      {
        auto m = (max_lateral_force - min_lateral_force) * 0.5;
        auto f = clamp(min_lateral_force < 0.0 ? -max_lateral_force : -min_lateral_force, -m, m);
        car.apply_force(make_vector2(f, 0.0), center_of_mass);
        net_force.x += f;
      }

      angular_velocity -= angular_velocity * handling.angular_damping * frame_duration;
      car.set_angular_velocity(angular_velocity);
      handling_state.net_force = net_force;
      return handling_state;
    }
  }
}


#include "car.h"
#include "genetic.h"
#include "layer.h"

#include <iostream>
#include <Eigen/Geometry>

Car::Car(): m_rotationToOriginal(0.0), m_mapSet(false), m_droveDistance(0.0),
    m_formerDistance(0.0), m_accumulatedRotation(0.0), m_carSize{4}
{
    setSpeed( 0.0 );
    setPosition( Eigen::Vector2d(0.0, 0.0));
    setAcceleration(0.0);
    setDirection(Eigen::Vector2d(1.0, 0.0));
    setRotationSpeed(0.0);

    setMeasureAngles( {-80, -50.0, -15.0, 0.0, 15.0, 50.0, 80} );

    std::vector<unsigned int> map = {8,4,2};
    m_network = NetworkPtr( new Network(map) );

    m_killer.start();
}

Car::~Car()
{

}

double Car::getSpeed() const
{
    return m_speed;
}

void Car::setSpeed(double speed)
{
    m_speed = speed;
}

const Eigen::Vector2d &Car::getPosition() const
{
    return m_position;
}

void Car::setPosition(const Eigen::Vector2d &position)
{
    m_position = position;
}

double Car::getAcceleration() const
{
    return m_acceleration;
}

void Car::setAcceleration(double acceleration)
{
    m_acceleration = acceleration;
}

const Eigen::Vector2d &Car::getDirection() const
{
    return m_direction;
}

void Car::setDirection(const Eigen::Vector2d &direction)
{
    m_direction = direction;
    m_direction.normalize();
    m_rotationToOriginal = computeAngleBetweenVectors(Eigen::Vector2d(1.0,0.0), m_direction);
}

void Car::update()
{
    double animTime = getTimeSinceLastUpdate();

    // rotate first
    double thisRotation =  m_rotationSpeedRad * animTime;
    Eigen::Rotation2D<double> r( thisRotation );
    m_direction = r.toRotationMatrix() * m_direction;
    Eigen::Vector2d(1.0,0.0);
    m_rotationToOriginal = computeAngleBetweenVectors(Eigen::Vector2d(1.0,0.0), m_direction);

    // adjust speed
    double newSpeed = std::max(m_speed + animTime*getAcceleration(), 0.0);
    newSpeed = std::min(newSpeed,600.0);
    Eigen::Vector2d effectiveSpeed = m_direction * (newSpeed + m_speed) * 0.5;

    // set new position
    Eigen::Vector2d newPosition = m_position + animTime * effectiveSpeed;
    newPosition = handleCollision(m_position, newPosition);

    // adjust drove distance and accumulated rotation
    m_droveDistance += (newPosition - getPosition()).norm();
    m_accumulatedRotation += std::abs(thisRotation);

    // important: measure distances before navigate and post move collision
    m_measuredDistances = measureDistances();
    navigate();

    // update
    m_speed = newSpeed;
    m_position = newPosition;
}

double Car::getFitness()
{
    return m_droveDistance;
}

double Car::getRotationSpeed() const
{
    return m_rotationSpeed;
}

void Car::setRotationSpeed(double rotationSpeed)
{
    m_rotationSpeed = rotationSpeed;
    m_rotationSpeedRad = -(M_PI / 180.0 * m_rotationSpeed);
}

double Car::getRotationRelativeToInitial() const
{
    return m_rotationToOriginal;
}

double Car::computeAngleBetweenVectors(const Eigen::Vector2d &a, const Eigen::Vector2d &b) const
{
    return std::acos( a.dot(b) / ( a.norm() * b.norm() ) ) / M_PI * 180.0;
}

std::shared_ptr<TrackMap> Car::getMap() const
{
    return m_map;
}

void Car::setMap(std::shared_ptr<TrackMap> map)
{
    m_map = map;
    m_mapSet = true;
}

Eigen::Vector2d Car::handleCollision(const Eigen::Vector2d& from, const Eigen::Vector2d& to)
{
    if( !m_mapSet )
        return to;

    if( !( m_map->isPositionValid(from) ) )
    {
        m_alive = false;
        return from;
    }

    Eigen::Vector2d dif = to - from;
    double l = dif.norm();

    if( l < 0.00000001 ) // no or very small move
        return to;

    Eigen::Vector2d du = dif * 1.0 / l;

    double tillEdge = distanceToEdge(from,du);

    if( tillEdge < l + m_carSize ) // collision
    {
        m_alive = false;
        return from;
    }

    return to;
}

void Car::handlePostMoveCollision()
{
    for(size_t k = 0; k < m_measuredDistances.rows(); k++)
    {
        if( m_measuredDistances(k,0) < m_carSize )
        {
            this->kill();
            return;
        }
    }
}

double Car::distanceToEdge(const Eigen::Vector2d &pos, const Eigen::Vector2d &direction) const
{
    if( !m_mapSet )
        return 0.0;

    Eigen::Vector2d d = direction.normalized();
    Eigen::Vector2d end = pos;

    bool goOn = true;

    int minDistToEdge = m_map->isPositionValid(end);

    while(minDistToEdge  > 0)
    {
        end = end + (d * minDistToEdge);
        minDistToEdge = m_map->isPositionValid(end);
    }

    return (end-pos).norm();
}

const std::vector<double> &Car::getMeasureAngles() const
{
    return m_measureAngles;
}

void Car::setMeasureAngles(const std::vector<double> &measureAngles)
{
    m_measureAngles = measureAngles;
}

Eigen::MatrixXd Car::measureDistances() const
{
    // distance, xP, yP
    Eigen::MatrixXd angs( m_measureAngles.size(), 3);
    for( size_t k = 0; k < m_measureAngles.size(); k++ )
    {
        double aRad = -(M_PI / 180.0 * m_measureAngles[k]);
        Eigen::Rotation2D<double> r( aRad );
        Eigen::Vector2d measureDir = r.toRotationMatrix() * m_direction;

        double dist = distanceToEdge(getPosition(),measureDir);

        Eigen::Vector2d edgePos = getPosition() + measureDir * dist;
        angs(k,0) = dist;
        angs(k,1) = edgePos(0);
        angs(k,2) = edgePos(1);
    }

    return angs;
}

Eigen::MatrixXd Car::getMeasuredDistances() const
{
    return m_measuredDistances;
}

void Car::considerSuicide()
{
    if(( getAge() > 1.0 && m_droveDistance < 3.0) || m_droveDistance < m_formerDistance * 1.01 )
    {
        m_alive = false;
    }
    else
    {
        m_formerDistance = m_droveDistance;
    }

    if( m_accumulatedRotation / getAge() > M_PI / 2.0) //90° per seconds average
    {
        m_alive = false;
    }
}

void Car::navigate()
{
    // decide what to do next
    Eigen::MatrixXd nnInput = m_measuredDistances.col(0);
    nnInput.conservativeResize(m_measuredDistances.rows()+1, 1); // additional input for speed
    nnInput(nnInput.rows()-1,0) = m_speed;

    // normalize input -> all values are positive -> scale them on a range -1 to +1
    double maxValInput = nnInput.maxCoeff();
    nnInput = (nnInput * 2.0/maxValInput).array() - 1.0;

    m_network->feedForward(nnInput);
    Eigen::MatrixXd nnOut = m_network->getOutputActivation();

    double maxRotationSpeed = 720.0;
    double maxAcceleration = 100.0;

    // scale output from 0 - 1 to -1 to +1
    double speedActivation = (nnOut(0,0) - 0.5) * 2;
    double rotationActivation = (nnOut(1,0) - 0.5) * 2;
    setAcceleration(maxAcceleration*speedActivation);
    setRotationSpeed(maxRotationSpeed*rotationActivation);

    if( m_killer.elapsed() > 1000 )
    {
        considerSuicide();
        m_killer.restart();
    }
}


#include <boost/utility/string_view.hpp>  // for boost::string_view
#include <ckpttn/FMKWayConstrMgr.hpp>     // for FMKWayConstrMgr
#include <ckpttn/FMKWayGainMgr.hpp>       // for FMKWayGainMgr
#include <ckpttn/FMPartMgr.hpp>           // for FMPartMgr
#include <cstdint>                        // for uint8_t
#include <vector>                         // for vector

#include "benchmark/benchmark.h"      // for BENCHMARK, State, BENCHMARK_MAIN
#include "ckpttn/FMKWayGainCalc.hpp"  // for FMKWayGainCalc
#include "ckpttn/netlist.hpp"         // for SimpleNetlist

extern auto create_test_netlist() -> SimpleNetlist;  // import create_test_netlist
extern auto create_dwarf() -> SimpleNetlist;         // import create_dwarf
extern auto readNetD(boost::string_view netDFileName) -> SimpleNetlist;
extern void readAre(SimpleNetlist& H, boost::string_view areFileName);

/**
 * @brief Run FM K-way partitioning
 *
 * @param[in] H
 * @param[in] K
 * @param[in] option
 */
void run_FMKWayPartMgr(SimpleNetlist& H, std::uint8_t K, bool option) {
    FMKWayGainMgr<SimpleNetlist> gainMgr{H, K};
    gainMgr.gainCalc.special_handle_2pin_nets = option;

    FMKWayConstrMgr<SimpleNetlist> constrMgr{H, 0.4, K};
    FMPartMgr<SimpleNetlist, FMKWayGainMgr<SimpleNetlist>, FMKWayConstrMgr<SimpleNetlist>> partMgr{
        H, gainMgr, constrMgr};
    std::vector<std::uint8_t> part(H.number_of_modules(), 0);

    partMgr.legalize(part);
    // auto totalcostbefore = partMgr.totalcost;
    partMgr.optimize(part);
    // CHECK(totalcostbefore >= 0);
    // CHECK(partMgr.totalcost <= totalcostbefore);
    // CHECK(partMgr.totalcost >= 0);
}

/**
 * @brief
 *
 * @param[in] state
 */
static void BM_with_2pin_nets(benchmark::State& state) {
    auto H = readNetD("../../testcases/ibm03.net");
    readAre(H, "../../testcases/ibm03.are");

    while (state.KeepRunning()) {
        run_FMKWayPartMgr(H, 3, true);
    }
}

// Register the function as a benchmark
BENCHMARK(BM_with_2pin_nets);

//~~~~~~~~~~~~~~~~

/**
 * @brief Define another benchmark
 *
 * @param[in] state
 */
static void BM_without_2pin_nets(benchmark::State& state) {
    auto H = readNetD("../../testcases/ibm03.net");
    readAre(H, "../../testcases/ibm03.are");

    while (state.KeepRunning()) {
        run_FMKWayPartMgr(H, 3, false);
    }
}
BENCHMARK(BM_without_2pin_nets);

BENCHMARK_MAIN();

/*
3: ---------------------------------------------------------------
3: Benchmark                     Time             CPU   Iterations
3: ---------------------------------------------------------------
3: BM_with_2pin_nets     463584957 ns    463577351 ns            2
3: BM_without_2pin_nets  492702676 ns    492693494 ns            2
3/4 Test #3: Bench_FMKWay_2pin_nets ...........   Passed    3.02 sec
*/


#define CGAL_data_type float
#define CGAL_GL_data_type GL_FLOAT
#include "Scene_points_with_normal_item.h"
#include <CGAL/Polygon_mesh_processing/compute_normal.h>

#include <CGAL/Point_set_3/IO.h>
#include <CGAL/Timer.h>
#include <CGAL/Memory_sizer.h>

#include <CGAL/Three/Viewer_interface.h>
#include <CGAL/Three/Three.h>
#include <CGAL/Three/Point_container.h>
#include <CGAL/Three/Edge_container.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Search_traits_adapter.h>
#include <CGAL/linear_least_squares_fitting_3.h>
#include <CGAL/algorithm.h>

#include <QObject>
#include <QApplication>
#include <QMenu>
#include <QSlider>
#include <QWidgetAction>
#include <CGAL/Qt/manipulatedCameraFrame.h>

#include <set>
#include <stack>
#include <algorithm>
#include <boost/array.hpp>

#include <CGAL/boost/graph/properties_Surface_mesh.h>

#ifdef CGAL_LINKED_WITH_TBB
#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
#include <tbb/scalable_allocator.h>
#endif // CGAL_LINKED_WITH_TBB

const std::size_t limit_fast_drawing = 300000; //arbitraty large value

typedef CGAL::Three::Point_container Pc;
typedef CGAL::Three::Edge_container Ec;
typedef CGAL::Three::Viewer_interface VI;
typedef Scene_points_with_normal_item_priv Priv;
struct Scene_points_with_normal_item_priv
{
  enum Point_container_id{
    Points =0,
    Shaded_points,
    Selected_points,
    Selected_shaded_points
  };

  void init_values(Scene_points_with_normal_item* parent)
  {
    item = parent;
    nb_points = 0;
    nb_selected_points = 0;
    nb_lines = 0;
    is_point_slider_moving = false;
    normal_Slider = new QSlider(Qt::Horizontal);
    normal_Slider->setValue(CGAL::Three::Three::getDefaultNormalLength());
    point_Slider = new QSlider(Qt::Horizontal);
    point_Slider->setMinimum(1);
    point_Slider->setValue(CGAL::Three::Three::getDefaultPointSize());
    point_Slider->setMaximum(25);
    item->setPointContainer(Priv::Selected_shaded_points, new Pc(VI::PROGRAM_WITH_LIGHT,
                                                 false));
    item->setPointContainer(Priv::Selected_points, new Pc(VI::PROGRAM_NO_SELECTION,
                                                 false));
    item->setPointContainer(Priv::Shaded_points, new Pc(VI::PROGRAM_WITH_LIGHT,
                                                 false));
    item->setPointContainer(Priv::Points, new Pc(VI::PROGRAM_NO_SELECTION,
                                                 false));
    item->setEdgeContainer(0, new Ec(VI::PROGRAM_NO_SELECTION,
                                                 false));

  }
  Scene_points_with_normal_item_priv(Scene_points_with_normal_item* parent)
    :m_points(new Point_set)
  {
    init_values(parent);
  }
  Scene_points_with_normal_item_priv(const Scene_points_with_normal_item& toCopy, Scene_points_with_normal_item* parent)
    : m_points(new Point_set(*toCopy.d->m_points))
  {
    init_values(parent);
  }

  Scene_points_with_normal_item_priv(const SMesh& input_mesh, Scene_points_with_normal_item* parent)
    : m_points(new Point_set)
  {
   init_values(parent);
   boost::graph_traits<SMesh>::vertex_iterator v;
    m_points->add_normal_map();
    for (v = const_cast<SMesh&>(input_mesh).vertices_begin();
         v != const_cast<SMesh&>(input_mesh).vertices_end(); v++)
    {
      boost::graph_traits<SMesh>::vertex_descriptor vd(*v);
      const Kernel::Point_3& p = input_mesh.point(vd);
      Kernel::Vector_3 n =
        CGAL::Polygon_mesh_processing::compute_vertex_normal(vd, input_mesh);
      m_points->insert(p,n);
    }
  }

  ~Scene_points_with_normal_item_priv()
  {
    if(m_points)
    {
      delete m_points;
      m_points = NULL;
    }
    delete normal_Slider;
    delete point_Slider;
  }
  bool isPointSliderMoving() { return is_point_slider_moving; }
  void initializeBuffers(CGAL::Three::Viewer_interface *viewer) const;
  void compute_normals_and_vertices() const;

  Point_set* m_points;
  std::string m_comments;
  QAction* actionDeleteSelection;
  QAction* actionResetSelection;
  QAction* actionSelectDuplicatedPoints;
  QSlider* normal_Slider;
  QSlider* point_Slider;
  mutable bool is_point_slider_moving;
  mutable std::vector<CGAL_data_type> positions_lines;
  mutable std::vector<CGAL_data_type> normals;
  mutable std::vector<CGAL_data_type> positions_normals;
  mutable std::vector<CGAL_data_type> positions_selected_normals;
  mutable std::vector<CGAL_data_type> colors_points;
  mutable std::size_t nb_points;
  mutable std::size_t nb_selected_points;
  mutable std::size_t nb_lines;
  mutable QOpenGLShaderProgram *program;

  Scene_points_with_normal_item* item;
};

class Fill_buffers {

  Point_set* point_set;
  std::vector<Point_set::Index>& indices;
  std::vector<CGAL_data_type>& positions_lines;
  std::vector<CGAL_data_type>& positions_normals;
  bool has_normals;
  const CGAL::qglviewer::Vec offset;
  double length;
  std::size_t size_p;
  std::size_t offset_normal_indices;

public:
  Fill_buffers(Point_set* point_set,
               std::vector<Point_set::Index>& indices,
               std::vector<CGAL_data_type>& positions_lines,
               std::vector<CGAL_data_type>& positions_normals,
               bool has_normals,
               const CGAL::qglviewer::Vec offset,
               double length,
               std::size_t offset_normal_indices = 0)
    : point_set (point_set)
    , indices (indices)
    , positions_lines (positions_lines)
    , positions_normals (positions_normals)
    , has_normals (has_normals)
    , offset (offset)
    , length (length)
    , offset_normal_indices (offset_normal_indices)
  {
    if (has_normals)
      size_p = 6;
    else
      size_p = 3;
  }

#ifdef CGAL_LINKED_WITH_TBB
  void operator()(const tbb::blocked_range<std::size_t>& r) const
  {
    for( std::size_t i = r.begin(); i != r.end(); ++i)
      apply (i);
  }
#endif // CGAL_LINKED_WITH_TBB

  void apply (std::size_t i) const
  {
    const Point_set::Index& idx = indices[i];
    const Kernel::Point_3& p = point_set->point(idx);

    positions_lines[i * size_p    ] = p.x() + offset.x;
    positions_lines[i * size_p + 1] = p.y() + offset.y;
    positions_lines[i * size_p + 2] = p.z() + offset.z;

    if(has_normals)
    {
      const Kernel::Vector_3& n = point_set->normal(idx);
      Kernel::FT normalizer = 1.0/CGAL::sqrt(n.squared_length());
      Point_set_3<Kernel>::Point q = p + length * n * normalizer;
      positions_lines[i * size_p + 3] = q.x() + offset.x;
      positions_lines[i * size_p + 4] = q.y() + offset.y;
      positions_lines[i * size_p + 5] = q.z() + offset.z;

      positions_normals[(i - offset_normal_indices) * 3    ] = n.x();
      positions_normals[(i - offset_normal_indices) * 3 + 1] = n.y();
      positions_normals[(i - offset_normal_indices) * 3 + 2] = n.z();
    }
  }
};

Scene_points_with_normal_item::Scene_points_with_normal_item()
{
    setRenderingMode(Points);
    is_selected = true;
    d = new Scene_points_with_normal_item_priv(this);
}

// Copy constructor
Scene_points_with_normal_item::Scene_points_with_normal_item(const Scene_points_with_normal_item& toCopy)
{

  d = new Scene_points_with_normal_item_priv(toCopy, this);

  if (!has_normals())
  {
    setRenderingMode(Points);
    is_selected = true;
  }
  else{
    setRenderingMode(CGAL::Three::Three::defaultPointSetRenderingMode());
    is_selected = true;
  }

  invalidateOpenGLBuffers();
}

// Converts polyhedron to point set

Scene_points_with_normal_item::Scene_points_with_normal_item(const SMesh& input_mesh)
{
  // Converts Polyhedron vertices to point set.
  // Computes vertices normal from connectivity.
  d = new Scene_points_with_normal_item_priv(input_mesh, this);
  setRenderingMode(CGAL::Three::Three::defaultPointSetRenderingMode());
  is_selected = true;
  invalidateOpenGLBuffers();
}

Scene_points_with_normal_item::~Scene_points_with_normal_item()
{
  delete d;
}

void Scene_points_with_normal_item_priv::
initializeBuffers(CGAL::Three::Viewer_interface *viewer) const
{
  item->getEdgeContainer(0)->initializeBuffers(viewer);
  item->getPointContainer(Priv::Points)->initializeBuffers(viewer);
  item->getPointContainer(Priv::Shaded_points)->initializeBuffers(viewer);
  item->getPointContainer(Priv::Selected_points)->initializeBuffers(viewer);
  item->getPointContainer(Priv::Selected_shaded_points)->initializeBuffers(viewer);

  ////Clean-up
  item->getPointContainer(Priv::Points)->setFlatDataSize(nb_points - nb_selected_points);
  item->getPointContainer(Priv::Shaded_points)->setFlatDataSize(nb_points - nb_selected_points);
  item->getPointContainer(Priv::Selected_points)->setFlatDataSize(nb_selected_points);
  item->getPointContainer(Priv::Selected_shaded_points)->setFlatDataSize(nb_selected_points);
  item->getEdgeContainer(0)->setFlatDataSize(nb_lines);

  positions_lines             .resize(0);
  normals                     .resize(0);
  positions_normals           .resize(0);
  positions_selected_normals  .resize(0);
  colors_points               .resize(0);

  positions_lines             .shrink_to_fit();
  normals                     .shrink_to_fit();
  positions_normals           .shrink_to_fit();
  positions_selected_normals  .shrink_to_fit();
  colors_points               .shrink_to_fit();
}

void Scene_points_with_normal_item_priv::compute_normals_and_vertices() const
{
    const CGAL::qglviewer::Vec offset = static_cast<CGAL::Three::Viewer_interface*>(
          CGAL::QGLViewer::QGLViewerPool().first())->offset();
    QApplication::setOverrideCursor(Qt::WaitCursor);
    positions_lines.resize(0);
    normals.resize(0);
    positions_normals.resize(0);
    positions_selected_normals.resize(0);
    normals.resize(0);
    colors_points.resize(0);

    //Shuffle container to allow quick display random points
    std::vector<Point_set::Index> indices;
    indices.reserve (m_points->size());
    std::copy (m_points->begin(), m_points->end(), std::back_inserter(indices));

    CGAL::cpp98::random_shuffle (indices.begin(), indices.end() - m_points->nb_selected_points());
    if (m_points->nb_selected_points() != 0)
      CGAL::cpp98::random_shuffle (indices.end() - m_points->nb_selected_points(), indices.end());

    //if item has normals, points will be one point out of two in the lines data.
    //else points will be lines and lines discarded.
    double average_spacing = 0;
    double normal_length =0;
    double length_factor =0;
    if (item->has_normals())
    {
      // Store normals
      Kernel::Sphere_3 region_of_interest = m_points->region_of_interest();
      positions_lines.resize(m_points->size() * 6);
      positions_normals.resize((m_points->size() - m_points->nb_selected_points()) * 3);
      positions_selected_normals.resize(m_points->nb_selected_points() * 3);

      // we can't afford computing real average spacing just for display, 0.5% of bbox will do
      average_spacing = 0.005 * item->diagonalBbox();
      normal_length = (std::min)(average_spacing, std::sqrt(
                                   region_of_interest.squared_radius() / 1000.));
      length_factor = 10.0/100*normal_Slider->value();
    }
    else
    {
      positions_lines.resize(m_points->size() * 3);
    }

    Fill_buffers fill_buffers (m_points, indices, positions_lines, positions_normals,
                               item->has_normals(), offset, normal_length * length_factor);
    Fill_buffers fill_buffers_2 (m_points, indices, positions_lines, positions_selected_normals,
                                 item->has_normals(), offset, normal_length * length_factor,
                                 m_points->first_selected() - m_points->begin());

#ifdef CGAL_LINKED_WITH_TBB
    tbb::parallel_for(tbb::blocked_range<size_t>(0,
                                                 m_points->size() - m_points->nb_selected_points()),
                      fill_buffers);
    tbb::parallel_for(tbb::blocked_range<size_t>(m_points->size() - m_points->nb_selected_points(),
                                                 m_points->size()),
                      fill_buffers_2);
#else
    for (std::size_t i = 0; i < indices.size() - m_points->nb_selected_points(); ++ i)
      fill_buffers.apply (i);
    for (std::size_t i = indices.size() - m_points->nb_selected_points(); i < indices.size(); ++ i)
      fill_buffers_2.apply (i);
#endif

    //The colors
    if (m_points->has_colors())
    {
        colors_points.reserve((m_points->size() - m_points->nb_selected_points()) * 6);

        for (std::size_t i = 0; i < indices.size() - m_points->nb_selected_points(); ++ i)
        {
          colors_points.push_back (m_points->red(indices[i]));
          colors_points.push_back (m_points->green(indices[i]));
          colors_points.push_back (m_points->blue(indices[i]));
          colors_points.push_back (m_points->red(indices[i]));
          colors_points.push_back (m_points->green(indices[i]));
          colors_points.push_back (m_points->blue(indices[i]));
        }
    }

    nb_lines = positions_lines.size();
    if(item->has_normals())
      nb_points = positions_lines.size()/2;
    else
      nb_points = positions_lines.size();
    nb_selected_points = m_points->nb_selected_points() * 3;
    //edges
    if(item->has_normals())
    {
      item->getEdgeContainer(0)->allocate(Ec::Vertices, positions_lines.data(),
                                       static_cast<int>(nb_lines*sizeof(CGAL_data_type)));
      if (!(colors_points.empty()))
      {
        item->getEdgeContainer(0)->allocate(Ec::Colors, colors_points.data(),
                                            static_cast<int>(colors_points.size()
                                                             *sizeof(CGAL_data_type)));
      }
      //shaded points
      item->getPointContainer(Priv::Shaded_points)->setStride(Pc::Vertices,
                                           static_cast<int>(6*sizeof(CGAL_GL_data_type)));
      item->getPointContainer(Priv::Shaded_points)->allocate(Pc::Vertices,  positions_lines.data(),
                                       static_cast<int>(nb_lines*sizeof(CGAL_data_type)));
      item->getPointContainer(Priv::Shaded_points)->allocate(Pc::Normals, positions_normals.data(),
                                           static_cast<int>(positions_normals.size()
                                                            *sizeof(CGAL_data_type)));
    }
    //points
    if(!item->has_normals()) {
      item->getPointContainer(Priv::Points)->setStride(Pc::Vertices,
                                            0);
    }
    else{
      item->getPointContainer(Priv::Points)->setStride(Pc::Vertices,
                                            static_cast<int>(6*sizeof(CGAL_GL_data_type)));
    }
    item->getPointContainer(Priv::Points)->allocate(Pc::Vertices, positions_lines.data(),
                                         static_cast<int>(positions_lines.size()
                                                          *sizeof(CGAL_data_type)));
    if (!(colors_points.empty()))
    {
      item->getPointContainer(Priv::Points)->setStride(Pc::Colors,6*sizeof(CGAL_data_type));
      item->getPointContainer(Priv::Points)->allocate(Pc::Colors, colors_points.data(),
                                           static_cast<int>(colors_points.size()
                                                            *sizeof(CGAL_data_type)));
    }
    //selected points
    if(!item->has_normals()) {
      item->getPointContainer(Priv::Selected_points)->setStride(Pc::Vertices,
                                            0);
      item->getPointContainer(Priv::Selected_points)->setOffset(Pc::Vertices,
                                            static_cast<int>(
                                              3*
                                              (m_points->size()
                                               -m_points->nb_selected_points())
                                              *sizeof(CGAL_data_type) ));
    }
    else{
      item->getPointContainer(Priv::Selected_points)->setStride(Pc::Vertices,
                                            static_cast<int>(6*sizeof(CGAL_GL_data_type)));
      item->getPointContainer(Priv::Selected_points)->setOffset(Pc::Vertices,
                                            static_cast<int>(
                                              6*
                                              (m_points->size()
                                               -m_points->nb_selected_points())
                                              *sizeof(CGAL_data_type) ));
      item->getPointContainer(Priv::Selected_shaded_points)->setStride(Pc::Vertices,
                                            static_cast<int>(6*sizeof(CGAL_GL_data_type)));
      item->getPointContainer(Priv::Selected_shaded_points)->setOffset(Pc::Vertices,
                                            static_cast<int>(
                                              6*
                                              (m_points->size()
                                               -m_points->nb_selected_points())
                                              *sizeof(CGAL_data_type) ));
      item->getPointContainer(Priv::Selected_shaded_points)->allocate(Pc::Vertices,
                                                                      positions_lines.data(),
                                           static_cast<int>(positions_lines.size()
                                                            *sizeof(CGAL_data_type)));
      item->getPointContainer(Priv::Selected_shaded_points)->allocate(Pc::Normals,
                                                                      positions_selected_normals.data(),
                                           static_cast<int>(positions_selected_normals.size()
                                                            *sizeof(CGAL_data_type)));
    }
    item->getPointContainer(Priv::Selected_points)->allocate(Pc::Vertices, positions_lines.data(),
                                         static_cast<int>(positions_lines.size()
                                                          *sizeof(CGAL_data_type)));
    if (!(colors_points.empty()))
    {
      item->getPointContainer(Priv::Points)->setStride(Pc::Colors,6*sizeof(CGAL_data_type));
      item->getPointContainer(Priv::Points)->allocate(Pc::Colors, colors_points.data(),
                                           static_cast<int>(colors_points.size()
                                                            *sizeof(CGAL_data_type)));
    }
    QApplication::restoreOverrideCursor();
}

// Duplicates scene item
Scene_points_with_normal_item*
Scene_points_with_normal_item::clone() const
{
  return new Scene_points_with_normal_item(*this);
}

// Is selection empty?
bool Scene_points_with_normal_item::isSelectionEmpty() const
{
  return (d->m_points->nb_selected_points() == 0);
}

// Delete selection
void Scene_points_with_normal_item::deleteSelection()
{
  CGAL::Timer task_timer; task_timer.start();
  std::cerr << "Delete " << d->m_points->nb_selected_points() << " points...";

  // Delete selected points
  d->m_points->delete_selection();

  std::size_t memory = CGAL::Memory_sizer().virtual_size();
  std::cerr << "done: " << task_timer.time() << " seconds, "
                        << (memory>>20) << " Mb allocated"
                        << std::endl;
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}

// Invert selection
void Scene_points_with_normal_item::invertSelection()
{
  d->m_points->invert_selection();
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}

// Select everything
void Scene_points_with_normal_item::selectAll()
{
  d->m_points->select_all();
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}
// Reset selection mark
void Scene_points_with_normal_item::resetSelection()
{
  // Un-select all points
  d->m_points->unselect_all();
  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}
  //Select duplicated points
void Scene_points_with_normal_item::selectDuplicates()
{
  std::set<Kernel::Point_3> unique_points;
  std::vector<Point_set::Index> unselected, selected;
  for (Point_set::iterator ptit = d->m_points->begin(); ptit!= d->m_points->end(); ++ ptit)
    if ( !unique_points.insert(d->m_points->point(*ptit)).second)
      selected.push_back (*ptit);
    else
      unselected.push_back (*ptit);

  for (std::size_t i = 0; i < unselected.size(); ++ i)
    *(d->m_points->begin() + i) = unselected[i];
  for (std::size_t i = 0; i < selected.size(); ++ i)
    *(d->m_points->begin() + (unselected.size() + i)) = selected[i];

  if (selected.empty ())
  {
    d->m_points->unselect_all();
  }
  else
  {
    d->m_points->set_first_selected
      (d->m_points->begin() + unselected.size());
  }

  invalidateOpenGLBuffers();
  Q_EMIT itemChanged();
}

#ifdef CGAL_LINKED_WITH_LASLIB
// Loads point set from .LAS file
bool Scene_points_with_normal_item::read_las_point_set(std::istream& stream)
{
  Q_ASSERT(d->m_points != NULL);

  d->m_points->clear();

  bool ok = stream &&
    CGAL::read_las_point_set (stream, *(d->m_points)) &&
            !isEmpty();

  std::cerr << d->m_points->info();

  if (!d->m_points->has_normal_map())
  {
    setRenderingMode(Points);
  }
  else{
    setRenderingMode(CGAL::Three::Three::defaultPointSetRenderingMode());
  }
  if (d->m_points->check_colors())
    std::cerr << "-> Point set has colors" << std::endl;

  invalidateOpenGLBuffers();
  return ok;
}

// Write point set to .LAS file
bool Scene_points_with_normal_item::write_las_point_set(std::ostream& stream) const
{
  Q_ASSERT(d->m_points != NULL);

  d->m_points->reset_indices();

  return stream &&
    CGAL::write_las_point_set (stream, *(d->m_points));
}

#endif // LAS

// Loads point set from .PLY file
bool Scene_points_with_normal_item::read_ply_point_set(std::istream& stream)
{
  Q_ASSERT(d->m_points != NULL);

  d->m_points->clear();

  bool ok = stream &&
    CGAL::read_ply_point_set (stream, *(d->m_points), d->m_comments) &&
            !isEmpty();
    d->point_Slider->setValue(CGAL::Three::Three::getDefaultPointSize());
  std::cerr << d->m_points->info();

  if (!d->m_points->has_normal_map())
  {
    setRenderingMode(Points);
  }
  else{
    setRenderingMode(CGAL::Three::Three::defaultPointSetRenderingMode());
  }
  if (d->m_points->check_colors())
    std::cerr << "-> Point set has colors" << std::endl;

  std::cerr << "[Comments from PLY input]" << std::endl << d->m_comments;

  invalidateOpenGLBuffers();
  return ok;
}

// Write point set to .PLY file
bool Scene_points_with_normal_item::write_ply_point_set(std::ostream& stream, bool binary) const
{
  Q_ASSERT(d->m_points != NULL);

  d->m_points->reset_indices();

  if (!stream)
    return false;

  if (binary)
    CGAL::set_binary_mode (stream);

  CGAL::write_ply_point_set (stream, *(d->m_points), d->m_comments);

  return true;
}

// Loads point set from .OFF file
bool Scene_points_with_normal_item::read_off_point_set(std::istream& stream)
{
  Q_ASSERT(d->m_points != NULL);

  d->m_points->clear();
  bool ok = stream &&
    CGAL::read_off_point_set(stream, *(d->m_points)) &&
            !isEmpty();
  d->point_Slider->setValue(CGAL::Three::Three::getDefaultPointSize());
  invalidateOpenGLBuffers();
  return ok;
}

// Write point set to .OFF file
bool Scene_points_with_normal_item::write_off_point_set(std::ostream& stream) const
{
  Q_ASSERT(d->m_points != NULL);

  d->m_points->reset_indices();

  return stream &&
    CGAL::write_off_point_set (stream, *(d->m_points));
}

// Loads point set from .XYZ file
bool Scene_points_with_normal_item::read_xyz_point_set(std::istream& stream)
{
  Q_ASSERT(d->m_points != NULL);

  d->m_points->clear();

  bool ok = stream &&
    CGAL::read_xyz_point_set (stream, *(d->m_points)) &&
    !isEmpty();
  d->point_Slider->setValue(CGAL::Three::Three::getDefaultPointSize());
  invalidateOpenGLBuffers();
  return ok;
}

// Write point set to .XYZ file
bool Scene_points_with_normal_item::write_xyz_point_set(std::ostream& stream) const
{
  Q_ASSERT(d->m_points != NULL);

  d->m_points->reset_indices();

  return stream &&
    CGAL::write_xyz_point_set (stream, *(d->m_points));
}

QString
Scene_points_with_normal_item::toolTip() const
{
  Q_ASSERT(d->m_points != NULL);

  return QObject::tr("<p><b>%1</b> (color: %4)<br />"
                     "<i>Point_set_3</i></p>"
                     "<p>Number of points: %2</p>")
    .arg(name())
    .arg(d->m_points->size())
    .arg(color().name());
}

bool Scene_points_with_normal_item::supportsRenderingMode(RenderingMode m) const
{
  switch ( m )
  {
  case Points:
    return true;
  case ShadedPoints:
  case PointsPlusNormals:
    return has_normals();

  default:
    return false;
  }
}

void Scene_points_with_normal_item::drawEdges(CGAL::Three::Viewer_interface* viewer) const
{
    double ratio_displayed = 1.0;
    if (viewer->inFastDrawing () &&
        (d->nb_lines/6 > limit_fast_drawing)) // arbitrary large value
      ratio_displayed = 6 * limit_fast_drawing / (double)(d->nb_lines);
    if(!isInit(viewer))
      initGL(viewer);
    if ( getBuffersFilled() &&
       ! getBuffersInit(viewer))
    {
      d->initializeBuffers(viewer);
      setBuffersInit(viewer, true);
    }
    if(!d->m_points->has_colors())
      getEdgeContainer(0)->setColor(color());
    std::size_t real_size =
        getEdgeContainer(0)->getFlatDataSize();
    getEdgeContainer(0)->setFlatDataSize(ratio_displayed * real_size);
    getEdgeContainer(0)->draw( viewer, !d->m_points->has_colors());
    getEdgeContainer(0)->setFlatDataSize(real_size);

}
void Scene_points_with_normal_item::
drawPoints(CGAL::Three::Viewer_interface* viewer) const
{

  GLfloat point_size;
  viewer->glGetFloatv(GL_POINT_SIZE, &point_size);
  viewer->setGlPointSize(GLfloat(d->point_Slider->value()));
  double ratio_displayed = 1.0;
  if ((viewer->inFastDrawing () || d->isPointSliderMoving())
      &&((d->nb_points )/3 > limit_fast_drawing)) // arbitrary large value
    ratio_displayed = 3 * limit_fast_drawing / (double)(d->nb_points);

  if(!isInit(viewer))
    initGL(viewer);
  if ( getBuffersFilled() &&
     ! getBuffersInit(viewer))
  {
    d->initializeBuffers(viewer);
    setBuffersInit(viewer, true);
  }
  if(!getBuffersFilled())
  {
    computeElements();
    d->initializeBuffers(viewer);
  }

  if(has_normals() && renderingMode() == ShadedPoints)
  {
    getPointContainer(Priv::Shaded_points)->setColor(color());
    std::size_t real_size =
        getPointContainer(Priv::Shaded_points)->getFlatDataSize();
    getPointContainer(Priv::Shaded_points)->setFlatDataSize(ratio_displayed * real_size);
    getPointContainer(Priv::Shaded_points)->draw( viewer, true);
    getPointContainer(Priv::Shaded_points)->setFlatDataSize(real_size);

    real_size =
        getPointContainer(Priv::Selected_shaded_points)->getFlatDataSize();
    getPointContainer(Priv::Selected_shaded_points)->setColor(QColor(Qt::red));
    getPointContainer(Priv::Selected_shaded_points)->setFlatDataSize(ratio_displayed * real_size);
    getPointContainer(Priv::Selected_shaded_points)->draw( viewer, true);
    getPointContainer(Priv::Selected_shaded_points)->setFlatDataSize(real_size);
  }
  else
  {
    if(!d->m_points->has_colors())
      getPointContainer(Priv::Points)->setColor(color());
    std::size_t real_size =
        getPointContainer(Priv::Points)->getFlatDataSize();
    getPointContainer(Priv::Points)->setFlatDataSize(ratio_displayed * real_size);
    getPointContainer(Priv::Points)->setFlatDataSize(real_size);
    getPointContainer(Priv::Points)->draw( viewer, !d->m_points->has_colors());

    real_size =
        getPointContainer(Priv::Selected_points)->getFlatDataSize();
    getPointContainer(Priv::Selected_points)->setColor(QColor(Qt::red));
    getPointContainer(Priv::Selected_points)->setFlatDataSize(ratio_displayed * real_size);
    getPointContainer(Priv::Selected_points)->setFlatDataSize(real_size);
    getPointContainer(Priv::Selected_points)->draw( viewer, true);
  }

  viewer->setGlPointSize(point_size);
}
// Gets wrapped point set
Point_set* Scene_points_with_normal_item::point_set()
{
  Q_ASSERT(d->m_points != NULL);
  return d->m_points;
}
const Point_set* Scene_points_with_normal_item::point_set() const
{
  Q_ASSERT(d->m_points != NULL);
  return d->m_points;
}

// Gets wrapped point set
std::string& Scene_points_with_normal_item::comments()
{
  return d->m_comments;
}
const std::string& Scene_points_with_normal_item::comments() const
{
  return d->m_comments;
}

bool
Scene_points_with_normal_item::isEmpty() const
{
  Q_ASSERT(d->m_points != NULL);
  return d->m_points->empty();
}

void
Scene_points_with_normal_item::compute_bbox()const
{
  Q_ASSERT(d->m_points != NULL);

  Kernel::Iso_cuboid_3 bbox = d->m_points->bounding_box();
  setBbox(Bbox(bbox.xmin(),bbox.ymin(),bbox.zmin(),
              bbox.xmax(),bbox.ymax(),bbox.zmax()));
}

void Scene_points_with_normal_item::computes_local_spacing(int k)
{
  typedef Kernel Geom_traits;

  typedef CGAL::Search_traits_3<Geom_traits> SearchTraits_3;
  typedef CGAL::Search_traits_adapter <Point_set::Index, Point_set::Point_map, SearchTraits_3> Search_traits;
  typedef CGAL::Orthogonal_k_neighbor_search<Search_traits> Neighbor_search;
  typedef Neighbor_search::Tree Tree;
  typedef Neighbor_search::Distance Distance;

  // build kdtree
  Tree tree(d->m_points->begin(),
            d->m_points->end(),
            Tree::Splitter(),
            Search_traits (d->m_points->point_map())
            );
  Distance tr_dist(d->m_points->point_map());

  if (!(d->m_points->has_property_map<double> ("radius")))
    d->m_points->add_radius();

  // Compute the radius of each point = (distance max to k nearest neighbors)/2.
  {
    int i=0;
    for (Point_set::iterator it=d->m_points->begin(); it!=d->m_points->end(); ++it, ++i)
    {
      Neighbor_search search(tree, d->m_points->point(*it), k+1, 0, true, tr_dist);
      double maxdist2 = (--search.end())->second; // squared distance to furthest neighbor
      d->m_points->radius(*it) = sqrt(maxdist2)/2.;
    }
  }

  d->m_points->set_radii_uptodate(true);
}

QMenu* Scene_points_with_normal_item::contextMenu()
{
    const char* prop_name = "Menu modified by Scene_points_with_normal_item.";

    QMenu* menu = Scene_item::contextMenu();

    //add a slider to modify the normals length
    // Use dynamic properties:
    // https://doc.qt.io/qt-5/qobject.html#property
    bool menuChanged = menu->property(prop_name).toBool();

    if(!menuChanged) {
      if(has_normals())
      {
        QMenu *container = new QMenu(tr("Normals Length"));
        QWidgetAction *sliderAction = new QWidgetAction(0);
        if((d->nb_points)/3 <= limit_fast_drawing)
        {
          connect(d->normal_Slider, &QSlider::valueChanged, this, &Scene_points_with_normal_item::invalidateOpenGLBuffers);
          connect(d->normal_Slider, &QSlider::valueChanged, this, &Scene_points_with_normal_item::itemChanged);
        }
        else
        {
          connect(d->normal_Slider, &QSlider::sliderReleased, this, &Scene_points_with_normal_item::invalidateOpenGLBuffers);
          connect(d->normal_Slider, &QSlider::sliderReleased, this, &Scene_points_with_normal_item::itemChanged);
        }
        sliderAction->setDefaultWidget(d->normal_Slider);
        container->menuAction()->setProperty("is_groupable", true);
        container->addAction(sliderAction);
        menu->addMenu(container);
      }
        QMenu *container = new QMenu(tr("Points Size"));
        QWidgetAction *sliderAction = new QWidgetAction(0);
        connect(d->point_Slider, &QSlider::sliderPressed, this, &Scene_points_with_normal_item::pointSliderPressed);
        connect(d->point_Slider, &QSlider::sliderReleased, this, &Scene_points_with_normal_item::pointSliderReleased);
        connect(d->point_Slider, &QSlider::valueChanged, this, &Scene_points_with_normal_item::itemChanged);

        sliderAction->setDefaultWidget(d->point_Slider);
        container->menuAction()->setProperty("is_groupable", true);
        container->addAction(sliderAction);
        menu->addMenu(container);

        d->actionDeleteSelection = menu->addAction(tr("Delete Selection"));
        d->actionDeleteSelection->setObjectName("actionDeleteSelection");
        connect(d->actionDeleteSelection, SIGNAL(triggered()),this, SLOT(deleteSelection()));

        d->actionResetSelection = menu->addAction(tr("Reset Selection"));
        d->actionResetSelection->setObjectName("actionResetSelection");
        connect(d->actionResetSelection, SIGNAL(triggered()),this, SLOT(resetSelection()));

        d->actionSelectDuplicatedPoints = menu->addAction(tr("Select duplicated points"));
        d->actionSelectDuplicatedPoints->setObjectName("actionSelectDuplicatedPoints");
        connect(d->actionSelectDuplicatedPoints, SIGNAL(triggered()),this, SLOT(selectDuplicates()));
        QAction* resetColorsAction = menu->addAction(tr("Make Unicolor"));
        resetColorsAction->setObjectName("resetColorsAction");
        connect(resetColorsAction, &QAction::triggered, this, &Scene_points_with_normal_item::resetColors);
        menu->setProperty(prop_name, true);
    }
    QAction* actionColor = menu->findChild<QAction*>(tr("resetColorsAction"));
    actionColor->setVisible(d->m_points->has_colors());
    if (isSelectionEmpty())
    {
        d->actionDeleteSelection->setDisabled(true);
        d->actionResetSelection->setDisabled(true);
    }
    else
    {
        d->actionDeleteSelection->setDisabled(false);
        d->actionResetSelection->setDisabled(false);
    }

    return menu;
}

bool Scene_points_with_normal_item::has_normals() const { return d->m_points->has_normal_map(); }

void Scene_points_with_normal_item::invalidateOpenGLBuffers()
{
    setBuffersFilled(false);
    getPointContainer(Priv::Points)->reset_vbos(Scene_item_rendering_helper::ALL);
    getPointContainer(Priv::Selected_points)->reset_vbos(Scene_item_rendering_helper::ALL);
    getPointContainer(Priv::Selected_shaded_points)->reset_vbos(Scene_item_rendering_helper::ALL);
    getPointContainer(Priv::Shaded_points)->reset_vbos(Scene_item_rendering_helper::ALL);
    getEdgeContainer(0)->reset_vbos(Scene_item_rendering_helper::ALL);

    Q_FOREACH(CGAL::QGLViewer* v, CGAL::QGLViewer::QGLViewerPool())
    {
      CGAL::Three::Viewer_interface* viewer = static_cast<CGAL::Three::Viewer_interface*>(v);
      if(viewer == NULL)
        continue;
      setBuffersInit(viewer, false);
    }
}

void Scene_points_with_normal_item::pointSliderPressed()
{
  d->is_point_slider_moving = true;
}

void Scene_points_with_normal_item::pointSliderReleased()
{
  d->is_point_slider_moving = false;
}

void Scene_points_with_normal_item::copyProperties(Scene_item *item)
{
  Scene_points_with_normal_item* point_item = qobject_cast<Scene_points_with_normal_item*>(item);
  if(!point_item)
    return;
  int value = point_item->getPointSliderValue();
  setPointSize(value);
  if(has_normals())
    d->normal_Slider->setValue(point_item->getNormalSliderValue());
}

int Scene_points_with_normal_item::getNormalSliderValue()
{
  return d->normal_Slider->value();
}

int Scene_points_with_normal_item::getPointSliderValue()
{
  return d->point_Slider->value();
}

void Scene_points_with_normal_item::itemAboutToBeDestroyed(Scene_item *item)
{
  Scene_item::itemAboutToBeDestroyed(item);
  if(d && d->m_points && item == this)
  {
    delete d->m_points;
    d->m_points = NULL;
  }
}

void Scene_points_with_normal_item::
zoomToPosition(const QPoint &, CGAL::Three::Viewer_interface *viewer) const
{
  if (point_set()->nb_selected_points() == 0)
    return;
  const CGAL::qglviewer::Vec offset = static_cast<CGAL::Three::Viewer_interface*>(CGAL::QGLViewer::QGLViewerPool().first())->offset();
  // fit plane to triangles
  Point_set points;
  Bbox selected_points_bbox;
  for(Point_set::const_iterator it = point_set()->first_selected();
      it != point_set()->end();
      ++it)
  {
    points.insert(point_set()->point(*it));
    selected_points_bbox += point_set()->point(*it).bbox();
  }
  Kernel::Plane_3 plane;
  Kernel::Point_3 center_of_mass;
  CGAL::linear_least_squares_fitting_3
      (points.points().begin(),points.points().end(),plane, center_of_mass,
       CGAL::Dimension_tag<0>());

  Kernel::Vector_3 plane_normal= plane.orthogonal_vector();
  plane_normal = plane_normal/(CGAL::sqrt(plane_normal.squared_length()));
  Kernel::Point_3 centroid(center_of_mass.x() + offset.x,
                           center_of_mass.y() + offset.y,
                           center_of_mass.z() + offset.z);

  CGAL::qglviewer::Quaternion new_orientation(CGAL::qglviewer::Vec(0,0,-1),
                                        CGAL::qglviewer::Vec(-plane_normal.x(), -plane_normal.y(), -plane_normal.z()));
  double max_side = (std::max)((std::max)(selected_points_bbox.xmax() - selected_points_bbox.xmin(),
                                          selected_points_bbox.ymax() - selected_points_bbox.ymin()),
                               selected_points_bbox.zmax() - selected_points_bbox.zmin());
  //put the camera in way we are sure the longest side is entirely visible on the screen
  //See openGL's frustum definition
  double factor = max_side/(tan(viewer->camera()->aspectRatio()/
                                  (viewer->camera()->fieldOfView()/2)));

  Kernel::Point_3 new_pos = centroid + factor*plane_normal ;
  viewer->camera()->setSceneCenter(CGAL::qglviewer::Vec(centroid.x(),
                                                  centroid.y(),
                                                  centroid.z()));
  viewer->moveCameraToCoordinates(QString("%1 %2 %3 %4 %5 %6 %7").arg(new_pos.x())
                                                                 .arg(new_pos.y())
                                                                 .arg(new_pos.z())
                                                                 .arg(new_orientation[0])
                                                                 .arg(new_orientation[1])
                                                                 .arg(new_orientation[2])
                                                                 .arg(new_orientation[3]));

}

void Scene_points_with_normal_item::setPointSize(int size)
{
  d->point_Slider->setValue(size);
}

void Scene_points_with_normal_item::setNormalSize(int size)
{
  d->normal_Slider->setValue(size);
}

void Scene_points_with_normal_item::resetColors()
{
  d->m_points->remove_colors();
  invalidateOpenGLBuffers();
  redraw();
}

void Scene_points_with_normal_item::computeElements()const
{
  d->compute_normals_and_vertices();
  setBuffersFilled(true);

}

void Scene_points_with_normal_item::initializeBuffers(Viewer_interface * v) const
{
  d->initializeBuffers(v);
}