/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <boost/math/constants/constants.hpp>

#include "NGLMExt.hpp"
#include "GLMPrint.hpp"
#include "NPY.hpp"

#include "NQuad.hpp"
#include "NBBox.hpp"
#include "NTrianglesNPY.hpp"
#include "NTesselate.hpp"
#include "NTriangle.hpp"

#include "NTris.hpp"

#include "PLOG.hh"

#ifdef _MSC_VER
// instanciations of 'NTrianglesNPY' raise warning:
//    object allocated on the heap may not be aligned 16
// https://github.com/g-truc/glm/issues/235
// apparently fixed by 0.9.7.1 Release : currently on 0.9.6.3

#pragma warning( disable : 4316 )
#endif

const char* NTrianglesNPY::PLACEHOLDER = "PLACEHOLDER" ;

NPY<float>* NTrianglesNPY::getTris() const
{
    return m_tris ;
}
NPY<float>* NTrianglesNPY::getNormals() const
{
    return m_normals ;
}

float NTrianglesNPY::maxdiff( const NTrianglesNPY* other, bool dump )
{
    return m_tris->maxdiff( other->getTris(), dump );
}

void NTrianglesNPY::dump(const char* msg) const
{
    LOG(info) << "NTrianglesNPY::dump"
              << " tris " << m_tris->getShapeString()
              ;

    m_tris->dump(msg);
}

void NTrianglesNPY::setPoly(const std::string& poly)
{
   m_poly = poly ;
}
const std::string& NTrianglesNPY::getPoly()
{
   return m_poly ;
}

void NTrianglesNPY::setMessage(const std::string& msg)
{
   m_message = msg ;
}
const std::string& NTrianglesNPY::getMessage()
{
   return m_message ;
}

bool NTrianglesNPY::hasMessage(const std::string& msg)
{
    return m_message.compare(msg) == 0 ;
}
bool NTrianglesNPY::isPlaceholder()
{
    return m_message.compare(PLACEHOLDER) == 0 ;
}

void NTrianglesNPY::setTransform(const glm::mat4& transform_)
{
    m_transform = transform_ ;
}
glm::mat4 NTrianglesNPY::getTransform()
{
    return m_transform ;
}

NTrianglesNPY::NTrianglesNPY(const NTriSource* tris)
   :
   m_tris(NPY<float>::make(0,3,3)),
   m_normals(NPY<float>::make(0,3,3)),
   m_transform(1.0f)
{
    glm::uvec3 t ;
    glm::vec3 a ;
    glm::vec3 b ;
    glm::vec3 c ;

    unsigned ntri = tris->get_num_tri();
    for(unsigned i=0 ; i < ntri ; i++)
    {
        tris->get_tri(i,t,a,b,c );
        add(a,b,c);
    }
}

NTrianglesNPY::NTrianglesNPY()
   :
   m_tris(NPY<float>::make(0,3,3)),
   m_normals(NPY<float>::make(0,3,3)),
   m_transform(1.0f)
{
}

NTrianglesNPY::NTrianglesNPY(NPY<float>* tris, NPY<float>* normals)
   :
   m_tris(tris),
   m_normals(normals),
   m_transform(1.0f)
{
}

NTrianglesNPY* NTrianglesNPY::transform(glm::mat4& m)
{
    NPY<float>* tbuf = m_tris->transform(m);

    NTrianglesNPY* t = new NTrianglesNPY(tbuf);

    return t ;
}

NTrianglesNPY* NTrianglesNPY::subdivide(unsigned int nsubdiv)
{
    NTesselate* tess = new NTesselate(m_tris);

    tess->subdivide(nsubdiv);

    NPY<float>* tbuf = tess->getBuffer();

    NTrianglesNPY* t = new NTrianglesNPY(tbuf);

    t->setTransform( getTransform() );

    return t ;
}

unsigned int NTrianglesNPY::getNumTriangles()
{
    return m_tris->getNumItems();
}

void NTrianglesNPY::add(const glm::vec3& a, const glm::vec3& b, const glm::vec3& c, const glm::vec3& d)
{
   /*
         a------d
         |    . |
         |  .   |
         |.     |
         b------c

   */
    add(a,b,d);
    add(d,b,c);
}

void NTrianglesNPY::add(const glm::vec3& a, const glm::vec3& b, const glm::vec3& c)
{
    ntriangle t(a,b,c);
    add(t);
}

void NTrianglesNPY::addNormal(const glm::vec3& nrm )  // in triplicate
{
    int n = 3*3 ;
    float* vals = new float[n] ;
    for(int i=0 ; i < 3 ; i++)
    {
       vals[i*3+0] = nrm.x ;
       vals[i*3+1] = nrm.y ;
       vals[i*3+2] = nrm.z ;
    }
    m_normals->add(vals, n);
    delete [] vals ;
}

void NTrianglesNPY::addNormal(const glm::vec3& a, const glm::vec3& b, const glm::vec3& c )
{
    float vals[9] ;

    vals[0*3 + 0 ] = a.x ;
    vals[0*3 + 1 ] = a.y ;
    vals[0*3 + 2 ] = a.z ;

    vals[1*3 + 0 ] = b.x ;
    vals[1*3 + 1 ] = b.y ;
    vals[1*3 + 2 ] = b.z ;

    vals[2*3 + 0 ] = c.x ;
    vals[2*3 + 1 ] = c.y ;
    vals[2*3 + 2 ] = c.z ;

    m_normals->add(vals, 9);
}

void NTrianglesNPY::add(const ntriangle& t )
{
    unsigned int n = 3*3 ;
    float* vals = new float[n] ;
    t.copyTo(vals);
    m_tris->add(vals, n);
    delete [] vals ;
}

void NTrianglesNPY::add(NTrianglesNPY* other )
{
    m_tris->add(other->getTris());
}

nbbox* NTrianglesNPY::findBBox()
{
    NPY<float>* buf = getTris();
    assert(buf && buf->hasItemShape(3,3));
    unsigned nitem = buf->getShape(0);
    nbbox* bb = NULL ;
    if(nitem > 0)
    {
        ntrange3<float> r = buf->minmax3();
        bb = new nbbox ;
        bb->min.x = r.min.x ;
        bb->min.y = r.min.y ;
        bb->min.z = r.min.z ;

        bb->max.x = r.max.x ;
        bb->max.y = r.max.y ;
        bb->max.z = r.max.z ;
    }
    return bb ;
}

/* for icosahedron

* 20 faces (triangular)
* 12 verts
* 30 edges

*/
#define CZ (0.89442719099991)   /*  2/sqrt(5) */
#define SZ (0.44721359549995)   /*  1/sqrt(5) */
#define C1 (0.951056516)        /* cos(18),  */
#define S1 (0.309016994)        /* sin(18) */
#define C2 (0.587785252)        /* cos(54),  */
#define S2 (0.809016994)        /* sin(54) */
#define X1 (C1*CZ)
#define Y1 (S1*CZ)
#define X2 (C2*CZ)
#define Y2 (S2*CZ)

const glm::vec3 NTrianglesNPY::Ip0 = glm::vec3(0,0,1.) ;
const glm::vec3 NTrianglesNPY::Ip1 = glm::vec3(-X2,-Y2,SZ) ;
const glm::vec3 NTrianglesNPY::Ip2 = glm::vec3( X2,-Y2,SZ) ;
const glm::vec3 NTrianglesNPY::Ip3 = glm::vec3( X1, Y1,SZ) ;
const glm::vec3 NTrianglesNPY::Ip4 = glm::vec3(  0, CZ,SZ) ;
const glm::vec3 NTrianglesNPY::Ip5 = glm::vec3(-X1, Y1,SZ) ;

const glm::vec3 NTrianglesNPY::Im0 = glm::vec3(-X1, -Y1,-SZ) ;
const glm::vec3 NTrianglesNPY::Im1 = glm::vec3(  0, -CZ,-SZ) ;
const glm::vec3 NTrianglesNPY::Im2 = glm::vec3( X1, -Y1,-SZ) ;
const glm::vec3 NTrianglesNPY::Im3 = glm::vec3( X2,  Y2,-SZ) ;
const glm::vec3 NTrianglesNPY::Im4 = glm::vec3(-X2,  Y2,-SZ) ;
const glm::vec3 NTrianglesNPY::Im5 = glm::vec3(0,0,-1.) ;

NTrianglesNPY* NTrianglesNPY::icosahedron()
{
    NTrianglesNPY* tris = new NTrianglesNPY();

    /* front pole */
    tris->add(Ip0, Ip1, Ip2);
    tris->add(Ip0, Ip5, Ip1);
    tris->add(Ip0, Ip4, Ip5);
    tris->add(Ip0, Ip3, Ip4);
    tris->add(Ip0, Ip2, Ip3);

    /* mid */
    tris->add(Ip1, Im0, Im1);
    tris->add(Im0, Ip1, Ip5);
    tris->add(Ip5, Im4, Im0);
    tris->add(Im4, Ip5, Ip4);
    tris->add(Ip4, Im3, Im4);
    tris->add(Im3, Ip4, Ip3);
    tris->add(Ip3, Im2, Im3);
    tris->add(Im2, Ip3, Ip2);
    tris->add(Ip2, Im1, Im2);
    tris->add(Im1, Ip2, Ip1);

    /* back pole */
    tris->add(Im3, Im2, Im5);
    tris->add(Im4, Im3, Im5);
    tris->add(Im0, Im4, Im5);
    tris->add(Im1, Im0, Im5);
    tris->add(Im2, Im1, Im5);

    return tris ;
}

NTrianglesNPY* NTrianglesNPY::from_indexed( NPY<float>* vtx, NPY<unsigned>* idx )
{
    assert( vtx->hasShape( -1, 3) && idx->hasShape(-1,3) );
    // each idx entry of 3 unsigned ints points to three vertices of the triangle

    unsigned nvtx = vtx->getShape(0) ;
    unsigned ntri = idx->getShape(0) ;

    LOG(info) << " idx " << idx->getShapeString() ;
    LOG(info) << " vtx " << vtx->getShapeString() ;

    NTrianglesNPY* tris = new NTrianglesNPY();

    for(unsigned i=0 ; i < ntri ; i++)
    {
        unsigned v0 = idx->getValue( i, 0, 0,  0 );
        unsigned v1 = idx->getValue( i, 0, 0,  1 );
        unsigned v2 = idx->getValue( i, 0, 0,  2 );

        assert( v0 < nvtx );
        assert( v1 < nvtx );
        assert( v2 < nvtx );

        glm::vec3 vtx0 ;
        vtx0.x = vtx->getValue( v0, 0, 0,  0 );
        vtx0.y = vtx->getValue( v0, 0, 0,  1 );
        vtx0.z = vtx->getValue( v0, 0, 0,  2 );

        glm::vec3 vtx1 ;
        vtx1.x = vtx->getValue( v1, 0, 0,  0 );
        vtx1.y = vtx->getValue( v1, 0, 0,  1 );
        vtx1.z = vtx->getValue( v1, 0, 0,  2 );

        glm::vec3 vtx2 ;
        vtx2.x = vtx->getValue( v2, 0, 0,  0 );
        vtx2.y = vtx->getValue( v2, 0, 0,  1 );
        vtx2.z = vtx->getValue( v2, 0, 0,  2 );

        tris->add( vtx0, vtx1, vtx2 );
    }
    return tris ;
}

void NTrianglesNPY::to_vtxidx(NVtxIdx& vtxidx)
{
    assert( m_tris->hasShape(-1,3,3) );  // items with 3*3 float for each tri
    unsigned ntri = m_tris->getShape(0) ;

    NPY<float>* vtx = NPY<float>::copy( m_tris ) ;
    vtx->reshape( ntri*3 , 3 );

    NPY<unsigned>* idx = NPY<unsigned>::make( ntri, 3 ) ;
    idx->zero();

    vtxidx.vtx = vtx ;   // duplicate vertices for each use in a face
    vtxidx.idx = idx ;

    for(unsigned i=0 ; i < ntri ; i++)
    {
        idx->setValue( i, 0, 0,  0,  i*3+0  );
        idx->setValue( i, 0, 0,  1,  i*3+1  );
        idx->setValue( i, 0, 0,  2,  i*3+2  );
    }
}

const glm::vec3 NTrianglesNPY::PX = glm::vec3(1,0,0) ;
const glm::vec3 NTrianglesNPY::PY = glm::vec3(0,1,0) ;
const glm::vec3 NTrianglesNPY::PZ = glm::vec3(0,0,1) ;
const glm::vec3 NTrianglesNPY::MX = glm::vec3(-1,0,0) ;
const glm::vec3 NTrianglesNPY::MY = glm::vec3(0,-1,0) ;
const glm::vec3 NTrianglesNPY::MZ = glm::vec3(0,0,-1) ;

NTrianglesNPY* NTrianglesNPY::octahedron()
{
    NTrianglesNPY* tris = new NTrianglesNPY();

    // PZ pyramid
    tris->add(PX,PZ,MY);
    tris->add(MY,PZ,MX);
    tris->add(MX,PZ,PY);
    tris->add(PY,PZ,PX);

    // MZ pyramid
    tris->add(PX,MY,MZ);
    tris->add(MY,MX,MZ);
    tris->add(MX,PY,MZ);
    tris->add(PY,PX,MZ);

    return tris ;
}

NTrianglesNPY* NTrianglesNPY::hemi_octahedron()
{
    NTrianglesNPY* tris = new NTrianglesNPY();

    // PZ pyramid
    tris->add(PX,PZ,MY);
    tris->add(MY,PZ,MX);
    tris->add(MX,PZ,PY);
    tris->add(PY,PZ,PX);

    return tris ;
}

NTrianglesNPY* NTrianglesNPY::sphere(unsigned int n_polar, unsigned int n_azimuthal)
{
    glm::vec4 param(-1.f, 1.f, 0.f, 1.f);
    return sphere(param, n_polar, n_azimuthal);
}

void NTrianglesNPY::setTransform(const glm::vec3& scale, const glm::vec3& translate)
{
    glm::mat4 m_scale = glm::scale(glm::mat4(1.0f), scale);
    glm::mat4 m_translate = glm::translate(glm::mat4(1.0f), translate);
    glm::mat4 mat = m_translate * m_scale ;

    //print(mat, "NTrianglesNPY::setTransform");

    setTransform(mat);
}

NTrianglesNPY* NTrianglesNPY::sphere(glm::vec4& param, unsigned int n_polar, unsigned int n_azimuthal)
{
    float ctmin = param.x ;
    float ctmax = param.y ;
    float zpos  = param.z ;
    float radius = param.w ;

    // unit sphere at origin
    NTris* ts = NTris::make_sphere( n_polar, n_azimuthal, ctmin, ctmax );

    NTrianglesNPY* tris = new NTrianglesNPY(ts);

    glm::vec3 scale(radius);
    glm::vec3 translate(0,0,zpos);
    tris->setTransform(scale, translate);

    // dont apply the transform yet, as whilst sphere at origin
    // can easily get normals from the positions

    return tris ;
}

/*
         p0     p1

     t0  x00----x10
          |   .  |
          | .    |
     t1  x01----x11

      t = 0  => t0 = 0
               x00 x10 degenerate (0,0,1)

      t = n_polar - 1 => t1 = pi
               x01 x11 degenerate (0,0,-1)

*/

NTrianglesNPY* NTrianglesNPY::disk(glm::vec4& param, unsigned int n_azimuthal)
{
    float ct = param.x ;
    double ct0, st0 ;
    sincos_<double>(acos(ct), st0, ct0 );
    float pi = boost::math::constants::pi<float>() ;

    NTrianglesNPY* tris = new NTrianglesNPY();
    for(unsigned int p=0 ; p < n_azimuthal ; p++)
    {
        float p0 = 2.0f*pi*float(p)/n_azimuthal ;
        float p1 = 2.0f*pi*float(p+1)/n_azimuthal ;

        double sp0,sp1,cp0,cp1 ;
        sincos_<double>(p0, sp0, cp0 );
        sincos_<double>(p1, sp1, cp1 );

        glm::vec3 x0(   st0*cp0,  st0*sp0,   ct0 );
        glm::vec3 x1(   st0*cp1,  st0*sp1,   ct0 );
        glm::vec3 xc(         0,        0,   ct0 );

        tris->add(x0,x1,xc); // winding order directs normal along -z
    }
    return tris ;
}

const glm::vec3 NTrianglesNPY::PXPYPZ = glm::vec3(1,1,1) ;
const glm::vec3 NTrianglesNPY::PXPYMZ = glm::vec3(1,1,-1) ;
const glm::vec3 NTrianglesNPY::PXMYPZ = glm::vec3(1,-1,1) ;
const glm::vec3 NTrianglesNPY::PXMYMZ = glm::vec3(1,-1,-1) ;

const glm::vec3 NTrianglesNPY::MXPYPZ = glm::vec3(-1,1,1) ;
const glm::vec3 NTrianglesNPY::MXPYMZ = glm::vec3(-1,1,-1) ;
const glm::vec3 NTrianglesNPY::MXMYPZ = glm::vec3(-1,-1,1) ;
const glm::vec3 NTrianglesNPY::MXMYMZ = glm::vec3(-1,-1,-1) ;

NTrianglesNPY* NTrianglesNPY::cube()
{
    NTrianglesNPY* tris = new NTrianglesNPY();

    tris->add(PXPYPZ, MXPYPZ, MXMYPZ, PXMYPZ);    // PZ face
    tris->add(PXPYPZ, PXMYPZ, PXMYMZ, PXPYMZ);    // PX face
    tris->add(PXPYPZ, PXPYMZ, MXPYMZ, MXPYPZ);    // PY face

    tris->add(MXMYMZ, PXMYMZ, PXPYMZ, MXPYMZ);    // MZ face
    tris->add(MXMYMZ, MXPYMZ, MXPYPZ, MXMYPZ);    // MX face
    tris->add(MXMYMZ, MXMYPZ, PXMYPZ, PXMYMZ);    // MY face

    return tris ;
}

NTrianglesNPY* NTrianglesNPY::box(const nbbox& bb)
{
    NTrianglesNPY* tris = new NTrianglesNPY();

    glm::vec3 P(bb.max.x, bb.max.y, bb.max.z );
    glm::vec3 M(bb.min.x, bb.min.y, bb.min.z );

    glm::vec3 _PXPYPZ( P.x, P.y, P.z );
    glm::vec3 _PXPYMZ( P.x, P.y, M.z );
    glm::vec3 _PXMYPZ( P.x, M.y, P.z );
    glm::vec3 _PXMYMZ( P.x, M.y, M.z );

    glm::vec3 _MXPYPZ( M.x, P.y, P.z );
    glm::vec3 _MXPYMZ( M.x, P.y, M.z );
    glm::vec3 _MXMYPZ( M.x, M.y, P.z );
    glm::vec3 _MXMYMZ( M.x, M.y, M.z ) ;

    tris->add(_PXPYPZ, _MXPYPZ, _MXMYPZ, _PXMYPZ);    // PZ face
    tris->add(_PXPYPZ, _PXMYPZ, _PXMYMZ, _PXPYMZ);    // PX face
    tris->add(_PXPYPZ, _PXPYMZ, _MXPYMZ, _MXPYPZ);    // PY face

    tris->add(_MXMYMZ, _PXMYMZ, _PXPYMZ, _MXPYMZ);    // MZ face
    tris->add(_MXMYMZ, _MXPYMZ, _MXPYPZ, _MXMYPZ);    // MX face
    tris->add(_MXMYMZ, _MXMYPZ, _PXMYPZ, _PXMYMZ);    // MY face

    return tris ;
}

NTrianglesNPY* NTrianglesNPY::prism(const glm::vec4& param)
{
/*
::

   .

               A'
                    A  (0,height)
                   /|\
                  / | \
                 /  |  \
                /   h   \
               /    |    \ (x,y)
         L'   M     |     N
             /      |      \
            L-------O-------R

     (-hwidth,0)  (0,0)    (hwidth, 0)

*/

    // hmm how to avoid duplication between here and hemi-pmt.cu/make_prism
    float angle = param.x > 0.f ? param.x : 60.f ;
    float height = param.y > 0.f ? param.y : param.w  ;
    float depth = param.z > 0.f ? param.z : param.w ;

    float pi = boost::math::constants::pi<float>() ;
    float hwidth = height*tan((pi/180.f)*angle/2.0f) ;

    float ymax =  height/2.0f ;
    float ymin = -height/2.0f ;

    glm::vec3 apex_near(0.f,     ymax, depth/2.f );
    glm::vec3 apex_far( 0.f,     ymax, -depth/2.f );

    glm::vec3 left_near(-hwidth,  ymin, depth/2.f);
    glm::vec3 left_far(-hwidth,   ymin, -depth/2.f);

    glm::vec3 right_near( hwidth, ymin, depth/2.f);
    glm::vec3 right_far( hwidth,  ymin, -depth/2.f);

    NTrianglesNPY* tris = new NTrianglesNPY();

    tris->add( apex_near, apex_far, left_far, left_near );
    tris->add( apex_near, right_near,  right_far,  apex_far );
    tris->add( left_near, left_far, right_far, right_near );
    tris->add( apex_near, left_near, right_near );
    tris->add( apex_far,  right_far, left_far );

    return tris ;
}


#ifndef JHMI_PT3_HPP_NRC_20141031
#define JHMI_PT3_HPP_NRC_20141031

#include <boost/functional/hash.hpp>
#include <boost/units/units_fwd.hpp>
#include <cmath>
#include <iostream>
#include <type_traits>

namespace jhmi {
  namespace jhmi_detail {
    template <typename Src, typename Dst>
    struct is_lossy_convertible
    {
      static const bool value =  (std::is_integral<Dst>::value && !std::is_integral<Src>::value)
        || (std::is_integral<Src>::value && !std::is_integral<Dst>::value
            && sizeof(Src) == sizeof(Dst))
        || sizeof(Src) > sizeof(Dst);
    };

    template <typename Src, typename Dst>
    struct is_non_narrowing_conversion
    {
      static const bool value = std::is_convertible<Src,Dst>::value
        && !((std::is_arithmetic<Src>::value && std::is_arithmetic<Dst>::value)
             || is_lossy_convertible<Src,Dst>::value);
    };

  }//end jhmi_detail

  template <typename T> struct pt3
  {
    using type = T;
    constexpr pt3() : x(), y(), z() {}
    constexpr pt3(T x, T y, T z) : x(x),y(y),z(z) {}

    template <typename U>
    constexpr pt3(pt3<U> const& u, typename std::enable_if<
        jhmi_detail::is_non_narrowing_conversion<U,T>::value>::type* = nullptr)
      : x(u.x),
        y(u.y),
        z(u.z)
    {}
    template <typename U>
    constexpr explicit pt3(pt3<U> const& u, typename std::enable_if<
        !jhmi_detail::is_non_narrowing_conversion<U,T>::value>::type* = nullptr)
      : x(static_cast<T>(u.x)),
        y(static_cast<T>(u.y)),
        z(static_cast<T>(u.z))
    {}

    constexpr pt3<T>& operator+=(pt3<T> const& rhs)
    { x += rhs.x; y += rhs.y; z += rhs.z; return *this; }
    constexpr pt3<T>& operator-=(pt3<T> const& rhs)
    { x -= rhs.x; y -= rhs.y; z -= rhs.z; return *this; }

    T x, y, z;
  };
  template <typename T> constexpr auto operator+(pt3<T> const& pt)
  { return pt; }
  template <typename T> constexpr auto operator-(pt3<T> const& pt)
  { return pt3<T>{-pt.x, -pt.y, -pt.z}; }
  template <typename T, typename U> constexpr auto operator+(pt3<T> const& lhs, pt3<U> const& rhs)
  { return pt3<decltype(lhs.x+rhs.x)>{lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z}; }
  template <typename T, typename U> constexpr auto operator-(pt3<T> const& lhs, pt3<U> const& rhs)
  { return pt3<decltype(lhs.x-rhs.x)>{lhs.x - rhs.x, lhs.y - rhs.y, lhs.z - rhs.z}; }
  template <typename T, typename U> constexpr auto operator*(pt3<T> const& lhs, U rhs)
  { return pt3<decltype(lhs.x*rhs)>{lhs.x * rhs, lhs.y * rhs, lhs.z * rhs}; }
  template <typename T, typename U> constexpr auto operator*(U lhs, pt3<T> const& rhs)
  { return pt3<decltype(lhs*rhs.x)>{lhs * rhs.x, lhs * rhs.y, lhs * rhs.z}; }
  template <typename T, typename D, typename S> constexpr auto operator*(pt3<T> const& lhs, boost::units::unit<D,S> rhs)
  { return pt3<decltype(lhs.x*rhs)>{lhs.x * rhs, lhs.y * rhs, lhs.z * rhs}; }
  template <typename T, typename D, typename S> constexpr auto operator*(boost::units::unit<D,S> lhs, pt3<T> const& rhs)
  { return pt3<decltype(lhs*rhs.x)>{lhs * rhs.x, lhs * rhs.y, lhs * rhs.z}; }
  template <typename T, typename U> constexpr auto operator/(pt3<T> const& lhs, U rhs)
  { return pt3<decltype(lhs.x/rhs)>{lhs.x / rhs, lhs.y / rhs, lhs.z / rhs}; }
  template <typename T, typename D, typename S> constexpr auto operator/(pt3<T> const& lhs, boost::units::unit<D,S> rhs)
  { return pt3<decltype(lhs.x/rhs)>{lhs.x / rhs, lhs.y / rhs, lhs.z / rhs}; }
  template <typename T> constexpr auto operator==(pt3<T> const& lhs, pt3<T> const& rhs)
  { return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z; }
  template <typename T> constexpr auto operator!=(pt3<T> const& lhs, pt3<T> const& rhs)
  { return !(lhs == rhs); }
  template <typename T> constexpr bool operator<(pt3<T> const& lhs, pt3<T> const& rhs)
  { return lhs.x < rhs.x || (lhs.x == rhs.x && (lhs.y < rhs.y || (lhs.y == rhs.y && lhs.z < rhs.z))); }
  template <typename T> std::ostream& operator<<(std::ostream& out, pt3<T> const& pt)
  { out << '[' << pt.x << ',' << pt.y << ',' << pt.z << ']'; return out; }
  template <typename T> std::size_t hash_value(pt3<T> const& pt) {
    std::size_t seed = 0;
    boost::hash_combine(seed, pt.x);
    boost::hash_combine(seed, pt.y);
    boost::hash_combine(seed, pt.z);
    return seed;
  }

  template <typename T> constexpr auto abs(pt3<T> const& v)
  { using std::abs; return pt3<T>{abs(v.x), abs(v.y), abs(v.z)}; }
  template <typename T> constexpr auto ceil(pt3<T> const& v)
  { using std::ceil; return pt3<T>{ceil(v.x), ceil(v.y), ceil(v.z)}; }
  template <typename T> constexpr auto cross(pt3<T> const& lhs, pt3<T> const& rhs) {
    return pt3<T>{lhs.y * rhs.z - lhs.z * rhs.y,
                  lhs.z * rhs.x - lhs.x * rhs.z,
                  lhs.x * rhs.y - lhs.y * rhs.x};
  }
  //Overloaded since we're losing precision with normalizing code.
  template <typename T, typename U> constexpr auto cross(pt3<boost::units::quantity<T,U>> const& lhs, pt3<boost::units::quantity<T,U>> const& rhs) {
    return pt3<boost::units::quantity<T,U>>{
      boost::units::quantity<T,U>::from_value(lhs.y.value() * rhs.z.value() - lhs.z.value() * rhs.y.value()),
      boost::units::quantity<T,U>::from_value(lhs.z.value() * rhs.x.value() - lhs.x.value() * rhs.z.value()),
      boost::units::quantity<T,U>::from_value(lhs.x.value() * rhs.y.value() - lhs.y.value() * rhs.x.value())};
  }
  template <typename T> auto distance(pt3<T> const& pt)
  { using std::sqrt; return sqrt(distance_squared(pt)); }
  template <typename T> constexpr auto distance_squared(pt3<T> const& pt)
  { return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z; }
  template <typename T> constexpr auto distance_squared(pt3<T> const& lhs, pt3<T> const& rhs)
  { return distance_squared(lhs - rhs); }
  template <typename T, typename U> constexpr auto dot(pt3<T> const& lhs, pt3<U> const& rhs)
  { return lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z; }
  template <typename T, typename U> constexpr auto element_multiply(pt3<T> const& lhs, pt3<U> const& rhs) {
    return pt3<decltype(lhs.x * rhs.x)>{lhs.x * rhs.x, lhs.y * rhs.y, lhs.z * rhs.z};
  }
  template <typename T, typename U> constexpr auto element_divide(pt3<T> const& lhs, pt3<U> const& rhs) {
    return pt3<decltype(lhs.x / rhs.x)>{lhs.x / rhs.x, lhs.y / rhs.y, lhs.z / rhs.z};
  }
  template <typename T, typename U> constexpr auto element_modulus(pt3<T> const& lhs, pt3<U> const& rhs) {
    return pt3<decltype(lhs.x % rhs.x)>{lhs.x % rhs.x, lhs.y % rhs.y, lhs.z % rhs.z};
  }
  template <typename T> auto element_max(pt3<T> const& lhs, pt3<T> const& rhs) {
    return pt3<T>{std::max(lhs.x, rhs.x), std::max(lhs.y, rhs.y), std::max(lhs.z, rhs.z)};
  }
  template <typename T> auto element_min(pt3<T> const& lhs, pt3<T> const& rhs) {
    return pt3<T>{std::min(lhs.x, rhs.x), std::min(lhs.y, rhs.y), std::min(lhs.z, rhs.z)};
  }
  template <typename T> pt3<T> floor(pt3<T> const& pt)
  { using std::floor; return {floor(pt.x), floor(pt.y), floor(pt.z)}; }
  template <typename T> auto normalize(pt3<T> const& pt)
  { return pt / distance(pt); }
  template <typename T> auto round(pt3<T> const& pt)
  { using std::round; return pt3<T>{round(pt.x), round(pt.y), round(pt.z)}; }
  template <typename U, typename T> auto round_to(pt3<T> const& pt)
  { using std::lround; return pt3<U>(lround(pt.x), lround(pt.y), lround(pt.z)); }

  typedef pt3<int> int3;
  typedef pt3<float> flt3;
  typedef pt3<double> dbl3;
  typedef pt3<long double> ldb3;
}//jhmi
#endif


// Copyright (C) 2019 Maciej Drwal
//
// Permission is granted to copy and distribute verbatim copies and modified
// versions of this file, provided that the copyright notice and this permission
// notice are preserved on all copies and modified versions of this file.
//

#include <iostream>

#include <boost/spirit/home/x3.hpp>

#include "parser.h"
#include "simplex.h"
#include "utils.h"

struct ParserState
{
    ParserState(LinearProgram& _lp) : lp(_lp), var_coeff_cache(0.0), sign_cache(1.0), label_cache("") {}

    LinearProgram& lp;
    double var_coeff_cache;
    double sign_cache;
    std::string label_cache;
};

namespace x3 = boost::spirit::x3;
namespace ascii = boost::spirit::x3::ascii;

namespace parser
{
    using x3::double_;
    using x3::eoi;
    using x3::lexeme;
    using x3::lit;
    using x3::no_case;
    using x3::skip;

    using ascii::alnum;
    using ascii::alpha;
    using ascii::char_;
    using ascii::space;

    // Semantic actions definitions
    auto set_minimize       = [](auto& ctx) { _val(ctx).lp.set_sense('m'); };
    auto set_maximize       = [](auto& ctx) { _val(ctx).lp.set_sense('M'); };
    auto set_obj_label      = [](auto& ctx) { _val(ctx).lp.set_objective_label(_attr(ctx)); };
    auto add_obj_fun_coeff  = [](auto& ctx) { _val(ctx).var_coeff_cache = _attr(ctx); };
    auto add_obj_fun_var    = [](auto& ctx)
    {
        // create new variable and store associated obj.fun. coefficient
        auto var_name = _attr(ctx);
        if (_val(ctx).lp.has_variable(var_name)) {
            throw "Parser: variable already defined in objective function.";
        }
        _val(ctx).lp.add_variable(var_name);
        _val(ctx).lp.objective_name_coeff[var_name] =
            _val(ctx).sign_cache * _val(ctx).var_coeff_cache;
    };
    auto neg_var_coeff      = [](auto& ctx) { _val(ctx).sign_cache = -1; };
    auto term_parsed        = [](auto& ctx)
    {
        _val(ctx).sign_cache = 1;
        _val(ctx).var_coeff_cache = 1;
    };
    auto add_obj_fun_const  = [](auto& ctx) { _val(ctx).lp.set_obj_value_shift(_attr(ctx)); };

    auto set_constr_label   = [](auto& ctx)
    {
        // create labeled constraint
        const auto& constraint = Constraint();
        _val(ctx).label_cache = _attr(ctx);
        _val(ctx).lp.constraints.emplace(_attr(ctx), constraint);
    };
    auto add_constr_coeff   = [](auto& ctx) { _val(ctx).var_coeff_cache = _attr(ctx); };
    auto add_constr_var     = [](auto& ctx)
    {
        auto& constr_label = _val(ctx).label_cache;
        if (constr_label == "") {
            // no label was defined by user; create new constraint with default label
            const auto& constraint = Constraint();
            constr_label = std::string("CONSTR") + tostr<size_t>(_val(ctx).lp.constraints.size());
            _val(ctx).label_cache = constr_label;
            _val(ctx).lp.constraints.emplace(constr_label, constraint);
        }

        auto var_name = _attr(ctx);

        if (_val(ctx).lp.constraints[constr_label].name_coeff.count(var_name) != 0) {
            throw (std::string("Parser: variable already defined in constraint") + constr_label).c_str();
        }

        if (!_val(ctx).lp.has_variable(var_name)) {
            // adding new variable via constraint
            _val(ctx).lp.add_variable(var_name);
        }

        auto coeff = _val(ctx).var_coeff_cache;
        auto sign  = _val(ctx).sign_cache;
        _val(ctx).lp.constraints[constr_label].name_coeff[var_name] = sign * coeff;
    };

    auto add_constr_type = [](auto& ctx)
    {
        _val(ctx).lp.constraints[_val(ctx).label_cache].type = _attr(ctx);
    };
    auto add_constr_rhs  = [](auto& ctx)
    {
        double _rhs = _attr(ctx);
        _val(ctx).lp.constraints[_val(ctx).label_cache].rhs = _rhs;
        auto op = _val(ctx).lp.constraints[_val(ctx).label_cache].type;
        if ((op == '<' && _rhs < 0.0) ||
            (op == '>' && _rhs > 0.0) ||
            (op == '=')) _val(ctx).lp.set_all_inequalities(false);
        _val(ctx).label_cache = "";
    };

    auto add_lb_constr   = [](auto& ctx)
    {
        auto lb = _val(ctx).var_coeff_cache;  // now it contains LB
        auto var_name = _attr(ctx);
        _val(ctx).label_cache = var_name;  // store var_name, in case we need to add UB
        if (_isfloatzero(lb)) return;
        _val(ctx).lp.var_lbnd[var_name] = lb;
    };
    auto add_ub_constr     = [](auto& ctx)
    {
        auto var_name = _val(ctx).label_cache;    // now it contains var_name
        auto ub = _attr(ctx);
        _val(ctx).lp.var_ubnd[var_name] = ub;
    };
    auto store_ub_var_name = [](auto& ctx) { _val(ctx).label_cache = _attr(ctx); };
    auto add_inf_lb        = [](auto& ctx)
    {
        auto lb = std::numeric_limits<double>::min();
        _val(ctx).var_coeff_cache = lb;
    };
    auto add_inf_ub        = [](auto& ctx)
    {
        auto var_name = _val(ctx).label_cache;    // now it contains var_name
        auto ub = std::numeric_limits<double>::max();
        _val(ctx).lp.var_ubnd[var_name] = ub;
    };

    auto print_attr = [](auto& ctx) { std::cout << _attr(ctx) << std::endl; };

    // Grammar rules definitions
    // TODO: handle properly the constant objective function, e.g., obj: 0

    struct keywords_t : x3::symbols<x3::unused_type>
    {
        keywords_t() {
            add("Subject")
               ("Bounds")
               ("End")
               ("Inf")
               ("Infinity");
        }
    } const keywords;

    const auto distinct_keywords = lexeme[no_case[keywords] >> !(alnum | '_')];

    x3::rule<struct identifier_tag, std::string> const identifier ("identifier");
    const auto identifier_def = lexeme[+(alpha | '_') >> *(alnum | '_')] - distinct_keywords;

    BOOST_SPIRIT_DEFINE(identifier);

    const auto obj_fun_term =
           -(double_ [add_obj_fun_coeff])
        >> identifier [add_obj_fun_var]
    ;

    const auto obj_fun_expr =
           -(identifier >> ':') [set_obj_label]
        >> -(char_('+') | char_('-')[neg_var_coeff]) >> obj_fun_term [term_parsed]
        >> *((char_('+') | char_('-')[neg_var_coeff]) > obj_fun_term [term_parsed])
    ;

    const auto constr_term =
           -(double_ [add_constr_coeff])
        >> identifier [add_constr_var]
    ;

    const auto constraint_expr =
        -(identifier >> ':') [set_constr_label]
        >>
        (
               -(char_('+') | char_('-')[neg_var_coeff]) >> constr_term [term_parsed]
            >> *((char_('+') | char_('-')[neg_var_coeff]) > constr_term [term_parsed])
            >> (char_("<>=")) [add_constr_type] >> -char_('=')
            >> double_ [add_constr_rhs]
        )
    ;

    const auto bound_expr =
        ((double_ [add_constr_coeff] | ('-' >> (no_case["Inf"] | no_case["Infinity"]) [add_inf_lb]))
            >> lit("<=") >> identifier [add_lb_constr]
            >> -(lit("<=") >> double_ [add_ub_constr]))
        |
        (identifier [store_ub_var_name]
            >> lit("<=")
            >> (double_[add_ub_constr] | (-char_('+') >> (no_case["Inf"] | no_case["Infinity"]) [add_inf_ub])))
    ;

    x3::rule<class lp_rules, ParserState> const lp_rules ("lp_rules");
    const auto lp_rules_def =
        skip(space)[
               (no_case["Minimize"] [set_minimize] | no_case["Maximize"] [set_maximize])
            >> -char_(':')
            >> obj_fun_expr
            >> no_case["Subject To"] >> -char_(':')
            >> +constraint_expr
            >> -(no_case["Bounds"] >> -char_(':') >> +bound_expr)
            >> no_case["End"] >> -char_('.')
            >> eoi
        ]
    ;

    BOOST_SPIRIT_DEFINE(lp_rules);
}

std::string remove_comments(const std::string& input)
{
    std::string result("");
    size_t pos_start = 0, pos_cur = 0;

    while (true)
    {
        pos_cur = input.find_first_of("/\\#", pos_start);
        if (pos_cur == std::string::npos) {
            pos_cur = input.length();
            break;
        }
        result += input.substr(pos_start, pos_cur - pos_start);
        pos_start = input.find_first_of('\n', pos_cur) + 1;
    }
    result += input.substr(pos_start, pos_cur - pos_start);

    return result;
}

bool run_parser_lp(const std::string& input, LinearProgram& lp)
{
    std::string stripped_input = remove_comments(input);
    auto iter = stripped_input.begin();
    const auto end = stripped_input.end();

    ParserState parser_state{ lp };

    bool r = parse(iter, end, parser::lp_rules, parser_state);

    if (r && iter == end) {
        std::cout << "Parser success." << std::endl;
        return true;
    }
    else {
        std::cout << "Parser fail." << std::endl;
        return false;
    }
}


#pragma once

#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/detail/iterator.hpp>

#include "permutations.hpp"

namespace tu
{

  /**
   * A matrix proxy with permuted rows and columns.
   */

  template <typename M>
  class matrix_permuted: public boost::numeric::ublas::matrix_expression <matrix_permuted <M> >
  {
  public:
    typedef matrix_permuted <M> self_type;
    typedef M matrix_type;
    typedef typename M::size_type size_type;
    typedef typename M::difference_type difference_type;
    typedef typename M::value_type value_type;
    typedef typename M::const_reference const_reference;
    typedef typename boost::mpl::if_ <boost::is_const <M>, typename M::const_reference, typename M::reference>::type reference;
    typedef typename boost::mpl::if_ <boost::is_const <M>, typename M::const_closure_type, typename M::closure_type>::type matrix_closure_type;
    typedef const self_type const_closure_type;
    typedef self_type closure_type;
    typedef permutation permutation_type;
    typedef typename M::orientation_category orientation_category;
    typedef typename M::storage_category storage_category;

  private:
    matrix_type& _data;
    permutation_type _perm1;
    permutation_type _perm2;

  public:

    /**
     * Constructs the matrix proxy.
     *
     * @param matrix The original matrix
     */

    matrix_permuted(matrix_type& matrix) :
      _data(matrix), _perm1(matrix.size1()), _perm2(matrix.size2())
    {

    }

    /**
     * @return Height of the matrix
     */

    inline size_type size1() const
    {
      return _perm1.size();
    }

    /**
     * @return Width of the matrix
     */

    inline size_type size2() const
    {
      return _perm2.size();
    }

    /**
     * @return Reference to the row permutation
     */

    inline const permutation_type& perm1() const
    {
      return _perm1;
    }

    /**
     * @return Reference to the row permutation
     */

    inline permutation_type& perm1()
    {
      return _perm1;
    }

    /**
     * @return Reference to the column permutation
     */

    inline const permutation_type& perm2() const
    {
      return _perm2;
    }

    /**
     * @return Reference to the column permutation
     */

    inline permutation_type& perm2()
    {
      return _perm2;
    }

    /**
     * Read-only access operator
     *
     * @param i Row index
     * @param j Column index
     * @return original(row-permutation(row), column-permutation(column))
     */

    inline const_reference operator ()(size_type i, size_type j) const
    {
      return _data(_perm1(i), _perm2(j));
    }

    /**
     * Access operator
     *
     * @param i Row index
     * @param j Column index
     * @return original(row-permutation(row), column-permutation(column))
     */

    inline reference operator ()(size_type i, size_type j)
    {
      return _data(_perm1(i), _perm2(j));
    }

    /**
     * @return Reference to original matrix
     */

    inline matrix_type& data()
    {
      return _data;
    }

    /**
     * @return Read-only reference to original matrix
     */

    inline const matrix_type& data() const
    {
      return _data;
    }

    typedef boost::numeric::ublas::detail::indexed_iterator1 <self_type, typename matrix_type::iterator1::iterator_category> iterator1;
    typedef boost::numeric::ublas::detail::indexed_iterator2 <self_type, typename matrix_type::iterator2::iterator_category> iterator2;
    typedef boost::numeric::ublas::detail::indexed_const_iterator1 <self_type, typename matrix_type::const_iterator1::iterator_category>
        const_iterator1;
    typedef boost::numeric::ublas::detail::indexed_const_iterator2 <self_type, typename matrix_type::const_iterator2::iterator_category>
        const_iterator2;
  };

  /**
   * Free function to set a matrix value of a permuted matrix.
   *
   * @param matrix The permuted matrix
   * @param row Row index
   * @param column Column index
   * @param value New value
   */

  template <typename MatrixType>
  inline void matrix_set_value(matrix_permuted <MatrixType>& matrix, size_t row, size_t column, typename MatrixType::value_type value)
  {
    matrix(row, column) = value;
  }

  /**
   * Dummy function for the version with writable orignal matrix.
   */

  template <typename MatrixType>
  inline void matrix_set_value(matrix_permuted <const MatrixType>& matrix, size_t row, size_t column, typename MatrixType::value_type value)
  {
    assert (false);
  }

  /**
   * Free function to permute two rows of a permuted matrix.
   *
   * @param matrix The permuted matrix
   * @param index1 First index
   * @param index2 Second index
   */

  template <typename MatrixType>
  inline void matrix_permute1(matrix_permuted <MatrixType>& matrix, size_t index1, size_t index2)
  {
    matrix.perm1().swap(index1, index2);
  }

  /**
   * Free function to permute two columns of a permuted matrix.
   *
   * @param matrix The permuted matrix
   * @param index1 First index
   * @param index2 Second index
   */

  template <typename MatrixType>
  inline void matrix_permute2(matrix_permuted <MatrixType>& matrix, size_t index1, size_t index2)
  {
    matrix.perm2().swap(index1, index2);
  }

  /**
   * Free function to perform a pivot on a permuted matrix.
   *
   * @param matrix The permuted matrix
   * @param i Row index
   * @param j Column index
   */

  template <typename MatrixType>
  inline void matrix_binary_pivot(matrix_permuted <MatrixType>& matrix, size_t i, size_t j)
  {
    matrix_binary_pivot(matrix.data(), matrix.perm1()(i), matrix.perm2()(j));
  }

  template <typename MatrixType>
  inline permutation matrix_get_perm1(matrix_permuted <MatrixType>& matrix)
  {
    return matrix.perm1();
  }

  template <typename MatrixType>
  inline permutation matrix_get_perm2(matrix_permuted <MatrixType>& matrix)
  {
    return matrix.perm2();
  }

  template <typename MatrixType>
  inline permutation matrix_get_perm1(matrix_transposed <MatrixType>& matrix)
  {
    return matrix_get_perm2(matrix.data());
  }

  template <typename MatrixType>
  inline permutation matrix_get_perm2(matrix_transposed <MatrixType>& matrix)
  {
    return matrix_get_perm1(matrix.data());
  }

  template <typename MatrixType>
  inline void matrix_set_perm1(matrix_permuted <MatrixType>& matrix, const permutation& permutation)
  {
    matrix.perm1() = permutation;
  }

  template <typename MatrixType>
  inline void matrix_set_perm2(matrix_permuted <MatrixType>& matrix, const permutation& permutation)
  {
    matrix.perm2() = permutation;
  }

  template <typename MatrixType>
  inline void matrix_set_perm1(matrix_transposed <MatrixType>& matrix, const permutation& permutation)
  {
    matrix_set_perm2(matrix.data(), permutation);
  }

  template <typename MatrixType>
  inline void matrix_set_perm2(matrix_transposed <MatrixType>& matrix, const permutation& permutation)
  {
    matrix_set_perm1(matrix.data(), permutation);
  }
} /* namespace tu */


#pragma once

/* =========================================================================
      Copyright (c) 2015-2017, COE of Peking University, Shaoqiang Tang.

                         -----------------
            cuarma - COE of Peking University, Shaoqiang Tang.
                         -----------------

                  Author Email    yangxianpku@pku.edu.cn

         Code Repo   https://github.com/yangxianpku/cuarma

                      License:    MIT (X11) License
============================================================================= */

/** @file cuarma/compressed_matrix.hpp
 *  @encoding:UTF-8 文档编码
 *  @brief Implementation of the compressed_matrix class
*/

#include <vector>
#include <list>
#include <map>
#include "cuarma/forwards.h"
#include "cuarma/vector.hpp"
#include "cuarma/blas/sparse_matrix_operations.hpp"
#include "cuarma/tools/tools.hpp"
#include "cuarma/tools/entry_proxy.hpp"

#ifdef CUARMA_WITH_UBLAS
#include <boost/numeric/ublas/matrix_sparse.hpp>
#endif

namespace cuarma
{
namespace detail
{

  /** @brief Implementation of the copy of a host-based sparse matrix to the device.
    *
    * See convenience copy() routines for type requirements of CPUMatrixT
    */
  template<typename CPUMatrixT, typename NumericT, unsigned int AlignmentV>
  void copy_impl(const CPUMatrixT & cpu_matrix,
                 compressed_matrix<NumericT, AlignmentV> & gpu_matrix,
                 arma_size_t nonzeros)
  {
    assert( (gpu_matrix.size1() == 0 || cuarma::traits::size1(cpu_matrix) == gpu_matrix.size1()) && bool("Size mismatch") );
    assert( (gpu_matrix.size2() == 0 || cuarma::traits::size2(cpu_matrix) == gpu_matrix.size2()) && bool("Size mismatch") );

    cuarma::backend::typesafe_host_array<unsigned int> row_buffer(gpu_matrix.handle1(), cpu_matrix.size1() + 1);
    cuarma::backend::typesafe_host_array<unsigned int> col_buffer(gpu_matrix.handle2(), nonzeros);
    std::vector<NumericT> elements(nonzeros);

    arma_size_t row_index  = 0;
    arma_size_t data_index = 0;

    for (typename CPUMatrixT::const_iterator1 row_it = cpu_matrix.begin1();
         row_it != cpu_matrix.end1();
         ++row_it)
    {
      row_buffer.set(row_index, data_index);
      ++row_index;

      for (typename CPUMatrixT::const_iterator2 col_it = row_it.begin();
           col_it != row_it.end();
           ++col_it)
      {
        col_buffer.set(data_index, col_it.index2());
        elements[data_index] = *col_it;
        ++data_index;
      }
      data_index = cuarma::tools::align_to_multiple<arma_size_t>(data_index, AlignmentV); //take care of alignment
    }
    row_buffer.set(row_index, data_index);

    gpu_matrix.set(row_buffer.get(),
                   col_buffer.get(),
                   &elements[0],
        cpu_matrix.size1(),
        cpu_matrix.size2(),
        nonzeros);
  }
}

//
// host to device:
//

//provide copy-operation:
/** @brief Copies a sparse matrix from the host to the  device (either GPU or multi-core CPU)
  *
  * There are some type requirements on the CPUMatrixT type (fulfilled by e.g. boost::numeric::ublas):
  * - .size1() returns the number of rows
  * - .size2() returns the number of columns
  * - const_iterator1    is a type definition for an iterator along increasing row indices
  * - const_iterator2    is a type definition for an iterator along increasing columns indices
  * - The const_iterator1 type provides an iterator of type const_iterator2 via members .begin() and .end() that iterates along column indices in the current row.
  * - The types const_iterator1 and const_iterator2 provide members functions .index1() and .index2() that return the current row and column indices respectively.
  * - Dereferenciation of an object of type const_iterator2 returns the entry.
  *
  * @param cpu_matrix   A sparse matrix on the host.
  * @param gpu_matrix   A compressed_matrix from cuarma
  */
template<typename CPUMatrixT, typename NumericT, unsigned int AlignmentV>
void copy(const CPUMatrixT & cpu_matrix,
          compressed_matrix<NumericT, AlignmentV> & gpu_matrix )
{
  if ( cpu_matrix.size1() > 0 && cpu_matrix.size2() > 0 )
  {
    //determine nonzeros:
    arma_size_t num_entries = 0;
    for (typename CPUMatrixT::const_iterator1 row_it = cpu_matrix.begin1();
         row_it != cpu_matrix.end1();
         ++row_it)
    {
      arma_size_t entries_per_row = 0;
      for (typename CPUMatrixT::const_iterator2 col_it = row_it.begin();
           col_it != row_it.end();
           ++col_it)
      {
        ++entries_per_row;
      }
      num_entries += cuarma::tools::align_to_multiple<arma_size_t>(entries_per_row, AlignmentV);
    }

    if (num_entries == 0) //we copy an empty matrix
      num_entries = 1;

    //set up matrix entries:
    cuarma::detail::copy_impl(cpu_matrix, gpu_matrix, num_entries);
  }
}

//adapted for std::vector< std::map < > > argument:
/** @brief Copies a sparse square matrix in the std::vector< std::map < > > format to an device. Use cuarma::tools::sparse_matrix_adapter for non-square matrices.
  *
  * @param cpu_matrix   A sparse square matrix on the host using STL types
  * @param gpu_matrix   A compressed_matrix from cuarma
  */
template<typename SizeT, typename NumericT, unsigned int AlignmentV>
void copy(const std::vector< std::map<SizeT, NumericT> > & cpu_matrix, compressed_matrix<NumericT, AlignmentV> & gpu_matrix )
{
  arma_size_t nonzeros = 0;
  arma_size_t max_col = 0;
  for (arma_size_t i=0; i<cpu_matrix.size(); ++i)
  {
    if (cpu_matrix[i].size() > 0)
      nonzeros += ((cpu_matrix[i].size() - 1) / AlignmentV + 1) * AlignmentV;
    if (cpu_matrix[i].size() > 0)
      max_col = std::max<arma_size_t>(max_col, (cpu_matrix[i].rbegin())->first);
  }

  cuarma::detail::copy_impl(tools::const_sparse_matrix_adapter<NumericT, SizeT>(cpu_matrix, cpu_matrix.size(), max_col + 1),
                              gpu_matrix,
                              nonzeros);
}

#ifdef CUARMA_WITH_UBLAS
/** @brief Convenience routine for copying a sparse uBLAS matrix to a cuarma matrix.
  *
  * Optimization which copies the data directly from the internal uBLAS buffers.
  */
template<typename ScalarType, typename F, arma_size_t IB, typename IA, typename TA>
void copy(const boost::numeric::ublas::compressed_matrix<ScalarType, F, IB, IA, TA> & ublas_matrix,
          cuarma::compressed_matrix<ScalarType, 1> & gpu_matrix)
{
  assert( (gpu_matrix.size1() == 0 || cuarma::traits::size1(ublas_matrix) == gpu_matrix.size1()) && bool("Size mismatch") );
  assert( (gpu_matrix.size2() == 0 || cuarma::traits::size2(ublas_matrix) == gpu_matrix.size2()) && bool("Size mismatch") );

  //we just need to copy the CSR arrays:
  cuarma::backend::typesafe_host_array<unsigned int> row_buffer(gpu_matrix.handle1(), ublas_matrix.size1() + 1);
  for (arma_size_t i=0; i<=ublas_matrix.size1(); ++i)
    row_buffer.set(i, ublas_matrix.index1_data()[i]);

  cuarma::backend::typesafe_host_array<unsigned int> col_buffer(gpu_matrix.handle2(), ublas_matrix.nnz());
  for (arma_size_t i=0; i<ublas_matrix.nnz(); ++i)
    col_buffer.set(i, ublas_matrix.index2_data()[i]);

  gpu_matrix.set(row_buffer.get(),
                 col_buffer.get(),
                 &(ublas_matrix.value_data()[0]),
      ublas_matrix.size1(),
      ublas_matrix.size2(),
      ublas_matrix.nnz());

}
#endif

//
// device to host:
//
/** @brief Copies a sparse matrix from the  device (either GPU or multi-core CPU) to the host.
  *
  * There are two type requirements on the CPUMatrixT type (fulfilled by e.g. boost::numeric::ublas):
  * - resize(rows, cols)  A resize function to bring the matrix into the correct size
  * - operator(i,j)       Write new entries via the parenthesis operator
  *
  * @param gpu_matrix   A compressed_matrix from cuarma
  * @param cpu_matrix   A sparse matrix on the host.
  */
template<typename CPUMatrixT, typename NumericT, unsigned int AlignmentV>
void copy(const compressed_matrix<NumericT, AlignmentV> & gpu_matrix,
          CPUMatrixT & cpu_matrix )
{
  assert( (cuarma::traits::size1(cpu_matrix) == gpu_matrix.size1()) && bool("Size mismatch") );
  assert( (cuarma::traits::size2(cpu_matrix) == gpu_matrix.size2()) && bool("Size mismatch") );

  if ( gpu_matrix.size1() > 0 && gpu_matrix.size2() > 0 )
  {
    //get raw data from memory:
    cuarma::backend::typesafe_host_array<unsigned int> row_buffer(gpu_matrix.handle1(), cpu_matrix.size1() + 1);
    cuarma::backend::typesafe_host_array<unsigned int> col_buffer(gpu_matrix.handle2(), gpu_matrix.nnz());
    std::vector<NumericT> elements(gpu_matrix.nnz());

    //std::cout << "GPU->CPU, nonzeros: " << gpu_matrix.nnz() << std::endl;

    cuarma::backend::memory_read(gpu_matrix.handle1(), 0, row_buffer.raw_size(), row_buffer.get());
    cuarma::backend::memory_read(gpu_matrix.handle2(), 0, col_buffer.raw_size(), col_buffer.get());
    cuarma::backend::memory_read(gpu_matrix.handle(),  0, sizeof(NumericT)* gpu_matrix.nnz(), &(elements[0]));

    //fill the cpu_matrix:
    arma_size_t data_index = 0;
    for (arma_size_t row = 1; row <= gpu_matrix.size1(); ++row)
    {
      while (data_index < row_buffer[row])
      {
        if (col_buffer[data_index] >= gpu_matrix.size2())
        {
          std::cerr << "cuarma encountered invalid data at colbuffer[" << data_index << "]: " << col_buffer[data_index] << std::endl;
          return;
        }

        if (std::fabs(elements[data_index]) > static_cast<NumericT>(0))
          cpu_matrix(row-1, static_cast<arma_size_t>(col_buffer[data_index])) = elements[data_index];
        ++data_index;
      }
    }
  }
}

/** @brief Copies a sparse matrix from an  device to the host. The host type is the std::vector< std::map < > > format .
  *
  * @param gpu_matrix   A compressed_matrix from cuarma
  * @param cpu_matrix   A sparse matrix on the host.
  */
template<typename NumericT, unsigned int AlignmentV>
void copy(const compressed_matrix<NumericT, AlignmentV> & gpu_matrix,
          std::vector< std::map<unsigned int, NumericT> > & cpu_matrix)
{
  assert( (cpu_matrix.size() == gpu_matrix.size1()) && bool("Size mismatch") );

  tools::sparse_matrix_adapter<NumericT> temp(cpu_matrix, gpu_matrix.size1(), gpu_matrix.size2());
  copy(gpu_matrix, temp);
}

#ifdef CUARMA_WITH_UBLAS
/** @brief Convenience routine for copying a cuarma sparse matrix back to a sparse uBLAS matrix
  *
  * Directly populates the internal buffer of the uBLAS matrix, thus avoiding a temporary STL matrix.
  */
template<typename ScalarType, unsigned int AlignmentV, typename F, arma_size_t IB, typename IA, typename TA>
void copy(cuarma::compressed_matrix<ScalarType, AlignmentV> const & gpu_matrix,
          boost::numeric::ublas::compressed_matrix<ScalarType> & ublas_matrix)
{
  assert( (cuarma::traits::size1(ublas_matrix) == gpu_matrix.size1()) && bool("Size mismatch") );
  assert( (cuarma::traits::size2(ublas_matrix) == gpu_matrix.size2()) && bool("Size mismatch") );

  cuarma::backend::typesafe_host_array<unsigned int> row_buffer(gpu_matrix.handle1(), gpu_matrix.size1() + 1);
  cuarma::backend::typesafe_host_array<unsigned int> col_buffer(gpu_matrix.handle2(), gpu_matrix.nnz());

  cuarma::backend::memory_read(gpu_matrix.handle1(), 0, row_buffer.raw_size(), row_buffer.get());
  cuarma::backend::memory_read(gpu_matrix.handle2(), 0, col_buffer.raw_size(), col_buffer.get());

  ublas_matrix.clear();
  ublas_matrix.reserve(gpu_matrix.nnz());

  ublas_matrix.set_filled(gpu_matrix.size1() + 1, gpu_matrix.nnz());

  for (arma_size_t i=0; i<ublas_matrix.size1() + 1; ++i)
    ublas_matrix.index1_data()[i] = row_buffer[i];

  for (arma_size_t i=0; i<ublas_matrix.nnz(); ++i)
    ublas_matrix.index2_data()[i] = col_buffer[i];

  cuarma::backend::memory_read(gpu_matrix.handle(),  0, sizeof(ScalarType) * gpu_matrix.nnz(), &(ublas_matrix.value_data()[0]));

}
#endif

//////////////////////// compressed_matrix //////////////////////////
/** @brief A sparse square matrix in compressed sparse rows format.
  *
  * @tparam NumericT    The floating point type (either float or double, checked at compile time)
  * @tparam AlignmentV     The internal memory size for the entries in each row is given by (size()/AlignmentV + 1) * AlignmentV. AlignmentV must be a power of two. Best values or usually 4, 8 or 16, higher values are usually a waste of memory.
  */
template<class NumericT, unsigned int AlignmentV /* see VCLForwards.h */>
class compressed_matrix
{
public:
  typedef cuarma::backend::mem_handle                                                              handle_type;
  typedef scalar<typename cuarma::tools::CHECK_SCALAR_TEMPLATE_ARGUMENT<NumericT>::ResultType>   value_type;
  typedef arma_size_t                                                                                size_type;

  /** @brief Default construction of a compressed matrix. No memory is allocated */
  compressed_matrix() : rows_(0), cols_(0), nonzeros_(0), row_block_num_(0) {}

  /** @brief Construction of a compressed matrix with the supplied number of rows and columns. If the number of nonzeros is positive, memory is allocated
      *
      * @param rows     Number of rows
      * @param cols     Number of columns
      * @param nonzeros Optional number of nonzeros for memory preallocation
      * @param ctx      Optional context in which the matrix is created (one out of multiple  contexts, CUDA, host)
      */
  explicit compressed_matrix(arma_size_t rows, arma_size_t cols, arma_size_t nonzeros = 0, cuarma::context ctx = cuarma::context())
    : rows_(rows), cols_(cols), nonzeros_(nonzeros), row_block_num_(0)
  {
    row_buffer_.switch_active_handle_id(ctx.memory_type());
    col_buffer_.switch_active_handle_id(ctx.memory_type());
    elements_.switch_active_handle_id(ctx.memory_type());
    row_blocks_.switch_active_handle_id(ctx.memory_type());

    if (rows > 0)
    {
      cuarma::backend::memory_create(row_buffer_, cuarma::backend::typesafe_host_array<unsigned int>().element_size() * (rows + 1), ctx);
      cuarma::vector_base<unsigned int> init_temporary(row_buffer_, size_type(rows+1), 0, 1);
      init_temporary = cuarma::zero_vector<unsigned int>(size_type(rows+1), ctx);
    }
    if (nonzeros > 0)
    {
      cuarma::backend::memory_create(col_buffer_, cuarma::backend::typesafe_host_array<unsigned int>().element_size() * nonzeros, ctx);
      cuarma::backend::memory_create(elements_, sizeof(NumericT) * nonzeros, ctx);
    }
  }

  /** @brief Construction of a compressed matrix with the supplied number of rows and columns. If the number of nonzeros is positive, memory is allocated
      *
      * @param rows     Number of rows
      * @param cols     Number of columns
      * @param ctx      Context in which to create the matrix
      */
  explicit compressed_matrix(arma_size_t rows, arma_size_t cols, cuarma::context ctx)
    : rows_(rows), cols_(cols), nonzeros_(0), row_block_num_(0)
  {
    row_buffer_.switch_active_handle_id(ctx.memory_type());
    col_buffer_.switch_active_handle_id(ctx.memory_type());
    elements_.switch_active_handle_id(ctx.memory_type());
    row_blocks_.switch_active_handle_id(ctx.memory_type());

    if (rows > 0)
    {
      cuarma::backend::memory_create(row_buffer_, cuarma::backend::typesafe_host_array<unsigned int>().element_size() * (rows + 1), ctx);
      cuarma::vector_base<unsigned int> init_temporary(row_buffer_, size_type(rows+1), 0, 1);
      init_temporary = cuarma::zero_vector<unsigned int>(size_type(rows+1), ctx);
    }
  }

  /** @brief Creates an empty compressed_matrix, but sets the respective context information.
    *
    * This is useful if you want to want to populate e.g. a cuarma::compressed_matrix<> on the host with copy(), but the default backend is .
    */
  explicit compressed_matrix(cuarma::context ctx) : rows_(0), cols_(0), nonzeros_(0), row_block_num_(0)
  {
    row_buffer_.switch_active_handle_id(ctx.memory_type());
    col_buffer_.switch_active_handle_id(ctx.memory_type());
    elements_.switch_active_handle_id(ctx.memory_type());
    row_blocks_.switch_active_handle_id(ctx.memory_type());
  }

  /** @brief Assignment a compressed matrix from the product of two compressed_matrix objects (C = A * B). */
  compressed_matrix(matrix_expression<const compressed_matrix, const compressed_matrix, op_prod> const & proxy)
    : rows_(0), cols_(0), nonzeros_(0), row_block_num_(0)
  {
    cuarma::context ctx = cuarma::traits::context(proxy.lhs());

    row_buffer_.switch_active_handle_id(ctx.memory_type());
    col_buffer_.switch_active_handle_id(ctx.memory_type());
    elements_.switch_active_handle_id(ctx.memory_type());
    row_blocks_.switch_active_handle_id(ctx.memory_type());

    cuarma::blas::prod_impl(proxy.lhs(), proxy.rhs(), *this);
    generate_row_block_information();
  }

  /** @brief Assignment a compressed matrix from possibly another memory domain. */
  compressed_matrix & operator=(compressed_matrix const & other)
  {
    assert( (rows_ == 0 || rows_ == other.size1()) && bool("Size mismatch") );
    assert( (cols_ == 0 || cols_ == other.size2()) && bool("Size mismatch") );

    rows_ = other.size1();
    cols_ = other.size2();
    nonzeros_ = other.nnz();
    row_block_num_ = other.row_block_num_;

    cuarma::backend::typesafe_memory_copy<unsigned int>(other.row_buffer_, row_buffer_);
    cuarma::backend::typesafe_memory_copy<unsigned int>(other.col_buffer_, col_buffer_);
    cuarma::backend::typesafe_memory_copy<unsigned int>(other.row_blocks_, row_blocks_);
    cuarma::backend::typesafe_memory_copy<NumericT>(other.elements_, elements_);

    return *this;
  }

  /** @brief Assignment a compressed matrix from the product of two compressed_matrix objects (C = A * B). */
  compressed_matrix & operator=(matrix_expression<const compressed_matrix, const compressed_matrix, op_prod> const & proxy)
  {
    assert( (rows_ == 0 || rows_ == proxy.lhs().size1()) && bool("Size mismatch") );
    assert( (cols_ == 0 || cols_ == proxy.rhs().size2()) && bool("Size mismatch") );

    cuarma::blas::prod_impl(proxy.lhs(), proxy.rhs(), *this);
    generate_row_block_information();

    return *this;
  }

  /** @brief Sets the row, column and value arrays of the compressed matrix
    *
    * Type of row_jumper and col_buffer is 'unsigned int' for CUDA and OpenMP (host) backend, but *must* be cl_uint for .
    * The reason is that 'unsigned int' might have a different bit representation on the host than 'unsigned int' on the  device.
    * cl_uint is guaranteed to have the correct bit representation for  devices.
    *
    * @param row_jumper     Pointer to an array holding the indices of the first element of each row (starting with zero). E.g. row_jumper[10] returns the index of the first entry of the 11th row. The array length is 'cols + 1'
    * @param col_buffer     Pointer to an array holding the column index of each entry. The array length is 'nonzeros'
    * @param elements       Pointer to an array holding the entries of the sparse matrix. The array length is 'elements'
    * @param rows           Number of rows of the sparse matrix
    * @param cols           Number of columns of the sparse matrix
    * @param nonzeros       Number of nonzeros
    */
  void set(const void * row_jumper,
           const void * col_buffer,
           const NumericT * elements,
           arma_size_t rows,
           arma_size_t cols,
           arma_size_t nonzeros)
  {
    assert( (rows > 0)     && bool("Error in compressed_matrix::set(): Number of rows must be larger than zero!"));
    assert( (cols > 0)     && bool("Error in compressed_matrix::set(): Number of columns must be larger than zero!"));
    assert( (nonzeros > 0) && bool("Error in compressed_matrix::set(): Number of nonzeros must be larger than zero!"));
    //std::cout << "Setting memory: " << cols + 1 << ", " << nonzeros << std::endl;

    //row_buffer_.switch_active_handle_id(cuarma::backend::_MEMORY);
    cuarma::backend::memory_create(row_buffer_, cuarma::backend::typesafe_host_array<unsigned int>(row_buffer_).element_size() * (rows + 1), cuarma::traits::context(row_buffer_), row_jumper);

    //col_buffer_.switch_active_handle_id(cuarma::backend::_MEMORY);
    cuarma::backend::memory_create(col_buffer_, cuarma::backend::typesafe_host_array<unsigned int>(col_buffer_).element_size() * nonzeros, cuarma::traits::context(col_buffer_), col_buffer);

    //elements_.switch_active_handle_id(cuarma::backend::_MEMORY);
    cuarma::backend::memory_create(elements_, sizeof(NumericT) * nonzeros, cuarma::traits::context(elements_), elements);

    nonzeros_ = nonzeros;
    rows_ = rows;
    cols_ = cols;

    //generate block information for CSR-adaptive:
    generate_row_block_information();
  }

  /** @brief Allocate memory for the supplied number of nonzeros in the matrix. Old values are preserved. */
  void reserve(arma_size_t new_nonzeros, bool preserve = true)
  {
    if (new_nonzeros > nonzeros_)
    {
      if (preserve)
      {
        handle_type col_buffer_old;
        handle_type elements_old;
        cuarma::backend::memory_shallow_copy(col_buffer_, col_buffer_old);
        cuarma::backend::memory_shallow_copy(elements_,   elements_old);

        cuarma::backend::typesafe_host_array<unsigned int> size_deducer(col_buffer_);
        cuarma::backend::memory_create(col_buffer_, size_deducer.element_size() * new_nonzeros, cuarma::traits::context(col_buffer_));
        cuarma::backend::memory_create(elements_,   sizeof(NumericT) * new_nonzeros,          cuarma::traits::context(elements_));

        cuarma::backend::memory_copy(col_buffer_old, col_buffer_, 0, 0, size_deducer.element_size() * nonzeros_);
        cuarma::backend::memory_copy(elements_old,   elements_,   0, 0, sizeof(NumericT)* nonzeros_);
      }
      else
      {
        cuarma::backend::typesafe_host_array<unsigned int> size_deducer(col_buffer_);
        cuarma::backend::memory_create(col_buffer_, size_deducer.element_size() * new_nonzeros, cuarma::traits::context(col_buffer_));
        cuarma::backend::memory_create(elements_,   sizeof(NumericT)            * new_nonzeros, cuarma::traits::context(elements_));
      }

      nonzeros_ = new_nonzeros;
    }
  }

  /** @brief Resize the matrix.
      *
      * @param new_size1    New number of rows
      * @param new_size2    New number of columns
      * @param preserve     If true, the old values are preserved. At present, old values are always discarded.
      */
  void resize(arma_size_t new_size1, arma_size_t new_size2, bool preserve = true)
  {
    assert(new_size1 > 0 && new_size2 > 0 && bool("Cannot resize to zero size!"));

    if (new_size1 != rows_ || new_size2 != cols_)
    {
      if (!preserve)
      {
        cuarma::backend::typesafe_host_array<unsigned int> host_row_buffer(row_buffer_, new_size1 + 1);
        cuarma::backend::memory_create(row_buffer_, cuarma::backend::typesafe_host_array<unsigned int>().element_size() * (new_size1 + 1), cuarma::traits::context(row_buffer_), host_row_buffer.get());
        // faster version without initializing memory:
        //cuarma::backend::memory_create(row_buffer_, cuarma::backend::typesafe_host_array<unsigned int>().element_size() * (new_size1 + 1), cuarma::traits::context(row_buffer_));
        nonzeros_ = 0;
      }
      else
      {
        std::vector<std::map<unsigned int, NumericT> > stl_sparse_matrix;
        if (rows_ > 0)
        {
          stl_sparse_matrix.resize(rows_);
          cuarma::copy(*this, stl_sparse_matrix);
        } else {
          stl_sparse_matrix.resize(new_size1);
          stl_sparse_matrix[0][0] = 0;      //enforces nonzero array sizes if matrix was initially empty
        }

        stl_sparse_matrix.resize(new_size1);

        //discard entries with column index larger than new_size2
        if (new_size2 < cols_ && rows_ > 0)
        {
          for (arma_size_t i=0; i<stl_sparse_matrix.size(); ++i)
          {
            std::list<unsigned int> to_delete;
            for (typename std::map<unsigned int, NumericT>::iterator it = stl_sparse_matrix[i].begin();
                 it != stl_sparse_matrix[i].end();
                 ++it)
            {
              if (it->first >= new_size2)
                to_delete.push_back(it->first);
            }

            for (std::list<unsigned int>::iterator it = to_delete.begin(); it != to_delete.end(); ++it)
              stl_sparse_matrix[i].erase(*it);
          }
        }

        cuarma::tools::sparse_matrix_adapter<NumericT> adapted_matrix(stl_sparse_matrix, new_size1, new_size2);
        rows_ = new_size1;
        cols_ = new_size2;
        cuarma::copy(adapted_matrix, *this);
      }

      rows_ = new_size1;
      cols_ = new_size2;
    }
  }

  /** @brief Resets all entries in the matrix back to zero without changing the matrix size. Resets the sparsity pattern. */
  void clear()
  {
    cuarma::backend::typesafe_host_array<unsigned int> host_row_buffer(row_buffer_, rows_ + 1);
    cuarma::backend::typesafe_host_array<unsigned int> host_col_buffer(col_buffer_, 1);
    std::vector<NumericT> host_elements(1);

    cuarma::backend::memory_create(row_buffer_, host_row_buffer.element_size() * (rows_ + 1), cuarma::traits::context(row_buffer_), host_row_buffer.get());
    cuarma::backend::memory_create(col_buffer_, host_col_buffer.element_size() * 1,           cuarma::traits::context(col_buffer_), host_col_buffer.get());
    cuarma::backend::memory_create(elements_,   sizeof(NumericT) * 1,                         cuarma::traits::context(elements_), &(host_elements[0]));

    nonzeros_ = 0;
  }

  /** @brief Returns a reference to the (i,j)-th entry of the sparse matrix. If (i,j) does not exist (zero), it is inserted (slow!) */
  entry_proxy<NumericT> operator()(arma_size_t i, arma_size_t j)
  {
    assert( (i < rows_) && (j < cols_) && bool("compressed_matrix access out of bounds!"));

    arma_size_t index = element_index(i, j);

    // check for element in sparsity pattern
    if (index < nonzeros_)
      return entry_proxy<NumericT>(index, elements_);

    // Element not found. Copying required. Very slow, but direct entry manipulation is painful anyway...
    std::vector< std::map<unsigned int, NumericT> > cpu_backup(rows_);
    tools::sparse_matrix_adapter<NumericT> adapted_cpu_backup(cpu_backup, rows_, cols_);
    cuarma::copy(*this, adapted_cpu_backup);
    cpu_backup[i][static_cast<unsigned int>(j)] = 0.0;
    cuarma::copy(adapted_cpu_backup, *this);

    index = element_index(i, j);

    assert(index < nonzeros_);

    return entry_proxy<NumericT>(index, elements_);
  }

  /** @brief  Returns the number of rows */
  const arma_size_t & size1() const { return rows_; }
  /** @brief  Returns the number of columns */
  const arma_size_t & size2() const { return cols_; }
  /** @brief  Returns the number of nonzero entries */
  const arma_size_t & nnz() const { return nonzeros_; }
  /** @brief  Returns the internal number of row blocks for an adaptive SpMV */
  const arma_size_t & blocks1() const { return row_block_num_; }

  /** @brief  Returns the  handle to the row index array */
  const handle_type & handle1() const { return row_buffer_; }
  /** @brief  Returns the  handle to the column index array */
  const handle_type & handle2() const { return col_buffer_; }
  /** @brief  Returns the  handle to the row block array */
  const handle_type & handle3() const { return row_blocks_; }
  /** @brief  Returns the  handle to the matrix entry array */
  const handle_type & handle() const { return elements_; }

  /** @brief  Returns the  handle to the row index array */
  handle_type & handle1() { return row_buffer_; }
  /** @brief  Returns the  handle to the column index array */
  handle_type & handle2() { return col_buffer_; }
  /** @brief  Returns the  handle to the row block array */
  handle_type & handle3() { return row_blocks_; }
  /** @brief  Returns the  handle to the matrix entry array */
  handle_type & handle() { return elements_; }

  /** @brief Switches the memory context of the matrix.
    *
    * Allows for e.g. an migration of the full matrix from  memory to host memory for e.g. computing a preconditioner.
    */
  void switch_memory_context(cuarma::context new_ctx)
  {
    cuarma::backend::switch_memory_context<unsigned int>(row_buffer_, new_ctx);
    cuarma::backend::switch_memory_context<unsigned int>(col_buffer_, new_ctx);
    cuarma::backend::switch_memory_context<unsigned int>(row_blocks_, new_ctx);
    cuarma::backend::switch_memory_context<NumericT>(elements_, new_ctx);
  }

  /** @brief Returns the current memory context to determine whether the matrix is set up for OpenMP, , or CUDA. */
  cuarma::memory_types memory_context() const
  {
    return row_buffer_.get_active_handle_id();
  }

private:

  /** @brief Helper function for accessing the element (i,j) of the matrix. */
  arma_size_t element_index(arma_size_t i, arma_size_t j)
  {
    //read row indices
    cuarma::backend::typesafe_host_array<unsigned int> row_indices(row_buffer_, 2);
    cuarma::backend::memory_read(row_buffer_, row_indices.element_size()*i, row_indices.element_size()*2, row_indices.get());

    //get column indices for row i:
    cuarma::backend::typesafe_host_array<unsigned int> col_indices(col_buffer_, row_indices[1] - row_indices[0]);
    cuarma::backend::memory_read(col_buffer_, col_indices.element_size()*row_indices[0], row_indices.element_size()*col_indices.size(), col_indices.get());

    for (arma_size_t k=0; k<col_indices.size(); ++k)
    {
      if (col_indices[k] == j)
        return row_indices[0] + k;
    }

    // if not found, return index past the end of the matrix (cf. matrix.end() in the spirit of the STL)
    return nonzeros_;
  }

public:
  /** @brief Builds the row block information needed for fast sparse matrix-vector multiplications.
   *
   *  Required when manually populating the memory buffers with values. Not necessary when using cuarma::copy() or .set()
   */
  void generate_row_block_information()
  {
    cuarma::backend::typesafe_host_array<unsigned int> row_buffer(row_buffer_, rows_ + 1);
    cuarma::backend::memory_read(row_buffer_, 0, row_buffer.raw_size(), row_buffer.get());

    cuarma::backend::typesafe_host_array<unsigned int> row_blocks(row_buffer_, rows_ + 1);

    arma_size_t num_entries_in_current_batch = 0;

    const arma_size_t shared_mem_size = 1024; // number of column indices loaded to shared memory, number of floating point values loaded to shared memory

    row_block_num_ = 0;
    row_blocks.set(0, 0);
    for (arma_size_t i=0; i<rows_; ++i)
    {
      arma_size_t entries_in_row = arma_size_t(row_buffer[i+1]) - arma_size_t(row_buffer[i]);
      num_entries_in_current_batch += entries_in_row;

      if (num_entries_in_current_batch > shared_mem_size)
      {
        arma_size_t rows_in_batch = i - row_blocks[row_block_num_];
        if (rows_in_batch > 0) // at least one full row is in the batch. Use current row in next batch.
          row_blocks.set(++row_block_num_, i--);
        else // row is larger than buffer in shared memory
          row_blocks.set(++row_block_num_, i+1);
        num_entries_in_current_batch = 0;
      }
    }
    if (num_entries_in_current_batch > 0)
      row_blocks.set(++row_block_num_, rows_);

    if (row_block_num_ > 0) //matrix might be empty...
      cuarma::backend::memory_create(row_blocks_,
                                       row_blocks.element_size() * (row_block_num_ + 1),
                                       cuarma::traits::context(row_buffer_), row_blocks.get());

  }

private:
  // /** @brief Copy constructor is by now not available. */
  //compressed_matrix(compressed_matrix const &);

private:

  arma_size_t rows_;
  arma_size_t cols_;
  arma_size_t nonzeros_;
  arma_size_t row_block_num_;
  handle_type row_buffer_;
  handle_type row_blocks_;
  handle_type col_buffer_;
  handle_type elements_;
};

/** @brief Output stream support for compressed_matrix. Output format is same as MATLAB, Octave, or SciPy
  *
  * @param os   STL output stream
  * @param A    The compressed matrix to be printed.
*/
template<typename NumericT, unsigned int AlignmentV>
std::ostream & operator<<(std::ostream & os, compressed_matrix<NumericT, AlignmentV> const & A)
{
  std::vector<std::map<unsigned int, NumericT> > tmp(A.size1());
  cuarma::copy(A, tmp);
  os << "compressed_matrix of size (" << A.size1() << ", " << A.size2() << ") with " << A.nnz() << " nonzeros:" << std::endl;

  for (arma_size_t i=0; i<A.size1(); ++i)
  {
    for (typename std::map<unsigned int, NumericT>::const_iterator it = tmp[i].begin(); it != tmp[i].end(); ++it)
      os << "  (" << i << ", " << it->first << ")\t" << it->second << std::endl;
  }
  return os;
}

//
// Specify available operations:
//

/** \cond */

namespace blas
{
namespace detail
{
  // x = A * y
  template<typename T, unsigned int A>
  struct op_executor<vector_base<T>, op_assign, vector_expression<const compressed_matrix<T, A>, const vector_base<T>, op_prod> >
  {
    static void apply(vector_base<T> & lhs, vector_expression<const compressed_matrix<T, A>, const vector_base<T>, op_prod> const & rhs)
    {
      // check for the special case x = A * x
      if (cuarma::traits::handle(lhs) == cuarma::traits::handle(rhs.rhs()))
      {
        cuarma::vector<T> temp(lhs);
        cuarma::blas::prod_impl(rhs.lhs(), rhs.rhs(), T(1), temp, T(0));
        lhs = temp;
      }
      else
        cuarma::blas::prod_impl(rhs.lhs(), rhs.rhs(), T(1), lhs, T(0));
    }
  };

  template<typename T, unsigned int A>
  struct op_executor<vector_base<T>, op_inplace_add, vector_expression<const compressed_matrix<T, A>, const vector_base<T>, op_prod> >
  {
    static void apply(vector_base<T> & lhs, vector_expression<const compressed_matrix<T, A>, const vector_base<T>, op_prod> const & rhs)
    {
      // check for the special case x += A * x
      if (cuarma::traits::handle(lhs) == cuarma::traits::handle(rhs.rhs()))
      {
        cuarma::vector<T> temp(lhs);
        cuarma::blas::prod_impl(rhs.lhs(), rhs.rhs(), T(1), temp, T(0));
        lhs += temp;
      }
      else
        cuarma::blas::prod_impl(rhs.lhs(), rhs.rhs(), T(1), lhs, T(1));
    }
  };

  template<typename T, unsigned int A>
  struct op_executor<vector_base<T>, op_inplace_sub, vector_expression<const compressed_matrix<T, A>, const vector_base<T>, op_prod> >
  {
    static void apply(vector_base<T> & lhs, vector_expression<const compressed_matrix<T, A>, const vector_base<T>, op_prod> const & rhs)
    {
      // check for the special case x -= A * x
      if (cuarma::traits::handle(lhs) == cuarma::traits::handle(rhs.rhs()))
      {
        cuarma::vector<T> temp(lhs);
        cuarma::blas::prod_impl(rhs.lhs(), rhs.rhs(), T(1), temp, T(0));
        lhs -= temp;
      }
      else
        cuarma::blas::prod_impl(rhs.lhs(), rhs.rhs(), T(-1), lhs, T(1));
    }
  };

  // x = A * vec_op
  template<typename T, unsigned int A, typename LHS, typename RHS, typename OP>
  struct op_executor<vector_base<T>, op_assign, vector_expression<const compressed_matrix<T, A>, const vector_expression<const LHS, const RHS, OP>, op_prod> >
  {
    static void apply(vector_base<T> & lhs, vector_expression<const compressed_matrix<T, A>, const vector_expression<const LHS, const RHS, OP>, op_prod> const & rhs)
    {
      cuarma::vector<T> temp(rhs.rhs(), cuarma::traits::context(rhs));
      cuarma::blas::prod_impl(rhs.lhs(), temp, lhs);
    }
  };

  // x = A * vec_op
  template<typename T, unsigned int A, typename LHS, typename RHS, typename OP>
  struct op_executor<vector_base<T>, op_inplace_add, vector_expression<const compressed_matrix<T, A>, vector_expression<const LHS, const RHS, OP>, op_prod> >
  {
    static void apply(vector_base<T> & lhs, vector_expression<const compressed_matrix<T, A>, vector_expression<const LHS, const RHS, OP>, op_prod> const & rhs)
    {
      cuarma::vector<T> temp(rhs.rhs(), cuarma::traits::context(rhs));
      cuarma::vector<T> temp_result(lhs);
      cuarma::blas::prod_impl(rhs.lhs(), temp, temp_result);
      lhs += temp_result;
    }
  };

  // x = A * vec_op
  template<typename T, unsigned int A, typename LHS, typename RHS, typename OP>
  struct op_executor<vector_base<T>, op_inplace_sub, vector_expression<const compressed_matrix<T, A>, const vector_expression<const LHS, const RHS, OP>, op_prod> >
  {
    static void apply(vector_base<T> & lhs, vector_expression<const compressed_matrix<T, A>, const vector_expression<const LHS, const RHS, OP>, op_prod> const & rhs)
    {
      cuarma::vector<T> temp(rhs.rhs(), cuarma::traits::context(rhs));
      cuarma::vector<T> temp_result(lhs);
      cuarma::blas::prod_impl(rhs.lhs(), temp, temp_result);
      lhs -= temp_result;
    }
  };

} // namespace detail
} // namespace blas
  /** \endcond */
}


#include "domain.hpp"

#include <deal.II/dofs/dof_tools.h>
#include <deal.II/lac/sparsity_tools.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/dofs/dof_renumbering.h>

namespace bart::domain {

namespace  {
template <int dim>
auto MeshSmoothing = typename dealii::Triangulation<dim>::MeshSmoothing(
    dealii::Triangulation<dim>::smoothing_on_refinement | dealii::Triangulation<dim>::smoothing_on_coarsening);
} // namespace

template <int dim>
Domain<dim>::Domain(std::unique_ptr<domain::mesh::MeshI<dim>> mesh,
                    std::shared_ptr<domain::finite_element::FiniteElementI<dim>> finite_element,
                    problem::DiscretizationType discretization)
    : mesh_(std::move(mesh)),
      finite_element_(finite_element),
      triangulation_(MPI_COMM_WORLD, MeshSmoothing<dim>),
      dof_handler_(triangulation_),
      discretization_type_(discretization) {
  AssertPointerNotNull(mesh_.get(), "mesh", "domain constructor");
  AssertPointerNotNull(finite_element_.get(), "finite_element", "domain constructor");

  std::string description{"Domain, " + std::to_string(dim) + "D"};
  if (discretization == problem::DiscretizationType::kContinuousFEM) {
    description += ", Continuous";
  } else if (discretization == problem::DiscretizationType::kDiscontinuousFEM ){
    description += ", Discontinuous";
  }
  this->set_description(description, utility::DefaultImplementation(true));
}

template <>
Domain<1>::Domain(
    std::unique_ptr<domain::mesh::MeshI<1>> mesh,
    std::shared_ptr<domain::finite_element::FiniteElementI<1>> finite_element,
    problem::DiscretizationType discretization)
    : mesh_(std::move(mesh)),
      finite_element_(finite_element),
      triangulation_(MeshSmoothing<1>),
      dof_handler_(triangulation_),
      discretization_type_(discretization) {
  AssertPointerNotNull(mesh_.get(), "mesh", "domain constructor");
  AssertPointerNotNull(finite_element_.get(), "finite_element", "domain constructor");
  std::string description{"Domain, 1D"};
  if (discretization == problem::DiscretizationType::kContinuousFEM) {
    description += ", Continuous";
  } else if (discretization == problem::DiscretizationType::kDiscontinuousFEM ){
    description += ", Discontinuous";
  }
  this->set_description(description, utility::DefaultImplementation(true));
}

template <int dim>
Domain<dim>& Domain<dim>::SetUpDOF() {
  // Setup dof Handler
  dof_handler_.distribute_dofs(*(finite_element_->finite_element()));
  // Populate dof IndexSets
  locally_owned_dofs_ = dof_handler_.locally_owned_dofs();
  dealii::DoFTools::extract_locally_relevant_dofs(dof_handler_, locally_relevant_dofs_);
  // Create constraint matrix
  constraint_matrix_.clear();
  constraint_matrix_.reinit(locally_relevant_dofs_);
  dealii::DoFTools::make_hanging_node_constraints(dof_handler_, constraint_matrix_);
  constraint_matrix_.close();

  for (auto cell = dof_handler_.begin_active(); cell != dof_handler_.end(); ++cell) {
    if (cell->is_locally_owned())
      local_cells_.push_back(cell);
  }

  // Set up dynamic sparsity pattern
  dynamic_sparsity_pattern_.reinit(locally_relevant_dofs_.size(), locally_relevant_dofs_.size(),
                                   locally_relevant_dofs_);

  if (discretization_type_ ==  problem::DiscretizationType::kDiscontinuousFEM) {
    dealii::DoFTools::make_flux_sparsity_pattern(dof_handler_, dynamic_sparsity_pattern_, constraint_matrix_, false);
  } else {
    dealii::DoFTools::make_sparsity_pattern(dof_handler_, dynamic_sparsity_pattern_, constraint_matrix_, false);
  }

  dealii::SparsityTools::distribute_sparsity_pattern(dynamic_sparsity_pattern_, locally_owned_dofs_,
                                                     MPI_COMM_WORLD, locally_relevant_dofs_);

  constraint_matrix_.condense(dynamic_sparsity_pattern_);

  return *this;
}

template <>
Domain<1>& Domain<1>::SetUpDOF() {
  const auto n_mpi_processes{ dealii::Utilities::MPI::n_mpi_processes(MPI_COMM_WORLD) };
  const auto this_process{ dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD) };

  dealii::GridTools::partition_triangulation(n_mpi_processes, triangulation_);
  dof_handler_.distribute_dofs(*(finite_element_)->finite_element());
  dealii::DoFRenumbering::subdomain_wise(dof_handler_);

  for (auto cell = dof_handler_.begin_active(); cell != dof_handler_.end(); ++cell) {
    if (cell->is_locally_owned())
      local_cells_.push_back(cell);
  }

  auto locally_owned_dofs_vector = dealii::DoFTools::locally_owned_dofs_per_subdomain(dof_handler_);
  locally_owned_dofs_ = locally_owned_dofs_vector.at(this_process);

  constraint_matrix_.clear();
  dealii::DoFTools::make_hanging_node_constraints(dof_handler_, constraint_matrix_);
  constraint_matrix_.close();

  dynamic_sparsity_pattern_.reinit(dof_handler_.n_dofs(), dof_handler_.n_dofs());

  if (discretization_type_ ==  problem::DiscretizationType::kDiscontinuousFEM) {
    dealii::DoFTools::make_flux_sparsity_pattern(dof_handler_, dynamic_sparsity_pattern_, constraint_matrix_, false);
  } else {
    dealii::DoFTools::make_sparsity_pattern(dof_handler_, dynamic_sparsity_pattern_, constraint_matrix_, false);
  }

  return *this;
}

template <int dim>
Domain<dim>& Domain<dim>::SetUpMesh() {return SetUpMesh(0); }

template<int dim>
Domain<dim>& Domain<dim>::SetUpMesh(const int global_refinements) {
  AssertThrow(mesh_->has_material_mapping(), dealii::ExcMessage("Mesh object must have initialized material mapping"));
  mesh_->FillTriangulation(triangulation_);
  mesh_->FillBoundaryID(triangulation_);
  mesh_->FillMaterialID(triangulation_);
  triangulation_.refine_global(global_refinements);
  return *this;
}

template<int dim>
auto Domain<dim>::GetCellMatrix() const -> dealii::FullMatrix<double> {
  const int cell_dofs{ finite_element_->dofs_per_cell() };
  return dealii::FullMatrix<double>(cell_dofs, cell_dofs);
//  dealii::FullMatrix<double> full_matrix(cell_dofs, cell_dofs);
//  return full_matrix;
}
template<int dim>
auto Domain<dim>::GetCellVector() const -> dealii::Vector<double> {
  return dealii::Vector<double>(finite_element_->dofs_per_cell());
//  int cell_dofs = finite_element_->dofs_per_cell();
//  dealii::Vector<double> vector(cell_dofs);
//  return vector;
}

template<int dim>
std::shared_ptr<system::MPISparseMatrix> Domain<dim>::MakeSystemMatrix() const {
  auto system_matrix_ptr = std::make_shared<system::MPISparseMatrix>();
  system_matrix_ptr->reinit(locally_owned_dofs_, locally_owned_dofs_, dynamic_sparsity_pattern_, MPI_COMM_WORLD);
  return system_matrix_ptr;
}

template<int dim>
std::shared_ptr<system::MPIVector> Domain<dim>::MakeSystemVector() const {
  auto system_vector_ptr = std::make_shared<system::MPIVector>();
  system_vector_ptr->reinit(locally_owned_dofs_, MPI_COMM_WORLD);
  return system_vector_ptr;
}

template <int dim>
int Domain<dim>::total_degrees_of_freedom() const {
  if (total_degrees_of_freedom_ == 0)
    total_degrees_of_freedom_ = dof_handler_.n_dofs();
  return total_degrees_of_freedom_;
}

template class Domain<1>;
template class Domain<2>;
template class Domain<3>;

} // namespace bart::domain


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

#include <pmp/algorithms/SurfaceParameterization.h>
#include <pmp/algorithms/DifferentialGeometry.h>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Sparse>

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

namespace pmp {

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

SurfaceParameterization::SurfaceParameterization(SurfaceMesh& mesh)
    : m_mesh(mesh)
{
}

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

bool SurfaceParameterization::setupBoundaryConstraints()
{
    // get properties
    auto points = m_mesh.vertexProperty<Point>("v:point");
    auto tex = m_mesh.vertexProperty<TextureCoordinate>("v:tex");

    SurfaceMesh::VertexIterator vit, vend = m_mesh.verticesEnd();
    SurfaceMesh::Vertex vh;
    SurfaceMesh::Halfedge hh;
    std::vector<SurfaceMesh::Vertex> loop;

    // Initialize all texture coordinates to the origin.
    for (auto v : m_mesh.vertices())
        tex[v] = TextureCoordinate(0.5, 0.5);

    // find 1st boundary vertex
    for (vit = m_mesh.verticesBegin(); vit != vend; ++vit)
        if (m_mesh.isSurfaceBoundary(*vit))
            break;

    // no boundary found ?
    if (vit == vend)
    {
        std::cerr << "Mesh has no boundary." << std::endl;
        return false;
    }

    // collect boundary loop
    vh = *vit;
    hh = m_mesh.halfedge(vh);
    do
    {
        loop.push_back(m_mesh.toVertex(hh));
        hh = m_mesh.nextHalfedge(hh);
    } while (hh != m_mesh.halfedge(vh));

    // map boundary loop to unit circle in texture domain
    unsigned int i, n = loop.size();
    Scalar angle, l, length;
    TextureCoordinate t;

    // compute length of boundary loop
    for (i = 0, length = 0.0; i < n; ++i)
        length += distance(points[loop[i]], points[loop[(i + 1) % n]]);

    // map length intervalls to unit circle intervals
    for (i = 0, l = 0.0; i < n;)
    {
        // go from 2pi to 0 to preserve orientation
        angle = 2.0 * M_PI * (1.0 - l / length);

        t[0] = 0.5 + 0.5 * cosf(angle);
        t[1] = 0.5 + 0.5 * sinf(angle);

        tex[loop[i]] = t;

        ++i;
        if (i < n)
        {
            l += distance(points[loop[i]], points[loop[(i + 1) % n]]);
        }
    }

    return true;
}

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

void SurfaceParameterization::harmonic(bool useUniformWeights)
{
    // map boundary to circle
    if (!setupBoundaryConstraints())
    {
        std::cerr << "Could not perform setup of boundary constraints.\n";
        return;
    }

    // get properties
    auto tex = m_mesh.vertexProperty<TextureCoordinate>("v:tex");
    auto eweight = m_mesh.addEdgeProperty<Scalar>("e:param");
    auto idx = m_mesh.addVertexProperty<int>("v:idx", -1);

    // compute Laplace weight per edge: cotan or uniform
    for (auto e : m_mesh.edges())
    {
        eweight[e] =
            useUniformWeights ? 1.0 : std::max(0.0, cotanWeight(m_mesh, e));
    }

    // collect free (non-boundary) vertices in array free_vertices[]
    // assign indices such that idx[ free_vertices[i] ] == i
    unsigned i = 0;
    std::vector<SurfaceMesh::Vertex> free_vertices;
    free_vertices.reserve(m_mesh.nVertices());
    for (auto v : m_mesh.vertices())
    {
        if (!m_mesh.isSurfaceBoundary(v))
        {
            idx[v] = i++;
            free_vertices.push_back(v);
        }
    }

    // setup matrix A and rhs B
    const unsigned int n = free_vertices.size();
    Eigen::SparseMatrix<double> A(n, n);
    Eigen::MatrixXd B(n, 2);
    std::vector<Eigen::Triplet<double>> triplets;
    double w, ww;
    SurfaceMesh::Vertex v, vv;
    SurfaceMesh::Edge e;
    for (i = 0; i < n; ++i)
    {
        v = free_vertices[i];

        // rhs row
        B(i, 0) = 0.0;
        B(i, 1) = 0.0;

        // lhs row
        ww = 0.0;
        for (auto h : m_mesh.halfedges(v))
        {
            vv = m_mesh.toVertex(h);
            e = m_mesh.edge(h);
            w = eweight[e];
            ww += w;

            if (m_mesh.isSurfaceBoundary(vv))
            {
                B(i, 0) -= -w * tex[vv][0];
                B(i, 1) -= -w * tex[vv][1];
            }
            else
            {
                triplets.emplace_back(i, idx[vv], -w);
            }
        }
        triplets.emplace_back(i, i, ww);
    }

    // build sparse matrix from triplets
    A.setFromTriplets(triplets.begin(), triplets.end());

    // solve A*X = B
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver(A);
    Eigen::MatrixXd X = solver.solve(B);
    if (solver.info() != Eigen::Success)
    {
        std::cerr << "SurfaceParameterization: Could not solve linear system\n";
    }
    else
    {
        // copy solution
        for (i = 0; i < n; ++i)
        {
            v = free_vertices[i];
            tex[v][0] = X(i, 0);
            tex[v][1] = X(i, 1);
        }
    }

    // clean-up
    m_mesh.removeVertexProperty(idx);
    m_mesh.removeEdgeProperty(eweight);
}

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

bool SurfaceParameterization::setupLSCMBoundary()
{
    // constrain the two boundary vertices farthest from each other to fix
    // the translation and rotation of the resulting parameterization

    // vertex properties
    auto pos = m_mesh.vertexProperty<Point>("v:point");
    auto tex = m_mesh.vertexProperty<TexCoord>("v:tex");
    auto locked = m_mesh.addVertexProperty<bool>("v:locked", false);

    // find boundary vertices and store handles in vector
    std::vector<SurfaceMesh::Vertex> boundary;
    for (auto v : m_mesh.vertices())
        if (m_mesh.isSurfaceBoundary(v))
            boundary.push_back(v);

    // no boundary?
    if (boundary.empty())
    {
        return false;
    }

    // find boundary vertices with largest distance
    Scalar diam(0.0), d;
    SurfaceMesh::Vertex v1, v2;
    for (auto vv1 : boundary)
    {
        for (auto vv2 : boundary)
        {
            d = distance(pos[vv1], pos[vv2]);
            if (d > diam)
            {
                diam = d;
                v1 = vv1;
                v2 = vv2;
            }
        }
    }

    // pin these two boundary vertices
    for (auto v : m_mesh.vertices())
    {
        tex[v] = TexCoord(0.5, 0.5);
        locked[v] = false;
    }
    tex[v1] = TexCoord(0.0, 0.0);
    tex[v2] = TexCoord(1.0, 1.0);
    locked[v1] = true;
    locked[v2] = true;

    return true;
}

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

void SurfaceParameterization::lscm()
{
    // boundary constraints
    if (!setupLSCMBoundary())
        return;

    // properties
    auto pos = m_mesh.vertexProperty<Point>("v:point");
    auto tex = m_mesh.vertexProperty<TexCoord>("v:tex");
    auto idx = m_mesh.addVertexProperty<int>("v:idx", -1);
    auto weight = m_mesh.addHalfedgeProperty<dvec2>("h:lscm");
    auto locked = m_mesh.getVertexProperty<bool>("v:locked");
    assert(locked);

    // compute weights/gradients per face/halfedge
    for (auto f : m_mesh.faces())
    {
        // collect face halfedge
        auto fh_it = m_mesh.halfedges(f);
        auto ha = *fh_it;
        ++fh_it;
        auto hb = *fh_it;
        ++fh_it;
        auto hc = *fh_it;

        // collect face vertices
        dvec3 a = (dvec3)pos[m_mesh.toVertex(ha)];
        dvec3 b = (dvec3)pos[m_mesh.toVertex(hb)];
        dvec3 c = (dvec3)pos[m_mesh.toVertex(hc)];

        // calculate local coordinate system
        dvec3 z = cross(c - b, a - b);
        dvec3 x = normalize(b - a);
        dvec3 y = normalize(cross(z, x));

        // calculate local vertex coordinates
        dvec2 a2D(0.0, 0.0);
        dvec2 b2D(norm(b - a), 0.0);
        dvec2 c2D(dot(c - a, x), dot(c - a, y));

        // calculate double triangle area
        double area = norm(z);
        if (area)
            area = 1.0 / area;

        // calculate W_j,Ti (index by corner a,b,c and real/imaginary)
        double War = c2D[0] - b2D[0];
        double Wbr = a2D[0] - c2D[0];
        double Wcr = b2D[0] - a2D[0];
        double Wai = c2D[1] - b2D[1];
        double Wbi = a2D[1] - c2D[1];
        double Wci = b2D[1] - a2D[1];

        // store matrix information per halfedge
        weight[ha] = dvec2(War * area, Wai * area);
        weight[hb] = dvec2(Wbr * area, Wbi * area);
        weight[hc] = dvec2(Wcr * area, Wci * area);
    }

    // collect free (non-boundary) vertices in array free_vertices[]
    // assign indices such that idx[ free_vertices[i] ] == i
    unsigned i = 0;
    std::vector<SurfaceMesh::Vertex> free_vertices;
    free_vertices.reserve(m_mesh.nVertices());
    for (auto v : m_mesh.vertices())
    {
        if (!locked[v])
        {
            idx[v] = i++;
            free_vertices.push_back(v);
        }
    }

    // build matrix and rhs
    const unsigned int nV2 = 2 * m_mesh.nVertices();
    const unsigned int nV = m_mesh.nVertices();
    const unsigned int N = free_vertices.size();
    SurfaceMesh::Vertex vi, vj;
    SurfaceMesh::Halfedge hh;
    double si, sj0, sj1, sign;
    int row(0), c0, c1;

    Eigen::SparseMatrix<double> A(2 * N, 2 * N);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(2 * N);
    std::vector<Eigen::Triplet<double>> triplets;

    for (unsigned int i = 0; i < nV2; ++i)
    {
        vi = SurfaceMesh::Vertex(i % nV);

        if (i < nV)
        {
            sign = 1.0;
            c0 = 0;
            c1 = 1;
        }
        else
        {
            sign = -1.0;
            c0 = 1;
            c1 = 0;
        }

        if (!locked[vi])
        {
            si = 0;

            for (auto h : m_mesh.halfedges(vi))
            {
                vj = m_mesh.toVertex(h);
                sj0 = sj1 = 0;

                if (!m_mesh.isSurfaceBoundary(h))
                {
                    const dvec2& wj = weight[h];
                    const dvec2& wi = weight[m_mesh.prevHalfedge(h)];

                    sj0 += sign * wi[c0] * wj[0] + wi[c1] * wj[1];
                    sj1 += -sign * wi[c0] * wj[1] + wi[c1] * wj[0];
                    si += wi[0] * wi[0] + wi[1] * wi[1];
                }

                h = m_mesh.oppositeHalfedge(h);
                if (!m_mesh.isSurfaceBoundary(h))
                {
                    const dvec2& wi = weight[h];
                    const dvec2& wj = weight[m_mesh.prevHalfedge(h)];

                    sj0 += sign * wi[c0] * wj[0] + wi[c1] * wj[1];
                    sj1 += -sign * wi[c0] * wj[1] + wi[c1] * wj[0];
                    si += wi[0] * wi[0] + wi[1] * wi[1];
                }

                if (!locked[vj])
                {
                    triplets.emplace_back(row, idx[vj], sj0);
                    triplets.emplace_back(row, idx[vj] + N, sj1);
                }
                else
                {
                    b[row] -= sj0 * tex[vj][0];
                    b[row] -= sj1 * tex[vj][1];
                }
            }

            triplets.emplace_back(row, idx[vi] + (i < nV ? 0 : N), 0.5 * si);

            ++row;
        }
    }

    // build sparse matrix from triplets
    A.setFromTriplets(triplets.begin(), triplets.end());

    // solve A*X = B
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver(A);
    Eigen::VectorXd x = solver.solve(b);
    if (solver.info() != Eigen::Success)
    {
        std::cerr << "SurfaceParameterization: Could not solve linear system\n";
    }
    else
    {
        // copy solution
        for (i = 0; i < N; ++i)
        {
            tex[free_vertices[i]] = TexCoord(x[i], x[i + N]);
        }
    }

    // scale tex coordiantes to unit square
    TexCoord bbmin(1, 1), bbmax(0, 0);
    for (auto v : m_mesh.vertices())
    {
        bbmin = min(bbmin, tex[v]);
        bbmax = max(bbmax, tex[v]);
    }
    bbmax -= bbmin;
    Scalar s = std::max(bbmax[0], bbmax[1]);
    for (auto v : m_mesh.vertices())
    {
        tex[v] -= bbmin;
        tex[v] /= s;
    }

    // clean-up
    m_mesh.removeVertexProperty(idx);
    m_mesh.removeVertexProperty(locked);
    m_mesh.removeHalfedgeProperty(weight);
}

//=============================================================================
} // namespace pmp
//=============================================================================