/********************************************************************
**                Image Component Library (ICL)                    **
**                                                                 **
** Copyright (C) 2006-2014 CITEC, University of Bielefeld          **
**                         Neuroinformatics Group                  **
** Website: www.iclcv.org and                                      **
**          http://opensource.cit-ec.de/projects/icl               **
**                                                                 **
** File   : ICLPhysics/src/ICLPhysics/PhysicsPaper.cpp             **
** Module : ICLPhysics                                             **
** Author : Christof Elbrechter, Matthias Esau                     **
**                                                                 **
**                                                                 **
** GNU LESSER GENERAL PUBLIC LICENSE                               **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the                  **
**                                                                 **
** Free Software Foundation and appearing in the file LICENSE.LGPL **
** included in the packaging of this file.  Please review the      **
** following information to ensure the license requirements will   **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt                **
**                                                                 **
** The development of this software was supported by the           **
** Excellence Cluster EXC 277 Cognitive Interaction Technology.    **
** The Excellence Cluster EXC 277 is a grant of the Deutsche       **
** Forschungsgemeinschaft (DFG) in the context of the German       **
** Excellence Initiative.                                          **
**                                                                 **
********************************************************************/
#include <ICLPhysics/PhysicsPaper.h>
#include <ICLPhysics/PhysicsWorld.h>
#include <ICLPhysics/PhysicsDefs.h>

#include <ICLPhysics/TriangleIntersectionEstimator.h>

#include <BulletSoftBody/btSoftBody.h>
#include <BulletSoftBody/btSoftBodyHelpers.h>
#include <BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h>
#include <BulletSoftBody/btSoftRigidDynamicsWorld.h>

#include <ICLGeom/Primitive.h>

#include <ICLMarkers/FiducialDetector.h>
#include <ICLQt/Quick.h>


namespace icl{
  using namespace utils;
  using namespace geom;
namespace physics{


  PhysicsPaper::PhysicsPaper(const PhysicsWorld &world,
                             int nxCells, int nyCells, const Vec *init,
                             bool initWithCorners,
                             const Img8u *texture,const Img8u *backfaceTexture):cells(nxCells,nyCells){
    //    const int dim = nxCells * nyCells;

    static const Vec stdcs[4]={
      Vec(-1,-1,0,1),Vec(1 ,-1,0,1),
      Vec(1 , 1,0,1),Vec(-1, 1,0,1)
    };
    const Vec *cs = initWithCorners ? init : stdcs;
    btVector3 bt[4];
    for(int i=0;i<4;++i){
      for(int j=0;j<3;++j){
        bt[i][j] = icl2bullet(cs[i][j]);
      }
    }
    
    btSoftBody *s=btSoftBodyHelpers::CreatePatch(*const_cast<btSoftBodyWorldInfo*>(world.getWorldInfo()),
                                                 bt[0],bt[1],bt[3],bt[2],nxCells,nyCells,0,true);
    setPhysicalObject(s);
    
    btSoftBody::tNodeArray & nodes = s->m_nodes;
    if(!initWithCorners){
      for(int i=0;i<nodes.size();++i){
        nodes[i].m_x = btVector3(icl2bullet(init[i][0]),icl2bullet(init[i][1]),icl2bullet(init[i][2]));
      }
    }
    
    //    s->getCollisionShape()->setMargin(0.18);
    s->getCollisionShape()->setMargin(icl2bullet(2));

    s->appendMaterial();
    s->m_materials[0]->m_kLST = 1.0;
    s->m_materials[0]->m_kAST = 1.0;
    s->m_materials[0]->m_kVST = 1.0;
    
    const Rect r(0,0,nxCells,nyCells);
    const int d = 4;

#define IDX(x,y) x+nxCells*y
    
    for(int y=0;y<nyCells;++y){
      for(int x=0;x<nxCells;++x){
        for(int i=-d;i<=d;++i){
          for(int j=-d;j<=d;++j){
            int tx = i+x, ty = j+y;
            //            if( !(i==0) && !(j==0) ) continue;

            if(r.contains(tx,ty) && (::abs(i)+::abs(j) >= 3)){ // changed
              btSoftBody::Material *m = s->appendMaterial();

              m->m_kVST = m->m_kAST = m->m_kLST = 0.9;

              int sizeBefore = s->m_links.size();
              s->appendLink(IDX(x,y),IDX(tx,ty), m, true); 
              if(s->m_links.size() == sizeBefore) continue; // link did already exist
              s->m_links[s->m_links.size()-1].m_bbending=1;
              
              constraints.push_back(BendingConstraint(&s->m_links[s->m_links.size()-1],
                                                      Point(x,y),Point(tx,ty)));


            }
          }
        }
      }
    }
    
    //std::cout << "created " << s->m_links.size() << " bending constraints" << std::endl;

    //s->randomizeConstraints();
    randomizeLinks();
    
    s->generateClusters(0);


    s->m_cfg.kLF = 0;
    s->m_cfg.kDG = 0;

    s->m_cfg.kMT = 10;
    s->m_cfg.kDP = 0.1;
    s->m_cfg.kDF = 1.0;

    //s->m_cfg.aeromodel = btSoftBody::eAeroModel::V_TwoSided;   
    //s->m_cfg.collisions	= btSoftBody::fCollision::CL_SS+btSoftBody::fCollision::CL_RS + btSoftBody::fCollision::CL_SELF;
    s->m_cfg.collisions	= (btSoftBody::fCollision::CL_SS | btSoftBody::fCollision::CL_RS | btSoftBody::fCollision::CL_SELF);
                           //btSoftBody::fCollision::SDF_RS + 
    //btSoftBody::fCollision::CL_SELF);
    
    /// creating the scene object
    for(int i=0;i<nxCells*nyCells;++i){
      addVertex(Vec(bullet2icl(nodes[i].m_x[0]),
                    bullet2icl(nodes[i].m_x[1]),
                    bullet2icl(nodes[i].m_x[2]),1),
                GeomColor(0,100,255,255));
      addNormal(Vec(nodes[i].m_n[0],nodes[i].m_n[1],nodes[i].m_n[2],1));
    }
   
    
    //for(int x=0;x<nxCells-1;++x){
    //  for(int y=0;y<nyCells-1;++y){
    //    int i = x+nxCells * y;
    //    addQuad(i,i+1,i+1+nxCells,i+nxCells,
    //            i,i+1,i+1+nxCells,i+nxCells,
    //            GeomColor(0,100,255,255));
    //          addLine(i,i+1,GeomColor(255,0,0,255));
    //    addLine(i+1,i+1+nxCells,GeomColor(255,0,0,255));
    //    addLine(i+1+nxCells,i+nxCells,GeomColor(255,0,0,255));
    //    addLine(i+nxCells,i,GeomColor(255,0,0,255));
    //  }
    //}
    addTwoSidedTGrid(nxCells, nyCells, m_vertices.data(), m_normals.data(),
                     GeomColor(0,100,255,255), GeomColor(255,0,100,255), 
                     GeomColor(0,255,100,255), false, true);
    
    hasBackfaceTexture = !! backfaceTexture;
    
    if(texture){
      setVisible(Primitive::quad,false);
      setVisible(Primitive::texture,false);
      if(backfaceTexture){
        SmartPtr<ImgBase> cpy = backfaceTexture->deepCopy();
#ifdef FLIP_MARKERS_ONLY
        FiducialDetector fd("bch","[0,4095]","size=1x1");
        std::vector<Fiducial> fids = fd.detect(cpy.get());
        for(size_t i=0;i<fids.size();++i){
          std::vector<Point32f> corners = fids[i].getCorners2D();
          Range32s rx(corners[0].x,corners[0].x), ry(corners[0].y,corners[0].y);
          for(int j=1;j<4;++j){
            rx.extend(corners[j].x);
            ry.extend(corners[j].y);
          }
          cpy->setROI(Rect(rx.minVal, ry.minVal, rx.getLength()+1, ry.getLength()+1));
          cpy->mirror(axisVert, true);
        }
        cpy->setFullROI();
        //        icl::show( (cvt(backfaceTexture), cvt(cpy.get()) ) );
#else
        cpy->mirror(axisVert, true);
#endif
        
        //struct DepthTestOnOffPrimitive : public Primitive{
        //  bool on;
        //  DepthTestOnOffPrimitive(bool on):Primitive(Primitive::texture),on(on){}
        //  virtual void render(const Primitive::RenderContext &ctx){
        //    if(on)glEnable(GL_DEPTH_TEST);
        //    else glDisable(GL_DEPTH_TEST);
        //  }
        //  virtual Primitive *copy() const { return new DepthTestOnOffPrimitive(*this); }
        //};

        
        //addCustomPrimitive(new DepthTestOnOffPrimitive(false));
        addTwoSidedTextureGrid(cells.width,cells.height,
                               texture,
                               cpy.get(),
                               &m_vertices[0][0], 
                               &m_vertices[0][1], 
                               &m_vertices[0][2],
                               &m_normals[0][0],
                               &m_normals[0][1],
                               &m_normals[0][2],4);
        TwoSidedTextureGridPrimitive * p = (TwoSidedTextureGridPrimitive*)(m_primitives.back());
        p->alphaFunc = (int)GL_ALWAYS;
        //addCustomPrimitive(new DepthTestOnOffPrimitive(true));
      }else{
        addTextureGrid(cells.width,cells.height,
                       texture,
                       &m_vertices[0][0], 
                       &m_vertices[0][1], 
                       &m_vertices[0][2],
                       &m_normals[0][0],
                       &m_normals[0][1],
                       &m_normals[0][2],
                       4);
      }

    }
    setVisible(Primitive::line,false);
    setColorsFromVertices(Primitive::quad,true);
    createAllProperties();

    s->setTotalMass( 0.01 ); // does not affect anything?

    for(int i=0;i<s->m_links.size();++i){
      originalRestLengths.push_back(s->m_links[i].m_rl);
    }

  }


  void PhysicsPaper::resetDeformation(){
    btSoftBody *s = getSoftBody();
    for(int i=0;i<s->m_links.size();++i){
      btSoftBody::Link &l = s->m_links[i];
      float o = originalRestLengths[i];
      l.m_rl = o;
      l.m_c1 = o*o;
    }
  }

 
  void PhysicsPaper::moveVertex(const Point &xy, const Vec &pos, float factor){
    btSoftBody::tNodeArray & nodes = getSoftBody()->m_nodes;
    int i = xy.x+cells.width*xy.y;
    btVector3 t(icl2bullet(pos[0]),icl2bullet(pos[1]),icl2bullet(pos[2]));
    
    nodes[i].m_v = (t-nodes[i].m_x)*factor;
  }

  void PhysicsPaper::movePosition(const Point32f &paperPos, const Vec &pos, float factor){
    const Vec currPos = getInterpolatedPosition(paperPos);

    const float x = paperPos.x, y = paperPos.y;
    const int x0 = floor(x), y0 = floor(y);
    const int x1 = ceil(x),  y1 = ceil(y);
    
    const Point32f ps[4] = { Point(x0,y0), Point(x1,y0), Point(x0,y1), Point(x1,y1) };
    for(int i=0;i<4;++i){
      float contribution = iclMax(1.0 - ::sqrt( ::sqr(ps[i].x-x) + ::sqr(ps[i].y-y) ), 0.0);
      moveVertex( ps[i], pos + (getNodePosition(ps[i])-currPos),factor * contribution);
    }
  }

  Point PhysicsPaper::getNodeIndex(const Vec &v){
    btSoftBody::tNodeArray & nodes = getSoftBody()->m_nodes;
    std::vector<float> ds(nodes.size());
    
    float x=icl2bullet(v[0]),y=icl2bullet(v[1]),z=icl2bullet(v[2]);
    for(int i=0;i<nodes.size();++i){
      ds[i] = sqr(nodes[i].m_x[0]-x) + sqr(nodes[i].m_x[1]-y) + sqr(nodes[i].m_x[2]-z); 
    }
    int minDistIdx = (int)(std::min_element(ds.begin(),ds.end())-ds.begin());
    return Point(minDistIdx%cells.width,minDistIdx/cells.width);
  }

  void PhysicsPaper::updateSceneObject(){
    btSoftBody::tNodeArray & nodes = getSoftBody()->m_nodes;
    const int &nxCells = cells.width;
    const int &nyCells = cells.height;
    const int dim = nxCells * nyCells;
    
    for(int i=0;i<dim;++i){
      for(int j=0;j<3;++j){
        m_vertices[i][j] = bullet2icl(nodes[i].m_x[j]);
        m_normals[i][j] = nodes[i].m_n[j];
      }
    }

#if 0
    if(hasBackfaceTexture){
#if 0
      for(int i=0;i<dim;++i){
        m_normals[i+dim] = m_normals[i];
        m_vertices[i+dim] = m_vertices[i] - m_normals[i]*-1;
      }
#else
      const int W = getDimensions().width, H = getDimensions().height;
      for(int y=0;y<H;++y){
        for(int x=0;x<W;++x){
          const int iSrc = x + W*y;
          const int iDst = dim + (W-1-x) + W*y;
          m_normals[iDst] = -m_normals[iSrc];
          m_vertices[iDst] = m_vertices[iSrc] - m_normals[iSrc]*-1;
        }
      }
#endif
    }
#endif
    
  }

  void PhysicsPaper::setDraggedNode(const Point &xy){
    if(xy.x < 0  && xy.y < 0 ){
      std::fill(m_vertexColors.begin(),m_vertexColors.end(), GeomColor(0,100,255,255)/255.);
    }else{
      m_vertexColors[xy.x+cells.width*xy.y] = GeomColor(1,0,0,1);
    }
  }
  
  
  Vec PhysicsPaper::getNodePosition(int x, int y) const{
    const btSoftBody::tNodeArray &nodes = getSoftBody()->m_nodes;
    const btVector3 &p = nodes[x+cells.width*y].m_x;
    return Vec(bullet2icl(p[0]),bullet2icl(p[1]),bullet2icl(p[2]),1);
  }

  void PhysicsPaper::setNodeMass(const Point &xy, float mass){
    getSoftBody()->setMass(xy.x+cells.width*xy.y,mass);
  }

  void PhysicsPaper::setNodeMass(const Vec &v, float mass){ 
    setNodeMass(getNodeIndex(v),mass); 
  }
  
  void PhysicsPaper::setTotalMass(float mass){
    btSoftBody::tNodeArray & nodes = getSoftBody()->m_nodes;
    for(int i=0;i<nodes.size();++i){
      nodes[i].m_im = mass/nodes.size();
    }
  }
  
  Img32f PhysicsPaper::getVelocityMap() const{
    Img32f m(cells,1);
    Channel32f c = m[0];
    const btSoftBody::tNodeArray & nodes = getSoftBody()->m_nodes;
    for(int i=0;i<nodes.size();++i){
      c[i] = ::sqrt( sqr(nodes[i].m_v[0]) + sqr(nodes[i].m_v[1]) + sqr(nodes[i].m_v[2]) );
    }
    return m;
  }

  void PhysicsPaper::randomizeLinks(){
    getSoftBody()->randomizeConstraints();
    btSoftBody::tLinkArray &links = getSoftBody()->m_links;

    std::map<BendingConstraint::Material*,BendingConstraint::Link*> lookup;

    for(int i=0;i<links.size();++i){
      lookup[links[i].m_material] = &links[i];
    }
    
    for(size_t i=0;i<constraints.size();++i){
      constraints[i].updateLinkPointer(lookup);
    }
  }
  
  void PhysicsPaper::adaptRowStiffness(float val, int row){
    for(size_t i=0;i<constraints.size();++i){
      BendingConstraint &c = constraints[i];
      const float y=c.a.y,ty=c.b.y;
      if( (y>row && ty<row) || (y<row && ty>row) ){
        c.setStiffness(val);
      }
    }
  }
  
  void PhysicsPaper::adaptColStiffness(float val, int col){
    for(size_t i=0;i<constraints.size();++i){
      BendingConstraint &c = constraints[i];
      const float x=c.a.x,tx=c.b.x;
      if( (x>col && tx<col) || (x<col && tx>col) ){
        c.setStiffness(val);
      }
    }
  }

  void PhysicsPaper::adaptGlobalStiffness(float val){
    btSoftBody::tLinkArray &links = getSoftBody()->m_links;
    for(int i=0;i<links.size();++i){
      BendingConstraint c(&links[i]);
      c.setStiffness(val);
    }
  }
  
  void PhysicsPaper::memorizeDeformation(){
    lock();
    getSoftBody()->updateConstants();
    unlock();
  }

  static int sign(float x){ return x > 0 ? 1 : -1; }

  void PhysicsPaper::adaptStiffnessAlongIntersection(const PlaneEquation &plane, float val){
    for(size_t i=0;i<constraints.size();++i){
      BendingConstraint &c = constraints[i];

      Vec a = getNodePosition(c.a.x,c.a.y) - plane.offset;
      Vec b = getNodePosition(c.b.x,c.b.y) - plane.offset;
      
      if( sign(sprod3(a,plane.normal)) != sign(sprod3(b,plane.normal)) ){
        c.setStiffness(val);
      }      
    }
  }

  namespace {
    struct GetPaperCoordinatesHit{
      Point32f coords;
      float distToCamera;
      bool operator<(const GetPaperCoordinatesHit &o) const{
        return distToCamera < o.distToCamera;
      }
    };
    
    static float squared_norm(const Vec &v){
      return sqr(v[0]) + sqr(v[1]) + sqr(v[2]);
    }
  }
  
  /// finds the optimal paper coordinates for a given point in the world
  Point32f PhysicsPaper::getPaperCoordinates(const geom::ViewRay &ray){
    typedef TriangleIntersectionEstimator TIE;
    
    std::vector<GetPaperCoordinatesHit> hits;
    
    for(int x=1;x<cells.width;++x){
      for(int y=1;y<cells.height;++y){
        Vec a = getNodePosition(x-1,y-1);
        Vec b = getNodePosition(x,y-1);
        Vec c = getNodePosition(x,y);
        Vec d = getNodePosition(x-1,y);
        
        TIE::Intersection i1 = TIE::find(TIE::Triangle(a,d,b),ray);
        TIE::Intersection i2 = TIE::find(TIE::Triangle(c,b,d),ray);
        
        if(i1){
          const float &ix = i1.trianglePosition.x, &iy = i1.trianglePosition.y;
          GetPaperCoordinatesHit h = {
            // orig: just wrong Point32f(x - iy,y - ix), 
            Point32f(x-1 + iy,y-1 + ix), 
            squared_norm(i1.position-ray.offset)
          };
          hits.push_back(h);
        }else if(i2){
          const float &ix = i2.trianglePosition.x, &iy = i2.trianglePosition.y;
          GetPaperCoordinatesHit h = {
            // orig: just wrong! Point32f(x-1 + iy, y-1 + ix), 
            Point32f(x - iy, y - ix), 
            squared_norm(i2.position-ray.offset)
          };
          hits.push_back(h);
        }
      }
    }
    if(!hits.size()) return Point32f(-1,-1);
    return std::min_element(hits.begin(),hits.end())->coords;
  }

  static inline Vec lin_interpolate(const Vec &a, const Vec &b, float f){
    const float f1 = 1.0-f;
    return  Vec (a[0] * f1 + b[0] * f,
                 a[1] * f1 + b[1] * f,
                 a[2] * f1 + b[2] * f,1);
                 //a * (1-f) + b * f; 
  }
  
  
  Vec PhysicsPaper::getInterpolatedPosition(const Point32f &p){
    float x = p.x, y = p.y;
    int x0 = floor(x), y0 = floor(y);
    int x1 = ceil(x),  y1 = ceil(y);
    
    Vec a = getNodePosition(x0,y0);
    Vec b = getNodePosition(x1,y0);
    Vec c = getNodePosition(x0,y1);
    Vec d = getNodePosition(x1,y1);
   
    //also wrong orig: float fx = x1 - p.x, fy = y1 - p.y;

    float fx = 1.0f - (x1 - p.x), fy = 1.0f - (y1 - p.y);
    return lin_interpolate( lin_interpolate(a,b,fx), lin_interpolate(c,d,fx), fy);
  }
  
  Vec PhysicsPaper::getPosFromPhysics(int x, int y) const{
    const btSoftBody::tNodeArray &nodes = getSoftBody()->m_nodes;
    int idx = x + getDimensions().width * y;
    return Vec(bullet2icl(nodes[idx].m_x[0]),
               bullet2icl(nodes[idx].m_x[1]),
               bullet2icl(nodes[idx].m_x[2]),1);
  }

  Vec PhysicsPaper::getNormalFromPhysics(int x, int y){
    const btSoftBody::tNodeArray &nodes = getSoftBody()->m_nodes;
    int idx = x + getDimensions().width * y;
    return Vec(nodes[idx].m_n[0],
               nodes[idx].m_n[1],
               nodes[idx].m_n[2],1);
  }


}
}