//=====================================================
// File   :  hand_vec_interface.hh
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
//=====================================================
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program 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 Public License for more details.
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
#ifndef HAND_VEC_INTERFACE_HH
#define HAND_VEC_INTERFACE_HH

#include <Eigen/Core>
#include "f77_interface.hh"

using namespace Eigen;

template<class real>
class hand_vec_interface : public f77_interface_base<real> {

public :

  typedef typename ei_packet_traits<real>::type Packet;
  static const int PacketSize = ei_packet_traits<real>::size;

  typedef typename f77_interface_base<real>::stl_matrix stl_matrix;
  typedef typename f77_interface_base<real>::stl_vector stl_vector;
  typedef typename f77_interface_base<real>::gene_matrix gene_matrix;
  typedef typename f77_interface_base<real>::gene_vector gene_vector;

  static void free_matrix(gene_matrix & A, int N){
    ei_aligned_free(A);
  }

  static void free_vector(gene_vector & B){
    ei_aligned_free(B);
  }

  static inline void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){
    int N = A_stl.size();
    A = (real*)ei_aligned_malloc(N*N*sizeof(real));
    for (int j=0;j<N;j++)
      for (int i=0;i<N;i++)
        A[i+N*j] = A_stl[j][i];
  }

  static inline void vector_from_stl(gene_vector & B, stl_vector & B_stl){
    int N = B_stl.size();
    B = (real*)ei_aligned_malloc(N*sizeof(real));
    for (int i=0;i<N;i++)
      B[i] = B_stl[i];
  }

  static inline std::string name() {
    #ifdef PEELING
    return "hand_vectorized_peeling";
    #else
    return "hand_vectorized";
    #endif
  }

  static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
  {
    asm("#begin matrix_vector_product");
    int AN = (N/PacketSize)*PacketSize;
    int ANP = (AN/(2*PacketSize))*2*PacketSize;
    int bound = (N/4)*4;
    for (int i=0;i<N;i++)
      X[i] = 0;

    for (int i=0;i<bound;i+=4)
    {
      register real* __restrict__ A0 = A + i*N;
      register real* __restrict__ A1 = A + (i+1)*N;
      register real* __restrict__ A2 = A + (i+2)*N;
      register real* __restrict__ A3 = A + (i+3)*N;

      Packet ptmp0 = ei_pset1(B[i]);
      Packet ptmp1 = ei_pset1(B[i+1]);
      Packet ptmp2 = ei_pset1(B[i+2]);
      Packet ptmp3 = ei_pset1(B[i+3]);
//       register Packet ptmp0, ptmp1, ptmp2, ptmp3;
//       asm(
//
//           "movss     (%[B],%[j],4), %[ptmp0]  \n\t"
//           "shufps   $0,%[ptmp0],%[ptmp0] \n\t"
//           "movss    4(%[B],%[j],4), %[ptmp1]  \n\t"
//           "shufps   $0,%[ptmp1],%[ptmp1] \n\t"
//           "movss    8(%[B],%[j],4), %[ptmp2]  \n\t"
//           "shufps   $0,%[ptmp2],%[ptmp2] \n\t"
//           "movss   12(%[B],%[j],4), %[ptmp3]  \n\t"
//           "shufps   $0,%[ptmp3],%[ptmp3] \n\t"
//           : [ptmp0] "=x" (ptmp0),
//             [ptmp1] "=x" (ptmp1),
//             [ptmp2] "=x" (ptmp2),
//             [ptmp3] "=x" (ptmp3)
//           : [B] "r" (B),
//             [j] "r" (size_t(i))
//           : );

      if (AN>0)
      {
//         for (size_t j = 0;j<ANP;j+=8)
//         {
//           asm(
//
//           "movaps     (%[A0],%[j],4), %%xmm8  \n\t"
//           "movaps   16(%[A0],%[j],4), %%xmm12 \n\t"
//           "movups     (%[A3],%[j],4), %%xmm11 \n\t"
//           "movups   16(%[A3],%[j],4), %%xmm15 \n\t"
//           "movups     (%[A2],%[j],4), %%xmm10 \n\t"
//           "movups   16(%[A2],%[j],4), %%xmm14 \n\t"
//           "movups     (%[A1],%[j],4), %%xmm9  \n\t"
//           "movups   16(%[A1],%[j],4), %%xmm13 \n\t"
//
//           "mulps %[ptmp0], %%xmm8  \n\t"
//           "addps (%[res0],%[j],4), %%xmm8  \n\t"
//           "mulps %[ptmp3], %%xmm11 \n\t"
//           "addps %%xmm11, %%xmm8  \n\t"
//           "mulps %[ptmp2], %%xmm10 \n\t"
//           "addps %%xmm10, %%xmm8  \n\t"
//           "mulps %[ptmp1], %%xmm9  \n\t"
//           "addps %%xmm9, %%xmm8   \n\t"
//           "movaps %%xmm8, (%[res0],%[j],4)  \n\t"
//
//           "mulps %[ptmp0], %%xmm12 \n\t"
//           "addps 16(%[res0],%[j],4), %%xmm12  \n\t"
//           "mulps %[ptmp3], %%xmm15 \n\t"
//           "addps %%xmm15, %%xmm12  \n\t"
//           "mulps %[ptmp2], %%xmm14 \n\t"
//           "addps %%xmm14, %%xmm12  \n\t"
//           "mulps %[ptmp1], %%xmm13 \n\t"
//           "addps %%xmm13, %%xmm12  \n\t"
//           "movaps %%xmm12, 16(%[res0],%[j],4) \n\t"
//           :
//           : [res0] "r" (X), [j] "r" (j),[A0] "r" (A0),
//             [A1] "r" (A1),
//             [A2] "r" (A2),
//             [A3] "r" (A3),
//             [ptmp0] "x" (ptmp0),
//             [ptmp1] "x" (ptmp1),
//             [ptmp2] "x" (ptmp2),
//             [ptmp3] "x" (ptmp3)
//           : "%xmm8", "%xmm9", "%xmm10", "%xmm11", "%xmm12", "%xmm13", "%xmm14", "%xmm15", "%r14");
//         }
          register Packet A00;
          register Packet A01;
          register Packet A02;
          register Packet A03;
          register Packet A10;
          register Packet A11;
          register Packet A12;
          register Packet A13;
          for (int j = 0;j<ANP;j+=2*PacketSize)
          {
//             A00 = ei_pload(&A0[j]);
//             A01 = ei_ploadu(&A1[j]);
//             A02 = ei_ploadu(&A2[j]);
//             A03 = ei_ploadu(&A3[j]);
//             A10 = ei_pload(&A0[j+PacketSize]);
//             A11 = ei_ploadu(&A1[j+PacketSize]);
//             A12 = ei_ploadu(&A2[j+PacketSize]);
//             A13 = ei_ploadu(&A3[j+PacketSize]);
//
//             A00 = ei_pmul(ptmp0, A00);
//             A01 = ei_pmul(ptmp1, A01);
//             A02 = ei_pmul(ptmp2, A02);
//             A03 = ei_pmul(ptmp3, A03);
//             A10 = ei_pmul(ptmp0, A10);
//             A11 = ei_pmul(ptmp1, A11);
//             A12 = ei_pmul(ptmp2, A12);
//             A13 = ei_pmul(ptmp3, A13);
//
//             A00 = ei_padd(A00,A01);
//             A02 = ei_padd(A02,A03);
//             A00 = ei_padd(A00,ei_pload(&X[j]));
//             A00 = ei_padd(A00,A02);
//             ei_pstore(&X[j],A00);
//
//             A10 = ei_padd(A10,A11);
//             A12 = ei_padd(A12,A13);
//             A10 = ei_padd(A10,ei_pload(&X[j+PacketSize]));
//             A10 = ei_padd(A10,A12);
//             ei_pstore(&X[j+PacketSize],A10);

            ei_pstore(&X[j],
              ei_padd(ei_pload(&X[j]),
                ei_padd(
                  ei_padd(ei_pmul(ptmp0,ei_pload(&A0[j])),ei_pmul(ptmp1,ei_ploadu(&A1[j]))),
                  ei_padd(ei_pmul(ptmp2,ei_ploadu(&A2[j])),ei_pmul(ptmp3,ei_ploadu(&A3[j]))) )));

            ei_pstore(&X[j+PacketSize],
              ei_padd(ei_pload(&X[j+PacketSize]),
                ei_padd(
                  ei_padd(ei_pmul(ptmp0,ei_pload(&A0[j+PacketSize])),ei_pmul(ptmp1,ei_ploadu(&A1[j+PacketSize]))),
                  ei_padd(ei_pmul(ptmp2,ei_ploadu(&A2[j+PacketSize])),ei_pmul(ptmp3,ei_ploadu(&A3[j+PacketSize]))) )));
          }
          for (int j = ANP;j<AN;j+=PacketSize)
            ei_pstore(&X[j],
              ei_padd(ei_pload(&X[j]),
                ei_padd(
                  ei_padd(ei_pmul(ptmp0,ei_pload(&A0[j])),ei_pmul(ptmp1,ei_ploadu(&A1[j]))),
                  ei_padd(ei_pmul(ptmp2,ei_ploadu(&A2[j])),ei_pmul(ptmp3,ei_ploadu(&A3[j]))) )));
      }
      // process remaining scalars
      for (int j=AN;j<N;j++)
        X[j] += ei_pfirst(ptmp0) * A0[j] + ei_pfirst(ptmp1) * A1[j] + ei_pfirst(ptmp2) * A2[j] + ei_pfirst(ptmp3) * A3[j];
    }
    for (int i=bound;i<N;i++)
    {
      real tmp0 = B[i];
      Packet ptmp0 = ei_pset1(tmp0);
      int iN0 = i*N;
      if (AN>0)
      {
        bool aligned0 = (iN0 % PacketSize) == 0;
        if (aligned0)
          for (int j = 0;j<AN;j+=PacketSize)
            ei_pstore(&X[j], ei_padd(ei_pmul(ptmp0,ei_pload(&A[j+iN0])),ei_pload(&X[j])));
        else
          for (int j = 0;j<AN;j+=PacketSize)
            ei_pstore(&X[j], ei_padd(ei_pmul(ptmp0,ei_ploadu(&A[j+iN0])),ei_pload(&X[j])));
      }
      // process remaining scalars
      for (int j=AN;j<N;j++)
        X[j] += tmp0 * A[j+iN0];
    }
    asm("#end matrix_vector_product");
  }

  static inline void symv(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
  {

//     int AN = (N/PacketSize)*PacketSize;
//     int ANP = (AN/(2*PacketSize))*2*PacketSize;
//     int bound = (N/4)*4;
    for (int i=0;i<N;i++)
      X[i] = 0;

    int bound = std::max(0,N-8) & 0xfffffffE;

    for (int j=0;j<bound;j+=2)
    {
      register real* __restrict__ A0 = A + j*N;
      register real* __restrict__ A1 = A + (j+1)*N;

      real t0 = B[j];
      Packet ptmp0 = ei_pset1(t0);
      real t1 = B[j+1];
      Packet ptmp1 = ei_pset1(t1);

      real t2 = 0;
      Packet ptmp2 = ei_pset1(t2);
      real t3 = 0;
      Packet ptmp3 = ei_pset1(t3);

      int starti = j+2;
      int alignedEnd = starti;
      int alignedStart = (starti) + ei_first_aligned(&X[starti], N-starti);
      alignedEnd = alignedStart + ((N-alignedStart)/(PacketSize))*(PacketSize);

      X[j]   += t0 * A0[j];
      X[j+1] += t1 * A1[j];

      X[j+1] += t0 * A0[j+1];
      t2 += A0[j+1] * B[j+1];

//       alignedStart = alignedEnd;
      for (int i=starti; i<alignedStart; ++i) {
        X[i] += t0 * A0[i] + t1 * A1[i];
        t2 += A0[i] * B[i];
        t3 += A1[i] * B[i];
      }
      asm("#begin symv");
      for (size_t i=alignedStart; i<alignedEnd; i+=PacketSize) {
        Packet A0i = ei_ploadu(&A0[i]);
        Packet A1i = ei_ploadu(&A1[i]);
//         Packet A0i1 = ei_ploadu(&A0[i+PacketSize]);
        Packet Xi = ei_pload(&X[i]);
        Packet Bi = ei_pload/*u*/(&B[i]);
//         Packet Xi1 = ei_pload(&X[i+PacketSize]);
//         Packet Bi1 = ei_pload/*u*/(&B[i+PacketSize]);
        Xi = ei_padd(ei_padd(Xi, ei_pmul(ptmp0, A0i)), ei_pmul(ptmp1, A1i));
        ptmp2 = ei_padd(ptmp2, ei_pmul(A0i, Bi));
        ptmp3 = ei_padd(ptmp3, ei_pmul(A1i, Bi));
//         Xi1 = ei_padd(Xi1, ei_pmul(ptmp1, A0i1));
//         ptmp2 = ei_padd(ptmp2, ei_pmul(A0i1, Bi1));
//
        ei_pstore(&X[i],Xi);
//         ei_pstore(&X[i+PacketSize],Xi1);
//         asm(
//           "prefetchnta   64(%[A0],%[i],4)   \n\t"
//           //"movups     (%[A0],%[i],4), %%xmm8  \n\t"
//           "movsd       (%[A0],%[i],4), %%xmm8  \n\t"
//           "movhps     8(%[A0],%[i],4), %%xmm8  \n\t"
// //           "movups   16(%[A0],%[i],4), %%xmm9  \n\t"
// //           "movups   64(%[A0],%[i],4), %%xmm15  \n\t"
//           "movaps     (%[B], %[i],4), %%xmm12 \n\t"
// //           "movaps   16(%[B], %[i],4), %%xmm13 \n\t"
//           "movaps     (%[X], %[i],4), %%xmm10 \n\t"
// //           "movaps   16(%[X], %[i],4), %%xmm11 \n\t"
//
//           "mulps %%xmm8, %%xmm12  \n\t"
// //           "mulps %%xmm9, %%xmm13  \n\t"
//
//           "mulps %[ptmp1], %%xmm8  \n\t"
//           "addps %%xmm12, %[ptmp2]  \n\t"
//           "addps %%xmm8, %%xmm10  \n\t"
//
//
//
//
// //           "mulps %[ptmp1], %%xmm9  \n\t"
//
// //           "addps %%xmm9, %%xmm11  \n\t"
// //           "addps %%xmm13, %[ptmp2]  \n\t"
//
//           "movaps %%xmm10,   (%[X],%[i],4) \n\t"
// //           "movaps %%xmm11, 16(%[X],%[i],4) \n\t"
//           :
//           : [X] "r" (X), [i] "r" (i), [A0] "r" (A0),
//             [B] "r" (B),
//             [ptmp1] "x" (ptmp1),
//             [ptmp2] "x" (ptmp2)
//           : "%xmm8", "%xmm9", "%xmm10", "%xmm11", "%xmm12", "%xmm13", "%xmm15");
      }
      asm("#end symv");
      for (int i=alignedEnd; i<N; i++) {
        X[i] += t0 * A0[i] + t1 * A1[i];
        t2 += A0[i] * B[i];
        t3 += A1[i] * B[i];
      }

      X[j]   += t2 + ei_predux(ptmp2);
      X[j+1] += t3 + ei_predux(ptmp3);
    }
    for (int j=bound;j<N;j++)
    {
      register real* __restrict__ A0 = A + j*N;

      real t1 = B[j];
      real t2 = 0;
      X[j] += t1 * A0[j];
      for (int i=j+1; i<N; i+=PacketSize) {
        X[i] += t1 * A0[i];
        t2 += A0[i] * B[i];
      }
      X[j] += t2;
    }

  }

//   static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
//   {
//     asm("#begin matrix_vector_product");
//     int AN = (N/PacketSize)*PacketSize;
//     int ANP = (AN/(2*PacketSize))*2*PacketSize;
//     int bound = (N/4)*4;
//     for (int i=0;i<N;i++)
//       X[i] = 0;
//
//     for (int i=0;i<bound;i+=4)
//     {
//       real tmp0 = B[i];
//       Packet ptmp0 = ei_pset1(tmp0);
//       real tmp1 = B[i+1];
//       Packet ptmp1 = ei_pset1(tmp1);
//       real tmp2 = B[i+2];
//       Packet ptmp2 = ei_pset1(tmp2);
//       real tmp3 = B[i+3];
//       Packet ptmp3 = ei_pset1(tmp3);
//       int iN0 = i*N;
//       int iN1 = (i+1)*N;
//       int iN2 = (i+2)*N;
//       int iN3 = (i+3)*N;
//       if (AN>0)
//       {
// //         int aligned0 = (iN0 % PacketSize);
//         int aligned1 = (iN1 % PacketSize);
//
//         if (aligned1==0)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             ei_pstore(&X[j],
//               ei_padd(ei_pload(&X[j]),
//                 ei_padd(
//                   ei_padd(ei_pmul(ptmp0,ei_pload(&A[j+iN0])),ei_pmul(ptmp1,ei_pload(&A[j+iN1]))),
//                   ei_padd(ei_pmul(ptmp2,ei_pload(&A[j+iN2])),ei_pmul(ptmp3,ei_pload(&A[j+iN3]))) )));
//           }
//         }
//         else if (aligned1==2)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             ei_pstore(&X[j],
//               ei_padd(ei_pload(&X[j]),
//                 ei_padd(
//                   ei_padd(ei_pmul(ptmp0,ei_pload(&A[j+iN0])),ei_pmul(ptmp1,ei_ploadu(&A[j+iN1]))),
//                   ei_padd(ei_pmul(ptmp2,ei_pload(&A[j+iN2])),ei_pmul(ptmp3,ei_ploadu(&A[j+iN3]))) )));
//           }
//         }
//         else
//         {
//           for (int j = 0;j<ANP;j+=2*PacketSize)
//           {
//             ei_pstore(&X[j],
//               ei_padd(ei_pload(&X[j]),
//                 ei_padd(
//                   ei_padd(ei_pmul(ptmp0,ei_pload(&A[j+iN0])),ei_pmul(ptmp1,ei_ploadu(&A[j+iN1]))),
//                   ei_padd(ei_pmul(ptmp2,ei_ploadu(&A[j+iN2])),ei_pmul(ptmp3,ei_ploadu(&A[j+iN3]))) )));
//
//             ei_pstore(&X[j+PacketSize],
//               ei_padd(ei_pload(&X[j+PacketSize]),
//                 ei_padd(
//                   ei_padd(ei_pmul(ptmp0,ei_pload(&A[j+PacketSize+iN0])),ei_pmul(ptmp1,ei_ploadu(&A[j+PacketSize+iN1]))),
//                   ei_padd(ei_pmul(ptmp2,ei_ploadu(&A[j+PacketSize+iN2])),ei_pmul(ptmp3,ei_ploadu(&A[j+PacketSize+iN3]))) )));
//
// //             ei_pstore(&X[j+2*PacketSize],
// //               ei_padd(ei_pload(&X[j+2*PacketSize]),
// //                 ei_padd(
// //                   ei_padd(ei_pmul(ptmp0,ei_pload(&A[j+2*PacketSize+iN0])),ei_pmul(ptmp1,ei_ploadu(&A[j+2*PacketSize+iN1]))),
// //                   ei_padd(ei_pmul(ptmp2,ei_ploadu(&A[j+2*PacketSize+iN2])),ei_pmul(ptmp3,ei_ploadu(&A[j+2*PacketSize+iN3]))) )));
// //
// //             ei_pstore(&X[j+3*PacketSize],
// //               ei_padd(ei_pload(&X[j+3*PacketSize]),
// //                 ei_padd(
// //                   ei_padd(ei_pmul(ptmp0,ei_pload(&A[j+3*PacketSize+iN0])),ei_pmul(ptmp1,ei_ploadu(&A[j+3*PacketSize+iN1]))),
// //                   ei_padd(ei_pmul(ptmp2,ei_ploadu(&A[j+3*PacketSize+iN2])),ei_pmul(ptmp3,ei_ploadu(&A[j+3*PacketSize+iN3]))) )));
//
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             ei_pstore(&X[j],
//               ei_padd(ei_pload(&X[j]),
//                 ei_padd(
//                   ei_padd(ei_pmul(ptmp0,ei_ploadu(&A[j+iN0])),ei_pmul(ptmp1,ei_ploadu(&A[j+iN1]))),
//                   ei_padd(ei_pmul(ptmp2,ei_ploadu(&A[j+iN2])),ei_pmul(ptmp3,ei_ploadu(&A[j+iN3]))) )));
//         }
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp0 * A[j+iN0] + tmp1 * A[j+iN1] + tmp2 * A[j+iN2] + tmp3 * A[j+iN3];
//     }
//     for (int i=bound;i<N;i++)
//     {
//       real tmp0 = B[i];
//       Packet ptmp0 = ei_pset1(tmp0);
//       int iN0 = i*N;
//       if (AN>0)
//       {
//         bool aligned0 = (iN0 % PacketSize) == 0;
//         if (aligned0)
//           for (int j = 0;j<AN;j+=PacketSize)
//             ei_pstore(&X[j], ei_padd(ei_pmul(ptmp0,ei_pload(&A[j+iN0])),ei_pload(&X[j])));
//         else
//           for (int j = 0;j<AN;j+=PacketSize)
//             ei_pstore(&X[j], ei_padd(ei_pmul(ptmp0,ei_ploadu(&A[j+iN0])),ei_pload(&X[j])));
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp0 * A[j+iN0];
//     }
//     asm("#end matrix_vector_product");
//   }

//   static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
//   {
//     asm("#begin matrix_vector_product");
//     int AN = (N/PacketSize)*PacketSize;
//     for (int i=0;i<N;i++)
//       X[i] = 0;
//
//     for (int i=0;i<N;i+=2)
//     {
//       real tmp0 = B[i];
//       Packet ptmp0 = ei_pset1(tmp0);
//       real tmp1 = B[i+1];
//       Packet ptmp1 = ei_pset1(tmp1);
//       int iN0 = i*N;
//       int iN1 = (i+1)*N;
//       if (AN>0)
//       {
//         bool aligned0 = (iN0 % PacketSize) == 0;
//         bool aligned1 = (iN1 % PacketSize) == 0;
//
//         if (aligned0 && aligned1)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             ei_pstore(&X[j],
//               ei_padd(ei_pmul(ptmp0,ei_pload(&A[j+iN0])),
//               ei_padd(ei_pmul(ptmp1,ei_pload(&A[j+iN1])),ei_pload(&X[j]))));
//           }
//         }
//         else if (aligned0)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             ei_pstore(&X[j],
//               ei_padd(ei_pmul(ptmp0,ei_pload(&A[j+iN0])),
//               ei_padd(ei_pmul(ptmp1,ei_ploadu(&A[j+iN1])),ei_pload(&X[j]))));
//           }
//         }
//         else if (aligned1)
//         {
//           for (int j = 0;j<AN;j+=PacketSize)
//           {
//             ei_pstore(&X[j],
//               ei_padd(ei_pmul(ptmp0,ei_ploadu(&A[j+iN0])),
//               ei_padd(ei_pmul(ptmp1,ei_pload(&A[j+iN1])),ei_pload(&X[j]))));
//           }
//         }
//         else
//         {
//           int ANP = (AN/(4*PacketSize))*4*PacketSize;
//           for (int j = 0;j<ANP;j+=4*PacketSize)
//           {
//             ei_pstore(&X[j],
//               ei_padd(ei_pmul(ptmp0,ei_ploadu(&A[j+iN0])),
//               ei_padd(ei_pmul(ptmp1,ei_ploadu(&A[j+iN1])),ei_pload(&X[j]))));
//
//             ei_pstore(&X[j+PacketSize],
//               ei_padd(ei_pmul(ptmp0,ei_ploadu(&A[j+PacketSize+iN0])),
//               ei_padd(ei_pmul(ptmp1,ei_ploadu(&A[j+PacketSize+iN1])),ei_pload(&X[j+PacketSize]))));
//
//             ei_pstore(&X[j+2*PacketSize],
//               ei_padd(ei_pmul(ptmp0,ei_ploadu(&A[j+2*PacketSize+iN0])),
//               ei_padd(ei_pmul(ptmp1,ei_ploadu(&A[j+2*PacketSize+iN1])),ei_pload(&X[j+2*PacketSize]))));
//
//             ei_pstore(&X[j+3*PacketSize],
//               ei_padd(ei_pmul(ptmp0,ei_ploadu(&A[j+3*PacketSize+iN0])),
//               ei_padd(ei_pmul(ptmp1,ei_ploadu(&A[j+3*PacketSize+iN1])),ei_pload(&X[j+3*PacketSize]))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             ei_pstore(&X[j],
//               ei_padd(ei_pmul(ptmp0,ei_ploadu(&A[j+iN0])),
//               ei_padd(ei_pmul(ptmp1,ei_ploadu(&A[j+iN1])),ei_pload(&X[j]))));
//         }
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp0 * A[j+iN0] + tmp1 * A[j+iN1];
//     }
//     int remaining = (N/2)*2;
//     for (int i=remaining;i<N;i++)
//     {
//       real tmp0 = B[i];
//       Packet ptmp0 = ei_pset1(tmp0);
//       int iN0 = i*N;
//       if (AN>0)
//       {
//         bool aligned0 = (iN0 % PacketSize) == 0;
//         if (aligned0)
//           for (int j = 0;j<AN;j+=PacketSize)
//             ei_pstore(&X[j], ei_padd(ei_pmul(ptmp0,ei_pload(&A[j+iN0])),ei_pload(&X[j])));
//         else
//           for (int j = 0;j<AN;j+=PacketSize)
//             ei_pstore(&X[j], ei_padd(ei_pmul(ptmp0,ei_ploadu(&A[j+iN0])),ei_pload(&X[j])));
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp0 * A[j+iN0];
//     }
//     asm("#end matrix_vector_product");
//   }

//   static inline void matrix_vector_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
//   {
//     asm("#begin matrix_vector_product");
//     int AN = (N/PacketSize)*PacketSize;
//     for (int i=0;i<N;i++)
//       X[i] = 0;
//     for (int i=0;i<N;i++)
//     {
//       real tmp = B[i];
//       Packet ptmp = ei_pset1(tmp);
//       int iN = i*N;
//       if (AN>0)
//       {
//         bool aligned = (iN % PacketSize) == 0;
//         if (aligned)
//         {
//           #ifdef PEELING
//           Packet A0, A1, A2, X0, X1, X2;
//           int ANP = (AN/(8*PacketSize))*8*PacketSize;
//           for (int j = 0;j<ANP;j+=PacketSize*8)
//           {
//             A0 = ei_pload(&A[j+iN]);
//             X0 = ei_pload(&X[j]);
//             A1 = ei_pload(&A[j+PacketSize+iN]);
//             X1 = ei_pload(&X[j+PacketSize]);
//             A2 = ei_pload(&A[j+2*PacketSize+iN]);
//             X2 = ei_pload(&X[j+2*PacketSize]);
//             ei_pstore(&X[j], ei_padd(X0, ei_pmul(ptmp,A0)));
//             A0 = ei_pload(&A[j+3*PacketSize+iN]);
//             X0 = ei_pload(&X[j+3*PacketSize]);
//             ei_pstore(&X[j+PacketSize], ei_padd(ei_pload(&X1), ei_pmul(ptmp,A1)));
//             A1 = ei_pload(&A[j+4*PacketSize+iN]);
//             X1 = ei_pload(&X[j+4*PacketSize]);
//             ei_pstore(&X[j+2*PacketSize], ei_padd(ei_pload(&X2), ei_pmul(ptmp,A2)));
//             A2 = ei_pload(&A[j+5*PacketSize+iN]);
//             X2 = ei_pload(&X[j+5*PacketSize]);
//             ei_pstore(&X[j+3*PacketSize], ei_padd(ei_pload(&X0), ei_pmul(ptmp,A0)));
//             A0 = ei_pload(&A[j+6*PacketSize+iN]);
//             X0 = ei_pload(&X[j+6*PacketSize]);
//             ei_pstore(&X[j+4*PacketSize], ei_padd(ei_pload(&X1), ei_pmul(ptmp,A1)));
//             A1 = ei_pload(&A[j+7*PacketSize+iN]);
//             X1 = ei_pload(&X[j+7*PacketSize]);
//             ei_pstore(&X[j+5*PacketSize], ei_padd(ei_pload(&X2), ei_pmul(ptmp,A2)));
//             ei_pstore(&X[j+6*PacketSize], ei_padd(ei_pload(&X0), ei_pmul(ptmp,A0)));
//             ei_pstore(&X[j+7*PacketSize], ei_padd(ei_pload(&X1), ei_pmul(ptmp,A1)));
// //
// //             ei_pstore(&X[j], ei_padd(ei_pload(&X[j]), ei_pmul(ptmp,ei_pload(&A[j+iN]))));
// //             ei_pstore(&X[j+PacketSize], ei_padd(ei_pload(&X[j+PacketSize]), ei_pmul(ptmp,ei_pload(&A[j+PacketSize+iN]))));
// //             ei_pstore(&X[j+2*PacketSize], ei_padd(ei_pload(&X[j+2*PacketSize]), ei_pmul(ptmp,ei_pload(&A[j+2*PacketSize+iN]))));
// //             ei_pstore(&X[j+3*PacketSize], ei_padd(ei_pload(&X[j+3*PacketSize]), ei_pmul(ptmp,ei_pload(&A[j+3*PacketSize+iN]))));
// //             ei_pstore(&X[j+4*PacketSize], ei_padd(ei_pload(&X[j+4*PacketSize]), ei_pmul(ptmp,ei_pload(&A[j+4*PacketSize+iN]))));
// //             ei_pstore(&X[j+5*PacketSize], ei_padd(ei_pload(&X[j+5*PacketSize]), ei_pmul(ptmp,ei_pload(&A[j+5*PacketSize+iN]))));
// //             ei_pstore(&X[j+6*PacketSize], ei_padd(ei_pload(&X[j+6*PacketSize]), ei_pmul(ptmp,ei_pload(&A[j+6*PacketSize+iN]))));
// //             ei_pstore(&X[j+7*PacketSize], ei_padd(ei_pload(&X[j+7*PacketSize]), ei_pmul(ptmp,ei_pload(&A[j+7*PacketSize+iN]))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             ei_pstore(&X[j], ei_padd(ei_pload(&X[j]), ei_pmul(ptmp,ei_pload(&A[j+iN]))));
//           #else
//           for (int j = 0;j<AN;j+=PacketSize)
//             ei_pstore(&X[j], ei_padd(ei_pload(&X[j]), ei_pmul(ptmp,ei_pload(&A[j+iN]))));
//           #endif
//         }
//         else
//         {
//           #ifdef PEELING
//           int ANP = (AN/(8*PacketSize))*8*PacketSize;
//           for (int j = 0;j<ANP;j+=PacketSize*8)
//           {
//             ei_pstore(&X[j], ei_padd(ei_pload(&X[j]), ei_pmul(ptmp,ei_ploadu(&A[j+iN]))));
//             ei_pstore(&X[j+PacketSize], ei_padd(ei_pload(&X[j+PacketSize]), ei_pmul(ptmp,ei_ploadu(&A[j+PacketSize+iN]))));
//             ei_pstore(&X[j+2*PacketSize], ei_padd(ei_pload(&X[j+2*PacketSize]), ei_pmul(ptmp,ei_ploadu(&A[j+2*PacketSize+iN]))));
//             ei_pstore(&X[j+3*PacketSize], ei_padd(ei_pload(&X[j+3*PacketSize]), ei_pmul(ptmp,ei_ploadu(&A[j+3*PacketSize+iN]))));
//             ei_pstore(&X[j+4*PacketSize], ei_padd(ei_pload(&X[j+4*PacketSize]), ei_pmul(ptmp,ei_ploadu(&A[j+4*PacketSize+iN]))));
//             ei_pstore(&X[j+5*PacketSize], ei_padd(ei_pload(&X[j+5*PacketSize]), ei_pmul(ptmp,ei_ploadu(&A[j+5*PacketSize+iN]))));
//             ei_pstore(&X[j+6*PacketSize], ei_padd(ei_pload(&X[j+6*PacketSize]), ei_pmul(ptmp,ei_ploadu(&A[j+6*PacketSize+iN]))));
//             ei_pstore(&X[j+7*PacketSize], ei_padd(ei_pload(&X[j+7*PacketSize]), ei_pmul(ptmp,ei_ploadu(&A[j+7*PacketSize+iN]))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             ei_pstore(&X[j], ei_padd(ei_pload(&X[j]), ei_pmul(ptmp,ei_ploadu(&A[j+iN]))));
//           #else
//           for (int j = 0;j<AN;j+=PacketSize)
//             ei_pstore(&X[j], ei_padd(ei_pload(&X[j]), ei_pmul(ptmp,ei_ploadu(&A[j+iN]))));
//           #endif
//         }
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         X[j] += tmp * A[j+iN];
//     }
//     asm("#end matrix_vector_product");
//   }

    static inline void atv_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
  {
    int AN = (N/PacketSize)*PacketSize;
    int bound = (N/4)*4;
    for (int i=0;i<bound;i+=4)
    {
      real tmp0 = 0;
      Packet ptmp0 = ei_pset1(real(0));
      real tmp1 = 0;
      Packet ptmp1 = ei_pset1(real(0));
      real tmp2 = 0;
      Packet ptmp2 = ei_pset1(real(0));
      real tmp3 = 0;
      Packet ptmp3 = ei_pset1(real(0));
      int iN0 = i*N;
      int iN1 = (i+1)*N;
      int iN2 = (i+2)*N;
      int iN3 = (i+3)*N;
      if (AN>0)
      {
        int align1 = (iN1 % PacketSize);
        if (align1==0)
        {
          for (int j = 0;j<AN;j+=PacketSize)
          {
            Packet b = ei_pload(&B[j]);
            ptmp0 = ei_padd(ptmp0, ei_pmul(b, ei_pload(&A[j+iN0])));
            ptmp1 = ei_padd(ptmp1, ei_pmul(b, ei_pload(&A[j+iN1])));
            ptmp2 = ei_padd(ptmp2, ei_pmul(b, ei_pload(&A[j+iN2])));
            ptmp3 = ei_padd(ptmp3, ei_pmul(b, ei_pload(&A[j+iN3])));
          }
        }
        else if (align1==2)
        {
          for (int j = 0;j<AN;j+=PacketSize)
          {
            Packet b = ei_pload(&B[j]);
            ptmp0 = ei_padd(ptmp0, ei_pmul(b, ei_pload(&A[j+iN0])));
            ptmp1 = ei_padd(ptmp1, ei_pmul(b, ei_ploadu(&A[j+iN1])));
            ptmp2 = ei_padd(ptmp2, ei_pmul(b, ei_pload(&A[j+iN2])));
            ptmp3 = ei_padd(ptmp3, ei_pmul(b, ei_ploadu(&A[j+iN3])));
          }
        }
        else
        {
          for (int j = 0;j<AN;j+=PacketSize)
          {
            Packet b = ei_pload(&B[j]);
            ptmp0 = ei_padd(ptmp0, ei_pmul(b, ei_pload(&A[j+iN0])));
            ptmp1 = ei_padd(ptmp1, ei_pmul(b, ei_ploadu(&A[j+iN1])));
            ptmp2 = ei_padd(ptmp2, ei_pmul(b, ei_ploadu(&A[j+iN2])));
            ptmp3 = ei_padd(ptmp3, ei_pmul(b, ei_ploadu(&A[j+iN3])));
          }
        }
        tmp0 = ei_predux(ptmp0);
        tmp1 = ei_predux(ptmp1);
        tmp2 = ei_predux(ptmp2);
        tmp3 = ei_predux(ptmp3);
      }
      // process remaining scalars
      for (int j=AN;j<N;j++)
      {
        tmp0 += B[j] * A[j+iN0];
        tmp1 += B[j] * A[j+iN1];
        tmp2 += B[j] * A[j+iN2];
        tmp3 += B[j] * A[j+iN3];
      }
      X[i+0] = tmp0;
      X[i+1] = tmp1;
      X[i+2] = tmp2;
      X[i+3] = tmp3;
    }

    for (int i=bound;i<N;i++)
    {
      real tmp0 = 0;
      Packet ptmp0 = ei_pset1(real(0));
      int iN0 = i*N;
      if (AN>0)
      {
        if (iN0 % PacketSize==0)
          for (int j = 0;j<AN;j+=PacketSize)
            ptmp0 = ei_padd(ptmp0, ei_pmul(ei_pload(&B[j]), ei_pload(&A[j+iN0])));
        else
          for (int j = 0;j<AN;j+=PacketSize)
            ptmp0 = ei_padd(ptmp0, ei_pmul(ei_pload(&B[j]), ei_ploadu(&A[j+iN0])));
        tmp0 = ei_predux(ptmp0);
      }
      // process remaining scalars
      for (int j=AN;j<N;j++)
        tmp0 += B[j] * A[j+iN0];
      X[i+0] = tmp0;
    }
  }

//   static inline void atv_product(const gene_matrix & A, const gene_vector & B, gene_vector & X, int N)
//   {
//     int AN = (N/PacketSize)*PacketSize;
//     for (int i=0;i<N;i++)
//       X[i] = 0;
//     for (int i=0;i<N;i++)
//     {
//       real tmp = 0;
//       Packet ptmp = ei_pset1(real(0));
//       int iN = i*N;
//       if (AN>0)
//       {
//         bool aligned = (iN % PacketSize) == 0;
//         if (aligned)
//         {
//           #ifdef PEELING
//           int ANP = (AN/(8*PacketSize))*8*PacketSize;
//           for (int j = 0;j<ANP;j+=PacketSize*8)
//           {
//             ptmp =
//               ei_padd(ei_pmul(ei_pload(&B[j]), ei_pload(&A[j+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+PacketSize]), ei_pload(&A[j+PacketSize+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+2*PacketSize]), ei_pload(&A[j+2*PacketSize+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+3*PacketSize]), ei_pload(&A[j+3*PacketSize+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+4*PacketSize]), ei_pload(&A[j+4*PacketSize+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+5*PacketSize]), ei_pload(&A[j+5*PacketSize+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+6*PacketSize]), ei_pload(&A[j+6*PacketSize+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+7*PacketSize]), ei_pload(&A[j+7*PacketSize+iN])),
//               ptmp))))))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             ptmp = ei_padd(ptmp, ei_pmul(ei_pload(&B[j]), ei_pload(&A[j+iN])));
//           #else
//           for (int j = 0;j<AN;j+=PacketSize)
//             ptmp = ei_padd(ptmp, ei_pmul(ei_pload(&B[j]), ei_pload(&A[j+iN])));
//           #endif
//         }
//         else
//         {
//           #ifdef PEELING
//           int ANP = (AN/(8*PacketSize))*8*PacketSize;
//           for (int j = 0;j<ANP;j+=PacketSize*8)
//           {
//             ptmp =
//               ei_padd(ei_pmul(ei_pload(&B[j]), ei_ploadu(&A[j+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+PacketSize]), ei_ploadu(&A[j+PacketSize+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+2*PacketSize]), ei_ploadu(&A[j+2*PacketSize+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+3*PacketSize]), ei_ploadu(&A[j+3*PacketSize+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+4*PacketSize]), ei_ploadu(&A[j+4*PacketSize+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+5*PacketSize]), ei_ploadu(&A[j+5*PacketSize+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+6*PacketSize]), ei_ploadu(&A[j+6*PacketSize+iN])),
//               ei_padd(ei_pmul(ei_pload(&B[j+7*PacketSize]), ei_ploadu(&A[j+7*PacketSize+iN])),
//               ptmp))))))));
//           }
//           for (int j = ANP;j<AN;j+=PacketSize)
//             ptmp = ei_padd(ptmp, ei_pmul(ei_pload(&B[j]), ei_ploadu(&A[j+iN])));
//           #else
//           for (int j = 0;j<AN;j+=PacketSize)
//             ptmp = ei_padd(ptmp, ei_pmul(ei_pload(&B[j]), ei_ploadu(&A[j+iN])));
//           #endif
//         }
//         tmp = ei_predux(ptmp);
//       }
//       // process remaining scalars
//       for (int j=AN;j<N;j++)
//         tmp += B[j] * A[j+iN];
//       X[i] = tmp;
//     }
//   }

  static inline void axpy(real coef, const gene_vector & X, gene_vector & Y, int N){
    int AN = (N/PacketSize)*PacketSize;
    if (AN>0)
    {
      Packet pcoef = ei_pset1(coef);
      #ifdef PEELING
      const int peelSize = 3;
      int ANP = (AN/(peelSize*PacketSize))*peelSize*PacketSize;
      float* X1 = X + PacketSize;
      float* Y1 = Y + PacketSize;
      float* X2 = X + 2*PacketSize;
      float* Y2 = Y + 2*PacketSize;
      Packet x0,x1,x2,y0,y1,y2;
      for (int j = 0;j<ANP;j+=PacketSize*peelSize)
      {
        x0 = ei_pload(X+j);
        x1 = ei_pload(X1+j);
        x2 = ei_pload(X2+j);

        y0 = ei_pload(Y+j);
        y1 = ei_pload(Y1+j);
        y2 = ei_pload(Y2+j);

        y0 = ei_pmadd(pcoef, x0, y0);
        y1 = ei_pmadd(pcoef, x1, y1);
        y2 = ei_pmadd(pcoef, x2, y2);

        ei_pstore(Y+j,  y0);
        ei_pstore(Y1+j, y1);
        ei_pstore(Y2+j, y2);
//         ei_pstore(&Y[j+2*PacketSize], ei_padd(ei_pload(&Y[j+2*PacketSize]), ei_pmul(pcoef,ei_pload(&X[j+2*PacketSize]))));
//         ei_pstore(&Y[j+3*PacketSize], ei_padd(ei_pload(&Y[j+3*PacketSize]), ei_pmul(pcoef,ei_pload(&X[j+3*PacketSize]))));
//         ei_pstore(&Y[j+4*PacketSize], ei_padd(ei_pload(&Y[j+4*PacketSize]), ei_pmul(pcoef,ei_pload(&X[j+4*PacketSize]))));
//         ei_pstore(&Y[j+5*PacketSize], ei_padd(ei_pload(&Y[j+5*PacketSize]), ei_pmul(pcoef,ei_pload(&X[j+5*PacketSize]))));
//         ei_pstore(&Y[j+6*PacketSize], ei_padd(ei_pload(&Y[j+6*PacketSize]), ei_pmul(pcoef,ei_pload(&X[j+6*PacketSize]))));
//         ei_pstore(&Y[j+7*PacketSize], ei_padd(ei_pload(&Y[j+7*PacketSize]), ei_pmul(pcoef,ei_pload(&X[j+7*PacketSize]))));
      }
      for (int j = ANP;j<AN;j+=PacketSize)
        ei_pstore(&Y[j], ei_padd(ei_pload(&Y[j]), ei_pmul(pcoef,ei_pload(&X[j]))));
      #else
      for (int j = 0;j<AN;j+=PacketSize)
        ei_pstore(&Y[j], ei_padd(ei_pload(&Y[j]), ei_pmul(pcoef,ei_pload(&X[j]))));
      #endif
    }
    // process remaining scalars
    for (int i=AN;i<N;i++)
      Y[i] += coef * X[i];
  }

};

#endif


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/function/simd/bitwise_cast.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_gez.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(bitfloating_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
      using result = bd::as_floating_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        A0 s = bitwise_cast<A0>(Signmask<result>());
        return bitwise_cast<result>(if_else(is_gez(a0) , a0, s-a0));
      }
   };

   BOOST_DISPATCH_OVERLOAD(bitfloating_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::unsigned_<A0>, X>
                          )
   {
      using result = bd::as_floating_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return simd::bitwise_cast<result>(a0);
      }
   };

} } }

#endif


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include "quick.h"
#include"graph.h"
#include"util.h"
#include"map.h"
using namespace std;
using namespace Eigen;

int main()
{
    //initialize
	int size = 60000000;
	//60000000
	MatrixXd keys_logits(size, 2);
	vector<float> *data = Initialize(size, 2);
	cout << "data_size:" << data->size() << endl;
	MatrixXd x_in = ArrayXXd::Zero(data->size(), 1);

	for (int i = 0; i < data->size(); ++i)
	{
		x_in(i) = (*data)[i];
	}
	InitResut initResult;
	vector<double> *finalResult = new vector<double>();

    //Prediction
    cout<<"Prediction start:\n"<<endl;
	steady_clock::time_point sortStart = steady_clock::now();
	steady_clock::time_point predictStart = steady_clock::now();
	graph_(&x_in, size, &keys_logits);
	steady_clock::time_point predictEnd = steady_clock::now();
    cout<<"Prediction end\n"<<endl;

	duration<double, std::milli> *timePredicte = new duration<double, std::milli>(predictEnd -predictStart);
	auto max = keys_logits.colwise().maxCoeff();

    //To bucket
	initResult.sortedList = new vector<double>(int(round(max[1])*3), FLT_MAX);
	initResult.waitedList = new vector<double>();
    cout<<"To bucket start:\n"<<endl;
	steady_clock::time_point sortBucketStart = steady_clock::now();
	ToBucket(&keys_logits, &initResult);
	steady_clock::time_point sortBucketEnd = steady_clock::now();
    cout<<"To bucket end\n"<<endl;

    // Quick sort & merge
    cout<<"Quick sort & merge start:\n"<<endl;

	steady_clock::time_point quicksortStart = steady_clock::now();
	QuickSort(initResult.waitedList, 0, initResult.waitedList->size() - 1);
	steady_clock::time_point quicksortEnd = steady_clock::now();

	steady_clock::time_point mergeStart = steady_clock::now();
	Merge(&initResult, finalResult);
	steady_clock::time_point mergeEnd = steady_clock::now();
	steady_clock::time_point sortEnd = steady_clock::now();
    cout<<"Quick sort & merge start end\n"<<endl;

    //Statistic & check
    cout<<"Statistic & check:\n"<<endl;
	duration<double, std::milli> *timeSpanBucket = new duration<double, std::milli>(sortBucketEnd - sortBucketStart);
	duration<double, std::milli> *timeSpanQuicksort = new duration<double, std::milli>(quicksortEnd - quicksortStart);
	duration<double, std::milli> *timeSpanMerg = new duration<double, std::milli>(mergeEnd - mergeStart);
	duration<double, std::milli> *timeSpan1 = new duration<double, std::milli>(sortEnd - sortStart);

	cout << "consumming of predict:" << timePredicte->count() << " ms" << endl;
	cout << "consumming of bucket:" << timeSpanBucket->count() << " ms" << endl;
	cout << "consumming of quicksort:" << timeSpanQuicksort->count() << " ms" << endl;
	cout << "consumming of merge:" << timeSpanMerg->count() << " ms" << endl;
	cout << "consumming of sort:" << timeSpan1->count() << " ms" << endl;

    check(finalResult);
	// cout << "--------------------finalReuslt---------------------"<< endl;
	// for(int i=0;i<finalResult->size();++i)
	// {
	// 	cout<<(*finalResult)[i]<<endl;
	// }
	//	graph_();
    cout<<"Sort finish";
}


#ifndef plane_calibration_SRC_PLANES_HPP_
#define plane_calibration_SRC_PLANES_HPP_

#include <memory>
#include <map>
#include <utility>
#include <Eigen/Dense>

#include "calibration_parameters.hpp"
#include "plane_to_depth_image.hpp"

namespace plane_calibration
{
typedef std::shared_ptr<Eigen::MatrixXf> MatrixPlanePtr;

class Planes
{
public:
  Planes(const CalibrationParameters::Parameters& parameters, const PlaneToDepthImage& plane_to_depth);

  std::pair<MatrixPlanePtr, MatrixPlanePtr> getFittingXTiltPlanes(const double& angle, const double& deviation);
  std::pair<MatrixPlanePtr, MatrixPlanePtr> getFittingYTiltPlanes(const double& angle, const double& deviation);

protected:
  void makePlanes();
  Eigen::MatrixXf makePlane(const Eigen::Vector2d& angles);

  void addPlanePairs(const double& angle);
  void addPlanes(const double& angle);

  std::pair<MatrixPlanePtr, MatrixPlanePtr> getFittingTiltPlanes(const std::map<double, MatrixPlanePtr>& planes,
                                                                 const double& angle, const double& deviation);
  std::pair<double, double> getDeviationPlaneKeys(const double& angle, const double& deviation);

  int pair_count_;
  double max_deviation_;
  PlaneToDepthImage plane_to_depth_;

  Eigen::Translation3d translation_;
  Eigen::AngleAxisd base_rotation_;

  std::map<double, MatrixPlanePtr> x_planes_;
  std::map<double, MatrixPlanePtr> y_planes_;
};
typedef std::shared_ptr<Planes> PlanesPtr;

} /* end namespace */

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "grad.h"
#include <Eigen/Geometry>
#include <vector>

#include "PI.h"
#include "per_face_normals.h"
#include "volume.h"
#include "doublearea.h"

namespace igl {

namespace {

template <typename DerivedV, typename DerivedF>
IGL_INLINE void grad_tet(
  const Eigen::MatrixBase<DerivedV>&V,
  const Eigen::MatrixBase<DerivedF>&T,
  Eigen::SparseMatrix<typename DerivedV::Scalar> &G,
  bool uniform)
{
  using namespace Eigen;
  assert(T.cols() == 4);
  const int n = V.rows(); int m = T.rows();

  /*
      F = [ ...
      T(:,1) T(:,2) T(:,3); ...
      T(:,1) T(:,3) T(:,4); ...
      T(:,1) T(:,4) T(:,2); ...
      T(:,2) T(:,4) T(:,3)]; */
  MatrixXi F(4*m,3);
  for (int i = 0; i < m; i++) {
    F.row(0*m + i) << T(i,0), T(i,1), T(i,2);
    F.row(1*m + i) << T(i,0), T(i,2), T(i,3);
    F.row(2*m + i) << T(i,0), T(i,3), T(i,1);
    F.row(3*m + i) << T(i,1), T(i,3), T(i,2);
  }
  // compute volume of each tet
  Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> vol;
  igl::volume(V,T,vol);

  Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> A(F.rows());
  Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> N(F.rows(),3);
  if (!uniform) {
    // compute tetrahedron face normals
    igl::per_face_normals(V,F,N); int norm_rows = N.rows();
    for (int i = 0; i < norm_rows; i++)
      N.row(i) /= N.row(i).norm();
    igl::doublearea(V,F,A); A/=2.;
  } else {
    // Use a uniform tetrahedra as a reference, with the same volume as the original one:
    //
    // Use normals of the uniform tet (V = h*[0,0,0;1,0,0;0.5,sqrt(3)/2.,0;0.5,sqrt(3)/6.,sqrt(2)/sqrt(3)])
    //         0         0    1.0000
    //         0.8165   -0.4714   -0.3333
    //         0          0.9428   -0.3333
    //         -0.8165   -0.4714   -0.3333
    for (int i = 0; i < m; i++) {
      N.row(0*m+i) << 0,0,1;
      double a = sqrt(2)*std::cbrt(3*vol(i)); // area of a face in a uniform tet with volume = vol(i)
      A(0*m+i) = (pow(a,2)*sqrt(3))/4.;
    }
    for (int i = 0; i < m; i++) {
      N.row(1*m+i) << 0.8165,-0.4714,-0.3333;
      double a = sqrt(2)*std::cbrt(3*vol(i));
      A(1*m+i) = (pow(a,2)*sqrt(3))/4.;
    }
    for (int i = 0; i < m; i++) {
      N.row(2*m+i) << 0,0.9428,-0.3333;
      double a = sqrt(2)*std::cbrt(3*vol(i));
      A(2*m+i) = (pow(a,2)*sqrt(3))/4.;
    }
    for (int i = 0; i < m; i++) {
      N.row(3*m+i) << -0.8165,-0.4714,-0.3333;
      double a = sqrt(2)*std::cbrt(3*vol(i));
      A(3*m+i) = (pow(a,2)*sqrt(3))/4.;
    }

  }

  /*  G = sparse( ...
      [0*m + repmat(1:m,1,4) ...
       1*m + repmat(1:m,1,4) ...
       2*m + repmat(1:m,1,4)], ...
      repmat([T(:,4);T(:,2);T(:,3);T(:,1)],3,1), ...
      repmat(A./(3*repmat(vol,4,1)),3,1).*N(:), ...
      3*m,n);*/
  std::vector<Triplet<double> > G_t;
  for (int i = 0; i < 4*m; i++) {
    int T_j; // j indexes : repmat([T(:,4);T(:,2);T(:,3);T(:,1)],3,1)
    switch (i/m) {
      case 0:
        T_j = 3;
        break;
      case 1:
        T_j = 1;
        break;
      case 2:
        T_j = 2;
        break;
      case 3:
        T_j = 0;
        break;
    }
    int i_idx = i%m;
    int j_idx = T(i_idx,T_j);

    double val_before_n = A(i)/(3*vol(i_idx));
    G_t.push_back(Triplet<double>(0*m+i_idx, j_idx, val_before_n * N(i,0)));
    G_t.push_back(Triplet<double>(1*m+i_idx, j_idx, val_before_n * N(i,1)));
    G_t.push_back(Triplet<double>(2*m+i_idx, j_idx, val_before_n * N(i,2)));
  }
  G.resize(3*m,n);
  G.setFromTriplets(G_t.begin(), G_t.end());
}

template <typename DerivedV, typename DerivedF>
IGL_INLINE void grad_tri(
  const Eigen::MatrixBase<DerivedV>&V,
  const Eigen::MatrixBase<DerivedF>&F,
  Eigen::SparseMatrix<typename DerivedV::Scalar> &G,
  bool uniform)
{
  // Number of faces
  const int m = F.rows();
  // Number of vertices
  const int nv = V.rows();
  // Number of dimensions
  const int dims = V.cols();
  Eigen::Matrix<typename DerivedV::Scalar,Eigen::Dynamic,3>
    eperp21(m,3), eperp13(m,3);

  for (int i=0;i<m;++i)
  {
    // renaming indices of vertices of triangles for convenience
    int i1 = F(i,0);
    int i2 = F(i,1);
    int i3 = F(i,2);

    // #F x 3 matrices of triangle edge vectors, named after opposite vertices
    typedef Eigen::Matrix<typename DerivedV::Scalar, 1, 3> RowVector3S;
    RowVector3S v32 = RowVector3S::Zero(1,3);
    RowVector3S v13 = RowVector3S::Zero(1,3);
    RowVector3S v21 = RowVector3S::Zero(1,3);
    v32.head(V.cols()) = V.row(i3) - V.row(i2);
    v13.head(V.cols()) = V.row(i1) - V.row(i3);
    v21.head(V.cols()) = V.row(i2) - V.row(i1);
    RowVector3S n = v32.cross(v13);
    // area of parallelogram is twice area of triangle
    // area of parallelogram is || v1 x v2 ||
    // This does correct l2 norm of rows, so that it contains #F list of twice
    // triangle areas
    double dblA = std::sqrt(n.dot(n));
    Eigen::Matrix<typename DerivedV::Scalar, 1, 3> u(0,0,1);
    if (!uniform) {
      // now normalize normals to get unit normals
      u = n / dblA;
    } else {
      // Abstract equilateral triangle v1=(0,0), v2=(h,0), v3=(h/2, (sqrt(3)/2)*h)

      // get h (by the area of the triangle)
      double h = sqrt( (dblA)/sin(igl::PI / 3.0)); // (h^2*sin(60))/2. = Area => h = sqrt(2*Area/sin_60)

      Eigen::Matrix<typename DerivedV::Scalar, 3, 1> v1,v2,v3;
      v1 << 0,0,0;
      v2 << h,0,0;
      v3 << h/2.,(sqrt(3)/2.)*h,0;

      // now fix v32,v13,v21 and the normal
      v32 = v3-v2;
      v13 = v1-v3;
      v21 = v2-v1;
      n = v32.cross(v13);
    }

    // rotate each vector 90 degrees around normal
    double norm21 = std::sqrt(v21.dot(v21));
    double norm13 = std::sqrt(v13.dot(v13));
    eperp21.row(i) = u.cross(v21);
    eperp21.row(i) = eperp21.row(i) / std::sqrt(eperp21.row(i).dot(eperp21.row(i)));
    eperp21.row(i) *= norm21 / dblA;
    eperp13.row(i) = u.cross(v13);
    eperp13.row(i) = eperp13.row(i) / std::sqrt(eperp13.row(i).dot(eperp13.row(i)));
    eperp13.row(i) *= norm13 / dblA;
  }

  // create sparse gradient operator matrix
  G.resize(dims*m,nv);
  std::vector<Eigen::Triplet<typename DerivedV::Scalar> > Gijv;
  Gijv.reserve(4*dims*m);
  for(int f = 0;f<F.rows();f++)
  {
    for(int d = 0;d<dims;d++)
    {
      Gijv.emplace_back(f+d*m,F(f,1), eperp13(f,d));
      Gijv.emplace_back(f+d*m,F(f,0),-eperp13(f,d));
      Gijv.emplace_back(f+d*m,F(f,2), eperp21(f,d));
      Gijv.emplace_back(f+d*m,F(f,0),-eperp21(f,d));
    }
  }
  G.setFromTriplets(Gijv.begin(), Gijv.end());
}

} // anonymous namespace

} // namespace igl

template <typename DerivedV, typename DerivedF>
IGL_INLINE void igl::grad(
  const Eigen::MatrixBase<DerivedV>&V,
  const Eigen::MatrixBase<DerivedF>&F,
  Eigen::SparseMatrix<typename DerivedV::Scalar> &G,
  bool uniform)
{
  assert(F.cols() == 3 || F.cols() == 4);
  switch(F.cols())
  {
    case 3:
      return grad_tri(V,F,G,uniform);
    case 4:
      return grad_tet(V,F,G,uniform);
    default:
      assert(false);
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::grad<Eigen::Matrix<double, -1, 2, 0, -1, 2>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 2, 0, -1, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::SparseMatrix<Eigen::Matrix<double, -1, 2, 0, -1, 2>::Scalar, 0, int>&, bool);
template void igl::grad<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::SparseMatrix<Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, 0, int>&, bool);
template void igl::grad<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::SparseMatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>::Scalar, 0, int>&, bool);
#endif


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include<mitkConnectomicsBetweennessHistogram.h>

#include <boost/numeric/conversion/converter.hpp>

#include <mitkConnectomicsConstantsManager.h>

mitk::ConnectomicsBetweennessHistogram::ConnectomicsBetweennessHistogram()
: m_Mode( UnweightedUndirectedMode )
, m_CentralityMap()
{
  m_Subject = "Node Betweenness";
}

mitk::ConnectomicsBetweennessHistogram::~ConnectomicsBetweennessHistogram()
{
}

void mitk::ConnectomicsBetweennessHistogram::SetBetweennessCalculationMode(
  const mitk::ConnectomicsBetweennessHistogram::BetweennessCalculationMode & mode )
{
  m_Mode = mode;
}

mitk::ConnectomicsBetweennessHistogram::BetweennessCalculationMode mitk::ConnectomicsBetweennessHistogram::GetBetweennessCalculationMode()
{
  return m_Mode;
}

void mitk::ConnectomicsBetweennessHistogram::ComputeFromConnectomicsNetwork( ConnectomicsNetwork* source )
{
  NetworkType* boostGraph = source->GetBoostGraph();
  IteratorType vertex_iterator_begin, vertex_iterator_end;

  m_CentralityMap.clear();
  m_CentralityMap.resize( source->GetNumberOfVertices() );

  switch( m_Mode )
  {
  case UnweightedUndirectedMode:
    {
      CalculateUnweightedUndirectedBetweennessCentrality( boostGraph, vertex_iterator_begin, vertex_iterator_end );
      break;
    }
  case WeightedUndirectedMode:
    {
      CalculateWeightedUndirectedBetweennessCentrality( boostGraph, vertex_iterator_begin, vertex_iterator_end );
      break;
    }
  }

  ConvertCentralityMapToHistogram();
}

void mitk::ConnectomicsBetweennessHistogram::CalculateUnweightedUndirectedBetweennessCentrality(
  NetworkType* boostGraph, IteratorType /*vertex_iterator_begin*/, IteratorType /*vertex_iterator_end*/ )
{
  boost::brandes_betweenness_centrality(
    *boostGraph,
    boost::centrality_map(
    boost::make_iterator_property_map( m_CentralityMap.begin(), boost::get( &mitk::ConnectomicsNetwork::NetworkNode::id, *boostGraph ), double() )
    ).vertex_index_map( boost::get( &mitk::ConnectomicsNetwork::NetworkNode::id, *boostGraph ) )
    );

}

void mitk::ConnectomicsBetweennessHistogram::CalculateWeightedUndirectedBetweennessCentrality(
  NetworkType* /*boostGraph*/, IteratorType /*vertex_iterator_begin*/, IteratorType /*vertex_iterator_end*/ )
{
  MBI_WARN << mitk::ConnectomicsConstantsManager::CONNECTOMICS_WARNING_UNIMPLEMENTED_FEATURE;
}

void mitk::ConnectomicsBetweennessHistogram::ConvertCentralityMapToHistogram()
{
  double maximumFloat( 0.0 );

  for ( unsigned int index( 0 ); index < m_CentralityMap.size(); index++ )
  {
    if( m_CentralityMap[ index ] > maximumFloat )
    {
      maximumFloat = m_CentralityMap[ index ];
    }
   }

  // use the boost double to int converter
  // it defaults to trunc
  typedef boost::numeric::converter<int,double> Double2Int ;

  // for rounding, reduces the number of nodes classed as zero
  maximumFloat += 0.5;

  int maximumInt( 0 );
  try
  {
    maximumInt = Double2Int::convert( maximumFloat );
  }
  catch ( boost::numeric::positive_overflow const& )
  {
    MBI_WARN << mitk::ConnectomicsConstantsManager::CONNECTOMICS_ERROR_OUTSIDE_INTEGER_RANGE;
  }

  m_HistogramVector.resize( maximumInt + 1 );

  for ( unsigned int index( 0 ); index < m_CentralityMap.size(); index++ )
  {
    int value( 0 );
    value = Double2Int::convert( ( m_CentralityMap[index ] + 0.5 ) );
    m_HistogramVector[ value ]++;
  }

  UpdateYMax();

  m_Valid = true;
}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/random.hpp>
#include <boost/random.hpp>
#include <stack>
#include <iostream>
#include <vector>

//==============================================================================
struct node_properties
{
  int component;
};
struct edge_properties
{
};
struct graph_properties
{
  int number_of_components;
};

//--------------------------------------
typedef boost::adjacency_list<
  boost::vecS,
  boost::vecS,
  boost::directedS,
  node_properties,
  edge_properties,
  graph_properties> graph_t;

typedef typename boost::graph_traits<graph_t>::vertex_descriptor vertex_t;
typedef typename boost::graph_traits<graph_t>::edge_descriptor edge_t;

graph_t graph;
std::vector<vertex_t> vertices;

int nn = 5;
std::vector<bool> visited(nn, false);
std::vector<int> r_index(nn, 0);
std::vector<bool> inComponent(nn, false);
std::stack<int> S;
int vindex = 0;
int c = 0; // component number

//==============================================================================
void print_graph(const graph_t &graph)
{
  std::cout << "Graph:" << std::endl;
  auto edges = boost::edges(graph);
  for (auto it = edges.first; it != edges.second; ++it)
  {
    std::cout << boost::source(*it, graph) << " -> "
              << boost::target(*it, graph) << std::endl;
  }
}

//==============================================================================
void print_components(const graph_t &graph)
{
  std::cout << "\nComponents: "
            << graph[boost::graph_bundle].number_of_components << std::endl;
  auto nodes = boost::vertices(graph);
  for (auto it = nodes.first; it != nodes.second; ++it)
  {
    std::cout << "Node: " << *it << " component: " << graph[*it].component
              << std::endl;
  }
}

//==============================================================================
void strong_components(graph_t &graph)
{
  std::vector<edge_t> temp_edges;

  auto edges = boost::edges(graph);

  for (auto it = edges.first; it != edges.second; ++it)
  {
    auto pair = boost::add_edge(boost::target(*it, graph),
                                boost::source(*it, graph),
                                graph);
    temp_edges.push_back(pair.first);
  }

  graph[boost::graph_bundle].number_of_components =
      boost::strong_components(graph,
                               boost::get(&node_properties::component, graph));

  for (const auto &e : temp_edges)
  {
    boost::remove_edge(e, graph);
  }
}

//==============================================================================

void visit(int v)
{
  bool root = true;
  visited[v] = true;
  r_index[v] = vindex;
  vindex = vindex + 1;
  inComponent[v] = false;
  auto edges = boost::edges(graph);
  for (auto it = edges.first; it != edges.second; ++it)
  {
    int w = boost::target(*it, graph);
    int s = boost::source(*it, graph);

    if (v == s)
    {
      //std::cout << "\n Visit for node: " << v << ", edge: " << s << "->" << w << "\n";
      if (!visited[w])
      {
        visit(w);
      }

      for (auto i = r_index.begin(); i != r_index.end(); ++i)
        //bool check = !(inComponent[w]) && (r_index[w] < r_index[v]);
        if (!(inComponent[w]) && (r_index[w] < r_index[v]))
        {
          r_index[v] = r_index[w];
          root = false;
        }
    }
  }

  if (root)
  {
    inComponent[v] = true;
    while (!S.empty())
    {
      while ((!S.empty()) && (r_index[v] <= r_index[S.top()]))
      {
        int w = S.top();
        S.pop();
        r_index[w] = c;
        inComponent[w] = true;
      }
    }
    r_index[v] = c;
    c = c + 1;
  }
  else
  {
    S.push(v);
  }
}

//==============================================================================
int main()
{

  //random generation;
  nn = 5;
  int e = 10;
  boost::mt19937 rng;
  boost::generate_random_graph(graph, nn, e, rng);
  // size_t e = boost::num_edges(graph);
  // size_t n = boost::num_vertices(graph);

  for (int i = 0; i < nn; i++)
  {
    if (!visited[i])
    {
      visit(i);
    }
  }

  std::cout << "\nComponents number SCC1: ----------" << c << std::endl;
  for (auto i = r_index.begin(); i != r_index.end(); ++i)
    std::cout << *i << ' ';
  //std::cout << "\n ***************************************" << std::endl;
  //std::cout << "\nTrue boost graph implementation : \n----------------------" << std::endl;

  print_graph(graph);

  return 0;
}


#include <algorithm>
#include <boost/functional/hash.hpp>
#include <iostream>
#include <sstream>
#include <unordered_set>
#include <vector>

size_t checksum(const std::vector<int>& nums)
{
    size_t seed = 0;
    for (auto n : nums) {
        boost::hash_combine(seed, n);
    }
    return seed;
}

int main()
{
    std::string line;
    while (std::getline(std::cin, line)) {
        int level = 0;
        int sum = 0;

        bool skip = false;
        bool garbage = false;

        for (auto it = std::cbegin(line); it != std::cend(line); ++it) {
            if (skip) {
                skip = false;
                continue;
            }
            if (*it == '!') {
                skip = true;
                continue;
            }
            if (garbage) {
                if (*it == '>') {
                    garbage = false;
                }
                continue;
            }
            if (*it == '<') {
                garbage = true;
                continue;
            }
            if (*it == '}') {
                sum += level--;
                continue;
            }
            if (*it == '{') {
                ++level;
                continue;
            }
        }
        std::cout << "score : " << sum << "\n";
    }
}


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#ifndef BOOST_ASTRONOMY_EQUATORIAL_HA_COORD_HPP
#define BOOST_ASTRONOMY_EQUATORIAL_HA_COORD_HPP

#include <iostream>
#include <boost/static_assert.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/units/get_dimension.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/dimensionless.hpp>
#include <boost/units/physical_dimensions/plane_angle.hpp>
#include <boost/astronomy/coordinate/coord_sys/coord_sys.hpp>

/**
 * The Equatorial Coordinates, are referred to the plane of the Earth’s equator
 *
 * Declination
 * Declination is analogous to latitude and indicates how far away an object is
 * from the celestial equator. Declination is int the range ±90◦ with positive
 * angles indicating locations north of the celestial equator and negative angles
 * indicating locations south of the celestial equator. Because declination is
 * measured with respect to the celestial equator, and the celestial equator’s
 * location does not vary with time of day or an observer’s location, declination
 * for an object is fixed and does not vary with the time of day or an
 * observer’s location.
 *
 * Hour Angle
 * If we use an observer’s meridian instead of the celestial prime meridian as a reference point,
 * we have another way to measure “celestial longitude" called “hour angle” (H). While right ascension
 * is an angular measurement (although expressed in HMS format) of an object’s distance from the First
 * Point of Aries, hour angle is very much a time measurement. The hour angle for an object is a measure
 * of how long it has been since the object crossed an observer’s meridian.
 * Because of the way that an hour angle is defined (i.e., relative to an observer’s local celestial meridian),
 * it varies both with time of day and an observer’s location.
 *
**/

namespace boost { namespace astronomy { namespace coordinate {

namespace bu = boost::units;
namespace bg = boost::geometry;

template
<
    typename CoordinateType = double,
    typename HourAngleQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename Declination = bu::quantity<bu::si::plane_angle, CoordinateType>
>
struct equatorial_ha_coord : public coord_sys
       <2, bg::cs::spherical<bg::radian>, CoordinateType>
{
  ///@cond INTERNAL
  BOOST_STATIC_ASSERT_MSG(
      ((std::is_same<typename bu::get_dimension<HourAngleQuantity>::type,
          bu::plane_angle_dimension>::value) &&
       (std::is_same<typename bu::get_dimension<Declination>::type,
           bu::plane_angle_dimension>::value)),
      "Hour Angle and Declination must be of plane angle type");
  BOOST_STATIC_ASSERT_MSG((std::is_floating_point<CoordinateType>::value),
                          "CoordinateType must be a floating-point type");
  ///@endcond
public:
    typedef HourAngleQuantity quantity1;
    typedef Declination quantity2;

    //Default constructor
    equatorial_ha_coord() {}

    equatorial_ha_coord
        (
            HourAngleQuantity const &Ha,
            Declination const &Dec
        )
    {
      this->set_ha_dec(Ha, Dec);
    }

    //Create tuple of Hour Angle and Declination
    std::tuple<HourAngleQuantity, Declination> get_ha_dec() const
    {
      return std::make_tuple(this->get_ha(), this->get_dec());
    }

    //Get Hour Angle
    HourAngleQuantity get_ha() const
    {
      return static_cast<HourAngleQuantity>
      (
          bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
              (bg::get<0>(this->point))
      );
    }

    //Get Declination
    Declination get_dec() const
    {
      return static_cast<Declination>
      (
          bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
              (bg::get<1>(this->point))
      );
    }

    //Set value of Hour Angle and Declination
    void set_ha_dec
        (
            HourAngleQuantity const &Ha,
            Declination const &Dec
        )
    {
      this->set_ha(Ha);
      this->set_dec(Dec);
    }

    //Set Hour Angle
    void set_ha(HourAngleQuantity const &Ha)
    {
      bg::set<0>
          (
              this->point,
              static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(Ha).value()
          );
    }

    //Set Declination
    void set_dec(Declination const &Dec)
    {
      bg::set<1>
          (
              this->point,
              static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(Dec).value()
          );
    }

}; //equatorial_ha_coord

//Make Equatorial Coordinate
template
<
    typename CoordinateType,
    template<typename Unit2, typename CoordinateType_> class HourAngleQuantity,
    template<typename Unit1, typename CoordinateType_> class Declination,
    typename Unit1,
    typename Unit2
>
equatorial_ha_coord
<
    CoordinateType,
    Declination<Unit1, CoordinateType>,
    HourAngleQuantity<Unit2, CoordinateType>
> make_equatorial_ha_coord
(
    HourAngleQuantity<Unit2, CoordinateType> const &Ha,
    Declination<Unit1, CoordinateType> const &Dec
)
{
  return equatorial_ha_coord
      <
          CoordinateType,
          HourAngleQuantity<Unit2, CoordinateType>,
          Declination<Unit1, CoordinateType>
      > (Ha, Dec);
}

//Print Equatorial Hour Angle Coordinates
template
<
    typename CoordinateType,
    class HourAngleQuantity,
    class Declination
>
std::ostream &operator << (std::ostream &out, equatorial_ha_coord
                          <CoordinateType, HourAngleQuantity, Declination> const &point) {
  out << "Equatorial Coordinate (Hour Angle: "
      << point.get_ha() << ", Declination: "
      << point.get_dec() << ")";

  return out;
}

}}}

#endif  // BOOST_ASTRONOMY_EQUATORIAL_HA_COORD_HPP


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

  @copyright 2012-2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_ONEOTWOEPS_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_ONEOTWOEPS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate \f$1/(2\epsilon)\f$

    @par Semantic:

    @code
    T r = Oneotwoeps<T>();
    @endcode

    is similar to:

    @code
    if T is integral
      r = T(0)
    else if T is double
      r =  pow(2.0, 52);
    else if T is float
      r =  pow(2.0f, 22);
    @endcode

    @return The Oneotwoeps constant for the proper type
  **/
  template<typename T> T Oneotwoeps();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant oneotwoeps.

      @return The Oneotwoeps constant for the proper type
    **/
    const boost::dispatch::functor<tag::oneotwoeps_> oneotwoeps = {};
  }
} }
#endif

#include <boost/simd/constant/definition/oneotwoeps.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "Simulation/SimulationModel.h"
#include "Simulation/TimeStepController.h"
#include <iostream>
#include "Demos/Visualization/Visualization.h"
#include "Simulation/DistanceFieldCollisionDetection.h"
#include "Utils/OBJLoader.h"
#include "Utils/Logger.h"
#include "Utils/Timing.h"
#include "Utils/FileSystem.h"
#include "Demos/Common/DemoBase.h"
#include "Demos/Common/TweakBarParameters.h"
#include "Simulation/Simulation.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
	#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;
using namespace Utilities;

void initParameters();
void timeStep ();
void buildModel ();
void createMesh();
void render ();
void reset();
void TW_CALL setStiffness(const void *value, void *clientData);
void TW_CALL getStiffness(void *value, void *clientData);
void TW_CALL setPoissonRatio(const void *value, void *clientData);
void TW_CALL getPoissonRatio(void *value, void *clientData);
void TW_CALL setNormalizeStretch(const void *value, void *clientData);
void TW_CALL getNormalizeStretch(void *value, void *clientData);
void TW_CALL setNormalizeShear(const void *value, void *clientData);
void TW_CALL getNormalizeShear(void *value, void *clientData);
void TW_CALL setSimulationMethod(const void *value, void *clientData);
void TW_CALL getSimulationMethod(void *value, void *clientData);
void TW_CALL setContactTolerance(const void *value, void *clientData);
void TW_CALL getContactTolerance(void *value, void *clientData);
void TW_CALL setContactStiffnessRigidBody(const void *value, void *clientData);
void TW_CALL getContactStiffnessRigidBody(void *value, void *clientData);
void TW_CALL setContactStiffnessParticleRigidBody(const void *value, void *clientData);
void TW_CALL getContactStiffnessParticleRigidBody(void *value, void *clientData);

DemoBase *base;
DistanceFieldCollisionDetection cd;

const unsigned int width = 30;
const unsigned int depth = 5;
const unsigned int height = 5;
short simulationMethod = 2;

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

	base = new DemoBase();
	base->init(argc, argv, "Bar collision demo");

	SimulationModel *model = new SimulationModel();
	model->init();
	Simulation::getCurrent()->setModel(model);

	buildModel();

	initParameters();

	Simulation::getCurrent()->setSimulationMethodChangedCallback([&]() { reset(); initParameters(); base->getSceneLoader()->readParameterObject(Simulation::getCurrent()->getTimeStep()); });

	// OpenGL
	MiniGL::setClientIdleFunc (50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0f, Vector3r (5.0, 10.0, 30.0), Vector3r (5.0, 0.0, 0.0));

	TwType enumType2 = TwDefineEnum("SimulationMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "SimulationMethod", enumType2, setSimulationMethod, getSimulationMethod, &simulationMethod,
			" label='Simulation method' enum='0 {None}, 1 {Volume constraints}, 2 {FEM based PBD}, 3 {Strain based dynamics (no inversion handling)}, 4 {Shape matching (no inversion handling)}' group=Simulation");
	TwAddVarCB(MiniGL::getTweakBar(), "Stiffness", TW_TYPE_REAL, setStiffness, getStiffness, model, " label='Stiffness'  min=0.0 step=0.1 precision=4 group='Simulation' ");
	TwAddVarCB(MiniGL::getTweakBar(), "PoissonRatio", TW_TYPE_REAL, setPoissonRatio, getPoissonRatio, model, " label='Poisson ratio XY'  min=0.0 step=0.1 precision=4 group='Simulation' ");
	TwAddVarCB(MiniGL::getTweakBar(), "NormalizeStretch", TW_TYPE_BOOL32, setNormalizeStretch, getNormalizeStretch, model, " label='Normalize stretch' group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "NormalizeShear", TW_TYPE_BOOL32, setNormalizeShear, getNormalizeShear, model, " label='Normalize shear' group='Strain based dynamics' ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactTolerance", TW_TYPE_REAL, setContactTolerance, getContactTolerance, &cd, " label='Contact tolerance'  min=0.0 step=0.001 precision=3 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactStiffnessRigidBody", TW_TYPE_REAL, setContactStiffnessRigidBody, getContactStiffnessRigidBody, model, " label='Contact stiffness RB'  min=0.0 step=0.1 precision=2 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactStiffnessParticleRigidBody", TW_TYPE_REAL, setContactStiffnessParticleRigidBody, getContactStiffnessParticleRigidBody, model, " label='Contact stiffness Particle-RB'  min=0.0 step=0.1 precision=2 group=Simulation ");

	glutMainLoop ();

	base->cleanup();

	Utilities::Timing::printAverageTimes();
	Utilities::Timing::printTimeSums();

	delete Simulation::getCurrent();
	delete base;
	delete model;

	return 0;
}

void initParameters()
{
	TwRemoveAllVars(MiniGL::getTweakBar());
	TweakBarParameters::cleanup();

	MiniGL::initTweakBarParameters();

	TweakBarParameters::createParameterGUI();
	TweakBarParameters::createParameterObjectGUI(base);
	TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent());
	TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent()->getModel());
	TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent()->getTimeStep());
}

void reset()
{
	Utilities::Timing::printAverageTimes();
	Utilities::Timing::reset();

	Simulation::getCurrent()->reset();
	base->getSelectedParticles().clear();

	Simulation::getCurrent()->getModel()->cleanup();
	Simulation::getCurrent()->getTimeStep()->getCollisionDetection()->cleanup();

	buildModel();
}

void timeStep ()
{
	const Real pauseAt = base->getValue<Real>(DemoBase::PAUSE_AT);
	if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime()))
		base->setValue(DemoBase::PAUSE, true);

	if (base->getValue<bool>(DemoBase::PAUSE))
		return;

	// Simulation code
	SimulationModel *model = Simulation::getCurrent()->getModel();
	const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
	for (unsigned int i = 0; i < numSteps; i++)
	{
		START_TIMING("SimStep");
		Simulation::getCurrent()->getTimeStep()->step(*model);
		STOP_TIMING_AVG;
	}

	for (unsigned int i = 0; i < model->getTetModels().size(); i++)
		model->getTetModels()[i]->updateMeshNormals(model->getParticles());
}

void loadObj(const std::string &filename, VertexData &vd, IndexedFaceMesh &mesh, const Vector3r &scale)
{
	std::vector<OBJLoader::Vec3f> x;
	std::vector<OBJLoader::Vec3f> normals;
	std::vector<OBJLoader::Vec2f> texCoords;
	std::vector<MeshFaceIndices> faces;
	OBJLoader::Vec3f s = { (float)scale[0], (float)scale[1], (float)scale[2] };
	OBJLoader::loadObj(filename, &x, &faces, &normals, &texCoords, s);

	mesh.release();
	const unsigned int nPoints = (unsigned int)x.size();
	const unsigned int nFaces = (unsigned int)faces.size();
	const unsigned int nTexCoords = (unsigned int)texCoords.size();
	mesh.initMesh(nPoints, nFaces * 2, nFaces);
	vd.reserve(nPoints);
	for (unsigned int i = 0; i < nPoints; i++)
	{
		vd.addVertex(Vector3r(x[i][0], x[i][1], x[i][2]));
	}
	for (unsigned int i = 0; i < nTexCoords; i++)
	{
		mesh.addUV(texCoords[i][0], texCoords[i][1]);
	}
	for (unsigned int i = 0; i < nFaces; i++)
	{
		// Reduce the indices by one
		int posIndices[3];
		int texIndices[3];
		for (int j = 0; j < 3; j++)
		{
			posIndices[j] = faces[i].posIndices[j] - 1;
			if (nTexCoords > 0)
			{
				texIndices[j] = faces[i].texIndices[j] - 1;
				mesh.addUVIndex(texIndices[j]);
			}
		}

		mesh.addFace(&posIndices[0]);
	}
	mesh.buildNeighbors();

	mesh.updateNormals(vd, 0);
	mesh.updateVertexNormals(vd);

	LOG_INFO << "Number of triangles: " << nFaces;
	LOG_INFO << "Number of vertices: " << nPoints;
}

void buildModel ()
{
	TimeManager::getCurrent ()->setTimeStepSize (static_cast<Real>(0.005));
	SimulationModel *model = Simulation::getCurrent()->getModel();

	createMesh();

	// create static rigid body
	string fileName = FileSystem::normalizePath(base->getDataPath() + "/models/cube.obj");
	IndexedFaceMesh mesh;
	VertexData vd;
	loadObj(fileName, vd, mesh, Vector3r::Ones());

	string fileNameTorus = FileSystem::normalizePath(base->getDataPath() + "/models/torus.obj");
	IndexedFaceMesh meshTorus;
	VertexData vdTorus;
	loadObj(fileNameTorus, vdTorus, meshTorus, Vector3r::Ones());

	SimulationModel::RigidBodyVector &rb = model->getRigidBodies();
	rb.resize(2);

	// floor
	rb[0] = new RigidBody();
	rb[0]->initBody(1.0,
		Vector3r(0.0, -5.5, 0.0),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vd, mesh,
		Vector3r(100.0, 1.0, 100.0));
	rb[0]->setMass(0.0);

	// torus
	rb[1] = new RigidBody();
	rb[1]->initBody(1.0,
		Vector3r(5.0, -1.5, 0.0),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vdTorus, meshTorus,
		Vector3r(3.0, 3.0, 3.0));
	rb[1]->setMass(0.0);
	rb[1]->setFrictionCoeff(static_cast<Real>(0.1));

	Simulation::getCurrent()->getTimeStep()->setCollisionDetection(*model, &cd);
	cd.setTolerance(static_cast<Real>(0.05));

	const std::vector<Vector3r> *vertices1 = rb[0]->getGeometry().getVertexDataLocal().getVertices();
	const unsigned int nVert1 = static_cast<unsigned int>(vertices1->size());
	cd.addCollisionBox(0, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices1)[0], nVert1, Vector3r(100.0, 1.0, 100.0));

	const std::vector<Vector3r> *vertices2 = rb[1]->getGeometry().getVertexDataLocal().getVertices();
	const unsigned int nVert2 = static_cast<unsigned int>(vertices2->size());
	cd.addCollisionTorus(1, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices2)[0], nVert2, Vector2r(3.0, 1.5));

	SimulationModel::TetModelVector &tm = model->getTetModels();
	ParticleData &pd = model->getParticles();
	for (unsigned int i = 0; i < tm.size(); i++)
	{
		const unsigned int nVert = tm[i]->getParticleMesh().numVertices();
		unsigned int offset = tm[i]->getIndexOffset();
		tm[i]->setFrictionCoeff(static_cast<Real>(0.1));
		cd.addCollisionObjectWithoutGeometry(i, CollisionDetection::CollisionObject::TetModelCollisionObjectType, &pd.getPosition(offset), nVert, true);
	}
}

void render ()
{
	base->render();
}

void createMesh()
{
	Vector3r points[width*height*depth];
	for (unsigned int i = 0; i < width; i++)
	{
		for (unsigned int j = 0; j < height; j++)
		{
			for (unsigned int k = 0; k < depth; k++)
			{
				points[i*height*depth + j*depth + k] = static_cast<Real>(0.3)*Vector3r((Real)i, (Real)j + static_cast<Real>(3.0), (Real)k);
			}
		}
	}

	vector<unsigned int> indices;
	for (unsigned int i = 0; i < width - 1; i++)
	{
		for (unsigned int j = 0; j < height - 1; j++)
		{
			for (unsigned int k = 0; k < depth - 1; k++)
			{
				// For each block, the 8 corners are numerated as:
				//     4*-----*7
				//     /|    /|
				//    / |   / |
				//  5*-----*6 |
				//   | 0*--|--*3
				//   | /   | /
				//   |/    |/
				//  1*-----*2
				unsigned int p0 = i*height*depth + j*depth + k;
				unsigned int p1 = p0 + 1;
				unsigned int p3 = (i + 1)*height*depth + j*depth + k;
				unsigned int p2 = p3 + 1;
				unsigned int p7 = (i + 1)*height*depth + (j + 1)*depth + k;
				unsigned int p6 = p7 + 1;
				unsigned int p4 = i*height*depth + (j + 1)*depth + k;
				unsigned int p5 = p4 + 1;

				// Ensure that neighboring tetras are sharing faces
				if ((i + j + k) % 2 == 1)
				{
					indices.push_back(p2); indices.push_back(p1); indices.push_back(p6); indices.push_back(p3);
					indices.push_back(p6); indices.push_back(p3); indices.push_back(p4); indices.push_back(p7);
					indices.push_back(p4); indices.push_back(p1); indices.push_back(p6); indices.push_back(p5);
					indices.push_back(p3); indices.push_back(p1); indices.push_back(p4); indices.push_back(p0);
					indices.push_back(p6); indices.push_back(p1); indices.push_back(p4); indices.push_back(p3);
				}
				else
				{
					indices.push_back(p0); indices.push_back(p2); indices.push_back(p5); indices.push_back(p1);
					indices.push_back(p7); indices.push_back(p2); indices.push_back(p0); indices.push_back(p3);
					indices.push_back(p5); indices.push_back(p2); indices.push_back(p7); indices.push_back(p6);
					indices.push_back(p7); indices.push_back(p0); indices.push_back(p5); indices.push_back(p4);
					indices.push_back(p0); indices.push_back(p2); indices.push_back(p7); indices.push_back(p5);
				}
			}
		}
	}
	SimulationModel *model = Simulation::getCurrent()->getModel();
	model->addTetModel(width*height*depth, (unsigned int)indices.size() / 4u, points, indices.data());

	ParticleData &pd = model->getParticles();
	for (unsigned int i = 0; i < pd.getNumberOfParticles(); i++)
	{
		pd.setMass(i, 1.0);
	}

	// init constraints
	for (unsigned int cm = 0; cm < model->getTetModels().size(); cm++)
	{
		const unsigned int nTets = model->getTetModels()[cm]->getParticleMesh().numTets();
		const unsigned int *tets = model->getTetModels()[cm]->getParticleMesh().getTets().data();
		const IndexedTetMesh::VertexTets *vTets = model->getTetModels()[cm]->getParticleMesh().getVertexTets().data();
		if (simulationMethod == 1)
		{
			const unsigned int offset = model->getTetModels()[cm]->getIndexOffset();
			const unsigned int nEdges = model->getTetModels()[cm]->getParticleMesh().numEdges();
			const IndexedTetMesh::Edge *edges = model->getTetModels()[cm]->getParticleMesh().getEdges().data();
			for (unsigned int i = 0; i < nEdges; i++)
			{
				const unsigned int v1 = edges[i].m_vert[0] + offset;
				const unsigned int v2 = edges[i].m_vert[1] + offset;

				model->addDistanceConstraint(v1, v2);
			}

			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v1 = tets[4 * i];
				const unsigned int v2 = tets[4 * i + 1];
				const unsigned int v3 = tets[4 * i + 2];
				const unsigned int v4 = tets[4 * i + 3];

				model->addVolumeConstraint(v1, v2, v3, v4);
			}
		}
		else if (simulationMethod == 2)
		{
			TetModel::ParticleMesh &mesh = model->getTetModels()[cm]->getParticleMesh();
			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v1 = tets[4 * i];
				const unsigned int v2 = tets[4 * i + 1];
				const unsigned int v3 = tets[4 * i + 2];
				const unsigned int v4 = tets[4 * i + 3];

				model->addFEMTetConstraint(v1, v2, v3, v4);
			}
		}
		else if (simulationMethod == 3)
		{
			TetModel::ParticleMesh &mesh = model->getTetModels()[cm]->getParticleMesh();
			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v1 = tets[4 * i];
				const unsigned int v2 = tets[4 * i + 1];
				const unsigned int v3 = tets[4 * i + 2];
				const unsigned int v4 = tets[4 * i + 3];

				model->addStrainTetConstraint(v1, v2, v3, v4);
			}
		}
		else if (simulationMethod == 4)
		{
			TetModel::ParticleMesh &mesh = model->getTetModels()[cm]->getParticleMesh();
			for (unsigned int i = 0; i < nTets; i++)
			{
				const unsigned int v[4] = { tets[4 * i], tets[4 * i + 1], tets[4 * i + 2], tets[4 * i + 3] };
				// Important: Divide position correction by the number of clusters
				// which contain the vertex.
				const unsigned int nc[4] = { vTets[v[0]].m_numTets, vTets[v[1]].m_numTets, vTets[v[2]].m_numTets, vTets[v[3]].m_numTets };
				model->addShapeMatchingConstraint(4, v, nc);
			}
		}
		model->getTetModels()[cm]->updateMeshNormals(pd);
	}

	LOG_INFO << "Number of tets: " << indices.size() / 4;
	LOG_INFO << "Number of vertices: " << width*height*depth;

}

void TW_CALL setStiffness(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*) clientData)->setValue<Real>(SimulationModel::SOLID_STIFFNESS, val);
}

void TW_CALL getStiffness(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getValue<Real>(SimulationModel::SOLID_STIFFNESS);
}

void TW_CALL setPoissonRatio(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setValue<Real>(SimulationModel::SOLID_POISSON_RATIO, val);
}

void TW_CALL getPoissonRatio(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getValue<Real>(SimulationModel::SOLID_POISSON_RATIO);
}

void TW_CALL setNormalizeStretch(const void *value, void *clientData)
{
	const bool val = *(const bool *)(value);
	((SimulationModel*)clientData)->setValue<Real>(SimulationModel::SOLID_NORMALIZE_STRETCH, val);
}

void TW_CALL getNormalizeStretch(void *value, void *clientData)
{
	*(bool *)(value) = ((SimulationModel*)clientData)->getValue<Real>(SimulationModel::SOLID_NORMALIZE_STRETCH);
}

void TW_CALL setNormalizeShear(const void *value, void *clientData)
{
	const bool val = *(const bool *)(value);
	((SimulationModel*)clientData)->setValue<Real>(SimulationModel::SOLID_NORMALIZE_SHEAR, val);
}

void TW_CALL getNormalizeShear(void *value, void *clientData)
{
	*(bool *)(value) = ((SimulationModel*)clientData)->getValue<Real>(SimulationModel::SOLID_NORMALIZE_SHEAR);
}

void TW_CALL setSimulationMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	*((short*)clientData) = val;
	reset();
}

void TW_CALL getSimulationMethod(void *value, void *clientData)
{
	*(short *)(value) = *((short*)clientData);
}

void TW_CALL setContactStiffnessRigidBody(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setContactStiffnessRigidBody(val);
}

void TW_CALL getContactStiffnessRigidBody(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getContactStiffnessRigidBody();
}

void TW_CALL setContactStiffnessParticleRigidBody(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setContactStiffnessParticleRigidBody(val);
}

void TW_CALL getContactStiffnessParticleRigidBody(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getContactStiffnessParticleRigidBody();
}

void TW_CALL setContactTolerance(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((DistanceFieldCollisionDetection*)clientData)->setTolerance(val);
}

void TW_CALL getContactTolerance(void *value, void *clientData)
{
	*(Real *)(value) = ((DistanceFieldCollisionDetection*)clientData)->getTolerance();
}


#include "../include/demos.h"
#include <Eigen/Eigen>
#include <iostream>

int main()
{
	//test_draw_trajectory();
	test_rangeimg2pc();
	return 0;
}


// Copyright (C) 2016-2018 T. Zachary Laine
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//[ calc2a
#include <boost/yap/expression.hpp>

#include <iostream>

int main ()
{
    using namespace boost::yap::literals;

    auto expr_1 = 1_p + 2.0;

    auto expr_1_fn = [expr_1](auto &&... args) {
        return evaluate(expr_1, args...);
    };

    auto expr_2 = 1_p * 2_p;

    auto expr_2_fn = [expr_2](auto &&... args) {
        return evaluate(expr_2, args...);
    };

    auto expr_3 = (1_p - 2_p) / 2_p;

    auto expr_3_fn = [expr_3](auto &&... args) {
        return evaluate(expr_3, args...);
    };

    // Displays "5"
    std::cout << expr_1_fn(3.0) << std::endl;

    // Displays "6"
    std::cout << expr_2_fn(3.0, 2.0) << std::endl;

    // Displays "0.5"
    std::cout << expr_3_fn(3.0, 2.0) << std::endl;

    return 0;
}
//]


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file knapsack_unbounded_two_app.hpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-10-07
 */
#ifndef PAAL_KNAPSACK_UNBOUNDED_TWO_APP_HPP
#define PAAL_KNAPSACK_UNBOUNDED_TWO_APP_HPP

#include "paal/utils/accumulate_functors.hpp"
#include "paal/utils/type_functions.hpp"
#include "paal/greedy/knapsack/knapsack_greedy.hpp"

#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/filter_iterator.hpp>

#include <type_traits>
#include <utility>

namespace paal {

namespace detail {
template <typename KnapsackData,
          typename ObjectIter = typename KnapsackData::object_iter,
          typename Size = typename KnapsackData::size,
          typename Value = typename KnapsackData::value>
std::tuple<Value, Size,
           std::pair<ObjectIter, unsigned>>
get_greedy_fill(KnapsackData knap_data, unbounded_tag) {

    auto density = knap_data.get_density();
    auto most_dense_iter = max_element_functor(
            knap_data.get_objects(), density).base();

    unsigned nr = knap_data.get_capacity() / knap_data.get_size(*most_dense_iter);
    Value value_sum = Value(nr) * knap_data.get_value(*most_dense_iter);
    Size  size_sum  = Size (nr) * knap_data.get_size (*most_dense_iter);
    return std::make_tuple(value_sum, size_sum,
                           std::make_pair(most_dense_iter, nr));
}

template <typename ObjectsIterAndNr, typename OutputIter>
void greedy_to_output(ObjectsIterAndNr most_dense_iter_and_nr, OutputIter & out,
                      unbounded_tag) {
    auto nr = most_dense_iter_and_nr.second;
    auto most_dense_iter = most_dense_iter_and_nr.first;
    for (unsigned i = 0; i < nr; ++i) {
        *out = *most_dense_iter;
        ++out;
    }
}

} //! detail

///this version of algorithm might permute, the input range
template <typename OutputIterator, typename Objects,
          typename ObjectSizeFunctor, typename ObjectValueFunctor,
          //this enable if assures that range can be permuted
          typename std::enable_if<!detail::is_range_const<Objects>::value>::type * = nullptr>

typename detail::knapsack_base<Objects, ObjectSizeFunctor,
                                  ObjectValueFunctor>::return_type
knapsack_unbounded_two_app(
    Objects && objects,
    typename detail::FunctorOnRangePValue<ObjectSizeFunctor, Objects> capacity,
    OutputIterator out, ObjectValueFunctor value, ObjectSizeFunctor size) {
    return detail::knapsack_general_two_app(
            detail::make_knapsack_data(std::forward<Objects>(objects), capacity, size, value, out),
            detail::unbounded_tag{});
}
} //! paal
#endif // PAAL_KNAPSACK_UNBOUNDED_TWO_APP_HPP


/* Copyright (c) 2016, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#include <vector>
#include <iostream>
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>

#include <jsCore/timer.hpp>
#include <jsCore/gpuMatrix.hpp>

void surfelRenderGPU(float* s, int32_t N, float f, int32_t w, int32_t h, float *d);
void surfelRenderGPU(double* s, int32_t N, double f, int32_t w, int32_t h, double *d);

namespace cudaPcl {

typedef Eigen::Map<Eigen::Matrix<float,
        Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>,
        0,Eigen::OuterStride<7> > SurfelMap;

struct Surfel {
  float x;
  float y;
  float z;
  float nx;
  float ny;
  float nz;
  float rSq;
//  float pDotn; // Dot product between point p and normal n
  Surfel(float x, float y, float z, float nx, float ny, float nz, float
      rSq) : x(x), y(y), z(z), nx(nx), ny(ny), nz(nz), rSq(rSq) {}
//      pDotn(x*nx+y*ny+z*nz) {}
  ~Surfel() {};
  void makeValid();
};

class SurfelStore {
 public:
  SurfelStore() {} ;
  ~SurfelStore() {};

  void AddSurfel(const Surfel& surfel) { ss_.push_back(surfel); }

  size_t Size() const { return ss_.size(); }

  const Surfel& GetSurfel(uint32_t i) { return ss_[i]; }
  SurfelMap GetXYZs() { return SurfelMap(&(ss_[0].x),ss_.size(),3); }
  SurfelMap GetNs() { return SurfelMap(&(ss_[0].nx),ss_.size(),3); }
  SurfelMap GetRSqs() { return SurfelMap(&(ss_[0].rSq),ss_.size(),1); }

  cv::Mat Render(const Eigen::Matrix3d& wRc, const Eigen::Vector3d&
      wtc, float f, uint32_t w, uint32_t h);

 private:
  std::vector<Surfel> ss_;
};

void Surfel::makeValid() {
  float len = sqrt(nx*nx+ny*ny+nz*nz);
  nx/=len;
  ny/=len;
  nz/=len;
}

cv::Mat SurfelStore::Render(const Eigen::Matrix3d& wRc, const Eigen::Vector3d& wtc,
    float f, uint32_t w, uint32_t h) {
  jsc::Timer t0;
  SurfelMap ss (&(ss_[0].x),ss_.size(),7);
  jsc::GpuMatrix<float> ss_d(ss.rows(), ss.cols());
  jsc::GpuMatrix<float> d_d(h,w);
  t0.toctic("prepare memory");
  ss_d.set(ss.data(), ss.rows(), ss.cols(), ss.outerStride(), ss.cols());
  t0.toctic("copy surfels");
  surfelRenderGPU(ss_d.data(), ss_.size(), f, w, h, d_d.data());
  cudaDeviceSynchronize();
  t0.toctic("render time");
  cv::Mat d(h,w,CV_32FC1);
  std::cout << d_d.cols() << "x" << d_d.rows() << std::endl;
  std::cout << w << "x" << h << std::endl;
  d_d.print();
  d_d.get(reinterpret_cast<float*>(d.data),h,w);
  d_d.print();
  t0.toctic("copy depth back");
  return d;
}

}


///////////////////////////////////////////////////////////////////////////////
// contingency_table1.cpp                                                    //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <iostream>

#include <string>
#include <boost/mpl/int.hpp>
#include <boost/mpl/vector/vector10.hpp>
#include <boost/mpl/detail/wrapper.hpp>
#include <boost/typeof/typeof.hpp>
#include <boost/fusion/include/make_map.hpp>
#include <boost/fusion/container/map/detail/sequence_to_map.hpp>
#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/include/factor.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/include/cells.hpp>

#include <boost/statistics/detail/non_parametric/contingency_table/include/pearson_chisq/independence.hpp>

#include <libs/statistics/detail/non_parametric/example/contingency_table1.h>

void example_contingency_table1()
{
	namespace ct = boost::statistics::detail::contingency_table;

    typedef double val_;
    typedef boost::mpl::int_<0> x_; typedef int data_x_;
    typedef boost::mpl::int_<1> y_; typedef std::string data_y_;
    typedef boost::mpl::int_<2> z_; typedef int data_z_;

    typedef boost::fusion::detail::sequence_to_map<
        boost::mpl::vector6<x_,data_x_,y_,data_y_,z_,data_z_>
    >::type sample_;

    typedef boost::mpl::vector1<x_> keys_; //,y_> keys_;
    typedef ct::tag::cells<keys_> cells_;
    typedef boost::accumulators::stats< cells_> stats_;
    typedef boost::accumulators::accumulator_set< sample_, stats_, long int > acc_;

    using namespace boost::assign;
    acc_ acc(( ct::_map_of_levels = boost::fusion::make_map<x_>(
        list_of(-1)(1) ) ));

    {
     	BOOST_ASSERT( ct::extract::levels<x_>( acc ).count( -1  ) == 1 );
    }
    {
        ct::extract::levels<x_>( acc ).begin();
    }
    {
        ct::initialize<keys_>( acc );
    }
    {
        using namespace boost::accumulators;
        //acc( boost::fusion::make_map<x_>( -1 ), weight = 0 );
    }

    long n1 = ct::cells_count<keys_>( acc );
    long n2 = ct::cells<keys_>( acc ).size();

    std::cout << '(' << n1 << ',' << n2 << ')';

//    BOOST_ASSERT( ct::cells_count<keys_>(acc) == ct::cells<keys_>( acc ).size() );

}