//------------------------------------------------------------------------------
//  Copyright 2007-2011 by Jyh-Ming Lien and George Mason University
//  See the file "LICENSE" for more information
//------------------------------------------------------------------------------

#include "polygon.h"
#include "intersection.h"
#include <vector>
#include <cmath>

#include <boost/polygon/polygon.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/algorithms/equals.hpp>

#include "adapt_boost_poly.h"

using namespace std;
namespace bp = boost::polygon;
namespace bg = boost::geometry;

#ifdef WIN32
extern "C"{
#include "triangulate.h"
}
#else
#include "triangulate.h"
#endif

template<typename Pt_>
ply_vertex<Pt_>::~ply_vertex()
{
    //doing nothing for now
}

// - compute normal
// - check if the vertex is reflex or not
template<typename Pt_>
void
ply_vertex<Pt_>::computeExtraInfo()
{
    //compute normal direction
    point_type v= next->pos;
    bg::subtract_point(v, pos);
    if( bg::get<0>(v) == 0 ){
        if (bg::get<1>(v)>0) { bg::set<0>(normal,1); bg::set<1>(normal,0); }
        else { bg::set<0>(normal,-1); bg::set<1>(normal,0); }
    }
    else if( bg::get<0>(v)>0 ){
      bg::set<1>(normal, -1);
      bg::set<0>(normal, bg::get<1>(v) / bg::get<0>(v));
    }
    else{//get<0>(v)<0
      bg::set<1>(normal, 1);
      bg::set<0>(normal, -(bg::get<1>(v) / bg::get<0>(v)));
    }

    // normalize
    normalize(normal);

    // compute left or right turn
    reflex = !leftTurn(pre->pos, pos, next->pos);
}

template<typename Pt_>
void
ply_vertex<Pt_>::negate()
{
    bg::multiply_value(normal, -1);
    bg::multiply_value(pos, -1);
}

template<typename Pt_>
void
ply_vertex<Pt_>::reverse()
{
    swap(next,pre);
    //normal=-normal;
    computeExtraInfo();
}

template<typename Pt_>
void
ply_vertex<Pt_>::copy(ply_vertex * other)
{
    pos=other->pos;
    normal=other->normal;
    reflex=other->reflex;
    vid=other->vid;
}

///////////////////////////////////////////////////////////////////////////////

//copy from the given ply
template<typename Pt_>
void
c_ply<Pt_>::copy(const c_ply<Pt_>& other)
{
    destroy();//detroy myself first

    vertex_type* ptr=other.head;
    beginPoly();
    do{
        vertex_type * v=new vertex_type();
        assert(v); //check for memory
        v->copy(ptr);
        addVertex(v);
        ptr=ptr->getNext();
    }while( ptr!=other.head );

    //endPoly();
    //finish up
    tail->setNext(head);

    //copy extra info
    area=other.area;
    bg::assign_point(center, other.center);
    radius=other.radius;
    type=other.type;
    triangulation=other.triangulation;
    extra_info=other.extra_info;
}

// clean up the space allocated
template<typename Pt_>
void
c_ply<Pt_>::destroy()
{
    if( head==NULL ) return;
    vertex_type* ptr=head;
    do{
        vertex_type * n=ptr->getNext();
        delete ptr;
        ptr=n;
    }while( ptr!=head );
    head=tail=NULL;

    all.clear();
    triangulation.clear();
}

// Create a empty polygon
template<typename Pt_>
void
c_ply<Pt_>::beginPoly()
{
    head=tail=NULL;
    all.clear();
    triangulation.clear();
}

// Add a vertex to the polygonal chian
template<typename Pt_>
void
c_ply<Pt_>::addVertex(
    typename c_ply<Pt_>::coordinate_type x,
    typename c_ply<Pt_>::coordinate_type y,
    bool remove_duplicate )
{
    point_type pt(x,y);

    if(tail!=NULL){
        if(bg::equals(tail->getPos(), pt) && remove_duplicate) return; //don't add
    }

    vertex_type * v=new vertex_type(pt);
    if( tail!=NULL ){
        tail->setNext(v);
    }
    tail=v;
    if( head==NULL ) head=tail;
    v->setVID(all.size()); //id of the vertex in this ply
	all.push_back(v);

}

// Add a vertex to the polygonal chian
template<typename Pt_>
void
c_ply<Pt_>::addVertex( vertex_type * v )
{
    if( tail!=NULL ){
        tail->setNext(v);
    }
    tail=v;
    if( head==NULL ) head=tail;
    v->setVID(all.size()); //id of the vertex in this ply
    all.push_back(v);
}

// finish building the polygon
template<typename Pt_>
void
c_ply<Pt_>::endPoly(bool remove_duplicate)
{
    if(head!=NULL && tail!=NULL){
        if(remove_duplicate){
            if(bg::equals(head->getPos(), tail->getPos())){ //remove tail..
                delete tail;
                all.pop_back();
                tail=all.back();
            }
        }//
    }

    tail->setNext(head);
    doInit();
}

// initialize property of the this polychain
// Compute normals and find reflective vertices
template<typename Pt_>
void
c_ply<Pt_>::doInit()
{
    //compute area
    getArea();
    if(this->area<0 && type==POUT){
       //cerr<<"! Warning: polygon type is POUT but has negative area. Reverse the vertex ordering."<<endl;
       reverse();
    }
    else if(this->area>0 && type==PIN){
       //cerr<<"! Warning: polygon type is PIN but has positive area. Reverse the vertex ordering."<<endl;
       reverse();
    }

    //compute normals
    vertex_type* ptr=head;
    do{
        ptr->computeExtraInfo();
        ptr=ptr->getNext();
    }while( ptr!=head );
}

template<typename Pt_>
const typename c_ply<Pt_>::point_type&
c_ply<Pt_>::getCenter()
{
    if(radius<0){
        center = point_type(0,0);
        vertex_type * ptr=head;
        const point_type& first=ptr->getPos();
        uint size=0;
        do{
            size++;
            point_type v = ptr->getPos();
            bg::subtract_point(v, first);
            bg::add_point(center, v);
            ptr=ptr->getNext();
        }while(ptr!=head); //end while
        bg::divide_value(center, size);
        bg::add_point(center, first);

        radius=0;
    }

    return center;
}

///////////////////////////////////////////////////////////////////////////
template<typename Pt_>
void
c_ply<Pt_>::negate()
{
    vertex_type * ptr=head;
    do{
        ptr->negate();
        ptr=ptr->getNext();
    }while(ptr!=head); //end while
}

//reverse the order of the vertices
template<typename Pt_>
void
c_ply<Pt_>::reverse()
{
    vertex_type * ptr=head;
    do{
        ptr->reverse();
        ptr=ptr->getNext();
    }
    while(ptr!=head); //end while

    this->area=-this->area;
    all.clear();
    triangulation.clear();
}

///////////////////////////////////////////////////////////////////////////
template<typename Pt_>
void
c_ply<Pt_>::translate(const point_type& p)
{
    vertex_type * ptr=head;
    do{
        ptr->translate(p);
        ptr=ptr->getNext();
    }while(ptr!=head); //end while

    //translate box
    extra_info.box[0] += bg::get<0>(p);
    extra_info.box[1] += bg::get<0>(p);
    extra_info.box[2] += bg::get<1>(p);
    extra_info.box[3] += bg::get<1>(p);
}

///////////////////////////////////////////////////////////////////////////
//compute the Radius of the poly chain
template<typename Pt_>
typename c_ply<Pt_>::coordinate_type
c_ply<Pt_>::getRadius()
{
    if(radius<0) getCenter();

    if(radius==0){
        vertex_type * ptr=head;
        do{
            point_type v = center;
            bg::subtract_point(v, ptr->getPos());
            coordinate_type d;
            normalize(v, d);
            if(d>radius) radius=d;
            ptr=ptr->getNext();
        }while(ptr!=head); //end while
        radius=sqrt(radius);
    }

    return radius;
}

template<typename Pt_>
typename c_ply<Pt_>::coordinate_type
c_ply<Pt_>::getArea()
{
    if (area == numeric_limits<coordinate_type>::lowest())
      area = bp::area(*this);

    return area;
}

template<typename Pt_>
bool
c_ply<Pt_>::enclosed(const typename c_ply<Pt_>::point_type& p)
{
    if(triangulation.empty())
        triangulate(triangulation);

    //find the largest triangle
    for(uint i=0;i<triangulation.size();i++){
        triangle & tri=triangulation[i];
        const point_type& p1=all[tri.v[0]]->getPos();
        const point_type& p2=all[tri.v[1]]->getPos();
        const point_type& p3=all[tri.v[2]]->getPos();
        coordinate_type area1=Area(p1,p2,p);
        coordinate_type area2=Area(p2,p3,p);
        coordinate_type area3=Area(p3,p1,p);
        if(area1>=0 && area2>=0 && area3>=0) return true; //in
        if(area1<=0 && area2<=0 && area3<=0) return true; //in
    }

    return false; //out
}

template<typename Pt_>
typename c_ply<Pt_>::point_type
c_ply<Pt_>::findEnclosedPt()
{
    if(triangulation.empty())
        triangulate(triangulation);

    if(triangulation.empty()){ //too few points...
        const point_type& p1=head->getPos();
        const point_type& p2=head->getNext()->getPos();
        point_type pt;
        bg::assign_point(pt, p1);
        bg::add_point(pt, p2);
        bg::divide_value(pt, 2);
        return pt;
    }

    //find the largest triangle
    coordinate_type largest_area=-1;
    uint   largest_tri=0;
    for(uint i=0;i<triangulation.size();i++){
        triangle & tri=triangulation[i];
        const point_type& p1=all[tri.v[0]]->getPos();
        const point_type& p2=all[tri.v[1]]->getPos();
        const point_type& p3=all[tri.v[2]]->getPos();
        coordinate_type area=fabs(Area(p1,p2,p3));
        if(area>largest_area){
            largest_area=area;
            largest_tri=i;
        }
    }

    //find a node near the vertex of the triangle
    triangle & tri=triangulation[largest_tri];
    const point_type& p1=(*this)[tri.v[0]]->getPos();
    const point_type& p2=(*this)[tri.v[1]]->getPos();
    const point_type& p3=(*this)[tri.v[2]]->getPos();

    point_type pt;
    bg::assign_point(pt, p1);
    bg::add_point(pt, p2);
    bg::add_point(pt, p3);
    bg::divide_value(pt, 3);

    return pt;
}

template<typename Pt_>
void
c_ply<Pt_>::triangulate(vector<triangle>& tris)
{
     indexing();
     if(triangulation.empty()){

         const point_type& O=getHead()->getPos();

         int * ringVN=new int[1];     //number of vertices for each ring
         assert(ringVN);
         ringVN[0]=getSize();
         int vN=ringVN[0];             //total number of vertices

         if( vN<3 ){
             triangle tri;
             for(short i=0;i<3;i++) tri.v[i]=i;
             tris.push_back(tri);
             return;
         }

         //more than 3 vertices
         int tN=(vN-2);                   //# of triangles
         double * V = new double[vN*2]; //to hold vertices pos
         int *T=new int[3*tN];            //to hold resulting triangles
         assert(T&&V);

         //copy vertices
         {
             int i=0;
             vertex_type * ptr=getHead();
             do{
                 point_type pt=ptr->getPos();
                 V[i*2]=bg::get<0>(pt)-bg::get<0>(O);    // potential implicit conversion
                 V[i*2+1]=bg::get<1>(pt)-bg::get<1>(O);
                 ptr=ptr->getNext();
                 i++;
             }while( ptr!=getHead() );
         }

         FIST_PolygonalArray(1, ringVN, (double (*)[2])V, &tN, (int (*)[3])T);

         for(int i=0;i<tN;i++){
             triangle tri;
             for(int j=0;j<3;j++){
                 tri.v[j]=T[i*3+j];
             }//end j
             triangulation.push_back(tri);
         }//end i

         delete [] ringVN;
         delete [] V;
         delete [] T;
     }//end if

     tris=triangulation;
}

//check if convex
template<typename Pt_>
bool
c_ply<Pt_>::is_convex() const
{
    vertex_type * ptr=head;
    do{
        if(ptr->isReflex()) return false;
        ptr=ptr->getNext();
    }while(ptr!=head); //end while

    return true;
}

template<typename Pt_>
void
c_ply<Pt_>::delete_vertex(vertex_type * v)
{
    vertex_type *pre=v->getPre();
    vertex_type *next=v->getNext();

    pre->setNext(next);
    next->setPre(pre);
    pre->computeExtraInfo(); //recompute info

    if(head==v){
        head=next;
        tail=pre;
    }
    delete v;

    triangulation.clear(); //not valid anymore
    all.clear(); //not valid anymore
}

template<typename Pt_>
void
c_ply<Pt_>::indexing()
{
    uint vid=0;
    all.clear();
    vertex_type * ptr=head;
    do{
        ptr->setVID(vid++);
        all.push_back(ptr);
        ptr=ptr->getNext();
    }while(ptr!=head); //end while
}

template<typename Pt_>
bool
c_ply<Pt_>::identical(const c_ply<Pt_>& other)
{

    //do some basic checks
    if((*this)==other) return true; //same head ptr
    if(type!=other.type) return false;
    if(getSize()!=other.getSize()) return false;
    if( fabs(getArea()-other.getArea())>SMALLNUMBER ) return false;

    //find the first match
    const vertex_type * o_ptr=other.head;
    bool found_first_match=false;
    do{
        const point_type& pos=o_ptr->getPos();
        coordinate_type dx=fabs(bg::get<0>(pos)-bg::get<0>(head->getPos()));
        coordinate_type dy=fabs(bg::get<1>(pos)-bg::get<0>(head->getPos()));

        if( dx<SMALLNUMBER && dy<SMALLNUMBER){
            found_first_match=true;
            break;
        }

        o_ptr=o_ptr->getNext();
    }while(o_ptr!=other.head); //end while

    if(!found_first_match) return false;

    //check the rest of the match
    const vertex_type * ptr=head;

    //since we are sure that ptr matches to o_ptr, we first advance
    ptr=ptr->getNext();
    o_ptr=o_ptr->getNext();

    do
    {
        const point_type& o_pos=o_ptr->getPos();
        const point_type& pos=ptr->getPos();

        coordinate_type dx=fabs(bg::get<0>(pos)-bg::get<0>(o_pos));
        coordinate_type dy=fabs(bg::get<1>(pos)-bg::get<1>(o_pos));

        if( dx>SMALLNUMBER || dy>SMALLNUMBER){
            return false; //don't match
        }

        //advance
        ptr=ptr->getNext();
        o_ptr=o_ptr->getNext();

    }
    while(ptr!=head);

    return true;
}

//
//
// Compute the center and the box of a list of plys
//
//
template<typename Pt_>
void
c_plylist<Pt_>::buildBoxAndCenter()
{
    box[0] = box[2] = std::numeric_limits<coordinate_type>::max();
    box[0] = box[3] = std::numeric_limits<coordinate_type>::lowest();
    for(auto i=begin();i!=end();i++){

        const vertex_type * ptr=i->getHead();
        do{
            const point_type& p=ptr->getPos();
            if(bg::get<0>(p)<box[0]) box[0]= bg::get<0>(p);
            if(bg::get<0>(p)>box[1]) box[1]= bg::get<0>(p);
            if(bg::get<1>(p)<box[2]) box[2]= bg::get<1>(p);
            if(bg::get<1>(p)>box[3]) box[3]= bg::get<1>(p);
            ptr=ptr->getNext();
        }
        while(ptr!=i->getHead()); //end while
    }

    bg::set<0>(center, (box[0]+box[1])/2);
    bg::set<1>(center, (box[2]+box[3])/2);

    is_buildboxandcenter_called=true;
}

template<typename Pt_>
void
c_plylist<Pt_>::translate(const typename c_plylist<Pt_>::point_type& p)
{
    for (auto i=begin();i!=end();i++) i->translate(p);
}

template<typename Pt_>
void
c_polygon<Pt_>::reverse()
{
    for(auto i=begin();i!=end();i++) i->reverse();
    triangulation.clear();
    all.clear();
}

template<typename Pt_>
bool
c_polygon<Pt_>::valid() const
{
    if(empty()) return false;
    if(front().getType()!=ring_type::POUT) return false;
    for(auto i=++begin();i!=end();i++)
      if(i->getType()!=ring_type::PIN)
        return false;

    return true;
}

//copy from the given polygon
template<typename Pt_>
void
c_polygon<Pt_>::copy(const c_polygon& other)
{
    destroy();

    for(auto i=other.cbegin();i!=other.cend();i++){
        this->emplace_back(i->getType());
        this->back().copy(*i);
    }
    indexing();
}

//check if a point is enclosed
//the behavior is unknown if pt is on the boundary of the polygon
template<typename Pt_>
bool
c_polygon<Pt_>::enclosed(const typename c_polygon<Pt_>::point_type& p) const
{
    //find the largest triangle
    for(uint i=0;i<triangulation.size();i++){
        const triangle & tri=triangulation[i];
        const point_type& p1=(*this)[tri.v[0]]->getPos();
        const point_type& p2=(*this)[tri.v[1]]->getPos();
        const point_type& p3=(*this)[tri.v[2]]->getPos();
        coordinate_type area1=Area(p1,p2,p);
        coordinate_type area2=Area(p2,p3,p);
        coordinate_type area3=Area(p3,p1,p);
        if(area1>=0 && area2>=0 && area3>=0) return true; //in
        if(area1<=0 && area2<=0 && area3<=0) return true; //in
    }

    return false; //out
}

template<typename Pt_>
typename c_polygon<Pt_>::point_type
c_polygon<Pt_>::findEnclosedPt()
{
    if(triangulation.empty())
        triangulate(triangulation);

    if(triangulation.empty())
        return front().findEnclosedPt();

    //find the largest triangle
    coordinate_type largest_area=-1;
    uint   largest_tri=0;
    for(uint i=0;i<triangulation.size();i++){
        triangle & tri=triangulation[i];
        const point_type& p1=(*this)[tri.v[0]]->getPos();
        const point_type& p2=(*this)[tri.v[1]]->getPos();
        const point_type& p3=(*this)[tri.v[2]]->getPos();
        coordinate_type area=fabs(Area(p1,p2,p3));
        if(area>largest_area){
            largest_area=area;
            largest_tri=i;
        }
    }

    //find a node near the vertex of the triangle
    triangle & tri=triangulation[largest_tri];
    const point_type& p1=(*this)[tri.v[0]]->getPos();
    const point_type& p2=(*this)[tri.v[1]]->getPos();
    const point_type& p3=(*this)[tri.v[2]]->getPos();

    point_type pt;
    bg::assign_point(pt, p1);
    bg::add_point(pt, p2);
    bg::add_point(pt, p3);
    bg::divide_value(pt, 3);

    return pt;
}

template<typename Pt_>
void
c_polygon<Pt_>::triangulate(vector<triangle>& tris)
{
    if(triangulation.empty())
    {
         const point_type& O=front().getHead()->getPos();

         int ringN=size();             //number of rings
         int * ringVN=new int[ringN];     //number of vertices for each ring
         assert(ringVN);

         int vN=0;             //total number of vertices
         {
             int i=0;
             for(auto ip=begin();ip!=end();ip++,i++){
                 vN+=ip->getSize();
                 ringVN[i]=ip->getSize();
             }
         }

         if( vN<3 ){
//             triangle tri;
//             for(short i=0;i<3;i++) tri.v[i]=i;
//             tris.push_back(tri);
             return;
         }

         int tN=(vN-2)+2*(ringN-1);       //# of triangles, (n-2)+2*(#holes)
         double * V=new double[vN*2];     //to hold vertices pos
         int *T=new int[3*tN];            //to hold resulting triangles
         assert(T&&V);

         //copy vertices
         int i=0;
         for(auto ip=begin();ip!=end();ip++){
             vertex_type * ptr=ip->getHead();
             do{
                 point_type pt=ptr->getPos();
                 V[i*2]=bg::get<0>(pt)-bg::get<0>(O);    // potential implicit conversion to double
                 V[i*2+1]=bg::get<1>(pt)-bg::get<1>(O);
                 ptr=ptr->getNext();
                 i++;
             }while( ptr!=ip->getHead() );
         }

         FIST_PolygonalArray(ringN, ringVN,
             (double (*)[2])V, &tN, (int (*)[3])T);

         for(int i=0;i<tN;i++){
             triangle tri;
             for(int j=0;j<3;j++){
                 tri.v[j]=T[i*3+j];
             }//end j
             triangulation.push_back(tri);
         }//end i

    }

    tris=triangulation;
}

template<typename Pt_>
void
c_polygon<Pt_>::pop_front()
{
  ring_type &ply = front();
  ply.indexing(); // reset vertex IDs
  c_plylist<Pt_>::pop_front();
  /* These must be recomputed */
  all.clear();
  triangulation.clear();
  indexing();
}

template<typename Pt_>
void
c_polygon<Pt_>::destroy()
{
    for(auto i=begin();i!=end();i++){
        i->destroy();
    }
    clear(); //remove all ply from this list
    all.clear();
    triangulation.clear();
}

template<typename Pt_>
bool
c_polygon<Pt_>::is_convex() const
{
    if(size()>1) return false; //contains hole
    return front().is_convex();
}

template<typename Pt_>
void
c_polygon<Pt_>::indexing()
{
    uint vid=0;
    for(auto i=begin();i!=end();i++){
        uint vsize=i->getSize();
        for(uint j=0;j<vsize;j++){
            (*i)[j]->setVID(vid++);       //id of vertex in the polygons
            all.push_back((*i)[j]);
        }
    }//end for i
}

template<typename Pt_>
typename c_polygon<Pt_>::coordinate_type
c_polygon<Pt_>::getArea()
{
  if (area == 0) {
    area = bp::area(*this);
  }

  return area;
}

template<typename Pt_>
bool
c_polygon<Pt_>::identical(c_polygon& p)
{
    if(size()!=p.size()) return false;

    coordinate_type a1=getArea();
    coordinate_type a2=p.getArea();
    if( fabs(a1-a2)>SMALLNUMBER ) return false;

    for(auto i=begin();i!=end();i++)
    {
        bool match=false;
        for(auto j=p.begin();j!=p.end();j++){
            if( i->identical(*j) ){
                match=true;
                break;
            }
        }
        if(!match) return false;
    }

    return true;
}

template<typename Pt_>
istream& operator>>( istream& is, c_ply<Pt_>& poly)
{
    typedef typename c_ply<Pt_>::coordinate_type coordinate_type;
    typedef typename c_ply<Pt_>::point_type point_type;

    int vsize; string str_type;
    is>>vsize>>str_type;

    if( str_type.find("out")!=string::npos )
        poly.type=c_ply<Pt_>::POUT;
    else poly.type=c_ply<Pt_>::PIN;

    poly.beginPoly();
    //read in all the vertices
    int iv;
    vector<point_type> pts; pts.reserve(vsize);
    for( iv=0;iv<vsize;iv++ ){
        coordinate_type x,y;
        is>>x>>y;
        pts.emplace_back(x,y);
        //coordinate_type d=x*x+y*y;
    }
    int id;
    for( iv=0;iv<vsize;iv++ ){
        is>>id; id=id-1;
        poly.addVertex(bg::get<0>(pts[id]), bg::get<1>(pts[id]));
    }

    poly.endPoly();
    return is;
}

template<typename Pt_>
istream& operator>>( istream& is, c_plylist<Pt_>& p)
{
    //remove header commnets
    do{
        char tmp[1024];
        char c=is.peek();
        if(isspace(c)) is.get(c); //eat it
        else if(c=='#') {
            is.getline(tmp,1024);
        }
        else break;
    }while(true);

    //start reading
    uint size;
    is>>size;
    uint vid=0;
    for(uint i=0;i<size;i++){
        c_ply<Pt_> poly(c_ply<Pt_>::UNKNOWN);
        is>>poly;
        p.push_back(poly);
        uint vsize=poly.getSize();
        for(uint j=0;j<vsize;j++){
            poly[j]->setVID(vid++);       //id of vertex in the polygons
            //p.all.push_back(poly[j]);
        }
    }
    return is;
}

template<typename Pt_>
ostream&
operator<<( ostream& os, const c_ply<Pt_>& p)
{
    os<<p.getSize()<<" "<<((p.type==c_ply<Pt_>::PIN)?"PIN  ":"POUT ");
    const typename c_ply<Pt_>::vertex_type * ptr=p.head;
    os << "< " << ptr->getPos();
    ptr = ptr->getNext();
    while(ptr!=p.head)
    {
        os<< ", " << ptr->getPos();
        ptr=ptr->getNext();
    }

    return os;
}

template<typename Pt_>
ostream&
operator<<( ostream& out, const c_plylist<Pt_>& p)
{
    out<<p.size()<<"\n";
    for(auto i=p.cbegin();i!=p.cend();i++) out<<*i;
    return out;
}

template<typename Pt_>
ostream &
operator<<(ostream &os, const c_polygon<Pt_> &p)
{
  unsigned int idx = 0u;
  os << "c_polygon({" << endl;
  for (auto ply_it = p.cbegin(); ply_it != p.cend(); ++ply_it)
    os << "    [" << setw(2) << idx++ << "] " << *ply_it << endl;
  os << "})";
  return os;
}

template<typename Pt_>
ostream &
operator<<(ostream &os, const vector<c_polygon<Pt_> > &v)
{
  os << "vector<c_polygon>({" << endl;
  unsigned int idx = 0u;
  for (auto it = v.begin(); it != v.end(); ++it)
    os << "  [" << setw(2) << idx++ << "] " << *it << endl;
  os << "})";
  return os;
}

template<typename Tp_>
ostream & operator<<(ostream &os,
    boost::geometry::model::d2::point_xy<Tp_> const&p)
{
  os << "(" << bg::get<0>(p) << "," << bg::get<1>(p) << ")";
  return os;
}

PLY_INSTANTIATE(boost::geometry::model::d2::point_xy<float>, );
PLY_INSTANTIATE(boost::geometry::model::d2::point_xy<double>, );

template std::ostream &operator<< <float>(std::ostream&,
    boost::geometry::model::d2::point_xy<float> const&);
template std::ostream &operator<< <double>(std::ostream&,
    boost::geometry::model::d2::point_xy<double> const&);


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <stan/math/prim/mat.hpp>
#include <math/prim/mat/prob/vector_rng_test_helper.hpp>
#include <math/prim/mat/prob/VectorIntRNGTestRig.hpp>
#include <limits>
#include <vector>

class NegativeBinomialTestRig : public VectorIntRNGTestRig {
 public:
  NegativeBinomialTestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1, 2, 3, 4, 5, 6}, {0.1, 1.7, 3.99},
                            {1, 2, 3}, {-2.1, -0.5, 0.0}, {-3, -1, 0},
                            {0.1, 1.1, 4.99}, {1, 2, 3}, {-3.0, -2.0, 0.0},
                            {-3, -1, 0}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& alpha, const T2& beta, const T3&,
                        T_rng& rng) const {
    return stan::math::neg_binomial_rng(alpha, beta, rng);
  }

  template <typename T1>
  double pmf(int y, T1 alpha, double beta, double) const {
    return std::exp(stan::math::neg_binomial_lpmf(y, alpha, beta));
  }
};

TEST(ProbDistributionsNegativeBinomial, errorCheck) {
  check_dist_throws_all_types(NegativeBinomialTestRig());
}

TEST(ProbDistributionsNegativeBinomial, distributionCheck) {
  check_counts_real_real(NegativeBinomialTestRig());
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_coordsys_prod);

BOOST_AUTO_TEST_CASE(simple_test)
{
  typedef OpenTissue::math::BasicMathTypes<double,size_t> math_types;

  typedef math_types::value_traits     value_traits;
  typedef math_types::real_type        T;
  typedef math_types::vector3_type     V;
  typedef math_types::quaternion_type  Q;
  typedef math_types::coordsys_type    X;

  {
    X I;
    I.identity();

    X L;
    L.T() = V(1.0,2.0,3.0);
    L.Q().Ru( value_traits::pi(), V(1.0, 2.0, 3.0) );

    X R = OpenTissue::math::prod( L, I );

    BOOST_CHECK( fabs( R.T()(0) - L.T()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(1) - L.T()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(2) - L.T()(2) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().s() - L.Q().s() ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().v()(0) - L.Q().v()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().v()(1) - L.Q().v()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().v()(2) - L.Q().v()(2) ) < 10e-10 );
  }
  {
    X I;
    I.identity();

    X L;
    L.T() = V(1.0,2.0,3.0);
    L.Q().Ru( value_traits::pi(), V(1.0, 2.0, 3.0) );

    X R = OpenTissue::math::prod( I, L );

    BOOST_CHECK( fabs( R.T()(0) - L.T()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(1) - L.T()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(2) - L.T()(2) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().s() - L.Q().s() ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().v()(0) - L.Q().v()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().v()(1) - L.Q().v()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().v()(2) - L.Q().v()(2) ) < 10e-10 );
  }
  {
    X I;
    I.identity();

    X R = OpenTissue::math::prod( I, I );

    BOOST_CHECK( fabs( R.T()(0) - I.T()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(1) - I.T()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(2) - I.T()(2) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().s() - I.Q().s() ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().v()(0) - I.Q().v()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().v()(1) - I.Q().v()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().v()(2) - I.Q().v()(2) ) < 10e-10 );
  }
  {
    X I;
    I.identity();

    X L;
    L.T() = V(1.0,2.0,3.0);
    L.Q().Ru( value_traits::pi(), V(1.0, 2.0, 3.0) );

    X invL = OpenTissue::math::inverse( L );
    X R = OpenTissue::math::prod( invL, L );

    BOOST_CHECK( fabs( R.T()(0) - I.T()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(1) - I.T()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.T()(2) - I.T()(2) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().s() - I.Q().s() ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().v()(0) - I.Q().v()(0) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().v()(1) - I.Q().v()(1) ) < 10e-10 );
    BOOST_CHECK( fabs( R.Q().v()(2) - I.Q().v()(2) ) < 10e-10 );
  }
}

BOOST_AUTO_TEST_SUITE_END();


#pragma once
#include <cmath>
#include <boost/random.hpp>

class PRNG{
  const size_t n_rng;
  boost::mt19937* gen;
  boost::uniform_real<> uni_dist; //range is [0,1].
  boost::normal_distribution<> *normal_dist; //mean=0 sd=1.0

public:
  PRNG(const size_t seed, const size_t th_numb) : n_rng(th_numb) {
    gen = new boost::mt19937 [th_numb];
    for (size_t i = 0; i < th_numb; i++) gen[i].seed(seed + i * 10);
    normal_dist = new boost::normal_distribution<> (0.0, 1.0);
  }
  PRNG(const size_t th_numb) : n_rng(th_numb) {
    gen = new boost::mt19937 [th_numb];
    const size_t base_seed = (size_t) time(NULL);
    for (size_t i = 0; i < th_numb; i++) gen[i].seed(base_seed + i * 10);
    normal_dist = new boost::normal_distribution<> (0.0, 1.0);
  }

  ~PRNG(){
    delete [] gen;
    delete normal_dist;
  }

  inline double Uniform(const int tid){
    return uni_dist(gen[tid]);
  }
  inline double Uniform(const int tid, const double up, const double dw){
    return dw + (up - dw) * uni_dist(gen[tid]);
  }

  inline double Normal(const int tid){
    return (*normal_dist)(gen[tid]);
  }
  inline double Normal(const int tid, const double mean, const double sd){
    return mean + (*normal_dist)(gen[tid]) * sqrt(sd);
  }
};


/*
  Total variation denoising based on the paper
  "The Split Bregman Method for L1-Regularized Problems" by Tom Goldstein and Stanley Osher.
  Siam J. Imaging Sciences. Vol. 2, No. 2, pp. 323-343.
*/

// Gadgetron includes
#include "cuNDArray.h"
#include "hoNDArray_fileio.h"
#include "cuSbCgSolver.h"
#include "cuCgSolver.h"
#include "identityOperator.h"
#include "cuPartialDerivativeOperator.h"
#include "parameterparser.h"
#include "cuNDDWT.h"
#include "cuDWTOperator.h"
#include <boost/make_shared.hpp>
// Std includes
#include <iostream>

using namespace std;
using namespace Gadgetron;

// Define desired precision
typedef float _real;

int main(int argc, char** argv)
{
  //
  // Parse command line
  //

  ParameterParser parms;
  parms.add_parameter( 'd', COMMAND_LINE_STRING, 1, "Noisy image file name (.real)", true );
  parms.add_parameter( 'r', COMMAND_LINE_STRING, 1, "Result file name", true, "denoised_image_TV.real" );
  parms.add_parameter( 'i', COMMAND_LINE_INT,    1, "Number of cg iterations", true, "20" );
  parms.add_parameter( 'I', COMMAND_LINE_INT,    1, "Number of sb inner iterations", true, "1" );
  parms.add_parameter( 'O', COMMAND_LINE_INT,    1, "Number of sb outer iterations", true, "10" );
  parms.add_parameter( 'l', COMMAND_LINE_FLOAT,  1, "Total variation weight (lambda)", true, "50.0" );
  parms.add_parameter( 'm', COMMAND_LINE_FLOAT,  1, "Regularization weight (mu)", true, "25.0" );
  parms.add_parameter('w', COMMAND_LINE_FLOAT, 1, "Wavelet weight" ,true, "0");

  parms.parse_parameter_list(argc, argv);
  if( parms.all_required_parameters_set() ){
    cout << " Running denoising with the following parameters: " << endl;
    parms.print_parameter_list();
  }
  else{
    cout << " Some required parameters are missing: " << endl;
    parms.print_parameter_list();
    parms.print_usage();
    return 1;
  }

  // Load sample data from disk
  boost::shared_ptr< hoNDArray<_real> > host_data =
    read_nd_array<_real>((char*)parms.get_parameter('d')->get_string_value());

  if( !host_data.get() ){
    cout << endl << "Input image not found. Quitting!\n" << endl;
    return 1;
  }

  if( host_data->get_number_of_dimensions() != 2 ){
    cout << endl << "Input image is not two-dimensional. Quitting!\n" << endl;
    return 1;
  }

  // Upload host data to device
  cuNDArray<_real> data(host_data.get());

  _real mu = (_real) parms.get_parameter('m')->get_float_value();
  _real lambda = (_real)parms.get_parameter('l')->get_float_value();

  if( mu <= (_real) 0.0 ) {
    cout << endl << "Regularization parameter mu should be strictly positive. Quitting!\n" << endl;
    return 1;
  }

  unsigned int num_cg_iterations = parms.get_parameter('i')->get_int_value();
  unsigned int num_inner_iterations = parms.get_parameter('I')->get_int_value();
  unsigned int num_outer_iterations = parms.get_parameter('O')->get_int_value();

 // Define encoding operator (identity)
  boost::shared_ptr< identityOperator<cuNDArray<_real> > > E( new identityOperator<cuNDArray<_real> >() );
  E->set_weight( mu );
  E->set_domain_dimensions(data.get_dimensions().get());
  E->set_codomain_dimensions(data.get_dimensions().get());

  // Setup split-Bregman solver
  cuSbCgSolver<_real> sb;
  sb.set_encoding_operator( E );
  sb.set_max_outer_iterations(num_outer_iterations);
  sb.set_max_inner_iterations(num_inner_iterations);
  sb.set_output_mode( cuCgSolver<_real>::OUTPUT_VERBOSE );
   // Setup regularization operators

  if (lambda > 0){
  boost::shared_ptr< cuPartialDerivativeOperator<_real,2> > Rx( new cuPartialDerivativeOperator<_real,2>(0) );
  Rx->set_weight( lambda );
  Rx->set_domain_dimensions(data.get_dimensions().get());
  Rx->set_codomain_dimensions(data.get_dimensions().get());

  boost::shared_ptr< cuPartialDerivativeOperator<_real,2> > Ry( new cuPartialDerivativeOperator<_real,2>(1) );
  Ry->set_weight( lambda );
  Ry->set_domain_dimensions(data.get_dimensions().get());
  Ry->set_codomain_dimensions(data.get_dimensions().get());
  //sb.add_regularization_operator( Rx ); // Anisotropic denoising
  //sb.add_regularization_operator( Ry ); // Anisotropic denoising
  sb.add_regularization_group_operator( Rx ); // Isotropic denoising
  sb.add_regularization_group_operator( Ry); // Isotropic denoising
  sb.add_group();
  }

  _real wavelet = parms.get_parameter('w')->get_float_value();
  if (wavelet > 0){
	  auto dwt = boost::make_shared<cuDWTOperator<_real,2>>();
	  dwt->set_levels(3);
	  dwt->set_weight(wavelet);
	  sb.add_regularization_operator(dwt);
	  dwt->set_domain_dimensions(data.get_dimensions().get());
	  dwt->set_codomain_dimensions(data.get_dimensions().get());
	  dwt->use_random(true);
  }

  // Setup inner conjugate gradient solver
  sb.get_inner_solver()->set_max_iterations( num_cg_iterations );
  sb.get_inner_solver()->set_tc_tolerance( 1e-4 );
  sb.get_inner_solver()->set_output_mode( cuCgSolver<_real>::OUTPUT_WARNINGS );

  // Run split-Bregman solver
  boost::shared_ptr< cuNDArray<_real> > sbresult = sb.solve(&data);

  /*
  boost::shared_ptr< cuNDArray<_real> > sbresult(new cuNDArray<_real>(data.get_dimensions()));
  clear(sbresult.get());

  vector_td<float,4> daubechies4({0.6830127f,1.1830127f,0.3169873f,-0.1830127f});
  vector_td<float,2> haahr(1.0f,1.0f);
  vector_td<float,6> daubechies6{0.47046721f,1.14111692f,0.650365f,-0.19093442f, -0.12083221f,0.0498175f};

  cuDWTOperator<float,2> dwt;
  dwt.set_levels(3);
  dwt.mult_M(&data,sbresult.get());
  //data = *sbresult;
  shrink1(sbresult.get(),30.0f,&data);
  dwt.mult_MH(&data,sbresult.get());*/
  //clear(sbresult.get());
 // All done, write out the result
  boost::shared_ptr< hoNDArray<_real> > host_result = sbresult->to_host();
  write_nd_array<_real>(host_result.get(), (char*)parms.get_parameter('r')->get_string_value());

  return 0;
}


/*
 * Copyright (C) 2015, Nils Moehrle
 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
 */

#include <cstdint>
#include <iostream>

#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <math/vector.h>

#include "poisson_blending.h"

typedef Eigen::SparseMatrix<float> SpMat;

math::Vec3f simple_laplacian(int i, mve::FloatImage::ConstPtr img) {
  const int width = img->width();
  assert(i > width + 1 && i < img->get_pixel_amount() - width - 1);

  return -4.0f * math::Vec3f(&img->at(i, 0))
         + math::Vec3f(&img->at(i - width, 0)) + math::Vec3f(&img->at(i - 1, 0))
         + math::Vec3f(&img->at(i + 1, 0))
         + math::Vec3f(&img->at(i + width, 0));
}

bool valid_mask(mve::ByteImage::ConstPtr mask) {
  const int width = mask->width();
  const int height = mask->height();

  for (int x = 0; x < width; ++x)
    if (mask->at(x, 0, 0) == 255 || mask->at(x, height - 1, 0) == 255)
      return false;

  for (int y = 0; y < height; ++y)
    if (mask->at(0, y, 0) == 255 || mask->at(width - 1, y, 0) == 255)
      return false;

  // TODO check for sane boundary conditions...

  return true;
}

void poisson_blend(
    mve::FloatImage::ConstPtr src,
    mve::ByteImage::ConstPtr mask,
    mve::FloatImage::Ptr dest,
    float alpha) {
  assert(src->width() == mask->width() && mask->width() == dest->width());
  assert(src->height() == mask->height() && mask->height() == dest->height());
  assert(src->channels() >= 3 && dest->channels() == src->channels());
  assert(mask->channels() == 1);
  assert(valid_mask(mask));

  const int n = dest->get_pixel_amount();
  const int width = dest->width();
  const int height = dest->height();
  // only do poisson on the color channels--first three
  // TODO dwh: get rid of hard coded color channels=3
  const int channels = std::min(dest->channels(), 3);

  mve::Image<int>::Ptr indices = mve::Image<int>::create(width, height, 1);
  indices->fill(-1);
  int index = 0;
  for (int i = 0; i < n; ++i) {
    if (mask->at(i) != 0) {
      indices->at(i) = index;
      index += 1;
    }
  }
  const int nnz = index;

  std::vector<math::Vec3f> coefficients_b;
  coefficients_b.resize(nnz);
  // std::cout << "blend 0 " << std::endl;
  std::vector<
      Eigen::Triplet<float, int>,
      Eigen::aligned_allocator<Eigen::Triplet<float, int>>>
      coefficients_A;
  coefficients_A.reserve(nnz);  // TODO better estimate...
  // std::cout << "blend A " << std::endl;
  for (int i = 0; i < n; ++i) {
    const int row = indices->at(i);
    if (mask->at(i) == 128 || mask->at(i) == 64) {
      Eigen::Triplet<float, int> t(row, row, 1.0f);
      coefficients_A.push_back(t);

      coefficients_b[row] = math::Vec3f(&dest->at(i, 0));
    }

    if (mask->at(i) == 255) {
      const int i01 = indices->at(i - width);
      const int i10 = indices->at(i - 1);
      const int i11 = indices->at(i);
      const int i12 = indices->at(i + 1);
      const int i21 = indices->at(i + width);

      /* All neighbours should be eighter border conditions or part of the
       * optimization. */
      assert(i01 != -1 && i10 != -1 && i11 != -1 && i12 != -1 && i21 != -1);

      Eigen::Triplet<float, int> t01(row, i01, 1.0f);

      Eigen::Triplet<float, int> t10(row, i10, 1.0f);
      Eigen::Triplet<float, int> t11(row, i11, -4.0f);
      Eigen::Triplet<float, int> t12(row, i12, 1.0f);

      Eigen::Triplet<float, int> t21(row, i21, 1.0f);

      Eigen::Triplet<float, int> triplets[] = {t01, t10, t11, t12, t21};

      coefficients_A.insert(coefficients_A.end(), triplets, triplets + 5);

      math::Vec3f l_d = simple_laplacian(i, dest);
      math::Vec3f l_s = simple_laplacian(i, src);

      coefficients_b[row] = (alpha * l_s + (1.0f - alpha) * l_d);
    }
  }
  // std::cout << "blend B " << std::endl;
  SpMat A(nnz, nnz);
  A.setFromTriplets(coefficients_A.begin(), coefficients_A.end());
  // std::cout << "blend C " << std::endl;
  Eigen::SparseLU<SpMat, Eigen::COLAMDOrdering<int>> solver;
  // std::cout << "blend Cp " << std::endl;
  // std::cout << nnz << std::endl;
  solver.compute(A);
  // std::cout << "blend D " << std::endl;
  for (int channel = 0; channel < channels; ++channel) {
    Eigen::VectorXf b(nnz);
    for (std::size_t i = 0; i < coefficients_b.size(); ++i)
      b[i] = coefficients_b[i][channel];

    Eigen::VectorXf x(n);
    x = solver.solve(b);

    for (int i = 0; i < n; ++i) {
      int index = indices->at(i);
      if (index != -1)
        dest->at(i, channel) = x[index];
    }
  }
  // std::cout << "blend E " << std::endl;
}


#include <Eigen/Dense>
#include <algorithm>
#include <cassert>
#include <iostream>
#include <vector>
#include "perf_test.hpp"
#include "svd.hpp"
#include "types.hpp"

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MatrixXd;
typedef Eigen::JacobiSVD<MatrixXd> SVD;

void run_jacobi(SVD&& svd, MatrixXd&& m, unsigned int flags) { svd.compute(m, flags); }

using SVDTolType = decltype(&run_jacobi);

double base_cost(size_t n, size_t n_iter) {
    // Ops in real_2x2_jacobi svd
    double svd_2x2_wild = 6;

    // Ops in m.applyOnTheLeft(..), part of real_2x2_jacobi svd
    double rot_2x2_add = 2;
    double rot_2x2_mul = 4;

    // Ops in makeJacobi(), part of real_2x2_jacobi svd
    double make_add = 5;
    double make_mul = 4;
    double make_div = 4;
    double make_sqrt = 2;
    double make_abs = 5;

    // Ops in rot1 * j_right->transpose(), part of real_2x2_jacobi svd
    double rot_mul_add = 2;
    double rot_mul_mul = 4;

    double svd_2x2 = svd_2x2_wild + rot_2x2_add + rot_2x2_mul + rot_mul_add + rot_mul_mul + make_add + make_mul +
                     make_div + make_sqrt + make_abs;

    // Ops in m_workMatrix.applyOnTheLeft(..)
    // Same for m_workMatrix.applyOnTheRight(..), m_matrixU.applyOnTheRight(..), m_matrixV.applyOnTheRight(..)
    double rot_add = 2 * n;
    double rot_mul = 4 * n;

    double loops = (n * n - n) / 2;
    double flops = n_iter * loops * (svd_2x2 + 4 * (rot_add + rot_mul));

    return flops;
}

using CostFuncType = decltype(&base_cost);

std::vector<SVDTolType> tol_based_versions = {
    run_jacobi,
};
std::vector<std::string> tol_based_names = {
    "svd_two_sided_eigen",
};
std::vector<CostFuncType> tol_based_cost_fns = {base_cost};

int main() {
    size_t n;

    std::ios_base::sync_with_stdio(false);  // disable synchronization between C and C++ standard streams
    std::cin.tie(NULL);                     // untie cin from cout

    std::cin >> n >> n;
    std::cerr << "Performance benchmark on array of size " << n << " by " << n << std::endl;

    MatrixXd Ap(n, n);
    SVD svd;

    for (size_t i = 0; i < n; ++i) {
        for (size_t j = 0; j < n; j++) {
            std::cin >> Ap(i, j);
        }
    }

    svd.compute(Ap, Eigen::ComputeFullU | Eigen::ComputeFullV);
    size_t n_iter = svd.getSweeps();

    std::vector<double> costs;
    for (const auto& cost_fn : tol_based_cost_fns) {
        costs.push_back(cost_fn(n, n_iter));
    }

    run_all(tol_based_versions, tol_based_names, costs, svd, Ap, Eigen::ComputeFullU | Eigen::ComputeFullV);
}


#pragma once

#include "SensorData.h"
#include "Poster.hpp"

//This needs to be in this order. We need to include Arduino.h (if not already included), then undefine the Arduino `abs` macro, then pull in Eigen. Pray Rust comes quickly.
#include "Arduino.h"
#undef abs
#include <Eigen.h>
#include <Eigen/Dense>

using namespace Eigen;
class AltFilter{
public:
  AltFilter();
  void update(SensorData& data);
  void init(SensorData& data);
  float getAltitude();
  float getVelocity();
  void logState();
  Poster<float> p_alt;
  Poster<float> p_vel;

private:

  uint32_t print_timer;

  Vector3f X;
  Vector2f Z;
  Matrix3f F;
  Matrix3f Q;
  Matrix3f P;
  Matrix<float, 3, 2> K;
  Matrix<float, 2, 3> H;
  Matrix2f R;

  uint32_t data_time;

  void prefilter(SensorData& data);
  void kalmanPredict();
  void kalmanUpdate();
  float p2alt(float p);
};


//
// Copyright (c) 2016 CNRS
//

#include "pinocchio/algorithm/energy.hpp"
#include "pinocchio/algorithm/crba.hpp"

#include "pinocchio/parsers/sample-models.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include <boost/test/floating_point_comparison.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE(test_kinetic_energy)
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Zero(model.nq);
  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd a = VectorXd::Ones(model.nv);

  data.M.fill(0);  crba(model,data,q);
  data.M.triangularView<Eigen::StrictlyLower>()
  = data.M.transpose().triangularView<Eigen::StrictlyLower>();

  double kinetic_energy_ref = 0.5*v.transpose() * data.M * v;
  double kinetic_energy = kineticEnergy(model, data, q, v);

  BOOST_CHECK_SMALL(kinetic_energy_ref - kinetic_energy, 1e-12);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef __PARTICLE_FILTER_HAND_PREDICTION_HPP
#define __PARTICLE_FILTER_HAND_PREDICTION_HPP

#include <ctime>
#include <Eigen/Dense>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/normal_distribution.hpp>

#include "particle_filter_hand.hpp"
#include "particle_filter_base.hpp"

#define SMOOTH_FACTOR (1.)

class HandPredictionModel: public PredictionModel<HAND_STATE_SIZE>
{
private:
  double initQSigma;
  double initWSigma;
  double updateQSigma;
  double updateWSigma;

  void _updateParticle(Particle<HAND_STATE_SIZE> &p,
		       double deltaT, double qSigma, double wSigma,
		       boost::random::mt19937 &rng) const
  {
    // Quaternion kinematics from
    // Sola, Joan. "Quaternion kinematics for the error-state KF." Laboratoire d’Analyse et d’Architecture des Systemes-Centre national de la recherche scientifique (LAAS-CNRS), Toulouse, France, Tech. Rep (2012).
    boost::random::normal_distribution<double> Nq(0., qSigma);
    boost::random::normal_distribution<double> Nw(0., wSigma);
    boost::random::uniform_01<double> U;

    // Current state quaternions (orientation and angular velocity)
    Eigen::Quaternion<double> qt, wt;
    qt.w() = p.state[0];
    qt.x() = p.state[1];
    qt.y() = p.state[2];
    qt.z() = p.state[3];
    wt.w() = 0.;
    wt.x() = p.state[4];
    wt.y() = p.state[5];
    wt.z() = p.state[6];

    // Perturbation quaternions
    Eigen::Quaternion<double> deltaqt, deltawt;
    //deltaqt.w() =1. +Nq(rng);
    //deltaqt.x() = Nq(rng);
    //deltaqt.y() = Nq(rng);
    //deltaqt.z() = Nq(rng);
    Eigen::AngleAxisd deltaaxt;
    deltaaxt.angle() = Nq(rng);
    deltaaxt.axis().x() = -1.+2.*U(rng);
    deltaaxt.axis().y() = -1.+2.*U(rng);
    deltaaxt.axis().z() = -1.+2.*U(rng);
    deltaqt = deltaaxt;
    deltawt.w() = 0;
    deltawt.x() = Nw(rng);
    deltawt.y() = Nw(rng);
    deltawt.z() = Nw(rng);

    // Update quaternion rotation
    Eigen::Quaternion<double> qtw,qt1;
    // - quaternion update (for non zero velocity)
    if (deltaT)
    {
      qtw.w() = cos(wt.norm()*deltaT/2);
      qtw.x() = wt.x()/wt.norm()*sin(wt.norm()*deltaT/2);
      qtw.y() = wt.y()/wt.norm()*sin(wt.norm()*deltaT/2);
      qtw.z() = wt.z()/wt.norm()*sin(wt.norm()*deltaT/2);
      // - multiply by perturbation and update
      qt1 = deltaqt * qtw * qt;
    }
    else
    {
      // Null velocity, apply the random perturbation only
      qt1 = deltaqt * qt;
    }
	// Normalize
	qt1.normalize();

    // Update velocity
    Eigen::Quaternion<double> deltaq, wt1;
    Eigen::AngleAxisd axang;
    if (deltaT)
    {
      deltaq = qt1 * qt.inverse();
      axang = deltaq;
      wt1.w() = 0.;
      wt1.x() = axang.axis().x()*axang.angle()/deltaT + deltawt.x();
      wt1.y() = axang.axis().y()*axang.angle()/deltaT + deltawt.y();
      wt1.z() = axang.axis().z()*axang.angle()/deltaT + deltawt.z();
    }
    else
    {
      wt1.w() = 0.;
      wt1.x() = wt.x()+deltawt.x();
      wt1.y() = wt.y()+deltawt.y();
      wt1.z() = wt.z()+deltawt.z();
    }

    // Done, update particle status
    Eigen::Quaternion<double> iq;
    iq = qt.slerp(SMOOTH_FACTOR, qt1);
    p.state[0] = iq.w();
    p.state[1] = iq.x();
    p.state[2] = iq.y();
    p.state[3] = iq.z();

    p.state[4] = SMOOTH_FACTOR*wt1.x() + (1.-SMOOTH_FACTOR)*wt.x();
    p.state[5] = SMOOTH_FACTOR*wt1.y() + (1.-SMOOTH_FACTOR)*wt.y();
    p.state[6] = SMOOTH_FACTOR*wt1.z() + (1.-SMOOTH_FACTOR)*wt.z();
  }

  void _updateParticles(std::vector<Particle<HAND_STATE_SIZE> > &particles,
			double deltaT, double qSigma, double wSigma) const
  {
    boost::random::mt19937 rng(time(NULL));
    for (std::vector<Particle<HAND_STATE_SIZE> >::iterator it=particles.begin();
	 it!=particles.end(); ++it)
    {
      Particle<HAND_STATE_SIZE> &p = *it;
      _updateParticle(p, deltaT, qSigma, wSigma, rng);
    }
  }

public:
  HandPredictionModel(double initQSigma, double initWSigma,
		      double updateQSigma, double updateWSigma):
    initQSigma(initQSigma),
    initWSigma(initWSigma),
    updateQSigma(updateQSigma),
    updateWSigma(updateWSigma)
  {}

  void init(const double *initState,
	    std::vector<Particle<HAND_STATE_SIZE> > &particles) const
  {
    // Set all particle to the initial state
    for (std::vector<Particle<HAND_STATE_SIZE> >::iterator it=particles.begin();
	 it!=particles.end(); ++it)
    {
      Particle<HAND_STATE_SIZE> &p = *it;
      std::copy(initState, initState+HAND_STATE_SIZE, p.state);
      p.likelihood = 0.;
    }

    _updateParticles(particles, 0., initQSigma, initWSigma);
  }

  void update(std::vector<Particle<HAND_STATE_SIZE> > &particles, double deltaT) const
  {
    _updateParticles(particles, deltaT, updateQSigma, updateWSigma);
  }
};

#endif // __PARTICLE_FILTER_HAND_PREDICTION_HPP


#include <kv/eig.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <kv/dd.hpp>
#include <kv/rdd.hpp>

namespace ub = boost::numeric::ublas;

typedef kv::interval< kv::dd > itvdd;
typedef kv::interval<double> itv;

int main()
{
	ub::matrix<double> a;
	ub::matrix< kv::complex<double> > v, d;

	std::cout.precision(17);

	a.resize(2,2);

	a(0,0) = 1.;
	a(0,1) = 2.;
	a(1,0) = 3.;
	a(1,1) = 4.;

	// approximation
	eig(a, v, d);
	std::cout << v << "\n";
	std::cout << d << "\n";

	// verified
	ub::vector< kv::complex<itv> > l;
	veig(a, l);
	std::cout << l << "\n";

	// verified (interval matrix)
	ub::matrix<itv> ia;
	ia = a;
	veig(ia, l);
	std::cout << l << "\n";

	// verified (interval dd matrix)
	ub::matrix<itvdd> iadd;
	ub::vector< kv::complex<itvdd> > ldd;
	std::cout.precision(34);
	iadd = a;
	veig(iadd, ldd);
	std::cout << ldd << "\n";
}


//#define ARMA_NO_DEBUG

#include <iostream>
#include <armadillo>
#include <float.h>
#include <ctime>

using namespace std;
using namespace arma;

typedef double (*callback_type)(int, double[]);
typedef bool (*is_terminate_type)(long, int, double); // runid, iter, value -> isTerminate

static uvec inverse(const uvec& indices) {
    uvec inverse = uvec(indices.size());
    for (int i = 0; i < indices.size(); i++)
        inverse(indices(i)) = i;
    return inverse;
}

static vec sequence(double start, double end, double step) {
    int size = (int) ((end - start) / step + 1);
    vec d(size);
    double value = start;
    for (int r = 0; r < size; r++) {
        d(r) = value;
        value += step;
    }
    return d;
}

// wrapper around the fittness function, scales according to boundaries

class Fittness {

public:

    Fittness(callback_type pfunc, const vec &lower_limit,
            const vec &upper_limit) {
        func = pfunc;
        lower = lower_limit;
        upper = upper_limit;
        evaluationCounter = 0;
        if (lower.size() > 0) { // bounds defined
            scale = 0.5 * (upper - lower);
            typx = 0.5 * (upper + lower);
        }
    }

    void closestFeasible(vec &X) const { // in place
        if (lower.size() > 0)
            X.for_each([](double &val) {
                val = max(min(val, 1.0), -1.0);
            });
    }

    vec getClosestFeasible(const vec &col) const {
        if (lower.size() > 0) {
            vec X(col);
            X.for_each([](double &val) {
                val = max(min(val, 1.0), -1.0);
            });
            return X;
        }
        return col;
    }

    double eval(const vec &X) {
        int n = X.size();
        double parg[n];
        for (int i = 0; i < n; i++)
            parg[i] = X(i);
        double res = func(n, parg);
        evaluationCounter++;
        return res;
    }

    double value(const vec &X) {
        double value;
        if (lower.size() > 0) {
            return eval(decode(getClosestFeasible(X)));
        } else
            return eval(X);
    }

    vec encode(const vec &X) const {
        if (lower.size() > 0)
            return (X - typx) / scale;
        else
            return X;
    }

    vec decode(const vec &X) const {
        if (lower.size() > 0)
            return (X % scale) + typx;
        else
            return X;
    }

    int getEvaluations() {
        return evaluationCounter;
    }

private:
   callback_type func;
   vec lower;
   vec upper;
   long evaluationCounter;
   vec scale;
   vec typx;
};

class AcmaesOptimizer {

public:

    AcmaesOptimizer(long runid_, Fittness* fitfun_, int popsize_, int mu_, const vec &guess_, const vec &inputSigma_,
            int maxIterations_, int maxEvaluations_, double accuracy_, double stopfitness_, is_terminate_type isTerminate_) {
// runid used in isTerminate callback to identify a specific run at different iteration
        runid = runid_;
// fitness function to minimize
        fitfun = fitfun_;
// initial guess for the arguments of the fitness function
        guess = guess_;
// accuracy = 1.0 is default, > 1.0 reduces accuracy
        accuracy = accuracy_;
// callback to check if to terminate
        isTerminate = isTerminate_;
// Number of objective variables/problem dimension
        dim = guess_.size();
//     Population size, offspring number. The primary strategy parameter to play
//     with, which can be increased from its default value. Increasing the
//     population size improves global search properties in exchange to speed.
//     Speed decreases, as a rule, at most linearly with increasing population
//     size. It is advisable to begin with the default small population size.
        if (popsize_ > 0)
            popsize = popsize_;
        else
            popsize = 4 + int(3. * log(dim));
//     Individual sigma values - initial search volume. inputSigma determines
//     the initial coordinate wise standard deviations for the search. Setting
//     SIGMA one third of the initial search region is appropriate.
        if (inputSigma_.size() == 1)
            inputSigma = vec(dim).fill(inputSigma_(0));
        else
            inputSigma = inputSigma_;
// Overall standard deviation - search volume.
        sigma = max(inputSigma);
// termination criteria
// maximal number of evaluations allowed.
        maxEvaluations = maxEvaluations_;
// maximal number of iterations allowed.
        maxIterations = maxIterations_;
// Limit for fitness value.
        stopfitness = stopfitness_;
// Stop if x-changes larger stopTolUpX.
        stopTolUpX = 1e3 * sigma;
// Stop if x-change smaller stopTolX.
        stopTolX = 1e-11 * sigma * accuracy;
// Stop if fun-changes smaller stopTolFun.
        stopTolFun = 1e-12 * accuracy;
// Stop if back fun-changes smaller stopTolHistFun.
        stopTolHistFun = 1e-13 * accuracy;
// selection strategy parameters
// Number of parents/points for recombination.
        mu = mu_ > 0 ? mu_ : popsize / 2;
// Array for weighted recombination.
        weights = (log(sequence(1, mu, 1)) * -1.) + log(mu + 0.5);
        double sumw = sum(weights);
        double sumwq = sum(weights % weights);
        weights *= 1. / sumw;
// Variance-effectiveness of sum w_i x_i.
        mueff = sumw * sumw / sumwq;

// dynamic strategy parameters and constants
// Cumulation constant.
        cc = (4. + mueff / dim) / (dim + 4. + 2. * mueff / dim);
// Cumulation constant for step-size.
        cs = (mueff + 2.) / (dim + mueff + 3.);
// Damping for step-size.
           damps = (1. + 2. * ::max(0., ::sqrt((mueff - 1.)
                    / (dim + 1.)) - 1.))
                    * max(0.3, 1.
                            - // modification for short runs
                            dim / (1e-6 + min(maxIterations, maxEvaluations
                                    / popsize))) + cs; // minor increment
// Learning rate for rank-one update.
        ccov1 = 2. / ((dim + 1.3) * (dim + 1.3) + mueff);
// Learning rate for rank-mu update'
        ccovmu = min(1. - ccov1,
                2. * (mueff - 2. + 1. / mueff)
                        / ((dim + 2.) * (dim + 2.) + mueff));
// Expectation of ||N(0,I)|| == norm(randn(N,1)).
        chiN = sqrt(dim) * (1. - 1. / (4. * dim) + 1 / (21. * dim * dim));
        ccov1Sep = min(1., ccov1 * (dim + 1.5) / 3.);
        ccovmuSep = min(1. - ccov1, ccovmu * (dim + 1.5) / 3.);

// CMA internal values - updated each generation
// Objective variables.
        xmean = fitfun->encode(guess);
// Evolution path.
        pc = zeros<vec>(dim);
// Evolution path for sigma.
        ps = zeros<vec>(dim);
// Norm of ps, stored for efficiency.
        normps = norm(ps);
// Coordinate system.
        B = mat(dim,dim).eye();
// Diagonal of sqrt(D), stored for efficiency.
        diagD = inputSigma / sigma;
        diagC = square(diagD);
// B*D, stored for efficiency.
        BD = B % repmat(diagD.t(), dim, 1);
// Covariance matrix.
        C = B * (mat(dim,dim).eye() * B.t());
// Number of iterations already performed.
        iterations = 0;
// Size of history queue of best values.
        historySize = 10 + int(3. * 10. * dim / popsize);
// stop criteria
        stop = 0;
// best value so far
        bestValue = fitfun->value(xmean);
// best parameters so far
        bestX = guess;
// History queue of best values.
        fitnessHistory = vec(historySize).fill(DBL_MAX);
        fitnessHistory(0) = bestValue;
    }

    // param zmean weighted row matrix of the gaussian random numbers generating the current offspring
    // param xold xmean matrix of the previous generation
    // return hsig flag indicating a small correction

    bool updateEvolutionPaths(const vec& zmean, const vec& xold) {
        ps = ps * (1. - cs) + ((B * zmean) * sqrt(cs * (2. - cs) * mueff));
        normps = norm(ps);
        bool hsig = normps / sqrt(1. - pow(1. - cs, 2. * iterations)) / chiN < 1.4 + 2. / (dim + 1.);
        pc *= (1. - cc);
        if (hsig)
            pc += (xmean - xold) * (sqrt(cc * (2. - cc) * mueff) / sigma);
        return hsig;
    }

    // param hsig flag indicating a small correction
    // param bestArx fitness-sorted matrix of the argument vectors producing the current offspring
    // param arz unsorted matrix containing the gaussian random values of the current offspring
    // param arindex indices indicating the fitness-order of the current offspring
    // param xold xmean matrix of the previous generation

    double updateCovariance(bool hsig, const mat& bestArx, const mat& arz,
            const uvec& arindex, const mat& xold) {
        double negccov = 0;
        if (ccov1 + ccovmu > 0) {
            mat arpos = (bestArx - repmat(xold, 1, mu)) * (1. / sigma); // mu difference vectors
            mat roneu = pc * pc.t() * ccov1;
            // minor correction if hsig==false
            double oldFac = hsig ? 0 : ccov1 * cc * (2. - cc);
            oldFac += 1. - ccov1 - ccovmu;
            // Adapt covariance matrix C active CMA
            negccov = (1. - ccovmu) * 0.25 * mueff
                    / (pow(dim + 2., 1.5) + 2. * mueff);
            double negminresidualvariance = 0.66;
            // keep at least 0.66 in all directions, small popsize are most critical
            double negalphaold = 0.5; // where to make up for the variance loss,
            // prepare vectors, compute negative updating matrix Cneg
            uvec arReverseIndex = reverse(arindex);
            mat arzneg = arz.cols(arReverseIndex.head(mu));
            vec arnorms = sqrt(sum(square(arzneg.t()), 1)); // rowsum
            uvec idxnorms = sort_index(arnorms);
            vec arnormsSorted = arnorms(idxnorms);
            uvec idxReverse = reverse(idxnorms);
            vec arnormsReverse = arnorms(idxReverse);
            arnorms = arnormsReverse / arnormsSorted;
            vec arnormsInv = arnorms(inverse(idxnorms));
            mat sqarnw = square(arnormsInv).t() * weights;
            double negcovMax = (1. - negminresidualvariance) / sqarnw(0);
            if (negccov > negcovMax)
                negccov = negcovMax;
            arzneg = arzneg % repmat(arnormsInv.t(), dim, 1);
            mat artmp = BD * arzneg;
            mat Cneg = artmp * diagmat(weights) * artmp.t();
            oldFac += negalphaold * negccov;
            C = (C * oldFac) + roneu
                    + ( arpos * (ccovmu + (1. - negalphaold) * negccov) *
                               (repmat(weights, 1, dim) % arpos.t()))
                    - (Cneg * negccov);
        }
        return negccov;
    }

    // Update B and diagD from C
    // param negccov Negative covariance factor.

    void updateBD(double negccov) {

        if (ccov1 + ccovmu + negccov > 0
                && (std::fmod(iterations, 1. / (ccov1 + ccovmu + negccov) / dim / 10.)) < 1.) {
            // to achieve O(N^2) enforce symmetry to prevent complex numbers
            C = trimatu(C) + trimatu(C, 1).t();
            // diagD defines the scaling
            vec eigval;
            mat eigvec;
            eig_sym(eigval, eigvec, C);

            diagD = reverse(eigval); // descending order of eigenvalues
            for (int i = 0; i < dim; i++)
                B.col(i) = eigvec.col(dim - 1 - i);

            if (diagD.min() <= 0) {
                for (int i = 0; i < dim; i++)
                    if (diagD(i, 0) < 0)
                        diagD(i, 0) = 0.;
                double tfac = diagD.max() / 1e14;
                C += mat(dim,dim).eye() * tfac;
                diagD += ones(dim, 1) * tfac;
            }
            if (diagD.max() > 1e14 * diagD.min()) {
                double tfac = diagD.max() / 1e14 - diagD.min();
                C += mat(dim,dim).eye() * tfac;
                diagD += ones(dim, 1) * tfac;
            }
            diagC = C.diag();
            diagD = sqrt(diagD); // D contains standard deviations now
            BD = B % repmat(diagD.t(), dim, 1);
        }
    }

    void doOptimize() {

        // -------------------- Generation Loop --------------------------------

        for (iterations = 1; iterations <= maxIterations &&
                    fitfun->getEvaluations() < maxEvaluations; iterations++) {
            // Generate and evaluate popsize offspring
            mat arz = mat(dim, popsize).randn();
            mat arx = mat(dim, popsize);
            vec fitness = vec(popsize).fill(DBL_MAX);
            // generate random offspring
            fitfun->closestFeasible(xmean);
            for (int k = 0; k < popsize; k++) {
                vec delta = (BD * arz.col(k)) * sigma;
                arx.col(k) = fitfun->getClosestFeasible(xmean + delta);
                fitness[k] = fitfun->value(arx.col(k)); // compute fitness
                if (!isfinite(fitness[k])) {
                    stop = -1;
                    break;
                }
            }
            if (stop != 0)
                break;
            // Sort by fitness and compute weighted mean into xmean
            uvec arindex = sort_index(fitness);

            // Calculate new xmean, this is selection and recombination
            vec xold = xmean; // for speed up of Eq. (2) and (3)
            uvec bestIndex = arindex.head(mu);
            mat bestArx = arx.cols(bestIndex);
            xmean = bestArx * weights;
            mat bestArz = arz.cols(bestIndex);
            mat zmean = bestArz * weights;

            bool hsig = updateEvolutionPaths(zmean, xold);
            double negccov = updateCovariance(hsig, bestArx, arz, arindex, xold);
            updateBD(negccov);
            // Adapt step size sigma - Eq. (5)
            sigma *= exp(min(1.0, (normps / chiN - 1.) * cs / damps));
            double bestFitness = fitness(arindex(0));
            double worstFitness = fitness(arindex(arindex.size()-1));
            if (bestValue > bestFitness) {
                bestValue = bestFitness;
                bestX = fitfun->decode(bestArx.col(0));
            }

            // handle termination criteria
            if (isfinite(stopfitness) && bestFitness < stopfitness) {
                stop = 1;
                break;
            }
            vec sqrtDiagC = sqrt(diagC);
            vec pcCol = pc;

            for (int i = 0; i < dim; i++) {
                if (sigma * (max(abs(pcCol[i]), sqrtDiagC[i])) > stopTolX)
                    break;
                if (i >= dim - 1)
                    stop = 2;
            }
            for (int i = 0; i < dim; i++)
                if (sigma * sqrtDiagC[i] > stopTolUpX)
                    stop = 3;
            if (stop > 0)
                break;
            double historyBest = min(fitnessHistory);
            double historyWorst = max(fitnessHistory);
            if (iterations > 2
                    && max(historyWorst, worstFitness)
                            - min(historyBest, bestFitness) < stopTolFun) {
                stop = 4;
                break;
            }
            if (iterations > fitnessHistory.size()
                    && historyWorst - historyBest < stopTolHistFun) {
                stop = 5;
                break;
            }
            // condition number of the covariance matrix exceeds 1e14
            if ( diagD.max() / diagD.min() > 1e7 * 1.0 / sqrt(accuracy)) {
                stop = 6;
                break;
            }
            if (isTerminate != NULL && isTerminate(runid, iterations, bestValue)) {
                stop = 7;
                break;
            }
            // Adjust step size in case of equal function values (flat fitness)
            if (bestValue == fitness[arindex[(int) (0.1 + popsize / 4.)]]) {
                sigma *= exp(0.2 + cs / damps);
            }
            if (iterations > 2
                    && max(historyWorst, bestFitness)
                            - ::min(historyBest, bestFitness) == 0) {
                sigma *= ::exp(0.2 + cs / damps);
            }
            // store best in history
            for (int i = 1; i < fitnessHistory.size(); i++)
                fitnessHistory[i] = fitnessHistory[i - 1];
            fitnessHistory[0] = bestFitness;
        }
    }

    vec getBestX() {
        return bestX;
    }

    double getBestValue() {
        return bestValue;
    }

    double getIterations() {
        return iterations;
    }

    double getStop() {
        return stop;
    }

private:
      long runid;
      Fittness* fitfun;
      vec guess;
      double accuracy;
      is_terminate_type isTerminate;
      int popsize; // population size
      vec inputSigma;
      int dim;
      int maxIterations;
      int maxEvaluations;
      double stopfitness;
      double stopTolUpX;
      double stopTolX;
      double stopTolFun;
      double stopTolHistFun;
      int mu; //
      vec weights;
      double mueff; //
      double sigma;
      double cc;
      double cs;
      double damps;
      double ccov1;
      double ccovmu;
      double chiN;
      double ccov1Sep;
      double ccovmuSep;
      vec xmean;
      vec pc;
      vec ps;
      double normps;
      mat B;
      mat BD;
      mat diagD;
      mat C;
      vec diagC;
      int iterations;
      vec fitnessHistory;
      int historySize;
      double bestValue;
      vec bestX;
      int stop;
};

// see https://cvstuff.wordpress.com/2014/11/27/wraping-c-code-with-python-ctypes-memory-and-pointers/

extern "C" {
    void seed(int s) {
        arma_rng::set_seed(s);
    }
}

extern "C" {
    void seedRandom() {
        arma_rng::set_seed_random();
    }
}

extern "C" {
    void free_mem(double* a) {
        delete[] a;
    }
}

extern "C" {
    double* optimizeACMA_C(long runid, callback_type func, int dim, double *init,
            double *lower, double *upper, double *sigma, int maxIter, int maxEvals,
            double stopfitness, int mu, int popsize, double accuracy,
            bool useTerminate, is_terminate_type isTerminate) {
        int n = dim;
        double *res = new double[n + 4];
        vec guess(n), lower_limit(n), upper_limit(n), inputSigma(n);
        bool useLimit = false;
        for (int i = 0; i < n; i++) {
            guess[i] = init[i];
            inputSigma[i] = sigma[i];
            lower_limit[i] = lower[i];
            upper_limit[i] = upper[i];
            useLimit |= (lower[i] != 0);
            useLimit |= (upper[i] != 0);
        }
        if (useLimit == false) {
            lower_limit.resize(0);
            upper_limit.resize(0);
        }
        Fittness fitfun(func, lower_limit, upper_limit);
        AcmaesOptimizer opt(
            runid,
            &fitfun,
            popsize,
            mu,
            guess,
            inputSigma,
            maxIter,
            maxEvals,
            accuracy,
            stopfitness,
            useTerminate ? isTerminate : NULL);
        try {
            opt.doOptimize();
            vec bestX = opt.getBestX();
            double bestY = opt.getBestValue();
            for (int i = 0; i < n; i++)
                res[i] = bestX[i];
            res[n] = bestY;
            res[n+1] = fitfun.getEvaluations();
            res[n+2] = opt.getIterations();
            res[n+3] = opt.getStop();
            return res;
        } catch (std::exception& e) {
            cout << e.what() << endl;
            return res;
        }
    }
}


// Copyright (C) 2011  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "tester.h"
#include <dlib/svm.h>
#include <dlib/data_io.h>
#include <dlib/sparse_vector.h>
#include "create_iris_datafile.h"
#include <vector>
#include <sstream>

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;
    dlib::logger dlog("test.data_io");

    class test_data_io : public tester
    {
        /*!
            WHAT THIS OBJECT REPRESENTS
                This object represents a unit test.  When it is constructed
                it adds itself into the testing framework.
        !*/
    public:
        test_data_io (
        ) :
            tester (
                "test_data_io",       // the command line argument name for this test
                "Run tests on the data_io stuff.", // the command line argument description
                0                     // the number of command line arguments for this test
            )
        {
        }

        template <typename sample_type>
        void run_test()
        {
            print_spinner();

            typedef typename sample_type::value_type::second_type scalar_type;

            std::vector<sample_type> samples;
            std::vector<scalar_type> labels;

            load_libsvm_formatted_data("iris.scale",samples, labels);
            save_libsvm_formatted_data("iris.scale2", samples, labels);

            DLIB_TEST(samples.size() == 150);
            DLIB_TEST(labels.size() == 150);
            DLIB_TEST(max_index_plus_one(samples) == 5);
            fix_nonzero_indexing(samples);
            DLIB_TEST(max_index_plus_one(samples) == 4);

            load_libsvm_formatted_data("iris.scale2",samples, labels);

            DLIB_TEST(samples.size() == 150);
            DLIB_TEST(labels.size() == 150);

            DLIB_TEST(max_index_plus_one(samples) == 5);
            fix_nonzero_indexing(samples);
            DLIB_TEST(max_index_plus_one(samples) == 4);

            one_vs_one_trainer<any_trainer<sample_type,scalar_type>,scalar_type> trainer;

            typedef sparse_linear_kernel<sample_type> kernel_type;
            trainer.set_trainer(krr_trainer<kernel_type>());

            randomize_samples(samples, labels);
            matrix<double> cv = cross_validate_multiclass_trainer(trainer, samples, labels, 4);

            dlog << LINFO << "confusion matrix: \n" << cv;
            const scalar_type cv_accuracy = sum(diag(cv))/sum(cv);
            dlog << LINFO << "cv accuracy: " << cv_accuracy;
            DLIB_TEST(cv_accuracy > 0.97);

            {
                print_spinner();
                typedef matrix<scalar_type,0,1> dsample_type;
                std::vector<dsample_type> dsamples = sparse_to_dense(samples);
                DLIB_TEST(dsamples.size() == 150);
                DLIB_TEST(dsamples[0].size() == 4);
                DLIB_TEST(max_index_plus_one(dsamples) == 4);

                one_vs_one_trainer<any_trainer<dsample_type,scalar_type>,scalar_type> trainer;

                typedef linear_kernel<dsample_type> kernel_type;
                trainer.set_trainer(rr_trainer<kernel_type>());

                cv = cross_validate_multiclass_trainer(trainer, dsamples, labels, 4);

                dlog << LINFO << "dense confusion matrix: \n" << cv;
                const scalar_type cv_accuracy = sum(diag(cv))/sum(cv);
                dlog << LINFO << "dense cv accuracy: " << cv_accuracy;
                DLIB_TEST(cv_accuracy > 0.97);
            }

        }

        void test_sparse_to_dense()
        {
            {
                std::map<unsigned long, double> temp;

                matrix<double,0,1> m, m2;

                m = sparse_to_dense(m);
                DLIB_TEST(m.size() == 0);
                m.set_size(2,1);
                m = 1, 2;
                m2 = sparse_to_dense(m);
                DLIB_TEST(m == m2);
                m2 = sparse_to_dense(m,1);
                DLIB_TEST(m2.size() == 1);
                DLIB_TEST(m2(0,0) == 1);
                m2 = sparse_to_dense(m,0);
                DLIB_TEST(m2.size() == 0);

                temp[3] = 2;
                temp[5] = 4;
                m2 = sparse_to_dense(temp);
                m.set_size(6);
                m = 0,0,0,2,0,4;
                DLIB_TEST(m2 == m);

                m2 = sparse_to_dense(temp, 5);
                m.set_size(5);
                m = 0,0,0,2,0;
                DLIB_TEST(m2 == m);

                m2 = sparse_to_dense(temp, 7);
                m.set_size(7);
                m = 0,0,0,2,0,4,0;
                DLIB_TEST(m2 == m);

                std::vector<std::vector<std::pair<unsigned long,double> > > vects;

                std::vector<std::pair<unsigned long,double> > v;
                v.push_back(make_pair(5,2));
                v.push_back(make_pair(3,1));
                v.push_back(make_pair(5,2));
                v.push_back(make_pair(3,1));
                v = make_sparse_vector(v);
                vects.push_back(v);
                vects.push_back(v);
                vects.push_back(v);
                vects.push_back(v);
                DLIB_TEST(max_index_plus_one(v) == 6);
                m2 = sparse_to_dense(v);
                m.set_size(6);
                m = 0,0,0,2,0,4;
                DLIB_TEST_MSG(m2 == m, m2 << "\n\n" << m );

                m2 = sparse_to_dense(v,7);
                m.set_size(7);
                m = 0,0,0,2,0,4,0;
                DLIB_TEST(m2 == m);

                m2 = sparse_to_dense(v,5);
                m.set_size(5);
                m = 0,0,0,2,0;
                DLIB_TEST(m2 == m);

                v.clear();
                m2 = sparse_to_dense(v);
                DLIB_TEST(m2.size() == 0);

                std::vector<matrix<double,0,1> > mvects = sparse_to_dense(vects);
                DLIB_TEST(mvects.size() == 4);
                m.set_size(6);
                m = 0,0,0,2,0,4;
                DLIB_TEST(mvects[0] == m);
                DLIB_TEST(mvects[1] == m);
                DLIB_TEST(mvects[2] == m);
                DLIB_TEST(mvects[3] == m);

                mvects = sparse_to_dense(vects, 7);
                DLIB_TEST(mvects.size() == 4);
                m.set_size(7);
                m = 0,0,0,2,0,4,0;
                DLIB_TEST(mvects[0] == m);
                DLIB_TEST(mvects[1] == m);
                DLIB_TEST(mvects[2] == m);
                DLIB_TEST(mvects[3] == m);

                mvects = sparse_to_dense(vects, 5);
                DLIB_TEST(mvects.size() == 4);
                m.set_size(5);
                m = 0,0,0,2,0;
                DLIB_TEST(mvects[0] == m);
                DLIB_TEST(mvects[1] == m);
                DLIB_TEST(mvects[2] == m);
                DLIB_TEST(mvects[3] == m);

            }
        }

        void perform_test (
        )
        {
            print_spinner();
            create_iris_datafile();

            test_sparse_to_dense();

            run_test<std::map<unsigned int, double> >();
            run_test<std::map<unsigned int, float> >();
            run_test<std::vector<std::pair<unsigned int, float> > >();
            run_test<std::vector<std::pair<unsigned long, double> > >();
        }
    };

    test_data_io a;

}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

//! \file
//!\brief Tests for sqrt(fixed_point) with round::fastest.

#include <cmath>

#define BOOST_TEST_MODULE test_negatable_func_sqrt_small_fastest
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

namespace local
{
  template<typename FixedPointType>
  const FixedPointType& tolerance_maker(const int fuzzy_bits)
  {
    static const FixedPointType the_tolerance = ldexp(FixedPointType(1), FixedPointType::resolution + fuzzy_bits);

    return the_tolerance;
  }

  template<typename FixedPointType,
           typename FloatPointType = typename FixedPointType::float_type>
  void test_sqrt(const int fuzzy_bits)
  {
    // Use at least 6 resolution bits.
    // Use at least 8 range bits.

    BOOST_STATIC_ASSERT(-FixedPointType::resolution >= 6);
    BOOST_STATIC_ASSERT( FixedPointType::range      >= 7);

    using std::sqrt;

    const FixedPointType a1 (  2L    );                                      const FloatPointType b1(  2L    );
    const FixedPointType a2 (  3L    );                                      const FloatPointType b2(  3L    );
    const FixedPointType a3 (  8.375L);                                      const FloatPointType b3(  8.375L);
    const FixedPointType a4 ( 64.125L);                                      const FloatPointType b4( 64.125L);
    const FixedPointType a5 (100.875L);                                      const FloatPointType b5(100.875L);
    const FixedPointType a6 (FixedPointType(  1) / 10);                      const FloatPointType b6(FloatPointType(  1) / 10);
    const FixedPointType a7 (FixedPointType( 12) / 10);                      const FloatPointType b7(FloatPointType( 12) / 10);
    const FixedPointType a8 (FixedPointType(111) / 10);                      const FloatPointType b8(FloatPointType(111) / 10);
    const FixedPointType a9 (boost::math::constants::phi<FixedPointType>()); const FloatPointType b9(boost::math::constants::phi<FloatPointType>());

    BOOST_CHECK_CLOSE_FRACTION(sqrt(a1), FixedPointType(sqrt(b1)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a2), FixedPointType(sqrt(b2)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a3), FixedPointType(sqrt(b3)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a4), FixedPointType(sqrt(b4)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a5), FixedPointType(sqrt(b5)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a6), FixedPointType(sqrt(b6)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a7), FixedPointType(sqrt(b7)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a8), FixedPointType(sqrt(b8)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a9), FixedPointType(sqrt(b9)), tolerance_maker<FixedPointType>(fuzzy_bits));
  }
}

BOOST_AUTO_TEST_CASE(test_negatable_func_sqrt_small_fastest)
{
  // Test sqrt() round::fastest for negatable in various key small digit ranges

  { typedef boost::fixed_point::negatable<7,  -8, boost::fixed_point::round::fastest> fixed_point_type; local::test_sqrt<fixed_point_type>(4); }
  { typedef boost::fixed_point::negatable<7, -16, boost::fixed_point::round::fastest> fixed_point_type; local::test_sqrt<fixed_point_type>(4); }
  { typedef boost::fixed_point::negatable<7, -24, boost::fixed_point::round::fastest> fixed_point_type; local::test_sqrt<fixed_point_type>(4); }

  { typedef boost::fixed_point::negatable<4, -11, boost::fixed_point::round::fastest> fixed_point_type; BOOST_CHECK_EQUAL(sqrt(fixed_point_type(1)), fixed_point_type(1)); }
  { typedef boost::fixed_point::negatable<7, -24, boost::fixed_point::round::fastest> fixed_point_type; BOOST_CHECK_EQUAL(sqrt(fixed_point_type(1)), fixed_point_type(1)); }
}


#include <boost/math/differentiation/autodiff.hpp>
#include <iostream>

using namespace std;
using namespace boost::math::differentiation;

constexpr double PI = M_PI;

template<typename X>
auto sqr(const X& x) { return x * x; }

#include <random>

double clamp(double x, double min_x, double max_x) {
    if (x > max_x) return max_x;
    if (x < min_x) return min_x;
    return x;
}

// TODO adam, vector as input, benchmarks

template<typename Func>
void optimize(const char* name, double range, bool newton, const Func& func) {
    cout << name << endl;
    std::mt19937 rng(0);

    double best = 1e100;
    std::uniform_real_distribution<double> unif(-range, range);
    double best_wx = unif(rng), best_wy = unif(rng);
    double dev = range / 3;
    size_t evals = 100 * 1000 * 1000;
    const double alpha = newton ? 1 : 1e-2;
    size_t misses = 0;

    for (size_t j = 0; evals > 0; j++) {
        double wx = clamp(std::normal_distribution<>(best_wx, dev)(rng), -range, range);
        double wy = clamp(std::normal_distribution<>(best_wy, dev)(rng), -range, range);
        bool improved = false;
        for (size_t i = 0; i < 10000; i++) {
            double result, dx, dy, d2x = 1, d2y = 1;
            if (newton) {
                const auto in = make_ftuple<double, 2, 2>(wx, wy);
                const auto& f = func(std::get<0>(in), std::get<1>(in));
                result = f.derivative(0, 0);
                dx = f.derivative(1, 0);
                dy = f.derivative(0, 1);
                d2x = f.derivative(2, 0);
                d2y = f.derivative(0, 2);
            } else {
                const auto in = make_ftuple<double, 1, 1>(wx, wy);
                const auto& f = func(std::get<0>(in), std::get<1>(in));
                result = f.derivative(0, 0);
                dx = f.derivative(1, 0);
                dy = f.derivative(0, 1);
            }
            if (result < best) {
                best = result;
                best_wx = wx;
                best_wy = wy;
                improved = true;
            }
            if (isnan(dx) || isnan(dy) || isnan(d2x) || isnan(d2y)) {
                cout << "nan gradients at " << wx << " " << wy << endl;
                return;
            }
            wx -= alpha * dx / d2x;
            wy -= alpha * dy / d2y;
            if (abs(dx) < 1e-15 && abs(dy) < 1e-15) break;
            if (--evals == 0) break;
            if (wx < -range || wx > range) break;
            if (wy < -range || wy > range) break;
        }
        if (improved) {
            misses = 0;
            dev *= 0.95;
            cout << setprecision(15) << "x=" << best_wx << " y=" << best_wy << " f=" << best << " dev=" << dev << endl;
        } else {
            misses += 1;
            if (misses == 1000)
                break;
        }
    }
    cout << endl;
}

#define Optimize(A, B, C) \
    optimize(A, B, false, [](const auto& x, const auto& y){C;}); \
    optimize(A, B, true, [](const auto& x, const auto& y){C;})

int main() {
    Optimize("sphere", 10, return sqr(x) + sqr(y));
    Optimize("booth", 10, return sqr(x + 2*y - 7) + sqr(2*x + y - 5));
    Optimize("rastrigin", 5.12, return 20 + (x*x - 10*cos(2*PI*x)) + (y*y - 10*cos(2*PI*y)));
    Optimize("rosenbrock", 10, return 100*sqr(y - x*x) + sqr(1 - x));
    Optimize("easom", 100, return -cos(x)*cos(y)*exp(-sqr(x - PI) - sqr(y - PI)));
    Optimize("himmelblau", 5, return sqr(x*x + y - 11) + sqr(x + y*y - 7));
    Optimize("styblinski–tang", 5, return x*x*x*x - 16*x*x + 5*x + y*y*y*y - 16*y*y + 5*y);
    Optimize("schaffer n.2", 100, return 0.5 + (sqr(sin(x*x - y*y)) - 0.5) / sqr(1 + 0.001*(x*x + y*y)));
    Optimize("hölder table", 10, return -abs(sin(x)*cos(y)*exp(abs(1 - sqrt(x*x + y*y)/PI))));
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Suites
#include <boost/test/unit_test.hpp>

#include<iostream>

#include"numerics.hpp"
#include"diag.hpp"
#include "files.hpp"
using namespace boost::unit_test;
using boost::unit_test_framework::test_suite;
using namespace Many_Body;
BOOST_AUTO_TEST_SUITE(timeevesting)
BOOST_AUTO_TEST_CASE(timeev)
{
  {  size_t size=5;
  Eigen::VectorXcd x = Eigen::VectorXcd::Random(size);
  x=x/x.norm();

    Eigen::MatrixXd AA = Eigen::MatrixXd::Random(size, size);
     Eigen::MatrixXd A = AA + AA.transpose();

  Eigen::MatrixXd B=A;
  Eigen::VectorXd evA(size);
  Eigen::VectorXd evB(size);
  Eigen::MatrixXcd Q(size, size);
  Many_Body::TriDiagMat tri=Many_Body::Lanczos(B, x, size, Q);

Eigen::MatrixXd S(size, size);
 S.setZero();
 diag(tri, S, evB);
  diag(A, evA);

  for (int i = 0; i < size; ++i)
  {
       BOOST_CHECK(std::abs(evA(i)-evB(i))<Many_Body::err);
  }

  }{
    size_t size=5;
  Eigen::VectorXcd x = Eigen::VectorXcd::Random(size);
  x=x/x.norm();

    Eigen::MatrixXcd AA = Eigen::MatrixXcd::Random(size, size);
    Eigen::MatrixXcd A = AA.adjoint()*AA;

  Eigen::MatrixXcd B=A;
  Eigen::VectorXd evA(size);
  Eigen::VectorXd evB(size);
  Eigen::MatrixXcd Q(size, size);
  Many_Body::TriDiagMat tri=Many_Body::Lanczos(B, x, size, Q);

Eigen::MatrixXd S(size, size);
  S.setZero();
  diag(tri, S, evB);
   diag(A, evA);
        std::cout << '\n';
  for (int i = 0; i < size; ++i)
  {
    BOOST_CHECK(std::abs(evA(i)-evB(i))<Many_Body::err);

  }
  }
  //
}

BOOST_AUTO_TEST_SUITE_END()
// EOF


#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/hypot.hpp>
#include <test/unit/math/rev/scal/fun/util.hpp>
#include <test/unit/math/mix/scal/fun/nan_util.hpp>

TEST(AgradFwdHypot, FvarVar_FvarVar_1stDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(3.0, 1.3);

  fvar<var> z(6.0, 1.0);
  fvar<var> a = hypot(x, z);

  EXPECT_FLOAT_EQ(hypot(3.0, 6.0), a.val_.val());
  EXPECT_FLOAT_EQ((1.3 * 3.0 + 6.0 * 1.0) / hypot(3.0, 6.0), a.d_.val());

  AVEC y = createAVEC(x.val_, z.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0, 6.0), g[0]);
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0, 6.0), g[1]);
}
TEST(AgradFwdHypot, FvarVar_Double_1stDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(3.0, 1.3);
  double z(6.0);
  fvar<var> a = hypot(x, z);

  EXPECT_FLOAT_EQ(hypot(3.0, 6.0), a.val_.val());
  EXPECT_FLOAT_EQ((1.3 * 3.0) / hypot(3.0, 6.0), a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0, 6.0), g[0]);
}
TEST(AgradFwdHypot, Double_FvarVar_1stDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  double x(3.0);
  fvar<var> z(6.0, 1.0);
  fvar<var> a = hypot(x, z);

  EXPECT_FLOAT_EQ(hypot(3.0, 6.0), a.val_.val());
  EXPECT_FLOAT_EQ((6.0 * 1.0) / hypot(3.0, 6.0), a.d_.val());

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0, 6.0), g[0]);
}
TEST(AgradFwdHypot, FvarVar_FvarVar_2ndDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(3.0, 1.3);
  fvar<var> z(6.0, 1.0);
  fvar<var> a = hypot(x, z);

  AVEC y = createAVEC(x.val_, z.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(
      (1.3 * 6.0 * 6.0 - 6.0 * 3.0) / hypot(3.0, 6.0) / (9.0 + 36.0), g[0]);
  EXPECT_FLOAT_EQ(
      (1.0 * 3.0 * 3.0 - 1.3 * 6.0 * 3.0) / hypot(3.0, 6.0) / (9.0 + 36.0),
      g[1]);
}
TEST(AgradFwdHypot, FvarVar_Double_2ndDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(3.0, 1.3);
  double z(6.0);
  fvar<var> a = hypot(x, z);

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(1.3 * 6.0 * 6.0 / hypot(3.0, 6.0) / (9.0 + 36.0), g[0]);
}
TEST(AgradFwdHypot, Double_FvarVar_2ndDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  double x(3.0);
  fvar<var> z(6.0, 1.0);
  fvar<var> a = hypot(x, z);

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(1.0 * 3.0 * 3.0 / hypot(3.0, 6.0) / (9.0 + 36.0), g[0]);
}

TEST(AgradFwdHypot, FvarFvarVar_FvarFvarVar_1stDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = hypot(x, y);

  EXPECT_FLOAT_EQ(hypot(3.0, 6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0, 6.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0, 6.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.059628479, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0, 6.0), g[0]);
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0, 6.0), g[1]);
}
TEST(AgradFwdHypot, FvarFvarVar_Double_1stDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  double y(6.0);

  fvar<fvar<var> > a = hypot(x, y);

  EXPECT_FLOAT_EQ(hypot(3.0, 6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0, 6.0), a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(3.0 / hypot(3.0, 6.0), g[0]);
}

TEST(AgradFwdHypot, Double_FvarFvarVar_1stDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  double x(3.0);
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = hypot(x, y);

  EXPECT_FLOAT_EQ(hypot(3.0, 6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0, 6.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0, 6.0), g[0]);
}
TEST(AgradFwdHypot, FvarFvarVar_FvarFvarVar_2ndDeriv_x) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = hypot(x, y);

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.val_.d_.grad(p, g);

  EXPECT_FLOAT_EQ(36.0 / hypot(3.0, 6.0) / (9.0 + 36.0), g[0]);
  EXPECT_FLOAT_EQ(-2.0 / 15.0 / std::sqrt(5.0), g[1]);
}
TEST(AgradFwdHypot, FvarFvarVar_FvarFvarVar_2ndDeriv_y) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = hypot(x, y);

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.d_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(-2.0 / 15.0 / std::sqrt(5.0), g[0]);
  EXPECT_FLOAT_EQ((3.0 * 3.0) / hypot(3.0, 6.0) / (9.0 + 36.0), g[1]);
}
TEST(AgradFwdHypot, FvarFvarVar_Double_2ndDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  double y(6.0);

  fvar<fvar<var> > a = hypot(x, y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.d_.grad(p, g);

  EXPECT_FLOAT_EQ(6.0 * 6.0 / hypot(3.0, 6.0) / (9.0 + 36.0), g[0]);
}

TEST(AgradFwdHypot, Double_FvarFvarVar_2ndDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  double x(3.0);
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = hypot(x, y);

  EXPECT_FLOAT_EQ(hypot(3.0, 6.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(6.0 / hypot(3.0, 6.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.val_.grad(p, g);
  EXPECT_FLOAT_EQ((3.0 * 3.0) / hypot(3.0, 6.0) / (9.0 + 36.0), g[0]);
}
TEST(AgradFwdHypot, FvarFvarVar_FvarFvarVar_3rdDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = hypot(x, y);

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.d_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.0079504643, g[0]);
  EXPECT_FLOAT_EQ(0.013913312, g[1]);
}
TEST(AgradFwdHypot, FvarFvarVar_Double_3rdDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 3.0;
  x.val_.d_ = 1.0;
  x.d_.val_ = 1.0;
  double y(6.0);

  fvar<fvar<var> > a = hypot(x, y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.d_.d_.grad(p, g);

  EXPECT_FLOAT_EQ(-0.02385139175999775676169785246647, g[0]);
}

TEST(AgradFwdHypot, Double_FvarFvarVar_3rdDeriv) {
  using boost::math::hypot;
  using stan::math::fvar;
  using stan::math::var;

  double x(3.0);
  fvar<fvar<var> > y;
  y.val_.val_ = 6.0;
  y.d_.val_ = 1.0;
  y.val_.d_ = 1.0;

  fvar<fvar<var> > a = hypot(x, y);

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.0119256958799988783808489262332, g[0]);
}

struct hypot_fun {
  template <typename T0, typename T1>
  inline typename boost::math::tools::promote_args<T0, T1>::type operator()(
      const T0 arg1, const T1 arg2) const {
    return hypot(arg1, arg2);
  }
};

TEST(AgradFwdHypot, nan) {
  hypot_fun hypot_;
  test_nan_mix(hypot_, 3.0, 5.0, false);
}


// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>
#include <OpenTissue/core/spline/spline.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>

// Boost Test declaration and Checking macros
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/test/floating_point_comparison.hpp>

typedef OpenTissue::spline::MathTypes<double, size_t>    math_types;
typedef math_types::vector_type                                vector_type;
typedef std::vector<double>                                    knot_container;
typedef std::vector<vector_type>                               point_container;

typedef OpenTissue::spline::NUBSpline<knot_container, point_container> spline_type;

BOOST_AUTO_TEST_SUITE(opentissue_spline_make_periodic);

BOOST_AUTO_TEST_CASE(test_make_periodic)
{
  knot_container U;

  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);  //k = 3
  U.push_back(1.0);
  U.push_back(2.0);
  U.push_back(3.0);
  U.push_back(4.0);  // n = 6  => |P| = 7
  U.push_back(5.0);
  U.push_back(5.0);
  U.push_back(5.0);  // m = 9  => |U| = 10

  point_container P;
  vector_type p0(2);  p0(0) = 0.0; p0(1) = 0.0;
  vector_type p1(2);  p1(0) = 1.0; p1(1) = 0.0;
  vector_type p2(2);  p2(0) = 2.0; p2(1) = 0.0;
  vector_type p3(2);  p3(0) = 3.0; p3(1) = 0.0;
  vector_type p4(2);  p4(0) = 4.0; p4(1) = 0.0;
  vector_type p5(2);  p5(0) = 5.0; p5(1) = 0.0;
  vector_type p6(2);  p6(0) = 6.0; p6(1) = 0.0;

  P.push_back(p0);
  P.push_back(p1);
  P.push_back(p2);
  P.push_back(p3);
  P.push_back(p4);
  P.push_back(p5);
  P.push_back(p6);

  spline_type spline(3,U,P);

  spline_type closed = OpenTissue::spline::make_periodic( spline );
}

BOOST_AUTO_TEST_SUITE_END();


#include <boost/lexical_cast.hpp>
#include "Poisson.hh"
#include "TMath.h"
#include <Eigen/Core>
#include <cmath>

using namespace Eigen;

Poisson::Poisson(bool ln_approx) {
  transformation_("poisson")
    .output("poisson")
    .types(&Poisson::checkTypes)
    .func(ln_approx ? &Poisson::calcPoissonApprox : &Poisson::calcPoisson)
    ;
  m_transform = t_["poisson"];
}

void Poisson::add(SingleOutput &theory, SingleOutput &data) {
  t_["poisson"].input(theory);
  t_["poisson"].input(data);
}

void Poisson::checkTypes(TypesFunctionArgs fargs) {
  auto& args=fargs.args;
  auto& rets=fargs.rets;
  if (args.size()%2 != 0) {
    throw args.undefined();
  }
  for (size_t i = 0; i < args.size(); i+=2) {
    if (args[i+0].shape.size() != 1) {
      throw rets.error(rets[0], "non-vector theory");
    }

    if (args[i+1].shape != args[i+0].shape) {
      throw rets.error(rets[0], "data and theory have different shape");
    }
  }
  rets[0] = DataType().points().shape(1);
}

double addOneInLnGamma(double in)
{
  return TMath::LnGamma(in + 1);
}

double lnFactorialApprox(double x)
{
  if (!(x == 0.0 || x == 1.0))
  {
    return x * std::log(x);
  }
  else
    return 0;
}

void Poisson::calcPoissonApprox(FunctionArgs fargs) {
  /***************************************************************************
   *       Formula: log of Poisson
   *
   *        -2 * ln(Poisson) =
   *            -2 * sum(data_i * log(theory_j) -  theory_j  - ln data_i! )
   *
   ****************************************************************************/
  auto& args=fargs.args;

  double res(0.0);
  for (size_t i = 0; i < args.size(); i+=2) {
    auto& theory=args[i+0].arr;
    auto& data=args[i+1].arr;
    res += (data*theory.log() - theory - data.unaryExpr(&lnFactorialApprox)).sum();
  }
  fargs.rets[0].arr(0) = -2*res;
}

void Poisson::calcPoisson(FunctionArgs fargs) {
  /***************************************************************************
   *       Formula: log of Poisson
   *
   *        -2 * ln(Poisson) =
   *               -2 * sum(data_i * log(theory_j) -  theory_j  - ln data_i! )
   *
   ****************************************************************************/
  auto& args=fargs.args;

  double res(0.0);
  for (size_t i = 0; i < args.size(); i+=2) {
    auto& theory=args[i+0].arr;
    auto& data=args[i+1].arr;
    res += (data*theory.log() - theory - data.unaryExpr(&addOneInLnGamma) ).sum();
  }
  fargs.rets[0].arr(0) = -2*res;
}