//=====================================================
// 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


// 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


/*=============================================================================
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


// Ben Martin
// October 11, 2005

// This currently works only on undirected graphs...
// Graph must model Adjacency Graph, Incidence Graph, VertexListGraph

#ifndef BOOST_GRAPH_SPECTRAL_EMBEDDING_LAYOUT_HPP
#define BOOST_GRAPH_SPECTRAL_EMBEDDING_LAYOUT_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/properties.hpp>
#include <vecLib/clapack.h>
//#include "f2c.h"
//#include "cblas.h"
//#include "clapack.h"
#include <utility> // for pair
//#include <boost/graph/point2d.hpp>
//#include <boost/graph/point3d.hpp>
#include <iostream.h>

typedef long int integer;
typedef double doublereal;

namespace boost {

  template <typename Graph, typename PositionMap>
  void spectral_embedding_layout(Graph& g, PositionMap pos_map) {

    typedef typename property_map<Graph, vertex_index_t>::const_type IndexMap;
    typedef typename Graph::vertex_iterator VertexIterator;
    typedef typename boost::graph_traits<Graph>::adjacency_iterator AdjacencyIterator;

    IndexMap index_map = get(vertex_index, g);

    VertexIterator v, vs, ve;
    using std::pair;
    std::pair<VertexIterator, VertexIterator> p;
    p = vertices(g);
    vs = p.first;
    ve = p.second;

    integer N = num_vertices(g);

    doublereal *A;
    //    A = (doublereal*)malloc( N*N*sizeof(double) );
    A = new double[N * N];

    int i;

    AdjacencyIterator a, as, ae;
    std::pair<AdjacencyIterator, AdjacencyIterator> ap;

    for (i = 0; i < N * N; i++)
      A[i] = 0;
    i = 0;
    for (v = vs; v != ve; ++v) {
      A[(index_map[*v] * N) + index_map[*v]] = (doublereal)out_degree(*v, g);
      ap = adjacent_vertices(*v, g);
      as = ap.first;
      ae = ap.second;
      for (a = as; a != ae; ++a) {
	A[index_map[*v] + index_map[*a]*N] = (doublereal)(-1);
	A[index_map[*v]*N + index_map[*a]] = (doublereal)(-1);
      }

      pos_map[*v][0] = i;
      pos_map[*v][1] = i;

      i++;
    }

    /*
    cout << "index_map = " << endl;
    for (v = vs; v != ve; ++v) {
      cout << index_map[*v] << " ";
    }
    cout << endl;
    cout << "A = " << endl;
    for (i = 0; i < N; i++)
    {
      for (int j = 0; j < N; j++)
      {
        cout << A[i*N + j] << " ";
      }
        cout << endl;
    }
    */

    // Set up the 8 billion parameters for CLAPACK
    char   JOBZ   = 'v';
    char   RANGE  = 'i';
    char   UPLO   = 'l'; // arbitrary at the moment...
    // N is defined
    // A is defined
    integer    LDA    = N;
    doublereal VL     = 0; // not needed
    doublereal VU     = 0; // not needed
    integer    IL     = 2;// 1; // first NON-ZERO eigenvalue
    integer    IU     = 3;// N; // 4 for the 3d case
    //    char   dlamch_cmach = 's';

    doublereal ABSTOL = 0.01;// dlamch_(&dlamch_cmach); // most likely SEVERE OVERKILL
    integer    M = (IU - IL + 1); // N; // should come back 2 or 3 (for 2d, 3d respectively)
    doublereal *W = new double[N]; // the eigenvalues
    doublereal *Z = new double[N*M]; // the eigenvectors
    integer    LDZ = N;
    integer    *ISUPPZ = new integer[2*M];// (integer*)0; // = new int[2*(IU-IL+1)]; ??? Not needed?
    doublereal *WORK = new double[26*N]; // should this be allocated?
    integer    LWORK  = 26*N; // -1
    integer    *IWORK = new integer[10*N];
    integer    LIWORK = 10*N; // -1
    integer    INFO;

    //    dstegr_()
    dsyevr_(&JOBZ, &RANGE, &UPLO, &N, A, &LDA, &VL, &VU, &IL, &IU, &ABSTOL,
            &M, W, Z, &LDZ, ISUPPZ, WORK, &LWORK, IWORK, &LIWORK, &INFO);

    /*
    cout << "N = " << N << endl;
    cout << "IL = " << INFO << endl;
    cout << "IU = " << INFO << endl;

    cout << "INFO = " << INFO << endl;
    cout << "M = " << M << endl;

    cout << "W = ";
    for (i = 0; i < M; i++)
      cout << W[i] << " ";
    cout << endl;
    for (i = 0; i < M; i++)
      for (int j = 0; j < N; j++)
      {
        cout << Z[i*N + j] << "";
        cout << endl;
      }
    */

    // Set position based on eigenvectors
    i = 0;
    for (v = vs; v != ve; ++v)
    {
      pos_map[*v][0] = Z[0*LDZ + i];
      i++;
    }
    i = 0;
    for (v = vs; v != ve; ++v)
    {
      pos_map[*v][1] = Z[1*LDZ + i];
      i++;
    }

  } // end spectral_embedding_layout()

} // end namespace boost

#endif // BOOST_GRAPH_SPECTRAL_EMBEDDING_LAYOUT_HPP


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef TRACKINGCONTROLIPOPT_HH
#define TRACKINGCONTROLIPOPT_HH

#include <memory>
#include <algorithm>
#include <cstdlib>

#include <boost/timer.hpp>

#include "IpTNLP.hpp"
#include "variationalproblemutil.hh"

struct FirstLess
{
  template <class T>
  bool operator()(std::pair<T,T> const& p) const { return p.first < p.second; }
};

/**
 * Ipopt interface implementation for optimal control problems. This
 * adapts Galerkin operator representations from variational
 * functionals to Ipopt.  Since Ipopt requires not only value and
 * derivatives of the Lagrange functional, the primal variables are
 * split into two quasi-identical groups: those that enter the cost
 * functional (u,y) and those that enter the constraints (v,z).
 */
template <class GOP>
class TrackingControlInterface: public Ipopt::TNLP
{
  typedef GOP Gop;
  typedef typename Gop::RT RT;
  typedef typename Gop::VariableSet VarSet;
  typedef typename VarSet::Grid Grid;
  typedef typename Gop::Functional Functional;

  typedef Ipopt::Index Index;
  typedef Ipopt::TNLP::IndexStyleEnum IndexStyleEnum;
  typedef Ipopt::SolverReturn SolverReturn;
  typedef Ipopt::Number Number;

  enum { U=0, Y, V, Z, LAMBDA, END};
  typedef int AssemblyParts;

public:
  TrackingControlInterface(VarSet const& varSet_,
                           Functional& f_):
    varSet(varSet_),
    gop(varSet),
    f(f_),
    nu(varSet.dimension(U,U+1)),
    ny(varSet.dimension(Y,Y+1)),
    nl(varSet.dimension(LAMBDA,LAMBDA+1)),
    n(nu+ny)
  {}

  /** overload this method to return the number of variables
   *  and constraints, and the number of non-zeros in the jacobian and
   *  the hessian. The index_style parameter lets you specify C or Fortran
   *  style indexing for the sparse matrix iRow and jCol parameters.
   *  C_STYLE is 0-based, and FORTRAN_STYLE is 1-based.
   */
  virtual bool get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
                            Index& nnz_h_lag, IndexStyleEnum& index_style)
  {
    std::cout << "nu=" << nu << " ny=" << ny << " nl=" << nl << '\n';

    n = this->n;
    m = nl;

    nnz_jac_g = gop.nnz(LAMBDA,LAMBDA+1,V,Z+1,false);

    int nnz1 = gop.nnz(U,Y+1,U,Y+1,true);
    int nnz2 = gop.nnz(V,Z+1,V,Z+1,true);
    int nnzMax = std::max(nnz1,nnz2);

    std::vector<int> rows(nnzMax), cols(nnzMax);
    std::vector<RT> data(nnzMax);
    std::vector<std::pair<int,int> > entries(nnz1+nnz2);

    gop.toTriplet(U,Y+1,U,Y+1,rows.begin(),cols.begin(),data.begin(),true);
    for (int i=0; i<nnz1; ++i)
      entries[i] = std::make_pair(rows[i],cols[i]);
    gop.toTriplet(V,Z+1,V,Z+1,rows.begin(),cols.begin(),data.begin(),true);
    for (int i=0; i<nnz2; ++i)
      entries[i+nnz1] = std::make_pair(rows[i],cols[i]);

    // remove upper diagonal entries
    entries.erase(std::remove_if(entries.begin(),entries.end(),FirstLess()),entries.end());
    // remove duplicate entries
    std::sort(entries.begin(),entries.end());
    entries.erase(std::unique(entries.begin(),entries.end()),entries.end());

    nnz_h_lag = entries.size();

    index_style = C_STYLE;

    return true;
  }

  /** overload this method to return the information about the bound
   *  on the variables and constraints. The value that indicates
   *  that a bound does not exist is specified in the parameters
   *  nlp_lower_bound_inf and nlp_upper_bound_inf.  By default,
   *  nlp_lower_bound_inf is -1e19 and nlp_upper_bound_inf is
   *  1e19. (see TNLPAdapter) */
  virtual bool get_bounds_info(Index n, Number* x_l, Number* x_u,
                               Index m, Number* g_l, Number* g_u)
  {
    assert(n == ny+nu);
    assert(m == nl);

    f.boxConstraints(x_l+nu,x_u+nu,x_l,x_u);

    std::fill_n(g_l,nl,0);
    std::fill_n(g_u,nl,0);

    return true;
  }

  /** overload this method to return the starting point. The bools
   *  init_x and init_lambda are both inputs and outputs. As inputs,
   *  they indicate whether or not the algorithm wants you to
   *  initialize x and lambda respectively. If, for some reason, the
   *  algorithm wants you to initialize these and you cannot, set
   *  the respective bool to false.
   */
  virtual bool get_starting_point(Index n, bool init_x, Number* x,
                                  bool init_z, Number* z_L, Number* z_U,
                                  Index m, bool init_lambda,
                                  Number* lambda)
  {
    if (init_x) {
      std::fill_n(x,n,0);
      for (size_t i=0; i<nu; ++i)
        x[i] = static_cast<Number>(std::rand())/RAND_MAX;
    }
    if (init_z) {
      std::fill_n(z_L,n,1);
      std::fill_n(z_U,n,1);
    }
    if (init_lambda) {
      std::fill_n(lambda,m,1);
    }

    return true;
  }

  /** overload this method to return the value of the objective function */
  virtual bool eval_f(Index n, const Number* x, bool new_x,
                      Number& obj_value)
  {
    std::vector<Number> lambda(nl,0);
    assemble(new_x,x,true,&lambda[0],GOP::VALUE);

    obj_value = gop.functional();

    return true;
  }

  /** overload this method to return the vector of the gradient of
   *  the objective w.r.t. x */
  virtual bool eval_grad_f(Index n, const Number* x, bool new_x,
                           Number* grad_f)
  {
    std::vector<Number> lambda(nl,0);
    assemble(new_x,x,true,&lambda[0],GOP::RHS);

    ToSequence<U,LAMBDA>::call(gop,grad_f);

    return true;
  }

  /** overload this method to return the vector of constraint values */
  virtual bool eval_g(Index n, const Number* x, bool new_x,
                      Index m, Number* g)
  {
    assemble(new_x,x,false,0,GOP::RHS);

    ToSequence<LAMBDA,END>::call(gop,g);

    return true;
  }

  /** overload this method to return the jacobian of the
   *  constraints. The vectors iRow and jCol only need to be set
   *  once. The first call is used to set the structure only (iRow
   *  and jCol will be non-NULL, and values will be NULL) For
   *  subsequent calls, iRow and jCol will be NULL. */
  virtual bool eval_jac_g(Index n, const Number* x, bool new_x,
                          Index m, Index nele_jac, Index* iRow,
                          Index *jCol, Number* values)
  {
    if (iRow != 0) {
      // provide sparsity structure
      std::vector<RT> data(nele_jac);
      typename std::vector<RT>::iterator v = data.begin();
      gop.toTriplet(LAMBDA,END,U,LAMBDA,iRow,jCol,v,false);
    } else {
      assert(x);
      assert(values);

      assemble(new_x,x,false,0,GOP::MATRIX);

      std::vector<Index> idx(nele_jac);
      typename std::vector<Index>::iterator i = idx.begin(), j = idx.begin();
      gop.toTriplet(LAMBDA,END,U,LAMBDA,i,j,values,false);
    }

    return true;
  }

  /** overload this method to return the hessian of the
   *  lagrangian. The vectors iRow and jCol only need to be set once
   *  (during the first call). The first call is used to set the
   *  structure only (iRow and jCol will be non-NULL, and values
   *  will be NULL) For subsequent calls, iRow and jCol will be
   *  NULL. This matrix is symmetric - specify the lower diagonal
   *  only.  A default implementation is provided, in case the user
   *  wants to se quasi-Newton approximations to estimate the second
   *  derivatives and doesn't not neet to implement this method. */
  virtual bool eval_h(Index n, const Number* x, bool new_x,
                      Number obj_factor, Index m, const Number* lambda,
                      bool new_lambda, Index nele_hess,
                      Index* iRow, Index* jCol, Number* values)
  {
    int nnz = gop.nnz(U,LAMBDA,U,LAMBDA,true);
    assert(nnz==nele_hess);

    if (values) {
      if (x || lambda) {
        // if (lambda) {
        //   std::cerr << "set lambda: ";
        //   std::copy(lambda,lambda+m,std::ostream_iterator<Number>(std::cerr," "));
        //   std::cerr << '\n';
        // }

        assemble(new_x,x,new_lambda,lambda,GOP::MATRIX,obj_factor);
      } else
        std::cerr << "eval_h called without x or lambda\n";

      std::vector<int> unused(nnz);
      std::vector<int>::iterator ri = unused.begin(), ci = unused.begin();
      gop.toTriplet(U,LAMBDA,U,LAMBDA,ri,ci,values,true);
    } else {
      std::vector<RT> unused(nnz);
      gop.toTriplet(U,LAMBDA,U,LAMBDA,iRow,jCol,unused.begin(),true);
    }

    return true;
  }

  /** This method is called when the algorithm is complete so the TNLP can store/write the solution */
  virtual void finalize_solution(SolverReturn status,
                                 Index n, const Number* x, const Number* z_L, const Number* z_U,
                                 Index m, const Number* g, const Number* lambda,
                                 Number obj_value)
  {
    int nnz = gop.nnz(U,END,U,END,false);
    std::vector<size_t> ri(nnz), ci(nnz);
    std::vector<double> di(nnz);
    gop.toTriplet(U,END,U,END,ri.begin(),ci.begin(),di.begin(),false);

    std::ofstream out("result.m");
    out << "function [A] = result()\n"
        << "ri = [\n";
    std::copy(ri.begin(),ri.end(),std::ostream_iterator<size_t>(out,"\n"));
    out << "];\nci = [\n";
    std::copy(ci.begin(),ci.end(),std::ostream_iterator<size_t>(out,"\n"));
    out << "];\ndi = [\n";
    std::copy(di.begin(),di.end(),std::ostream_iterator<double>(out,"\n"));
    out << "];\n"
        << "A = spconvert([ri+1 ci+1 di]);\n";

    std::vector<RT> data(n+nl,0);
    std::copy(x,x+n,data.begin());
    if (lambda)
      std::copy(lambda,lambda+nl,data.begin()+n);
    f.vars.read(data.begin());
    writeVTKFile(varSet,f.vars,"solution");
  }

  // /** Intermediate Callback method for the user.  Providing dummy
  //  *  default implementation.  For details see IntermediateCallBack
  //  *  in IpNLP.hpp. */
  // virtual bool intermediate_callback(AlgorithmMode mode,
  //                                    Index iter, Number obj_value,
  //                                    Number inf_pr, Number inf_du,
  //                                    Number mu, Number d_norm,
  //                                    Number regularization_size,
  //                                    Number alpha_du, Number alpha_pr,
  //                                    Index ls_trials,
  //                                    const IpoptData* ip_data,
  //                                    IpoptCalculatedQuantities* ip_cq);

private:
  void assemble(bool new_x, Number const* x, bool new_lambda, Number const* lambda, AssemblyParts ap, double sigma = 1.0)
  {
    if (new_x || new_lambda || (sigma!=f.ipoptSigma)) {

      std::vector<RT> data(n+nl);
      f.vars.write(data.begin());

      if (new_x) {
        assert(x);
        std::copy(x,x+n,data.begin());
        // std::cout << "u = " << x[0] << '\n';

      }

      if (new_lambda) {
        assert(lambda);
        std::copy(lambda,lambda+nl,data.begin()+n);
      }

      f.vars.read(data.begin());
      f.ipoptSigma = sigma;

      // it's more efficient to simultaneously assemble functional/rhs/matrix
      boost::timer timer;
      gop.assemble(f);

      // std::cout << "\nAssembled (sigma=" << sigma << "):\nu=";
      // std::copy(data.begin(),data.begin()+nu,std::ostream_iterator<RT>(std::cout," "));
      // std::cout << "\ny="; std::copy(data.begin()+nu,data.begin()+n,std::ostream_iterator<RT>(std::cout," "));
      // std::cout << "\nl="; std::copy(data.begin()+n,data.begin()+n+nl,std::ostream_iterator<RT>(std::cout," "));
      // toSequence<0,3>(gop,data.begin());
      // std::cout << "\nLagrange gradient:\ndu="; std::copy(data.begin(),data.begin()+nu,std::ostream_iterator<RT>(std::cout," "));
      // std::cout << "\ndy="; std::copy(data.begin()+nu,data.begin()+n,std::ostream_iterator<RT>(std::cout," "));
      // std::cout << "\ndl="; std::copy(data.begin()+n,data.begin()+n+nl,std::ostream_iterator<RT>(std::cout," "));
      // std::cout << "\n";

      //std::cout << "assembly time: " << timer.elapsed() << "s\n";
      // std::cout << "As. u=[";
      // std::cout.precision(2);
      // std::copy(x+ny,x+ny+nu,std::ostream_iterator<Number>(std::cout,","));
      // std::cout << "]\n";
    }
  }

  VarSet const& varSet;
  Gop gop;
  Functional& f;
  int nu, ny, nl, n;
};

#endif