// Copyright (C) 2013 The Regents of the University of California (Regents).
// 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 Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/triangulation/triangulation.h"

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <Eigen/SVD>
// #include <glog/logging.h>
#include <vector>

#include "theia/matching/feature_correspondence.h"
#include "theia/math/util.h"
#include "theia/sfm/pose/essential_matrix_utils.h"
#include "theia/sfm/pose/fundamental_matrix_util.h"
#include "theia/sfm/pose/util.h"

namespace theia {
namespace {

using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;

// Given either a fundamental or essential matrix and two corresponding images
// points such that ematrix * point2 produces a line in the first image,
// this method finds corrected image points such that
// corrected_point1^t * ematrix * corrected_point2 = 0.
void FindOptimalImagePoints(const Matrix3d& ematrix,
                            const Vector2d& point1,
                            const Vector2d& point2,
                            Vector2d* corrected_point1,
                            Vector2d* corrected_point2) {
  const Vector3d point1_homog = point1.homogeneous();
  const Vector3d point2_homog = point2.homogeneous();

  // A helper matrix to isolate certain coordinates.
  Matrix<double, 2, 3> s_matrix;
  s_matrix << 1, 0, 0, 0, 1, 0;

  const Eigen::Matrix2d e_submatrix = ematrix.topLeftCorner<2, 2>();

  // The epipolar line from one image point in the other image.
  Vector2d epipolar_line1 = s_matrix * ematrix * point2_homog;
  Vector2d epipolar_line2 = s_matrix * ematrix.transpose() * point1_homog;

  const double a = epipolar_line1.transpose() * e_submatrix * epipolar_line2;
  const double b =
      (epipolar_line1.squaredNorm() + epipolar_line2.squaredNorm()) / 2.0;
  const double c = point1_homog.transpose() * ematrix * point2_homog;

  const double d = sqrt(b * b - a * c);

  double lambda = c / (b + d);
  epipolar_line1 -= e_submatrix * lambda * epipolar_line1;
  epipolar_line2 -= e_submatrix.transpose() * lambda * epipolar_line2;

  lambda *=
      (2.0 * d) / (epipolar_line1.squaredNorm() + epipolar_line2.squaredNorm());

  *corrected_point1 =
      (point1_homog - s_matrix.transpose() * lambda * epipolar_line1)
          .hnormalized();
  *corrected_point2 =
      (point2_homog - s_matrix.transpose() * lambda * epipolar_line2)
          .hnormalized();
}

}  // namespace

// Triangulates 2 posed views
bool Triangulate(const Matrix3x4d& pose1,
                 const Matrix3x4d& pose2,
                 const Vector2d& point1,
                 const Vector2d& point2,
                 Vector4d* triangulated_point) {
  Eigen::Matrix3d ematrix;
  EssentialMatrixFromTwoProjectionMatrices(pose1, pose2, &ematrix);

  Vector2d corrected_point1, corrected_point2;
  FindOptimalImagePoints(
      ematrix, point1, point2, &corrected_point1, &corrected_point2);

  // Now the two points are guaranteed to intersect. We can use the DLT method
  // since it is easy to construct.
  return TriangulateDLT(
      pose1, pose2, corrected_point1, corrected_point2, triangulated_point);
}

// Triangulates a 3D point by determining the closest point between the two
// rays. This method is known to be suboptimal in terms of reprojection error
// but it is extremely fast.
bool TriangulateMidpoint(const std::vector<Vector3d>& ray_origin,
                         const std::vector<Vector3d>& ray_direction,
                         Eigen::Vector4d* triangulated_point) {
  assert(triangulated_point != nullptr);
  assert(ray_origin.size() >= 2);
  assert(ray_origin.size() == ray_direction.size());

  Eigen::Matrix4d A;
  A.setZero();
  Eigen::Vector4d b;
  b.setZero();
  for (int i = 0; i < ray_origin.size(); i++) {
    const Eigen::Vector4d ray_direction_homog(
        ray_direction[i].x(), ray_direction[i].y(), ray_direction[i].z(), 0);
    const Eigen::Matrix4d A_term =
        Eigen::Matrix4d::Identity() -
        ray_direction_homog * ray_direction_homog.transpose();
    A += A_term;
    b += A_term * ray_origin[i].homogeneous();
  }

  Eigen::LLT<Eigen::Matrix4d> linear_solver(A);
  if (linear_solver.info() != Eigen::Success) {
    return false;
  }
  *triangulated_point = linear_solver.solve(b);
  return linear_solver.info() == Eigen::Success;
}

// Triangulates 2 posed views
bool TriangulateDLT(const Matrix3x4d& pose1,
                    const Matrix3x4d& pose2,
                    const Vector2d& point1,
                    const Vector2d& point2,
                    Vector4d* triangulated_point) {
  Matrix4d design_matrix;
  design_matrix.row(0) = point1[0] * pose1.row(2) - pose1.row(0);
  design_matrix.row(1) = point1[1] * pose1.row(2) - pose1.row(1);
  design_matrix.row(2) = point2[0] * pose2.row(2) - pose2.row(0);
  design_matrix.row(3) = point2[1] * pose2.row(2) - pose2.row(1);

  // Extract nullspace.
  *triangulated_point =
      design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
  return true;
}

// Triangulates N views by computing SVD that minimizes the error.
bool TriangulateNViewSVD(const std::vector<Matrix3x4d>& poses,
                         const std::vector<Vector2d>& points,
                         Vector4d* triangulated_point) {
  assert(poses.size() == points.size());

  MatrixXd design_matrix(3 * points.size(), 4 + points.size());

  for (int i = 0; i < points.size(); i++) {
    design_matrix.block<3, 4>(3 * i, 0) = -poses[i].matrix();
    design_matrix.block<3, 1>(3 * i, 4 + i) = points[i].homogeneous();
  }

  *triangulated_point = design_matrix.jacobiSvd(Eigen::ComputeFullV)
                            .matrixV()
                            .rightCols<1>()
                            .head(4);
  return true;
}

bool TriangulateNView(const std::vector<Matrix3x4d>& poses,
                      const std::vector<Vector2d>& points,
                      Vector4d* triangulated_point) {
  assert(poses.size() == points.size());

  Matrix4d design_matrix = Matrix4d::Zero();
  for (int i = 0; i < points.size(); i++) {
    const Vector3d norm_point = points[i].homogeneous().normalized();
    const Eigen::Matrix<double, 3, 4> cost_term =
        poses[i].matrix() -
        norm_point * norm_point.transpose() * poses[i].matrix();
    design_matrix = design_matrix + cost_term.transpose() * cost_term;
  }

  Eigen::SelfAdjointEigenSolver<Matrix4d> eigen_solver(design_matrix);
  *triangulated_point = eigen_solver.eigenvectors().col(0);
  return eigen_solver.info() == Eigen::Success;
}

bool IsTriangulatedPointInFrontOfCameras(
    const FeatureCorrespondence& correspondence,
    const Matrix3d& rotation,
    const Vector3d& position) {
  const Vector3d dir1 = correspondence.feature1.homogeneous();
  const Vector3d dir2 =
      rotation.transpose() * correspondence.feature2.homogeneous();

  const double dir1_sq = dir1.squaredNorm();
  const double dir2_sq = dir2.squaredNorm();
  const double dir1_dir2 = dir1.dot(dir2);
  const double dir1_pos = dir1.dot(position);
  const double dir2_pos = dir2.dot(position);

  return (dir2_sq * dir1_pos - dir1_dir2 * dir2_pos > 0 &&
          dir1_dir2 * dir1_pos - dir1_sq * dir2_pos > 0);
}

// Returns true if the triangulation angle between any two observations is
// sufficient.
bool SufficientTriangulationAngle(
    const std::vector<Eigen::Vector3d>& ray_directions,
    const double min_triangulation_angle_degrees) {
  // Test that the angle between the rays is sufficient.
  const double cos_of_min_angle =
      cos(DegToRad(min_triangulation_angle_degrees));
  for (int i = 0; i < ray_directions.size(); i++) {
    for (int j = i + 1; j < ray_directions.size(); j++) {
      if (ray_directions[i].dot(ray_directions[j]) < cos_of_min_angle) {
        return true;
      }
    }
  }
  return false;
}

}  // namespace theia


// faster alternative to ExpPlusPPTrajectory.eval() in Matlab
// Michael Kaess, June 2013

#include <mex.h>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>
#include <iostream>

using namespace Eigen;
using namespace std;

// Initialization:
//   obj = ExpPlusPPTrajectoryEvalmex(breaks,K,A,alpha,gamma)
// Evaluation:
//   [y,jj] = ExpPlusPPTrajectoryEvalmex(obj, t)
//
// t:      m      vector of evaluation points
//
// breaks: n+1
// K:      d x a
// A:      a x a
// alpha:  a x n
// gamma:  dxn x p - reshape!
//
// y:      p x m
// jj:     m

class Eval {

private:

  const VectorXd m_breaks;
  const MatrixXd m_K, m_A, m_alpha, m_gamma;
  const int m_n, m_d, m_p;

public:

  MatrixXd expm(const MatrixXd& A) {
    MatrixXd F;
    MatrixExponential<MatrixXd>(A).compute(F);
    return F;
  }

  Eval(const VectorXd& breaks, const MatrixXd& K, const MatrixXd& A, const MatrixXd& alpha, const MatrixXd& gamma) : m_breaks(breaks), m_K(K), m_A(A), m_alpha(alpha), m_gamma(gamma), m_n(alpha.cols()), m_d(K.rows()), m_p(gamma.cols()) {}

  int dim() {
    return m_d;
  }

  VectorXd term(int j, double trel) {
    VectorXd trels(m_p);
    trels(0) = (trel<0)?-1.:1.;
    for (int i=1; i<m_p; i++) {
      trels(i) = trels(i-1) * trel;
    }
    const MatrixXd gammaj = m_gamma.block(j*m_d,0,m_d,m_p);
    return m_K*expm(m_A*trel)*m_alpha.col(j) + gammaj*trels;
  }

  void compute(const VectorXd& t, Map<MatrixXd>& y, Map<MatrixXd>& jj) {
    int m = t.rows();
    for(int k=0; k<m; k++) {
      double tk = t(k);
      // find the right interval
      int j = 0;
      for (int b=1; b<m_n; b++) { // todo: binary search
        if (tk >= m_breaks(b)) j=b;
      }
      double trel = tk - m_breaks(j);
      y.col(k) = term(j,trel);
      jj(k) = j+1; // convert to Matlab convention
    }
  }

};

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  if (nrhs == 5 && nlhs == 1) {

    // create object
    Map<VectorXd> breaks(mxGetPr(prhs[0]), mxGetNumberOfElements(prhs[0]));
    Map<MatrixXd>      K(mxGetPr(prhs[1]), mxGetM(prhs[1]), mxGetN(prhs[1]));
    Map<MatrixXd>      A(mxGetPr(prhs[2]), mxGetM(prhs[2]), mxGetN(prhs[2]));
    Map<MatrixXd>  alpha(mxGetPr(prhs[3]), mxGetM(prhs[3]), mxGetN(prhs[3]));
    Map<MatrixXd>  gamma(mxGetPr(prhs[4]), mxGetM(prhs[4]), mxGetN(prhs[4]));

    Eval *eval = new Eval(breaks, K, A, alpha, gamma);
    mxClassID cid;
    if (sizeof(eval)==4) cid = mxUINT32_CLASS;
    else if (sizeof(eval)==8) cid = mxUINT64_CLASS;
    else mexErrMsgIdAndTxt("Drake:ExpPlusPPTmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
    plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
    memcpy(mxGetData(plhs[0]),&eval,sizeof(eval));

    //    mexPrintf("constructor\n"); mexCallMATLAB(0,NULL,0,NULL,"drawnow");

  } else {

    // retrieve object
    Eval *eval = NULL;
    if (nrhs==0 || !mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
      mexErrMsgIdAndTxt("Drake:ExpPlusPPTmex:BadInputs","the first argument should be the mex_ptr");
    memcpy(&eval,mxGetData(prhs[0]),sizeof(eval));

    if (nrhs == 1) {
      //      mexPrintf("delete\n"); mexCallMATLAB(0,NULL,0,NULL,"drawnow");

      // delete object
      if (eval)
	delete eval;

    } else {

      //      mexPrintf("eval\n"); mexCallMATLAB(0,NULL,0,NULL,"drawnow");

      // eval() function call
      if (nrhs != 2 || nlhs != 2) {
        mexErrMsgIdAndTxt("Drake:ExpPlusPPTmex:WrongNumberOfInputs","Usage obj = ExpPlusPPTmex(breaks,K,A,alpha,gamma) or [y,jj] = ExpPlusPPTmex(obj,t)");
      }

      Map<VectorXd> t(mxGetPr(prhs[1]), mxGetNumberOfElements(prhs[1]));

      int m = t.rows();
      int d = eval->dim();

      plhs[0] = mxCreateDoubleMatrix(d,m,mxREAL);
      Map<MatrixXd> y(mxGetPr(plhs[0]),d,m);
      plhs[1] = mxCreateDoubleMatrix(m,1,mxREAL);
      Map<MatrixXd> jj(mxGetPr(plhs[1]),m,1);

      eval->compute(t, y, jj);

    }

  }

}


//
// Created by Yohsuke Murase on 2020/06/04.
//

#include <iostream>
#include <ostream>
#include <vector>
#include <array>
#include <random>
#include <cassert>
#include <mpi.h>
#include <Eigen/Dense>
#include <fstream>
#include "StrategyN3M5.hpp"

// calculate the distribution of fixation probability rho
// against randomly selected N2M3 deterministic strategies

std::array<double,3> CalcPayoffs(const std::array<double,StrategyN3M5::N>& stationary_state, double benefit) {
  const double cost = 1.0;
  std::array<double,3> ans = {0.0, 0.0, 0.0};
  for (size_t i = 0; i < StrategyN3M5::N; i++) {
    StateN3M5 s(i);
    double pa = 0.0, pb = 0.0, pc = 0.0;
    if (s.ha[0] == false) { pa -= cost; pb += benefit / 2.0; pc += benefit / 2.0; }
    if (s.hb[0] == false) { pb -= cost; pa += benefit / 2.0; pc += benefit / 2.0; }
    if (s.hc[0] == false) { pc -= cost; pa += benefit / 2.0; pb += benefit / 2.0; }
    ans[0] += stationary_state[i] * pa;
    ans[1] += stationary_state[i] * pb;
    ans[2] += stationary_state[i] * pc;
  }
  return ans;
}

double FixationProb(size_t N, double sigma, double e, double benefit, const StrategyN3M5 &res, const StrategyN3M5 &mut, double s_yyy) {
  auto a_xxx = mut.StationaryState(e);
  auto a_xxy = mut.StationaryState(e, &mut, &res);
  auto a_xyy = mut.StationaryState(e, &res, &res);

  double s_xxx = CalcPayoffs(a_xxx, benefit)[0];
  auto _xxy = CalcPayoffs(a_xxy, benefit);
  double s_xxy = _xxy[0];
  double s_yxx = _xxy[2];
  auto _xyy = CalcPayoffs(a_xyy, benefit);
  double s_xyy = _xyy[0];
  double s_yyx = _xyy[1];

  // rho_inv = \sum_{i=0}^{N-1} exp(sigma[S]),
  // where S is defined as
  // S =  i/6(i^2−3iN+6i+3N^2−12N+11)s_{yyy}
  //     −i/6(i+1)(2i−3N+4)s_{yyx}
  //     +i/6(i−1)(i+1)s_{yxx}
  //     −i/6(i^2−3i(N−1)+3N^2−6N+2)s_{xyy}
  //     +i/6(i−1)(2i−3N+2))s_{xxy}
  //     −i/6(i^2−3i+2)s_{xxx}

  double num_games = (N-1) * (N-2) / 2.0;
  double rho_inv = 0.0;
  for (int i=0; i < N; i++) {
    double x = sigma / num_games * (i / 6.0) * (
        (i*i - 3.0*i*N + 6.0*i + 3.0*N*N - 12.0*N + 11.0) * s_yyy
            - (i+1.0) * (2.0*i - 3.0*N + 4) * s_yyx
            + (i-1.0) * (i+1.0) * s_yxx
            - (i*i - 3.0*i*(N-1.0) + 3.0*N*N - 6.0*N + 2.0) * s_xyy
            + (i-1.0) * (2.0*i-3.0*N+2.0) * s_xxy
            - (i*i - 3.0*i + 2.0) * s_xxx
    );
    rho_inv += std::exp(x);
  }

  return 1.0 / rho_inv;
}

std::bitset<StrategyN3M5::N> DrawRandomBit32768(std::mt19937_64 & rnd) {
  std::uniform_int_distribution<uint64_t > dist(0, std::numeric_limits<uint64_t>::max() );

  std::bitset<StrategyN3M5::N> ans;
  int rep = StrategyN3M5::N / 64;
  assert(StrategyN3M5::N % 64 == 0);
  for (int i = 0; i < rep; i++) {
    std::bitset<StrategyN3M5::N> mask = dist(rnd);
    mask <<= (64 * i);
    ans |= mask;
  }
  return ans;
}

int main(int argc, char *argv[]) {
  MPI_Init(&argc, &argv);
  Eigen::initParallel();
  if( argc != 8 ) {
    std::cerr << "Error : invalid argument" << std::endl;
    std::cerr << "  Usage : " << argv[0] << " <N> <sigma> <e> <benefit> <resident 0:caprin, +:num_resident_samples> <num_mutants> <seed>" << std::endl;
    MPI_Finalize();
    return 1;
  }

  size_t N = std::strtoul(argv[1], nullptr,0);
  double sigma = std::strtod(argv[2], nullptr);
  double e = std::strtod(argv[3], nullptr);
  double benefit = std::strtod(argv[4], nullptr);

  long n_resident = std::strtol(argv[5], nullptr, 0);
  long n_mutants = std::strtol(argv[6], nullptr, 0);
  int seed = std::strtol(argv[7], nullptr, 0);

  const size_t NUM_BINS = 1000;
  std::vector<size_t> counts(NUM_BINS, 0ul);
  size_t robust_count = 0ul;

  int my_rank = 0;
  MPI_Comm_rank(MPI_COMM_WORLD, &my_rank);
  int num_procs = 0;
  MPI_Comm_size(MPI_COMM_WORLD, &num_procs);

  if (n_resident > 0) {
    {
      std::seed_seq seq = {seed, my_rank};
      std::mt19937_64 rnd(seq);
      size_t my_n_resident = n_resident / num_procs;
      if (my_rank < n_resident % num_procs) { my_n_resident++; }
      // std::cerr << "my_n_resident: " << my_n_resident << " " << my_rank << ' ' << th << std::endl;

      for (size_t i = 0; i < my_n_resident; i++) {
        std::cerr << "i: " << i << " " << my_rank << std::endl;
        StrategyN3M5 res( DrawRandomBit32768(rnd) );
        auto a_yyy = res.StationaryState(e);
        double s_yyy = CalcPayoffs(a_yyy, benefit)[0];
        for (size_t j = 0; j < n_mutants; j++) {
          std::cerr << "j: " << j << " " << my_rank << std::endl;
          StrategyN3M5 mut( DrawRandomBit32768(rnd) );
          double rho = FixationProb(N, sigma, e, benefit, res, mut, s_yyy);
          std::cerr << "  rho: " << rho << std::endl;
          size_t b = static_cast<size_t>(rho * NUM_BINS);
          counts[b]++;
          if (rho <= 1.0 / N) { robust_count++; }
        }
      }
    }
  }
  else {
    {
      std::seed_seq seq = {seed, my_rank};
      std::mt19937_64 rnd(seq);

      StrategyN3M5 res = StrategyN3M5::CAPRI3();
      auto a_yyy = res.StationaryState(e);
      double s_yyy = CalcPayoffs(a_yyy, benefit)[0];

      size_t my_n_mutants = n_mutants / num_procs;
      if (my_rank < n_mutants % num_procs) { my_n_mutants++; }
      //std::cerr << "my_n_mutants: " << my_n_mutants << ' ' << my_rank << ' ' << th << std::endl;

      for (size_t j = 0; j < my_n_mutants; j++) {
        StrategyN3M5 mut( DrawRandomBit32768(rnd) );
        double rho = FixationProb(N, sigma, e, benefit, res, mut, s_yyy);
        size_t b = static_cast<size_t>(rho * NUM_BINS);
        counts[b] += 1;
        if (rho <= 1.0 / N) { robust_count++; }
      }
    }
  }

  // reduce counts
  std::vector<size_t> all_counts(NUM_BINS, 0ul);
  size_t all_robust_count = 0ul;
  MPI_Reduce(counts.data(), all_counts.data(), all_counts.size(), MPI_UNSIGNED_LONG, MPI_SUM, 0, MPI_COMM_WORLD);
  MPI_Reduce(&robust_count, &all_robust_count, 1, MPI_UNSIGNED_LONG, MPI_SUM, 0, MPI_COMM_WORLD);

  // print counts
  if (my_rank == 0) {
    std::ofstream fout("dist.dat");
    double dx = 1.0 / NUM_BINS;
    double total = (n_resident == 0) ? n_mutants : n_resident * n_mutants;
    for (size_t i = 0; i < NUM_BINS; i++) {
      fout << i * dx << ' ' << (double)all_counts[i]/total << std::endl;
    }

    std::cerr << "robust_count/total: " << all_robust_count << " / " << total << " : " << (double)all_robust_count/total << std::endl;
  }

  MPI_Finalize();
  return 0;
}


//
// Created by xiang on 1/4/18.
// this program shows how to perform direct bundle adjustment
//
#include <iostream>

using namespace std;

#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

#include <Eigen/Core>
#include <sophus/se3.hpp>
#include <opencv2/opencv.hpp>

#include <pangolin/pangolin.h>
#include <boost/format.hpp>

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> VecSE3;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVec3d;

// global variables
string pose_file = "../poses.txt";
string points_file = "../points.txt";

// intrinsics
float fx = 277.34;
float fy = 291.402;
float cx = 312.234;
float cy = 239.777;

// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
            (1 - xx) * (1 - yy) * data[0] +
            xx * (1 - yy) * data[1] +
            (1 - xx) * yy * data[img.step] +
            xx * yy * data[img.step + 1]
    );
}

// g2o vertex that use sophus::SE3 as pose
class VertexSophus : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    VertexSophus() {}

    ~VertexSophus() {}

    bool read(std::istream &is) {}

    bool write(std::ostream &os) const {}

    virtual void setToOriginImpl() {
        _estimate = Sophus::SE3d();
    }

    virtual void oplusImpl(const double *update_) {
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> update(update_);
        setEstimate(Sophus::SE3d::exp(update) * estimate());
    }
};

// TODO edge of projection error, implement it
// 16x1 error, which is the errors in patch
typedef Eigen::Matrix<double,16,1> Vector16d;
class EdgeDirectProjection : public g2o::BaseBinaryEdge<16, Vector16d, g2o::VertexSBAPointXYZ, VertexSophus> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeDirectProjection(float *color, cv::Mat &target) {
        this->origColor = color;
        this->targetImg = target;
    }

    ~EdgeDirectProjection() {}

    virtual void computeError() override {
        // TODO START YOUR CODE HERE
        // compute projection error ...
        const g2o::VertexSBAPointXYZ *vertexPw = static_cast<const g2o::VertexSBAPointXYZ * >(vertex(0));
        const VertexSophus *vertexTcw = static_cast<const VertexSophus * >(vertex(1));
        Eigen::Vector3d p_nsp = vertexTcw->estimate() * vertexPw->estimate();
        p_nsp /= p_nsp[2];
        double u = p_nsp[0]*fx+cx;
        double v = p_nsp[1]*fy+cy;
        if (u-2>=0 && v-2>=0 &&u+1<=targetImg.cols-1 && v+1<=targetImg.rows-1){
            int index=0;
            for (int du=-2;du<=1;du++){
                for (int dv=-2;dv<=1;dv++)
                {
                    _error[index] = origColor[index]-GetPixelValue(targetImg, u+du, v+dv);
                    index++;
                }
            }
        }
        else{
            this->setLevel(1);
            for(int i=0;i<16;i++) _error[i]=0;
        }
        // END YOUR CODE HERE
    }

    // Let g2o compute jacobian for you

    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}

private:
    cv::Mat targetImg;  // the target image
    float *origColor = nullptr;   // 16 floats, the color of this point
};

// plot the poses and points for you, need pangolin
void Draw(const VecSE3 &poses, const VecVec3d &points);

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

    // read poses and points
    VecSE3 poses;
    VecVec3d points;
    ifstream fin(pose_file);

    while (!fin.eof()) {
        double timestamp = 0;
        fin >> timestamp;
        if (timestamp == 0) break;
        double data[7];
        for (auto &d: data) fin >> d;
        poses.push_back(Sophus::SE3d(
                Eigen::Quaterniond(data[6], data[3], data[4], data[5]),
                Eigen::Vector3d(data[0], data[1], data[2])
        ));
        if (!fin.good()) break;
    }
    fin.close();

    vector<float *> color;
    fin.open(points_file);
    while (!fin.eof()) {
        double xyz[3] = {0};
        for (int i = 0; i < 3; i++) fin >> xyz[i];
        if (xyz[0] == 0) break;
        points.push_back(Eigen::Vector3d(xyz[0], xyz[1], xyz[2]));
        float *c = new float[16];
        for (int i = 0; i < 16; i++) fin >> c[i];
        color.push_back(c);

        if (fin.good() == false) break;
    }
    fin.close();

    cout << "poses: " << poses.size() << ", points: " << points.size() << endl;

    // read images
    vector<cv::Mat> images;
    boost::format fmt("../%d.png");
    for (int i = 0; i < 7; i++) {
        images.push_back(cv::imread((fmt % i).str(), 0));
    }

    // build optimization problem
    // typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> DirectBlock;  // 求解的向量是6＊1的
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // 求解的向量是6＊1的
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    // TODO add vertices, edges into the graph optimizer
    // START YOUR CODE HERE
    vector<VertexSophus *> vertex_poses;
    for (int i=0; i<poses.size(); i++)
    {
        VertexSophus *v = new VertexSophus();
        v->setId(i);
        v->setEstimate(poses[i]);
        optimizer.addVertex(v);
        vertex_poses.push_back(v);
    }

    vector<g2o::VertexSBAPointXYZ *> vertex_points;
    for (int i=0;i<points.size();i++){
        g2o::VertexSBAPointXYZ *v = new g2o::VertexSBAPointXYZ();
        v->setId(i+poses.size());
        v->setEstimate(points[i]);

        v->setMarginalized(true);
        optimizer.addVertex(v);
        vertex_points.push_back(v);
    }

    for(int i=0;i<vertex_poses.size();i++){
        for (int j=0;j<vertex_points.size();j++)
        {
            EdgeDirectProjection *edge = new EdgeDirectProjection(color[j], images[i]);
            edge->setVertex(0, vertex_points[j]);
            edge->setVertex(1, vertex_poses[i]);
            edge->setInformation(Eigen::Matrix<double, 16, 16>::Identity());
            edge->setRobustKernel(new g2o::RobustKernelHuber());
            optimizer.addEdge(edge);
        }
    }

    // END YOUR CODE HERE

    // perform optimization
    optimizer.initializeOptimization(0);
    optimizer.optimize(200);

    // TODO fetch data from the optimizer
    // START YOUR CODE HERE
    for (int i=0;i<poses.size();i++){
        poses[i] = vertex_poses[i]->estimate();
    }
    for (int i=0;i<points.size();i++){
        points[i] = vertex_points[i]->estimate();
    }
    // END YOUR CODE HERE

    // plot the optimized points and poses
    Draw(poses, points);

    // delete color data
    for (auto &c: color) delete[] c;
    return 0;
}

void Draw(const VecSE3 &poses, const VecVec3d &points) {
    if (poses.empty() || points.empty()) {
        cerr << "parameter is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(0.0f, 0.0f, 0.0f, 0.0f);

        // draw poses
        float sz = 0.1;
        int width = 640, height = 480;
        for (auto &Tcw: poses) {
            glPushMatrix();
            Sophus::Matrix4f m = Tcw.inverse().matrix().cast<float>();
            glMultMatrixf((GLfloat *) m.data());
            glColor3f(1, 0, 0);
            glLineWidth(2);
            glBegin(GL_LINES);
            glVertex3f(0, 0, 0);
            glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
            glVertex3f(0, 0, 0);
            glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
            glVertex3f(0, 0, 0);
            glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
            glVertex3f(0, 0, 0);
            glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
            glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
            glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
            glVertex3f(sz * (width - 1 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
            glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
            glVertex3f(sz * (0 - cx) / fx, sz * (height - 1 - cy) / fy, sz);
            glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
            glVertex3f(sz * (0 - cx) / fx, sz * (0 - cy) / fy, sz);
            glVertex3f(sz * (width - 1 - cx) / fx, sz * (0 - cy) / fy, sz);
            glEnd();
            glPopMatrix();
        }

        // points
        glPointSize(2);
        glBegin(GL_POINTS);
        for (size_t i = 0; i < points.size(); i++) {
            glColor3f(0.0, points[i][2]/4, 1.0-points[i][2]/4);
            glVertex3d(points[i][0], points[i][1], points[i][2]);
        }
        glEnd();

        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
}


#pragma once

#include <vector>
#include <type_traits>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>

#include "Bounds.hh"
#include "LineSegment/linesegment.hh"
#include "LineSegment/LineSegment2/linesegment2.hh"
#include "../../util/assert.hh"
#include "../../util/log.hh"
#include "../../util/Maybe.hh"

namespace bold
{
  typedef unsigned int uint;

  template<typename T>
  class Polygon2
  {
  public:
    typedef Eigen::Matrix<T, 2, 1> Point;
    typedef std::vector<Point,Eigen::aligned_allocator<Point>> PointVector;

    Polygon2(PointVector const& vertices)
    : d_vertices(vertices)
    {
      if (vertices.size() < 3)
      {
        log::error("Polygon2::Polygon2") << "Cannot create a polygon with number of vertices: " << vertices.size();
        throw std::runtime_error("A polygon must have at least three vertices");
      }
    }

    Polygon2(Bounds<T,2> bounds)
    : d_vertices()
    {
      d_vertices.push_back(bounds.min());
      d_vertices.emplace_back(Point(bounds.max().x(), bounds.min().y()));
      d_vertices.push_back(bounds.max());
      d_vertices.emplace_back(Point(bounds.min().x(), bounds.max().y()));
    }

    bool contains(Point const& point)
    {
      bool isInside = false;
      for (unsigned i = 0, j = d_vertices.size() - 1; i < d_vertices.size(); j = i++)
      {
        Point const& a = d_vertices[i];
        Point const& b = d_vertices[j];

        if (((a.y() > point.y()) != (b.y() > point.y()))
          && (point.x() < (b.x() - a.x()) * (point.y() - a.y()) / (b.y() - a.y()) + a.x()))
        {
          isInside = !isInside;
        }
      }
      return isInside;
    }

    uint vertexCount() const { return d_vertices.size(); }

    Point operator[](uint i)
    {
      return d_vertices[i];
    }

    typename PointVector::iterator begin() { return d_vertices.begin(); }
    typename PointVector::iterator end() { return d_vertices.end(); }
    typename PointVector::const_iterator begin() const { return d_vertices.begin(); }
    typename PointVector::const_iterator end() const { return d_vertices.end(); }

    /// Returns the subsection of the provided that that resides within this polygon.
    /// Assumes the poly is convex. Returns an empty result if no intersection exists.
    Maybe<LineSegment2<T>> clipLine(LineSegment2<T> const& line)
    {
      bool contains1 = contains(line.p1());
      bool contains2 = contains(line.p2());

      // Line is completely inside the polygon.
      // As we're convex, there cannot be any intersection.
      if (contains1 && contains2)
        return line;

      PointVector intersectionPoints;
      for (unsigned i = 0, j = d_vertices.size() - 1; i < d_vertices.size(); j = i++)
      {
        Point const& a = d_vertices[i];
        Point const& b = d_vertices[j];
        LineSegment2<T> l(a, b);
        auto result = l.tryIntersect(line);
        if (result.hasValue())
        {
          // Ensure unique members of the list
          if (intersectionPoints.size() > 0 && intersectionPoints[0] == result.value())
            continue;
          if (intersectionPoints.size() > 1 && intersectionPoints[1] == result.value())
            continue;

          intersectionPoints.push_back(std::move(result.value()));

          // In a convex polygon, we should never see more than two intersections
          if (intersectionPoints.size() == 2)
            break;
        }
      }

      if (intersectionPoints.size() == 0)
      {
        // Line is completely outside and does not intersect with polygon
        return Maybe<LineSegment2<T>>::empty();
      }

      if (intersectionPoints.size() == 2)
      {
        // Line is completely outside and intersects with polygon
        return LineSegment2<T>(intersectionPoints[0], intersectionPoints[1]);
      }

      // One end of the line is outside, and we have a single intersection
      ASSERT(intersectionPoints.size() == 1);
      if (contains1)
        return LineSegment2<T>(line.p1(), intersectionPoints[0]);
      else
        return LineSegment2<T>(intersectionPoints[0], line.p2());
    }

  private:
    PointVector d_vertices;
  };

  typedef Polygon2<int> Polygon2i;
  typedef Polygon2<float> Polygon2f;
  typedef Polygon2<double> Polygon2d;

  template<typename T>
  Maybe<Polygon2<T>> make_polygon2(typename Polygon2<T>::PointVector const& vertices)
  {
    return vertices.size() < 3
      ? Maybe<Polygon2<T>>::empty()
      : Polygon2<T>(vertices);
  }

  inline Maybe<Polygon2d> make_polygon2d(typename Polygon2d::PointVector const& vertices)
  {
    return make_polygon2<double>(vertices);
  }
}


#ifndef AMPAR_GILLESPIE_CLASS_HPP_INCLUDED
#define AMPAR_GILLESPIE_CLASS_HPP_INCLUDED

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

#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/random/linear_congruential.hpp>

#include "stl_vector_operation_functions.hpp"

class ampar_gillespie_class
{
   private:
      unsigned int seed;
      boost::minstd_rand rng;   //Boost random number generator
      boost::uniform_real<double> uni_dist;   //Uniform random number distribution which produces values b/w 0 and 1 (0 inclusive, 1 exlusive).
      boost::variate_generator<boost::minstd_rand&, boost::uniform_real< double> > uni_rand;// variate generator
      //--------------
      double kf1 = 4.59E06;    //M^-1 S^-1
      double kb1 = 4.26E03;    //S^-1
      double kf2 = 28.4E06;    //M^-1 * S^-1
      double kb2 = 3.26E03;    //S^-1
      double kf3 = 1.27E06;    //M^-1 S^-1
      double kb3 = 45.7;       //S^-1
      double a0 = 4.24E03;     //S^-1
      double b0 = 900.0;       //S^-1
      double a1 = 2.89E03;     //S^-1
      double b1 = 39.2;        //S^-1
      double a2 = 172.0;       //S^-1
      double b2 = 0.727;       //S^-1
      double a3 = 17.7;        //S^-1
      double b3 = 4.0;         //S^-1
      double a4 = 16.8;        //S^-1
      double b4 = 190.4;       //S^-1
      double Volume = 1.0;
   public:
      const double avagadro = 6.022e+23;
      const int Nstates = 8;
      const int Nreactions = 16;
      double r1,r2;
      //Construct vectors
      std::vector<int> X;      // A vector to hold the initial number of the receptors in each states [Glu,C0,C1,C2,C3,C4,C5,Open]
      std::vector<double> C;  //A vector to hold the Cmew values
      std::vector<double> H;  //A vector to hold the Hmew values
      //Class constructorsampar_gillespie_class(unsigned int seed_ = std::time(0), std::vector<int> X_):   seed(seed_),
       ampar_gillespie_class(std::vector<int> X_, unsigned int seed_=std::time(0)):   seed(seed_),
                                                                        uni_dist(boost::uniform_real<double>(0,1)),
                                                                        uni_rand(boost::variate_generator<boost::minstd_rand&, boost::uniform_real<double> >(rng, uni_dist)),
                                                                        X(std::move(X_))
      {
         rng.seed(seed);
         C = {kf1*Volume,kb1,kf2*Volume,kb2,a1,b1,kf3*Volume,kb3,a2,b2,a4,b4,a0,b0,b3,a3};
         H = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
      }
      void updateX (const int rx_id);
      void updateH (void);
};

/* Function update H_mew vector with the current number of molecules in each state from the state vector X */
void ampar_gillespie_class::updateH(void)
{
   //Genrate two random numbers from the distribution by means of STL generator interface.
   r1 = uni_rand();
   r2 = uni_rand();

   H[0] = X[0] * X[1];
   H[1] = X[2];
   H[2] = X[2] * X[0];
   H[3] = X[3];
   H[4] = X[2];
   H[5] = X[4];
   H[6] = X[4] * X[0];
   H[7] = X[5];
   H[8] = X[3];
   H[9] = X[5];
   H[10] = X[5];
   H[11] = X[6];
   H[12] = X[3];
   H[13] = X[7];
   H[14] = X[6];
   H[15] = X[7];
}
//Function update the state vector X for a given reaction rx_id
void ampar_gillespie_class::updateX(const int rx_id)
{
   //std::cout << "Reaction " << rx_id << " X in " << X;
   switch(rx_id)
   {
      case 0:
      {
         if((X[0] > 0) && (X[1] > 0)){
            --X[0]; --X[1];
            ++X[2];
         }
         break;
      }
      case 1:
      {
         if(X[2] > 0){
         --X[2];
         ++X[0]; ++X[1];
         }
         break;
      }
      case 2:
      {
         if((X[2] > 0) && (X[0] > 0)){
         --X[2]; --X[0];
         ++X[3];
         }
         break;
      }
      case 3:
      {
         if(X[3] > 0){
         --X[3];
         ++X[2]; ++X[0];
         }
         break;
      }
      case 4:
      {
         if(X[2] > 0){
         --X[2];
         ++X[4];
         }
         break;
      }
      case 5:
      {
         if(X[4] > 0){
         --X[4];
         ++X[2];
         }
         break;
      }
      case 6:
      {
         if((X[4] > 0) && (X[0] > 0)){
         --X[4];
         --X[0];
         ++X[5];
         }
         break;
      }
      case 7:
      {
         if(X[5] > 0){
         --X[5];
         ++X[4];
         ++X[0];
         }
         break;
      }
      case 8:
      {
         if(X[3] > 0){
         --X[3];
         ++X[5];
         }
         break;
      }
      case 9:
      {
         if(X[5] > 0){
         --X[5];
         ++X[3];
         }
         break;
      }
      case 10:
      {
         if(X[5] > 0){
         --X[5];
         ++X[6];
         }
         break;
      }
      case 11:
      {
         if(X[6] > 0){
         --X[6];
         ++X[5];
         }
         break;
      }
      case 12:
      {
         if(X[3] > 0){
         --X[3];
         ++X[7];
         }
         break;
      }
      case 13:
      {
         if(X[7] > 0){
         --X[7];
         ++X[3];
         }
         break;
      }
      case 14:
      {
         if(X[6] > 0){
         --X[6];
         ++X[7];
         }
         break;
      }
      case 15:
      {
         if(X[7] > 0){
         --X[7];
         ++X[6];
         }
         break;
      }
   }
   for(size_t i=0; i<X.size();++i){
      if(X[i] < 0){
         X[i] = 0;
      }
   }
}

#endif // AMPAR_GILLESPIE_CLASS_HPP_INCLUDED


#ifndef STAN_MATH_PRIM_MAT_FUN_VARIANCE_HPP
#define STAN_MATH_PRIM_MAT_FUN_VARIANCE_HPP

#include <stan/math/prim/arr/err/check_nonzero_size.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/mean.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace stan {
namespace math {

/**
 * Returns the sample variance (divide by length - 1) of the
 * coefficients in the specified standard vector.
 * @param v Specified vector.
 * @return Sample variance of vector.
 * @throws std::domain_error if the size of the vector is less
 * than 1.
 */
template <typename T>
inline return_type_t<T> variance(const std::vector<T>& v) {
  check_nonzero_size("variance", "v", v);
  if (v.size() == 1) {
    return 0.0;
  }
  T v_mean(mean(v));
  T sum_sq_diff(0);
  for (size_t i = 0; i < v.size(); ++i) {
    T diff = v[i] - v_mean;
    sum_sq_diff += diff * diff;
  }
  return sum_sq_diff / (v.size() - 1);
}

/**
 * Returns the sample variance (divide by length - 1) of the
 * coefficients in the specified column vector.
 * @param m Specified vector.
 * @return Sample variance of vector.
 */
template <typename T, int R, int C>
inline return_type_t<T> variance(const Eigen::Matrix<T, R, C>& m) {
  check_nonzero_size("variance", "m", m);

  if (m.size() == 1) {
    return 0.0;
  }
  return_type_t<T> mn(mean(m));
  return_type_t<T> sum_sq_diff(0);
  for (int i = 0; i < m.size(); ++i) {
    return_type_t<T> diff = m(i) - mn;
    sum_sq_diff += diff * diff;
  }
  return sum_sq_diff / (m.size() - 1);
}

}  // namespace math
}  // namespace stan
#endif


#pragma once
#include <vector>
#include <random>
#include <Eigen/Core>

namespace mdk {
    /**
     * A random number generator. We use two versions: a legacy version taken
     * from Fortran, and a modern version. Aside from sampling from [0, 1], we
     * add sampling from [a, b], N(0, 1), N(mu, sigma^2) or points on S^2.
     */
    class Random {
#ifdef LEGACY_MODE
    private:
        static constexpr int
            im1 = 2147483563,
            im2 = 2147483399,
            imm1 = im1-1,
            ia1 = 40014,
            ia2 = 40692,
            iq1 = 53668,
            iq2 = 52774,
            ir1 = 12211,
            ir2 = 3791,
            ntab = 32,
            ndiv = 1+imm1/ntab;

        static constexpr double
            eps = 1.2e-7,
            rnmx = 1.-eps,
            am = (float) 1.0/im1;

        int iy = 0, idum = -448, idum2 = 123456789;
        int iv[ntab];
    public:
        Random(int seed) {
            idum = -seed;
            for (auto& x: iv) x = 0;
        }
        inline double uniform() {
            int k, j;
            if (idum <= 0) {
                idum2 = idum = std::max(-idum, 1);
                for (j = ntab + 7; j >= 0; --j) {
                    k = idum / iq1;
                    idum = ia1 * (idum - k * iq1) - k * ir1;
                    if (idum < 0) idum += im1;
                    if (j < ntab) iv[j] = idum;
                }
                iy = iv[0];
            }

            k = idum / iq1;
            idum = ia1 * (idum - k * iq1) - k * ir1;
            if (idum < 0) idum += im1;

            k = idum2 / iq2;
            idum2 = ia2 * (idum2 - k * iq2) - k * ir2;
            if (idum2 < 0) idum2 += im2;

            j = iy / ndiv;
            iy = iv[j] - idum2;
            iv[j] = idum;
            if (iy < 1) iy += imm1;

            return std::min(am * iy, rnmx);
        }
#else
    private:
        uint64_t state;
        Random() = default;

    public:
        Random(int seed) {
            assert (seed > 0);
            state = seed;

            // Shuffle the state a bit
            for (int i = 0; i < 100; ++i) uniform();
        }

        inline double uniform() {
            static const double inv = 1.0 / (double) (1ull << 32);
            uint64_t result = state * 0xd989bcacc137dcd5ull;
            state ^= state >> 11;
            state ^= state << 31;
            state ^= state >> 18;
            double res = (uint32_t) (result >> 32ull);
            return res * inv;
        }

        /* This function is used to create a new random object
           that won't generate similar values.
           Uses splitmix64 to create new state */
        Random getNewRandom() {
            uint64_t result = state;
            result = (result ^ (result >> 30)) * 0xBF58476D1CE4E5B9;
            result = (result ^ (result >> 27)) * 0x94D049BB133111EB;
            result = result ^ (result >> 31);
            Random rng;
            rng.state = result;
            return rng;
        }

#endif
        Random(Random const& oth) = default;

        inline double uniform(double a, double b) {
            return a + (b - a) * uniform();
        }

        inline double normal() {
            double r1 = uniform();
            double r2 = uniform();
            return sqrt(-2.0  * log(r1)) * cos(2.0 * M_PI * r2);
        }

        inline std::pair<double, double> two_normals() {
            double r1 = uniform();
            double r2 = uniform();
            double r = sqrt(-2.0  * log(r1));
            double ang = 2.0 * M_PI * r2;
            return {r * cos(ang), r * sin(ang)};
        }

        inline double normal(double mu, double sigma) {
            return mu + sigma * normal();
        }

        inline Eigen::Vector3d sphere() {
            Eigen::Vector3d r { normal(), normal(), normal() };
            return r.normalized();
        }
    };
}


#include <iostream>
#include <time.h>
#include <vector>
#include <typeinfo>
#include <math.h>
#include <algorithm>
#include <numeric>
#include <fstream>
#include <string>
#include <vector>
#include <sstream>
#include <iterator>
//#include <boost/archive/binary_iarchive.hpp>
//#include <boost/archive/binary_oarchive.hpp>
//#include <boost/serialization/vector.hpp>
//#include <pybind11/pybind11.h>
//#include <Python.h>
//#include <pybind11/embed.h>

using namespace std;

class ANG
{

	vector<vector<float>> activations;
	vector<vector<vector<float>>> derivatives;
	vector<vector<vector<float>>> weights;
	vector<vector<vector<float>>> seed_weights, seed_derivatives;
	vector<vector<float>> seed_activations;
	/*
private:
	friend class boost::serialization::access;
	template<class Archive>
	void serialize(Archive& ar, const unsigned int version) {
		ar& weights;
		ar& derivatives;
		ar& activations;
		ar& seed_weights;
		ar& seed_derivatives;
		ar& seed_activations;
	}
	*/

public:
	//Default Constructor
	ANG(){
		activations = {};
		derivatives = {};
		weights = {};
		seed_weights = {};
		seed_derivatives = {};
		seed_activations = {};
	}
	//Parameterized Constructor
	ANG(vector<int> hidden_layers, int num_outputs = 1, int num_inputs = 10) {
		vector<int> layers;
		layers.push_back(num_inputs);
		for (int i = 0; i < hidden_layers.size(); i++) layers.push_back(hidden_layers[i]);
		layers.push_back(num_outputs);
		//for (int lay = 0; lay < layers.size(); lay++) cout << layers[lay] << endl;
		vector<vector<float>> w;
		for (int i = 0; i < layers.size() - 1; i++) {
			int row_len, col_len;
			row_len = layers[i];
			col_len = layers[i + 1];
			//srand(time(0));
			vector < vector <float>> Matrix(row_len, vector<float>(col_len, 0));
			for (auto it_row = Matrix.begin(); it_row != Matrix.end(); it_row++)
			{
				// Getting each (i,j) element and assigning random value to it
				for (auto it_col = it_row->begin(); it_col != it_row->end(); it_col++)
				{
					*it_col = (float)rand() / RAND_MAX;
					//cout << *it_col;
				}
			}
			w = Matrix;
			weights.push_back(w);
		}
		//for (int i = 0; i < w.size(); i++) {
			//for (int j = 0; j < w[i].size(); j++)
				//cout << w[i][j] << " ";
			//cout << endl;
		//}

		vector<vector<float>> d;
		for (int i = 0; i < layers.size() - 1; i++) {
			int row_len, col_len;
			row_len = layers[i];
			col_len = layers[i + 1];
			vector < vector <float>> Matrix(row_len, vector<float>(col_len, 0.0));
			d = Matrix;
			derivatives.push_back(d);
		}
		//for (int i = 0; i < w.size(); i++) {
			//for (int j = 0; j < w[i].size(); j++)
				//cout << w[i][j] << " ";
			//cout << endl;
		//}

		vector<float> a;
		for (int i = 0; i < layers.size(); i++) {
			int row_len, col_len;
			row_len = layers[i];
			vector<float> Matrix(row_len, 0);
			a = Matrix;
			activations.push_back(a);
		}
		//for (int i = 0; i < w.size(); i++) {
			//for (int j = 0; j < w[i].size(); j++)
				//cout << w[i][j] << " ";
			//cout << endl;
		//}
		//cout << weights.size() << " ";
		//for (int i = 0; i < weights.size(); i++) {
			//for (int j = 0; j < weights[i].size(); j++) {
			//	for (int k = 0; k < weights[i][j].size(); k++)
			//		cout << weights[i][j][k] << " ";
			//	cout << endl;
			//}
		//}
	}
	//friend class boost::serialization::access;
	vector<int> sorted_index(int size, vector<float> b) {
		vector<int> indices(size);
		std::iota(indices.begin(), indices.end(), 0);
		sort(indices.begin(), indices.end(), [&](int A, int B) -> bool {return b[A] < b[B];});
		return indices;
	}

	void Print3D(vector<vector<vector<float>>> a) {
		for (int i = 0; i < a.size(); i++) {
			for (int j = 0; j < a[i].size(); j++) {
				for (int k = 0; k < a[i][j].size(); k++) {
					cout << a[i][j][k] << " ";
				}
				cout << endl;
			}
			cout << "#" << endl;
		}
	}
	float stdev(vector<float> data) {
		int size = data.size();
		float mean, sd = 0.0, sum = 0.0;
		for (int i = 0; i < size; i++) sum += data[i];
		mean = (float)sum / size;
		for (int i = 0; i < size; i++) sd += pow(data[i] - mean, 2);
		return sqrt(sd / size);

	}
	void PrintMatrix(vector<vector<float>> a) {
		for (int i = 0; i < a.size(); i++) {

			for (int j = 0; j < a[i].size(); j++) cout << a[i][j] << " ";
			cout << endl;
		}
	}

	void PrintVector(vector<float> a) {
		for (int i = 0; i < a.size(); i++) cout << a[i] << " ";
	}

	vector<vector<float>> transpose(vector<vector<float>> b)
	{
		if (b.size() == 0)
			return { {} };
		vector<vector<float>> trans_vec(b[0].size(), vector<float>());
		for (int i = 0; i < b.size(); i++)
		{
			for (int j = 0; j < b[i].size(); j++)
			{
				trans_vec[j].push_back(b[i][j]);
			}
		}
		return trans_vec;
	}
	//init not in use anymore
	void Init(vector<int> hidden_layers, int num_outputs = 1, int num_inputs = 10)
	{
		vector<int> layers;
		layers.push_back(num_inputs);
		for (int i = 0; i < hidden_layers.size(); i++) layers.push_back(hidden_layers[i]);
		layers.push_back(num_outputs);
		//for (int lay = 0; lay < layers.size(); lay++) cout << layers[lay] << endl;
		vector<vector<float>> w;
		for (int i = 0; i < layers.size() - 1; i++) {
			int row_len, col_len;
			row_len = layers[i];
			col_len = layers[i + 1];
			//srand(time(0));
			vector < vector <float>> Matrix(row_len, vector<float>(col_len, 0));
			for (auto it_row = Matrix.begin(); it_row != Matrix.end(); it_row++)
			{
				// Getting each (i,j) element and assigning random value to it
				for (auto it_col = it_row->begin(); it_col != it_row->end(); it_col++)
				{
					*it_col = (float)rand()/RAND_MAX;
					//cout << *it_col;
				}
			}
			w = Matrix;
			weights.push_back(w);
		}
		//for (int i = 0; i < w.size(); i++) {
			//for (int j = 0; j < w[i].size(); j++)
				//cout << w[i][j] << " ";
			//cout << endl;
		//}

		vector<vector<float>> d;
		for (int i = 0; i < layers.size() - 1; i++) {
			int row_len, col_len;
			row_len = layers[i];
			col_len = layers[i + 1];
			vector < vector <float>> Matrix(row_len, vector<float>(col_len, 0.0));
			d = Matrix;
			derivatives.push_back(d);
		}
		//for (int i = 0; i < w.size(); i++) {
			//for (int j = 0; j < w[i].size(); j++)
				//cout << w[i][j] << " ";
			//cout << endl;
		//}

		vector<float> a;
		for (int i = 0; i < layers.size(); i++) {
			int row_len, col_len;
			row_len = layers[i];
			vector<float> Matrix(row_len, 0);
			a = Matrix;
			activations.push_back(a);
		}
		//for (int i = 0; i < w.size(); i++) {
			//for (int j = 0; j < w[i].size(); j++)
				//cout << w[i][j] << " ";
			//cout << endl;
		//}
		//cout << weights.size() << " ";
		//for (int i = 0; i < weights.size(); i++) {
			//for (int j = 0; j < weights[i].size(); j++) {
			//	for (int k = 0; k < weights[i][j].size(); k++)
			//		cout << weights[i][j][k] << " ";
			//	cout << endl;
			//}
		//}
	}/**//**/
	vector<vector<float>> dot_new(vector<vector<float>>& a, vector<vector<float>>& b) {
		int row_a, row_b, col_a, col_b;
		row_a = a.size();
		row_b = b.size();
		col_a = a[0].size();
		col_b = b[0].size();
		vector<vector<float>> dot;
		for (int i = 0; i < row_a; i++) dot.push_back({});
		for (int i = 0; i < row_a; i++) {
			vector<float> col0;
			for (int j = 0; j < col_b; j++) col0.push_back(0);
			dot[i] = col0;
		}
		//cout << row_a << " " << col_a << " " << row_b << " " << col_b << endl;
		for (int i = 0; i < row_a; i++) {
			for (int j = 0; j < col_b; j++) {
				dot[i][j] = 0;
				for (int k = 0; k < col_a; k++)
					dot[i][j] = dot[i][j] + (a[i][k] * b[k][j]);
			}
		}
		return dot;

	}
	//Dot product done
	vector<vector<float>> dot_product(vector<vector<float>> &a, vector<vector<float>> &b) {
		int row_a, row_b, col_a, col_b;
		row_a = a.size();
		row_b = b.size();
		col_a = a[0].size();
		col_b = b[0].size();
		vector<vector<float>> dot;
		dot.push_back({});
		for (int i = 0; i < col_b; i++)
			dot[0].push_back(0);
		for (int i = 0; i < row_a; i++) {
			for (int j = 0; j < col_b; j++) {
				dot[i][j] = 0;
				for(int k = 0; k< col_a; k++)
					dot[i][j] = dot[i][j] + (a[i][k] * b[k][j]);
			}
		}

		return dot;

	}

	float _mse(vector<float> target, vector<float> output) {
		float sum = 0;
		int n = target.size();
		for (int i = 0; i < target.size(); i++) {
			sum = sum + ((target[i] - output[i]) * (target[i] - output[i]));
		}
		return sum / n;
	}
	//sigmoid done
	vector<float> _sigmoid(vector<float>& a) {
		vector<float> sigmoid;
		for (int i = 0; i < a.size(); i++) sigmoid.push_back(1.0 / (1.0 + (float)exp(-1 * a[i])));
		return sigmoid;
	}

	// sigmoid derivative done
	vector<float> _sigmoid_derivative(vector<float>& a) {
		vector<float> sigmoid;
		for (int i = 0; i < a.size(); i++) sigmoid.push_back(a[i] * (1 - a[i]));
		return sigmoid;
	}
	//forward propogation done
	vector<float> forward_propogate(vector<float> inputs) {
		activations[0] = inputs;
		vector<vector<float>> net_inputs;
		vector<vector<float>> activ;
		activ.push_back(inputs);
		for (int i = 0; i < weights.size(); i++) {
			//cout << weights[i].size() << " " << endl;
			net_inputs = dot_new(activ, weights[i]);
			vector<float> temp_activ;
			temp_activ = _sigmoid(net_inputs[0]);
			activ.pop_back();
			activ.push_back(temp_activ);
			activations[i + 1] = temp_activ;
		}
		//return activations and change return type of function
		return activ[0];
	}

	vector<float> forward_propogate_test(vector<float> inputs) {
		activations[0] = inputs;
		vector<vector<float>> net_inputs;
		vector<vector<float>> activ;
		activ.push_back(inputs);
		for (int i = 0; i < weights.size(); i++) {
			//cout << weights[i].size() << " " << endl;
			net_inputs = dot_new(activ, weights[i]);
			vector<float> temp_activ;
			temp_activ = _sigmoid(net_inputs[0]);
			activ.pop_back();
			activ.push_back(temp_activ);
			activations[i + 1] = temp_activ;
		}
		//return activations and change return type of function
		return activ[0];
	}

	void gradient_descent(float learning_rate = 1.0) {
		for (int i = 0; i < weights.size(); i++) {
			//vector<vector<float>> w = weights[i];
			//weights[i] += weights[i] * learning_rate;
			for (int j = 0; j < weights[i].size(); j++) {
				//cout << weights[i][j].size();
				//transform(weights[i][j].begin(), weights[i][j].end(), temp.begin(), [learning_rate](float& c) {return c * learning_rate; });
				for (int k = 0; k < weights[i][j].size(); k++) {
					weights[i][j][k] = weights[i][j][k] + derivatives[i][j][k] * learning_rate;
				}
				//transform(weights[i][j].begin(), weights[i][j].end(), temp.begin(), weights[i][j].begin(), plus<float>());
			}
		}
	}

	void back_propogate(vector<float> error) {
		vector<float> activation;
		//vector<float> delta;
		vector<float> sigmoid_derivative;
		//cout << activations.size();
		for (int i = derivatives.size()-1; i >= 0; i--) {
			//cout << i;
			vector<float> delta;
			activation = activations[i + 1];
			//cout << activation.size() << endl;
			sigmoid_derivative = _sigmoid_derivative(activation);
			//cout << sigmoid_derivative.size() <<endl;
			//PrintVector(error);
			for (int j = 0; j < sigmoid_derivative.size(); j++) {
				delta.push_back(sigmoid_derivative[j] * error[j]);
			}
			//cout << delta.size();
			vector<vector<float>> delta_re;
			delta_re.push_back(delta);
			vector<float> current_acc = activations[i];
			vector<vector<float>> current_activations;
			for (int k = 0; k < current_acc.size(); k++) current_activations.push_back({ current_acc[k] });
			//PrintMatrix(current_activations);
			//delta = std::transform(sigmoid_derivative.begin(), sigmoid_derivative.end(), error, std::multiplies<float>());
			//#cout << delta_re[0].size()<< endl;
			vector<vector<float>> dotp = dot_new(current_activations, delta_re);
			derivatives[i] = dotp;
			vector<vector<float>> t_weights = transpose(weights[i]);
			vector<vector<float>> delta_mul = { delta };
			error = dot_new(delta_mul, t_weights)[0];
			//PrintVector(error);
		}
	}

	void train(vector<vector<float>> inputs, vector<vector<float>> targets, int epochs, float learning_rate) {
		for (int i = 0; i < epochs; i++) {
			float sum_errors = 0;
			for (int j = 0; j < inputs.size(); j++) {
				vector<float> target = targets[j];
				vector<float> output = forward_propogate(inputs[j]);
				vector<float> error;
				for (int k = 0; k < output.size(); k++)
					error.push_back(target[k] - output[k]);
				back_propogate(error);
				gradient_descent(learning_rate);
				sum_errors = sum_errors + _mse(target, output);
			}
			cout << "Error: " << sum_errors / inputs.size() << " at epoch" << i + 1 << endl;
		}
		cout << "Training complete" << endl;
		cout << "=======" << endl;
	}

	void create_seed() {
		seed_weights = weights;
		seed_activations = activations;
		//cout << activations.size() << endl;
		seed_derivatives = derivatives;
		seed_weights.resize(seed_weights.size() - 2);
		seed_activations.resize(seed_activations.size() - 2);
		//cout << seed_activations.size() << endl;
		seed_derivatives.resize(seed_derivatives.size() - 2);
	}

	void prime_base_network(vector<vector<float>> inputs, vector<vector<float>> targets, int cycles, float learning_rate = 0.5) {
		train(inputs, targets, cycles, learning_rate);
		Print3D(weights);
	}

	void remove_temp_classifier() {
		weights.resize(weights.size() - 1);
		derivatives.resize(derivatives.size() - 1);
		activations.resize(activations.size() - 1);
	}

	void add_destination_layer() {
		vector<vector<float>> temp;
		int r, c;
		// add new weights
		temp = weights.back();
		//PrintMatrix(temp);
		r = temp[0].size(); //changed to set the size of the destionation layer to a minimum 3
		c = 1;// temp[0].size();
		vector < vector <float>> Matrix(r, vector<float>(c, 0));
		for (auto it_row = Matrix.begin(); it_row != Matrix.end(); it_row++)
		{
			// Getting each (i,j) element and assigning random value to it
			for (auto it_col = it_row->begin(); it_col != it_row->end(); it_col++)
			{
				*it_col = (float)rand() / RAND_MAX;
				//cout << *it_col;
			}
		}
		temp = Matrix;
		weights.push_back(temp);
		//for (int wei = 0; wei < weights.size(); wei++) cout << "The matrices are of shape " << weights[wei].size() << " x " << weights[wei][0].size() << endl;
		// add new derivatives
		temp = derivatives.back();
		vector < vector <float>> Matrix_dr(r, vector<float>(c, 0.0));
		temp = Matrix_dr;
		derivatives.push_back(temp);

		// add new activations
		vector<float> temp_acc = activations.back();
		int row_len = 1;// temp_acc.size();
		vector<float> Matrix_acc(row_len, 0);
		temp_acc = Matrix_acc;
		activations.push_back(temp_acc);
		//for (int wei = 0; wei < activations.size(); wei++) cout << "The matrices are of shape " << activations.size() << " x " << activations[wei].size() << endl;
	}
    //done
	void add_class_layer(vector<vector<float>> targets) {
		vector<float> temp;
		for (int i = 0; i < targets.size(); i++) temp.push_back(targets[i][0]);
		sort(temp.begin(), temp.end());
		int num_classes = std::unique(temp.begin(), temp.end()) - temp.begin();
		num_classes = num_classes - 1;
		vector<vector<float>> temp_w;
		int r, c;
		temp_w = weights.back();
		r = temp_w[0].size();
		c = num_classes;
		vector < vector <float>> Matrix(r, vector<float>(c, 0));
		for (auto it_row = Matrix.begin(); it_row != Matrix.end(); it_row++)
		{
			// Getting each (i,j) element and assigning random value to it
			for (auto it_col = it_row->begin(); it_col != it_row->end(); it_col++)
			{
				*it_col = (float)rand() / RAND_MAX;
				//cout << *it_col;
			}
		}
		temp_w = Matrix;
		weights.push_back(temp_w);

		vector<vector<float>> temp_d;
		temp_d = derivatives.back();
		vector < vector <float>> Matrix_dr(r, vector<float>(c, 0.0));
		temp_d = Matrix_dr;
		derivatives.push_back(temp_d);

		vector<float> temp_acc = activations.back();
		int row_len = num_classes;
		vector<float> Matrix_acc(row_len, 0);
		temp_acc = Matrix_acc;
		activations.push_back(temp_acc);

	}
	// not done
	void add_class_layer_final() {}

	void create_temp_classifier_seed(vector<vector<float>> targets) {
		vector<float> temp;
		//cout << seed_weights.size() << endl;
		for (int i = 0; i < targets.size(); i++) temp.push_back(targets[i][0]);
		sort(temp.begin(), temp.end());
		int num_classes = std::unique(temp.begin(), temp.end()) - temp.begin();
		num_classes = num_classes - 1;
		vector<vector<float>> temp_w;
		int r, c;
		temp_w = seed_weights.back();
		r = temp_w[0].size();
		c = num_classes;
		vector < vector <float>> Matrix(r, vector<float>(c, 0));
		//PrintMatrix(Matrix);
		for (auto it_row = Matrix.begin(); it_row != Matrix.end(); it_row++)
		{
			// Getting each (i,j) element and assigning random value to it
			for (auto it_col = it_row->begin(); it_col != it_row->end(); it_col++)
			{
				*it_col = (float)rand() / RAND_MAX;
				//cout << *it_col;
			}
		}
		temp_w = Matrix;
		//PrintMatrix(temp_w);
		seed_weights.push_back(temp_w);
		//cout << seed_weights.size()<< endl;
		vector<vector<float>> temp_d;
		temp_d = seed_derivatives.back();
		vector < vector <float>> Matrix_dr(r, vector<float>(c, 0.0));
		temp_d = Matrix_dr;
		seed_derivatives.push_back(temp_d);

		vector<float> temp_acc = seed_activations.back();
		int row_len = num_classes;
		vector<float> Matrix_acc(row_len, 0);
		temp_acc = Matrix_acc;
		seed_activations.push_back(temp_acc);

	}

	vector<float> forward_propogate_seed(vector<float> inputs) {
		seed_activations[0] = inputs;
		vector<vector<float>> net_inputs;
		vector<vector<float>> activ;
		activ.push_back(inputs);
		for (int i = 0; i < seed_weights.size(); i++) {
			//cout << weights[i].size() << " " << endl;
			net_inputs = dot_new(activ, seed_weights[i]);
			vector<float> temp_activ;
			temp_activ = _sigmoid(net_inputs[0]);
			activ.pop_back();
			activ.push_back(temp_activ);
			seed_activations[i + 1] = temp_activ;
		}
		//return activations and change return type of function
		return activ[0];
	}

	void back_propogate_seed(vector<float> error) {
		vector<float> activation;
		//vector<float> delta;
		vector<float> sigmoid_derivative;
		//cout << activations.size();
		for (int i = seed_derivatives.size() - 1; i >= 0; i--) {
			//cout << i;
			vector<float> delta;
			activation = seed_activations[i + 1];
			//cout << activation.size() << endl;
			sigmoid_derivative = _sigmoid_derivative(activation);
			//cout << sigmoid_derivative.size() <<endl;
			//PrintVector(error);
			for (int j = 0; j < sigmoid_derivative.size(); j++) {
				delta.push_back(sigmoid_derivative[j] * error[j]);
			}
			//cout << delta.size();
			vector<vector<float>> delta_re;
			delta_re.push_back(delta);
			vector<float> current_acc = seed_activations[i];
			vector<vector<float>> current_activations;
			for (int k = 0; k < current_acc.size(); k++) current_activations.push_back({ current_acc[k] });
			//PrintMatrix(current_activations);
			//delta = std::transform(sigmoid_derivative.begin(), sigmoid_derivative.end(), error, std::multiplies<float>());
			//#cout << delta_re[0].size()<< endl;
			vector<vector<float>> dotp = dot_new(current_activations, delta_re);
			seed_derivatives[i] = dotp;
			vector<vector<float>> t_weights = transpose(weights[i]);
			vector<vector<float>> delta_mul = { delta };
			error = dot_new(delta_mul, t_weights)[0];
			//PrintVector(error);
		}
	}

	void train_seed(vector<vector<float>> inputs, vector<vector<float>> targets, int epochs, float learning_rate) {
		for (int i = 0; i < epochs; i++) {
			float sum_errors = 0;
			for (int j = 0; j < inputs.size(); j++) {
				vector<float> target = targets[j];
				vector<float> output = forward_propogate_seed(inputs[j]);
				vector<float> error;
				//PrintVector(output);
				for (int k = 0; k < target.size(); k++)
					error.push_back(target[k] - output[k]);
				back_propogate_seed(error);
				gradient_descent(learning_rate);
				sum_errors = sum_errors + _mse(target, output);
			}
			//cout << "Error: " << sum_errors / inputs.size() << " at epoch" << i + 1 << endl;
		}
		cout << "Training complete" << endl;
		cout << "=======" << endl;
	}

	void prime_seed_network(vector<vector<float>> inputs, vector<vector<float>> targets, int cycles, float learning_rate = 0.5) {
		train_seed(inputs, targets, cycles, learning_rate);
	}

	void remove_temp_classifier_seed() {
		seed_weights.resize(seed_weights.size() - 1);
		seed_derivatives.resize(seed_derivatives.size() - 1);
		seed_activations.resize(seed_activations.size() - 1);
	}

	vector<vector<vector<float>>> extreme_member_classes(vector<vector<float>> inputs, vector<vector<float>> targets) {
		//cout << weights.size() << endl;
		create_seed();
		create_temp_classifier_seed(targets);
		prime_seed_network(inputs, targets, 10);
		remove_temp_classifier_seed();
		//cout << weights.size() << endl;
		//cout << weights.size() << endl;
		vector<float> classes, temp;
		for (int i = 0; i < targets.size(); i++) classes.push_back(targets[i][0]);
		temp = classes;
		sort(classes.begin(), classes.end());
		vector<float>::iterator ip = std::unique(classes.begin(), classes.end());
		classes.resize(std::distance(classes.begin(), ip));
		//PrintVector(classes);
		vector<vector<vector<float>>> sorted_classes_list;
		for (int i = 0; i < classes.size(); i++) {
			int count = 0;
			for (int j = 0; j < targets.size(); j++) {
					if (targets[j][0] == classes[i]) count = count + 1;
			}
			//cout << count << endl;
			vector<float> sum_perceptron, avg_perceptron;
			for (int k = 0; k < seed_weights.back()[0].size(); k++) sum_perceptron.push_back(0);
			//filter indices
			vector<float> filter_indices;
			//cout << temp.size() << endl;
			for (int ind = 0; ind < temp.size(); ind++) {
				if (temp[ind] == classes[i]) filter_indices.push_back(ind);
			}
			//PrintVector(filter_indices);
			//Class_inputs
			vector<vector<float>> Class_inputs;
			for (int cinp = 0; cinp < filter_indices.size(); cinp++) {
				Class_inputs.push_back(inputs[filter_indices[cinp]]);
			}

			for (int inp = 0; inp < Class_inputs.size(); inp++) {
				vector<float> output_single = forward_propogate_seed(Class_inputs[inp]);
				for (int sw = 0; sw < seed_activations.back().size(); sw++) sum_perceptron[sw] += output_single[sw];
			}

			for (int s = 0; s < sum_perceptron.size(); s++) avg_perceptron.push_back((float)sum_perceptron[s] / count);

			vector<float> Error_list;
			for (int cimp = 0; cimp < Class_inputs.size(); cimp++) {
				int err = 0;
				vector<float> output_single = forward_propogate_seed(Class_inputs[cimp]);
				for (int o = 0; o < output_single.size(); o++) err += avg_perceptron[o] - output_single[o];
				Error_list.push_back(err);
			}
			//sort_indices = np.argsort(Error_list)
			vector<int> sorted_indices = sorted_index(Class_inputs.size(), Error_list);
			vector<vector<float>> Classes_sorted;
			//for (int is = 0; is < sorted_indices.size(); is++) cout << sorted_indices[is] << endl;
			for (int sc = 0; sc < sorted_indices.size(); sc++) Classes_sorted.push_back(Class_inputs[sorted_indices[sc]]);
			//PrintMatrix(Class_inputs);
			sorted_classes_list.push_back(Classes_sorted);
		}
		return sorted_classes_list;

	}

	vector<float> return_source_activations() {
		return activations.rbegin()[1];
	}

	float return_acc(vector<vector<float>> inputs, vector<vector<float>> targets) {

		vector<int> pred;
		for (int i = 0; i < inputs.size(); i++) {
			vector<float> output = forward_propogate(inputs[i]);
			//cout << output[0] << endl;
			if (output[0] >= 0.5) pred.push_back(1);
			else pred.push_back(0);
		}
		float acc = 0.0;
		int count = 0;
		for (int j = 0; j < pred.size(); j++) {
			//cout << pred[j] << endl;
			if (pred[j] == targets[j][0]) count += 1;
		}
		//cout << "acc: " << acc << endl;
		acc = (float)count / pred.size();
		cout << "acc: " << acc << endl;
		return acc;

	}

	void ANG_grow(vector<vector<float>> inputs, vector<vector<float>> targets, vector<int> hidden_layers) {
		//Init(hidden_layers, 1, inputs[0].size());
		//cout << weights.size() << endl;
		prime_base_network(inputs, targets, 10);
		//cout << weights.size() << endl;
		remove_temp_classifier();
		//cout << weights.size() << endl;
		add_destination_layer();
		//Print3D(weights);
		//cout << weights.size() << endl;
		add_class_layer(targets);
		//Print3D(weights);
		//cout << weights.size() << endl;
		float accuracy = 0.0;
		int percep = 0;
		vector<vector<float>> items = inputs, test_targets = targets;

		while (accuracy < 0.9) {
			vector<vector<vector<float>>> sorted_classes = extreme_member_classes(inputs, targets);
			//cout << weights.size() << endl;
			remove_temp_classifier();
			//cout << activations.back().size() << endl;
			for (int i = 0; i < sorted_classes.size(); i++) {
				vector<vector<float>> extremes;
				//PrintMatrix(sorted_classes[i]);
				extremes.push_back(sorted_classes[i][0]);
				extremes.push_back(sorted_classes[i].back());
				//PrintMatrix(extremes);
				for (int ext = 0; ext < extremes.size(); ext++) {
					vector<float> output = forward_propogate(extremes[ext]);
					vector<float> fp_output = return_source_activations();
					float sum_op = 0.0;
					for (int fpo = 0; fpo < fp_output.size(); fpo++) sum_op += fp_output[fpo];
					int len_op = fp_output.size();
					float average = (float)sum_op / len_op;
					float sum_avg = 0.0;
					for (int sa = 0; sa < fp_output.size(); sa++) sum_avg += (average - fp_output[sa]) * (average - fp_output[sa]);
					float sd = stdev(fp_output);
					int x = 1;
					//cout << "percep " << percep << endl;
					if(percep<activations.back().size()) {
						activations.back()[percep] = 0;
					}
					else {
						vector<vector<float>> temp_weights;
						vector<float> tw;
						for (int w = 0; w < weights.back().size(); w++) {
							tw = weights.back()[w];
							tw.push_back((float)rand() / RAND_MAX);
							temp_weights.push_back(tw);
						}
						//PrintMatrix(temp_weights);
						weights.back() = temp_weights;
						//PrintMatrix(weights.back());
						vector<vector<float>> temp_derivatives;
						vector<float> td;
						for (int d = 0; d < derivatives.back().size(); d++) {
							td = derivatives.back()[d];
							td.push_back(0);
							temp_derivatives.push_back(td);
						}
						derivatives.back() = temp_derivatives;
						activations.back().push_back(0);
					}
					percep = percep + 1;
					for (int conn = 0; conn < fp_output.size(); conn++) {
						if (fp_output[conn] >= x * sd || fp_output[conn] < -x * sd) cout << "This is a critical connection" << endl;
						else {
							cout << "This is not a critical connection" << endl;
							cout << fp_output.size() << endl;
							for (int wl = 0; wl < weights.back().size(); wl++)
								for(int c = 0; c< weights.back()[0].size(); c++)
									weights.back()[wl][c] = 0;
						}
					}
				}
			}
			cout << "I am here" << endl;
			add_class_layer(targets);
			cout << weights.back().size() << endl;
			train(inputs, targets, 50, 0.5);
			cout << "I am here" << endl;
			accuracy = return_acc(items, test_targets);
			vector<vector<float>> extreme_members;
			for (int scl = 0; scl < sorted_classes.size(); scl++) {
				extreme_members.push_back(sorted_classes[scl][0]);
				extreme_members.push_back(sorted_classes[scl].back());
			}
			vector<int> rm_index;
			for (int em = 0; em < extreme_members.size(); em++) {
				for (int pos = 0; pos < inputs.size(); pos++) {
					if (extreme_members[em] == inputs[pos]) {
						//rm_index.push_back(pos);
						inputs.erase(inputs.begin() + pos);
						targets.erase(targets.begin() + pos);
						break;
					}
				}
			}
			//Work on this
			/*for (auto rm : rm_index) {
				inputs.erase(inputs.begin() + rm);
				targets.erase(targets.begin() + rm);
			}*/
			cout << "accuracy: " << accuracy << endl;
		}
		//cout << "activations size: " << activations.size() << endl;
	}
	/*
	void save(ostringstream& oss)
	{
		boost::archive::binary_oarchive oa(oss);
		oa&* (this);
	}

	void load(istringstream& iss)
	{
		//std::string str_data = oss.str();
		//std::istringstream iss(str_data);
		boost::archive::binary_iarchive ia(iss);
		ia&* (this);
	}
	*/
	void save_weights_der(vector<vector<vector<float>>> Matrix, string name) {
		ofstream output_file(name + ".txt");
		ostream_iterator<float> output_iterator(output_file, ",");
		for (int i = 0; i < Matrix.size(); i++) {

			for (int j = 0; j <Matrix[i].size(); j++) {
				//cout << j;
				copy(Matrix[i][j].begin(), Matrix[i][j].end() - 1, output_iterator);
				output_file << Matrix[i][j].back();
				output_file << "\n";
			}
			output_file << "#\n";
		}

		output_file.close();

	}

	void save_activations(vector<vector<float>> Matrix, string name) {

		ofstream output_file(name + ".txt");
		ostream_iterator<float> output_iterator(output_file, ",");

		for (int j = 0; j < Matrix.size(); j++) {
			//cout << j;
			copy(Matrix[j].begin(), Matrix[j].end() - 1, output_iterator);
			output_file << Matrix[j].back();
			output_file << "\n";
		}

		output_file.close();

	}

	void read_weights_der(vector<vector<vector<float>>>& Matrix, string name) {

		std::ifstream in(name + ".txt");
		std::string str;
		vector<vector<vector<float>>> weights;
		vector<vector<float>> st;
		while (in.good())
		{
			string line;
			vector<float> row;
			getline(in, line, '\n');
			if (line == "") break;
			if (line == "#") {
				weights.push_back(st);
				st = {};
				//cout <<"size" << st.size() << endl;
				continue;
			}
			//cout << "size" << st.size() << endl;
			stringstream ss(line);

			while (ss.good()) {
				//std::string::size_type sz;
				string sbstr;
				getline(ss, sbstr, ',');
				float val = stof(sbstr);
				row.push_back(val);
			}

			st.push_back(row);

		}

		Matrix = weights;
	}

	void read_activations(vector<vector<float>>& Act, string name) {
		vector<vector<float>> A;
		ifstream ip(name + ".txt");
		while (ip.good()) {
			vector<float> st;
			string line;
			getline(ip, line, '\n');
			if (line == "") break;
			stringstream ss(line);
			//cout << line << endl;
			while (ss.good()) {
				std::string::size_type sz;
				string sbstr;
				getline(ss, sbstr, ',');
				float val = stof(sbstr);
				st.push_back(val);
			}
			A.push_back(st);
		}

		Act = A;

	}

	void save_model(string name) {
		save_weights_der(weights, name + "_weights");
		save_weights_der(derivatives, name + "_derivatives");
		save_weights_der(seed_weights, name + "_seed_weights");
		save_weights_der(seed_derivatives, name + "_seed_derivatives");
		save_activations(activations, name + "_activations");
		save_activations(seed_activations, name + "_seed_activations");

	}

	void load_model(string name) {
		read_weights_der(weights, name + "_weights");
		read_weights_der(seed_weights, name + "_seed_weights");
		read_weights_der(derivatives, name + "_derivatives");
		read_weights_der(seed_derivatives, name + "_seed_derivatives");
		read_activations(activations, name + "_activations");
		read_activations(seed_activations, name + "_seed_activations");
	}

};

//new stuff
void read_files(string filename, vector<vector<float>>& inputs, vector<vector<float>>& targets) {
	ifstream ip(filename);

	if (!ip.is_open()) cout << "Error" << endl;
	else cout << "Yay" << endl;

	ip.ignore(500, '\n');
	//vector<vector<float>> inputs;
	vector<string> st;
	while (ip.good()) {
		vector<float> st;
		string line;
		getline(ip, line, '\n');
		if (line == "") break;
		stringstream ss(line);
		//cout << line << endl;
		while (ss.good()) {
			std::string::size_type sz;
			string sbstr;
			getline(ss, sbstr, ',');
			float val = stof(sbstr);
			st.push_back(val);
		}
		//cout << st.size() << endl;
		inputs.push_back(st);
	}
	ip.close();
	cout <<"number of rows: " << inputs.size() << endl;
	//vector<vector<float>> targets;
	for (int tar = 0; tar < inputs.size(); tar++) {
		vector<float> t;
		t.push_back(inputs[tar].back());
		targets.push_back(t);
		inputs[tar].pop_back();
	}
	return;
}

ANG Train_ng(vector<vector<float>> inputs, vector<vector<float>> targets, vector<int> hidden_layers) {

	ANG ang(hidden_layers, targets[0].size(), inputs[0].size());
	ang.ANG_grow(inputs, targets, hidden_layers);

	return ang;
}

vector<int> test(ANG ang, vector<vector<float>> inputs) {
	vector<int> pred;

	for (int i = 0; i < inputs.size(); i++) {
		vector<float> output = ang.forward_propogate(inputs[i]);
		//cout << output[0] << endl;
		if (output[0] >= 0.5) pred.push_back(1);
		else pred.push_back(0);
	}
	return pred;
}

int main()
{
	cout << "Hello \n";
	ANG ang;
	//vector<vector<float>> inputs, targets;
	//string filename = "D:/Tests/Titanic/train_titanic.csv";
	//read_files(filename, inputs, targets);
	//ang.PrintMatrix(inputs);
	vector<vector<float>>  i = { {1, 1, 1}, {0, 0, 0}, {1, 1, 1} , {0,0,0} ,{1,1,1}, {1,1,1} , {0,0,0}, {0,0,0} ,{1,1,1} , {0,0,0}, {1, 1, 1}, {0, 0, 0}, {1, 1, 1} , { 0,0,0} ,{1,1,1}, {1,1,1} , {0,0,0}, {0,0,0} ,{1,1,1} , {0,0,0}, {1, 1, 1}, {0, 0, 0}, {1, 1, 1} , { 0,0,0} ,{1,1,1}, {1,1,1} , {0,0,0}, {0,0,0} ,{1,1,1} , {0,0,0}, {1, 1, 1}, {0, 0, 0}, {1, 1, 1} , { 0,0,0} ,{1,1,1}, {1,1,1} , {0,0,0}, {0,0,0} ,{1,1,1} , {0,0,0}, {1, 1, 1}, {0, 0, 0}, {1, 1, 1} , { 0,0,0} ,{1,1,1}, {1,1,1} , {0,0,0}, {0,0,0} ,{1,1,1} , {0,0,0} };
	vector<vector<float>> t = { {1} , {0}, {0} , {0}, {1}, {1}, {0} , {0}, {1}, {0}, {1} , {0}, {1} , {0}, {1}, {1}, {0} , {0}, {1}, {0}, {1} , {0}, {1} , {0}, {1}, {1}, {0} , {0}, {1}, {0}, {1} , {0}, {1} , {0}, {1}, {1}, {0} , {0}, {1}, {0}, {1} , {0}, {1} , {0}, {1}, {1}, {0} , {0}, {1}, {0} };
	ang = Train_ng(i, t, {3,4});
	//std::ostringstream oss;
	//ang.save(oss);
	ang.save_model("saveloadtest");
	//string s = oss.str();
	ANG ang2;
	//std::istringstream iss;
	//iss.str(s);
	//ang2.load(iss);
	ang2.load_model("saveloadtest");
	/*
	cout << "weights" << endl;
	ang2.Print3D(ang2.weights);
	cout << "seed_weights" << endl;
	ang2.Print3D(ang2.seed_weights);
	cout << "derivatives" << endl;
	ang2.Print3D(ang2.derivatives);
	cout << "seed_derivatives" << endl;
	ang2.Print3D(ang2.seed_derivatives);
	cout << "activations" << endl;
	ang2.PrintMatrix(ang2.activations);
	cout << "seed_activations" << endl;
	ang2.PrintMatrix(ang2.seed_activations);
	*/
	vector<int> pred1 = test(ang, i);
	for (int i = 0; i < pred1.size(); i++) cout << pred1[i] << " ";
	cout << endl;
	vector<int> pred = test(ang2, i);
	for (int i = 0; i < pred.size(); i++) cout << pred[i] << " ";
	//ang.ANG_grow(i, t, {3,3});
	//ang.create_seed();
	//ang.create_temp_classifier_seed(t);
	//cout << ang.seed_activations.size() << endl;
	//ang.train_seed(i, t, 10, 0.1);
	//vector<vector<vector<float>>> emc = ang.extreme_member_classes(i, t);
	//cout << emc.size() << endl;
	//ang.add_class_layer(t);
	//ang.train(i, t, 10, 1.0);
	//ang.PrintVector(ang.activations.back());
	//vector<vector<float>> p = ang.dot_new(a, b);
	//ang.PrintMatrix(p);
	//vector<vector<float>> trans = ang.transpose(b);
	//ang.PrintMatrix(trans);
	//vector<float> t = {1 ,0, 1, 4, 5};
	//sort(t.begin(), t.end());
	//vector<float>::iterator ip = std::unique(t.begin(), t.end());
	//t.resize(std::distance(t.begin(), ip));
	//ang.PrintVector(t);
	//vector<int> ind;
	//ind = ang.sorted_index(5, t);
	//for (int i = 0; i < ind.size(); i++) cout << ind[i] << " ";
	return 0;
}


#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <cmath>

#include <Eigen/Dense>

#include "variablesExt.h"
#include "recContours.h"
#include "pathfinding.h"

using namespace cv;
using namespace std;
using namespace Eigen;

void pathfinding(double offset)
{
	cout << "--- Nouveau path ---" << endl;
	convexHull(approxPoly, approxConvex, false, false); // on approxime par un poly convexe
	if(approxConvex.size() < 3) return; // si ce n'est pas au moins un triangle on sort

	Point A1,A2,B1,B2,L1,L2;

	A1 = approxConvex[0];
	A2 = approxConvex[approxConvex.size()-1];
	B1 = approxConvex[1];
	B2 = approxConvex[2];
	size_t iB2 = 2;
	size_t iA2 = approxConvex.size()-1;

	Vector2d vOffset = offsetVector(A1,B1,offset);
	Vector2d vAB = points2Vec(A1,B1);

	path.clear();

	// on commence par aller de A1 à B1

	path.push_back(A1);
	path.push_back(B1);

	bool continuer = true;
	bool altern = true;

	Point B = B1;
	int cmpt = 1;

	while(continuer)
	{
		// on recherche les points d'intersection de la parallèle à A1B1 décalée de vOffset

		Point O = vec2Point(B, vOffset*cmpt); // à partir du point 0, on cherche ces points
		Vector2d vB12 = points2Vec(B1,B2); // on a besoin de croiser vB12 avec vAB, on vérifie qu'ils ne sont pas colinéaires
		Vector2d vA12 = points2Vec(A1,A2);

		if(vAB == vB12 || vAB == vA12 || vAB == vB12 * -1 || vAB == vA12 * -1)
		{
			cout << "vecteurs colineaires" << endl;
			continuer = false; // c'est possible mais ça signifie qu'on est arrivé au bout du champ
		}
		else
		{
			Matrix4d MB;
			MB << 	1,-1*vAB[0],0,0,
				1,0,0,-1*vB12[0],
				0,-1*vAB[1],1,0,
				0,0,1,-1*vB12[1];
			//cout << "MB : " << endl << MB << endl;

			Vector4d CB;
			CB << O.x,B1.x,O.y,B1.y;
			//cout << "CB : " << endl << CB << endl;

			FullPivLU<Matrix4d> decompositionB(MB);

		    	Vector4d XB = decompositionB.solve(CB); // X : (x, t, y, t')
		    	cout << "XB : " << endl << XB << endl;

		    	Matrix4d MA;
			MA << 	1,-1*vAB[0],0,0,
				1,0,0,-1*vA12[0],
				0,-1*vAB[1],1,0,
				0,0,1,-1*vA12[1];
			//cout << "MA : " << endl << MA << endl;

			Vector4d CA;
			CA << O.x,A1.x,O.y,A1.y;
			//cout << "CA : " << endl << CA << endl;

			FullPivLU<Matrix4d> decompositionA(MA);

		    	Vector4d XA = decompositionA.solve(CA); // X : (x, t, y, t')
		    	cout << "XA : " << endl << XA << endl;

		    	Point BB = Point(XB[0],XB[2]);
		    	Point AA = Point(XA[0],XA[2]);

		    	// il faut regarder si on est sorti de l'une des deux arêtes

		    	Vector2d vNBB = points2VecN(B1,BB);
		    	Vector2d vNAA = points2VecN(A1,AA);
		    	Vector2d vNB12 = points2VecN(B1,B2);
		    	Vector2d vNA12 = points2VecN(A1,A2);

		    	if(vNBB.squaredNorm() > vNB12.squaredNorm())
		    	{
		    		iB2++;
		    		B1 = B2;
		    		B2 = approxConvex[iB2];
		    		if(iA2 >= iB2) continue;
		    	}
		    	else
		    	{
		    		B1 = BB;
		    	}

		    	if(vNAA.squaredNorm() > vNA12.squaredNorm())
		    	{
		    		iA2--;
		    		A1 = A2;
		    		A2 = approxConvex[iA2];
		    		if(iA2 >= iB2) continue;
		    	}
		    	else
		    	{
		    		A1 = AA;
		    	}

		    	if(iA2 < iB2) // on est arrivé au bout du champ
		    	{
		    		continuer = false;
		    		cout << "iA2:"<<iA2<<"<iB2:"<<iB2<<endl;
		    		break;
		    	}

		    	// ici, on vérifie que nos points sont dans l'approxPoly, s'ils n'y sont pas, on les force à y rentrer.
		    	double distAA = pointPolygonTest(approxPoly, AA, true);
		    	if(distAA < 0)
		    	{
		    		// on cherche le point d'intersection entre AA-BB et l'approxPoly (à la bourrin)
		    		int t = 0;
		    		while(true)
		    		{
		    			t++;
			    		//Point AAA = vec2Point(AA, t * vAB);
			    		Point AAA = vec2Point(AA, t * points2Vec(AA,BB));
			    		double distAAA = pointPolygonTest(approxPoly, AAA, true);

			    		AA = AAA;

			    		if(distAAA >= 0)
			    		{
			    			cout << "On est rentré dans le polygone pour AA" << endl;
			    			break;
			    		}
			    		else if(distAAA < distAA - 1000)
			    		{
			    			cout << "ERREUR : on s'eloigne !" << endl;
			    			break;
			    		}
		    		}
		    	}
		    	double distBB = pointPolygonTest(approxPoly, BB, true);
		    	if(distBB < 0)
		    	{
		    		// on cherche le point d'intersection entre AA-BB et l'approxPoly (à la bourrin)
		    		int t = 0;
		    		while(true)
		    		{
		    			t++;
			    		//Point BBB = vec2Point(BB, (-1*t) * vAB);
			    		Point BBB = vec2Point(BB, t * points2Vec(BB,AA));
			    		double distBBB = pointPolygonTest(approxPoly, BBB, true);

			    		BB = BBB;

			    		if(distBBB >= 0)
			    		{
			    			cout << "On est rentré dans le polygone pour BB" << endl;
			    			break;
			    		}
			    		else if(distBBB < distBB - 1000)
			    		{
			    			cout << "ERREUR : on s'eloigne !" << endl;
			    			break;
			    		}
		    		}
		    	}

		    	if(altern)
		    	{
			    	path.push_back(BB);
			    	path.push_back(AA);
			    	cout << "nouveau trait : " << BB<< "-"<< AA<<endl;
		    	}
		    	else
		    	{
		    		path.push_back(AA);
			    	path.push_back(BB);
			    	cout << "nouveau trait : " << AA<< "-"<< BB<<endl;
		    	}
	    	}
    	altern = !altern;
    	cmpt++;

    	cv::polylines(rgb_copy, path, false, Scalar(0,255,0), 1, LINE_8, 0);
	displayPicture(0,NULL);

    	//cv::waitKey(0);
    	}
}

Point vec2Point(Point depart, Vector2d vec)
{
	double lx = (double)depart.x + vec[0];
	double ly = (double)depart.y + vec[1];

	return Point(ceil(lx),ceil(ly));
}

Vector2d points2Vec(Point A, Point B) // transforme AB en vecteur unitaire
{
	double moduleX = (B.x - A.x);
	double moduleY = (B.y - A.y);
	double module = sqrt(moduleX*moduleX + moduleY*moduleY);

	Vector2d V;
	V[0] = moduleX/module;
	V[1] = moduleY/module;

	return V;
}

Vector2d points2VecN(Point A, Point B) // transforme AB en vecteur normé
{
	double moduleX = (B.x - A.x);
	double moduleY = (B.y - A.y);

	Vector2d V;
	V[0] = moduleX;
	V[1] = moduleY;

	return V;
}

Vector2d offsetVector(Point A, Point B, double offset) // (unitaire) A et B sont les points de l'arrête à couper, leur ordre spatial ne change pas, et le polygone est codé en sens horraire
{
	Vector2d edge = points2Vec(A,B);

	Vector2d voffset;

	voffset[0] = (-1 * edge[1]) * offset;
	voffset[1] = (edge[0]) * offset;

	return voffset;
}