/*
    Copyright (c) 2010, Luis Alvarez <lalvarez@dis.ulpgc.es>
	All rights reserved.
	
	This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

/**
 * \page code_level_sets.h \link level_sets.h \endlink
 * \brief Code of  \ref code_level_sets.h "level_sets.h"
 * \includelineno level_sets.h
 */

/**
 * \file level_sets.h
 * \brief level_sets class definition
 * \author Luis Alvarez \n \n
 * \brief \b Code: \n
 * - \ref code_level_sets.h "level_sets.h"
*/


#ifndef level_sets_H
#define level_sets_H

#ifndef NOMINMAX
#define NOMINMAX
#endif


#include <iostream>
#include <math.h>
#include <string.h>
#include <vector>
#include <sstream>
#include "contours.h"
#include "image.h"
#include "utilities.h"
#include "filters.h"


//#define LEVEL_SETS_DEBUG


using namespace std;

namespace ami
{

/**
 * \class level_sets
 * \brief class to store 2D level_sets to manage contours and basic methods
 * \author Luis Alvarez
 */


class level_sets{

  public :

  template <class T>
  int clean_contours(ami::contours<T> &c);

  template <class T>
  vector < contours<T> > conexe_component_contour_separation(const ami::contours<T> &c, const float min_area );

  template <class T,class U>
  int construct_contours(ami::contours<T> &c,const ami::image<U> &img, const U &level,const point2d<T> &OffSet,
    const int channel);

  template <class T>
  ami::image<unsigned char> transparency_image(const ami::contours<T> &c,point2d<T> &OffSet);
};

}


/**
 * \fn template <class T> vector < ami::contours<T> > ami::level_sets::conexe_component_contour_separation(const ami::contours<T> &c, const float min_area )
 * \brief function separate a non-ordered contour in its conexe component ordered contours
 * \author Luis Alvarez
 * \return The collection of conexe component ordered contours extracted from the original contour
**/
template <class T>
vector < ami::contours<T> > ami::level_sets::conexe_component_contour_separation(
const ami::contours<T> &c /** non ordered contour composed of a collection of points enclosing several conexe regions */,
const float min_area /** min area of the regions to be considered to build the associated contour */)
{

  vector < ami::contours<T> >  cc; // VECTOR TO STORE THE OUTPUT CONTOUR COLLECTION

  if(c.size()<3) return(cc);

  int border_size=4; // DISTANCE FROM THE CONTOUR TO AUXILIARY WINDOW BORDER
  int i0,j0,i1,j1;

  // WE COMPUTE THE MIN AND MAX OF THE CONTOUR POINTS
  point2d<T> p2d_min=c.min();
  point2d<T> p2d_max=c.max();

  // WE COMPUTE THE IMAGE SIZE AND THE INITIAL POINT COORDINATES
  i0=p2d_min.y-border_size;
  j0=p2d_min.x-border_size;
  i1=p2d_max.y+border_size;
  j1=p2d_max.x+border_size;

  // IMAGE DIMENSIONS
  int width2=j1-j0;
  int height2=i1-i0;
  int size=width2*height2;

  // WE DEFINE AND ALLOCATE MEMORY FOR LOCAL VECTOR
  ami::image<unsigned char> snake_image(width2,height2,1,255);

  // WE FILL THE SNAKE IMAGE
  for(int m=0;m<c.size();m++){
    int i=(int) (c[m].y-i0);
    int j=(int) (c[m].x-j0);
    snake_image[i*width2+j]= 0;
  }

  #ifdef LEVEL_SETS_DEBUG
  {
    static int cont_debug9=1000;
    ami::image<unsigned char> img_debug(width2,height2,3,255);
    int size2=width2*height2;
    for(int m=0;m!=size2;++m){
      img_debug[m]=img_debug[m+size2]=img_debug[m+2*size2]=snake_image[m];
    }
    std::stringstream ss;//create a stringstream
    ss << "conexe_component_contour_separation_original_contour_image" << cont_debug9++ <<".jpg";
    img_debug.write(ss.str());
  }
  #endif

  ami::utilities u;
  int contador=u.scan_line_filling(snake_image,(int) 0,(int) 0, (int) 0, (unsigned char) 255, (unsigned char) 128);

  // WE KEEP ONLY EXTERNAL CONEXE COMPONENTS
  ami::image<unsigned char> snake_image_paso=snake_image;
  #pragma omp parallel for shared(snake_image_paso,snake_image,width2,height2)
  for(int i=0;i<height2;++i){
    int m=i*width2;
    for(int j=0;j<width2;++j,++m){
      if(i==0 || i==height2-1 || j==0 || j== width2-1)
        snake_image[m]=0;
      else{
        if(snake_image_paso[m]==128 ||
        snake_image_paso[m+width2]==128 ||
        snake_image_paso[m-width2]==128 ||
        snake_image_paso[m+1]==128 ||
        snake_image_paso[m-1]==128)
          snake_image[m]=0;
        else{
          if(snake_image[m]!=255) snake_image[m]=255;
        }
      }
    }
  }

  // WE REGULARIZE THE CONTOUR

   #ifdef LEVEL_SETS_DEBUG
  {
    static int cont_debug3=1000;
    ami::image<unsigned char> img_debug(width2,height2,3,255);
    int size2=width2*height2;
    for(int m=0;m!=size2;++m){
      img_debug[m]=img_debug[m+size2]=img_debug[m+2*size2]=snake_image[m];
    }
    std::stringstream ss;//create a stringstream
    ss << "conexe_component_contour_separation_level_set" << cont_debug3++ <<".jpg";
    img_debug.write(ss.str());
  }
  #endif


  // WE SEPARATE THE CONEXE COMPONENTS OF SNAKE IMAGE
  int Nlevel;
  int level=1;
  for(int i=0,m=0;i<height2;++i){
    for(int j=0;j<width2;++j,++m){
      if(snake_image[m]==255){
        contador=u.scan_line_filling(snake_image,j,i, (int) 0,(unsigned char) 255, (unsigned char) level);
        if(contador> min_area){
         level++;
        }
        if(level==255) break;
      }
    }
  }
  Nlevel=level;

  for(level=1;level<Nlevel;level++){
    snake_image_paso=snake_image;
    #pragma omp parallel for shared(level,size,snake_image_paso)
    for(int m=0;m<size;m++){
      if(snake_image_paso[m]==level) snake_image_paso[m]=255;
      else snake_image_paso[m]=1;
    }
    u.scan_line_filling(snake_image_paso,(int) 0,(int) 0, (int) 0,(unsigned char) 1, (unsigned char) 0);

    #pragma omp parallel for shared(size,snake_image_paso)
    for(int m=0;m<size;m++) if(snake_image_paso[m]==1)  snake_image_paso[m]=255;

    // WE RECOMPUTE THE CONTOUR
    ami::level_sets ls;
    point2d <T> OffSet( (T) 0, (T) 0 );
    ami::contours<T> c2;
    int error=ls.construct_contours(c2,snake_image_paso,(unsigned char) level,OffSet,(int) 0);
    if(error<100){
      for(int m=0,m_end=c2.size();m!=m_end;m++){
        c2[m].x+=j0;
        c2[m].y+=i0;
      }
      // WE ADD THE CONTOUR TO THE COLLECTION
      cc.push_back(c2);
    }
  }

  #ifdef LEVEL_SETS_DEBUG
  {
    static int cont_debug11=1000;
    ami::image<unsigned char> img_debug(width2,height2,3,255);
    int size2=width2*height2;
    for(int k=0;k<cc.size();k++){
      for(int l=0;l<cc[k].size();l++){
        int m=(cc[k][l].y-i0)*width2+(cc[k][l].x-j0);
        img_debug[m]=126*(k%3);
        img_debug[m+size2]=126*((k+1)%3);
        img_debug[m+2*size2]=126*((k+2)%3);
      }
    }
    std::stringstream ss;//create a stringstream
    ss << "conexe_component_contour_separation_final" << cont_debug11++ <<".jpg";
    img_debug.write(ss.str());
  }
  #endif
  return(cc);

}


/**
 * \fn template <class T> ami::image<unsigned char> ami::level_sets::transparency_image(const ami::contours<T> &c,ami::point2d<T> &OffSet){
 * \brief function to construct a transparence image outside a contour. Outside the contour the transparence
   image is equal to 0. Inside the contour 255.
 * \author Luis Alvarez
 * \return transparence image.
**/
template <class T>
ami::image<unsigned char> ami::level_sets::transparency_image(
const ami::contours<T> &c /** INPUT CONTOUR */,
ami::point2d<T> &OffSet /** OUTPUT OFFSET TO LOCATE TRANSPARENCE IMAGE POINTS IN THE ORIGINAL IMAGE */)
{
  // WE CHECK THE NUMBER OF CONTOUR POINTS
  if(c.size()<3) return(ami::image<unsigned char>());

  int border_size=4; // DISTANCE FROM THE CONTOUR TO AUXILIARY WINDOW BORDER
  int i0,j0,i1,j1; // SUBWINDOW COORDINATES INCLUDING THE CONTOUR

  // WE COMPUTE THE MIN AND MAX OF THE CONTOUR POINTS
  ami::point2d<T> p2d_min=c.min();
  ami::point2d<T> p2d_max=c.max();

  // WE COMPUTE THE IMAGE SIZE AND THE INITIAL POINT COORDINATES
  i0=p2d_min.y-border_size;
  j0=p2d_min.x-border_size;
  i1=p2d_max.y+border_size;
  j1=p2d_max.x+border_size;

  // IMAGE DIMENSIONS
  int width2=j1-j0;
  int height2=i1-i0;
  //int size=width2*height2;  //CHANGED ***

  // WE DEFINE AND ALLOCATE MEMORY FOR LOCAL VECTOR
  ami::image<unsigned char> snake_image(width2,height2,3,255);

  // WE FILL THE LEVEL SET IMAGE
  for(int m=0;m<c.size();m++){
    int i=(int) (c[m].y-i0);
    int j=(int) (c[m].x-j0);
    snake_image[i*width2+j]= 0;
  }

  // WE FILL THE CONTOUR OUTSIDE
  ami::utilities u;
/*int contador=*/ //CHANGED ***
u.scan_line_filling(snake_image,(int) 0,(int) 0, (int) 0, (unsigned char) 255, (unsigned char) 0);

  // WE FILL THE CONTOUR POINTS AS INSIDE POINTS
  for(int m=0;m<c.size();m++){
    int i=(int) (c[m].y-i0);
    int j=(int) (c[m].x-j0);
    snake_image[i*width2+j]= 255;
  }

  // WE REMOVE THE IMAGE BORDER BY TAKING A ROI FROM THE ORIGINAL IMAGE
  snake_image.set_roi(border_size-2,width2-border_size+3,border_size-2,height2-border_size+3);
  ami::image<unsigned char> snake_image2;
  snake_image.get_roi_image(snake_image2);

   #ifdef LEVEL_SETS_DEBUG
  {
    static int cont_debug4=1000;
    int width3=snake_image2.width();
    int height3=snake_image2.height();
    ami::image<unsigned char> img_debug(width3,height3,3,255);
    int size3=width3*height3;
    for(int m=0;m!=size3;++m){
      img_debug[m]=img_debug[m+size3]=img_debug[m+2*size3]=snake_image2[m];
    }
    std::stringstream ss;//create a stringstream
    ss << "transparency_image" << cont_debug4++ <<".jpg";
    img_debug.write(ss.str());
  }
  #endif

  // WE FIT OFFSET
  OffSet.x=(T) p2d_min.x-2;
  OffSet.y=(T) p2d_min.y-2;

  return(snake_image2);

}


/**
  * \fn template <class T> int ami::level_sets::clean_contours(ami::contours<T> &c)
  * \brief function to clean the contour (in the case the contour has collapsed or has interior points)
  * \return 0 if function finish properly
  * \author Luis Alvarez

*/
template <class T>
int ami::level_sets::clean_contours(ami::contours<T> &c){

  if(c.size()<3) return(-1);

  int border_size=4; // DISTANCE FROM THE CONTOUR TO AUXILIARY WINDOW BORDER
  int i0,j0,i1,j1;

  // WE COMPUTE THE MIN AND MAX OF THE CONTOUR POINTS
  point2d<T> p2d_min=c.min();
  point2d<T> p2d_max=c.max();

  // WE COMPUTE THE IMAGE SIZE AND THE INITIAL POINT COORDINATES
  i0=p2d_min.y-border_size;
  j0=p2d_min.x-border_size;
  i1=p2d_max.y+border_size;
  j1=p2d_max.x+border_size;

  // IMAGE DIMENSIONS
  int width2=j1-j0;
  int height2=i1-i0;
  int size=width2*height2;

  // WE DEFINE AND ALLOCATE MEMORY FOR LOCAL VECTOR
  ami::image<unsigned char> snake_image(width2,height2,1,255);

  // WE FILL THE SNAKE IMAGE
  for(int m=0;m<c.size();m++){
    int i=(int) (c[m].y-i0);
    int j=(int) (c[m].x-j0);
    snake_image[i*width2+j]= 0;
  }

  #ifdef LEVEL_SETS_DEBUG
  {
    static int cont_debug9=1000;
    ami::image<unsigned char> img_debug(width2,height2,3,255);
    int size2=width2*height2;
    for(int m=0;m!=size2;++m){
      img_debug[m]=img_debug[m+size2]=img_debug[m+2*size2]=snake_image[m];
    }
    std::stringstream ss;//create a stringstream
    ss << "clean_contours_" << cont_debug9++ <<".jpg";
    img_debug.write(ss.str());
  }
  #endif

  ami::utilities u;
  int contador=u.scan_line_filling(snake_image,(int) 0,(int) 0, (int) 0, (unsigned char) 255, (unsigned char) 128);

  // WE KEEP ONLY EXTERNAL CONEXE COMPONENTS
  ami::image<unsigned char> snake_image_paso=snake_image;
  #pragma omp parallel for shared(snake_image_paso,snake_image,width2,height2)
  for(int i=0;i<height2;++i){
    int m=i*width2;
    for(int j=0;j<width2;++j,++m){
      if(i==0 || i==height2-1 || j==0 || j== width2-1)
        snake_image[m]=0;
      else{
        if(snake_image_paso[m]==128 ||
        snake_image_paso[m+width2]==128 ||
        snake_image_paso[m-width2]==128 ||
        snake_image_paso[m+1]==128 ||
        snake_image_paso[m-1]==128)
          snake_image[m]=0;
        else{
          if(snake_image[m]!=255) snake_image[m]=255;
        }
      }
    }
  }

  // WE REGULARIZE THE CONTOUR

   #ifdef LEVEL_SETS_DEBUG
  {
    static int cont_debug3=1000;
    ami::image<unsigned char> img_debug(width2,height2,3,255);
    int size2=width2*height2;
    for(int m=0;m!=size2;++m){
      img_debug[m]=img_debug[m+size2]=img_debug[m+2*size2]=snake_image[m];
    }
    std::stringstream ss;//create a stringstream
    ss << "clean_contours_level_set" << cont_debug3++ <<".jpg";
    img_debug.write(ss.str());
  }
  #endif


  // WE KEEP ONLY THE BIG CONEXE COMPONENT OF SNAKE IMAGE
  int cont_max=0;
  int level=1;
  int level_max=255;
  for(int i=0,m=0;i<height2;++i){
    for(int j=0;j<width2;++j,++m){
      if(snake_image[m]==255){
        contador=u.scan_line_filling(snake_image,j,i, (int) 0,(unsigned char) 255, (unsigned char) level);
        if(contador>cont_max){
          cont_max=contador;
          level_max=level;
        }
        level+=1;
        if(level==255) break;
      }
    }
  }

  { int m;
    #pragma omp parallel for shared(level_max,size,snake_image) private(m)
    for(m=0;m<size;m++){
      if(snake_image[m]==level_max) snake_image[m]=255;
      else snake_image[m]=1;
    }
  }
  u.scan_line_filling(snake_image,(int) 0,(int) 0, (int) 0,(unsigned char) 1, (unsigned char) 0);

  { int m;
    #pragma omp parallel for shared(size,snake_image) private(m)
    for(/*int*/ m=0;m<size;m++) if(snake_image[m]==1)  snake_image[m]=255; //CHANGED ***
  }
  // WE RECOMPUTE THE CONTOUR
  ami::level_sets ls;
  point2d <T> OffSet( (T) 0, (T) 0 );
  ami::contours<T> c2;
  int error=ls.construct_contours(c2,snake_image,(unsigned char) level,OffSet,(int) 0);
  if(error<100){
    c.resize(c2.size());
    for(int m=0,m_end=c.size();m!=m_end;m++){
      c[m].x=c2[m].x+j0;
      c[m].y=c2[m].y+i0;
    }
    return(0);
  }

  return(error);

}

/**
  * \fn itemplate <class T,class U> int ami::level_sets::construct_contours(ami::contours<T> &c,const ami::image<U> &img, const U &level,
const point2d<T> &OffSet,const int channel)
  * \brief function to construct a contour for a level set image
  * \return 0 if function finish properly
  * \author Luis Alvarez
*/
template <class T,class U>
int ami::level_sets::construct_contours(
ami::contours<T> &c /** OUTPUT CONTOUR */,
const ami::image<U> &img /** INPUT LEVEL SET IMAGE */,
const U &level /** INPUT LEVEL TO TAKE THE CONTOUR */,
const point2d<T> &OffSet /** OFFSET TO ADD TO CONTOUR POINTS */,
const int channel /** CHANNEL OF IMAGE img TO COMPUTE THE CONTOUR */)
{
  // WE CHECK IF THE IMAGE IS DEFINED
  int width2=img.width();
  int height2=img.height();
  if(width2==0 || height2==0 || channel>=img.nChannels() ) return(-1);

  //AUXILIARY VARIABLES
  int m;

  // WE SAVE INPUT LEVEL SET IMAGE
  #ifdef LEVEL_SETS_DEBUG
  {
    static int cont_debug=1000;
    ami::image<unsigned char> img_debug(width2,height2,3);
    for( int m=0,m_end=width2*height2;m!=m_end;++m){
      img_debug[m]=img_debug[m+m_end]=img_debug[m+2*m_end]=img[m];
    }
    std::stringstream ss;//create a stringstream
    ss << "construct_contours_level_set_" << cont_debug++ <<".jpg";
    img_debug.write(ss.str());
  }
  #endif


  // WE DEFINE SOME AUXILIARY VARIABLES
  int i2,j2,cont=0;
  int n2=channel*height2*width2;

  // WE BUILD NEIGHBORHOOD POINTS
  ami::utilities u;
  int neighborhood_radius=10;
  std::vector < std::vector <ami::point2d<int> > > neigh=u.construct_neighborhood(neighborhood_radius);

  // WE DEFINE AN IMAGE TO STORE POTENTIAL CONTOUR POINTS
  ami::image<unsigned char> level_set_image(width2,height2,1,255);


  { // WE COMPUTE THE POTENTIAL CONTOUR POINTS
    int c=n2,width2_1=width2-1,y,k,k_end; //CHANGED *** ,width=width2
    int size_image_width2=width2*height2-width2;
    #pragma omp parallel for schedule(dynamic) shared(c,width2,width2_1,size_image_width2,level_set_image,i2,j2,cont) \
    private(y,m,k,k_end)
    for(y=width2;y<size_image_width2;y+=width2){
      m=c+y;
      k_end=m+width2_1;
      for(k=m+1; k<k_end; k++){
        if(img[k]<=level && (img[k+width2]>level || img[k-width2]>level || img[k-1]>level || img[k+1]>level) ){
          level_set_image[k-c]=0;
          if(cont==0){i2=y; j2=k%width2;}
          #pragma omp atomic
          cont++;
        }
      }
    }
  }

  // IF WE DO NOT FIND CONTOUR POINTS WE RETURN
  if(cont==0) return(-2);
  bool new_point=true;


  std::vector < std::vector <int> > b=u.boundary_neighborhood_5n(width2,height2);
  { // WE LOOK FOR CONTOUR POINTS IN THE IMAGE BOUNDARY
    int n,n_end=b.size();
    #pragma omp parallel for private(n) shared(b,n_end,n2,level_set_image)
    for(n=0;n<n_end;++n){
      if(img[n2+b[n][0]]<=level &&
        (img[n2+b[n][1]]>level || img[n2+b[n][2]]>level || img[n2+b[n][3]]>level || img[n2+b[n][4]]>level) ){
          level_set_image[b[n][0]]=0;
      }
    }
  }


  // WE DEFINE LEVELS OF CONTOUR POINTS AND VISITED CONTOUR POINTS
  U level_0=0; // LEVEL OF THE CONTOUR POINTS
  U level_1=128; // LEVEL TO CONTROL VISITED CONTOUR POINTS

  // WE KEEP ONLY POTENTIAL CONTOUR POINTS IN THE BOUNDARY OF REGIONS
   for(unsigned n=0,n_end=b.size();n!=n_end;++n){
    if(level_set_image[b[n][0]]==255){
       int x2=b[n][0]%width2;
       int y2=b[n][0]/width2;
       /*int contador=*/ //CHANGED ***
       u.scan_line_filling(level_set_image,x2,y2, (int) 0, (unsigned char) 255, (unsigned char) 1);
    }
  }
  { // WE GO ACROSS THE IMAGE
    int y,k,y_end=height2-1,k_end,m; // m=width2+n2;
    #pragma omp parallel for private(y,k,k_end,m) shared(width2,level_set_image,y_end)
    for(y=1; y<y_end;++y){
      m=y*width2;
      k_end=m+width2-1;
      for(k=m+1;k<k_end;++k){
        if(level_set_image[k]==0 && level_set_image[k+1]!=1 && level_set_image[k-1]!=1 &&
        level_set_image[k+width2]!=1 && level_set_image[k-width2]!=1 && level_set_image[k-width2-1]!=1 &&
        level_set_image[k+width2+1]!=1 && level_set_image[k-width2-1]!=1 && level_set_image[k+width2-1]!=1 ){
          level_set_image[k]=255;
        }
      }
    }
  }

  int size2=width2*height2;
  #pragma omp parallel for private(m) shared(size2,level_set_image)
  for(m=0;m<size2;m++) if(level_set_image[m]==1) level_set_image[m]=255;

  //#pragma omp parallel for default(none) private(m) shared(size2,level_set_image,width2,i2,j2)
  for(m=0;m<size2;m++){
    if(level_set_image[m]==0) {
      //#pragma omp critical (update_point2)
      {
        i2=m/width2;
        j2=m%width2;
      }
      break;
    }
    if(m==size2) return(-5);
  }



  //OffSet POINT (CORNER OF THE SUBWINDOW)
  T i0= OffSet.y;
  T j0= OffSet.x;

  // WE DEFINE THE CONTOURS
  ami::contours<T> c2;
  // WE INCLUDE  THE FIRST CONTOUR POINT
  //c.clear();
  c2.push_back(point2d<T>((T) j0+j2,(T) i0+i2)); // WE ADD THE POINT TO THE CONTOUR
  level_set_image[i2*width2+j2]=level_1;

  // WE BUILD NEIGHBOURHOOD PIXEL CONTOURS
  int Nd=2;
  vector < vector <int> > x(Nd);
  vector < vector <int> > y(Nd);
  x[1].push_back(-1); y[1].push_back(-1);
  x[0].push_back(-1); y[0].push_back(0);
  x[1].push_back(-1); y[1].push_back(1);
  x[0].push_back(0); y[0].push_back(1);
  x[1].push_back(1); y[1].push_back(1);
  x[0].push_back(1); y[0].push_back(0);
  x[1].push_back(1); y[1].push_back(-1);
  x[0].push_back(0); y[0].push_back(-1);

  // WE INSERT POINTS IN THE CONTOUR
  int k0=0;
  int n0=0;
  int i_1=0,j_1=0,i_2=0,j_2=0,i_3=0,j_3=0; // VARIABLES TO TRACK CONTOUR LAST POINTS
  while(new_point==true){
    new_point=false;
    // WE SET INITIAL POINT TO IMAGE SNAKE CONTOUR POINT
    if(c2.size()==4){
      int m= ((int) (c2[0].y-i0))*width2+ (int) (c2[0].x-j0);
      level_set_image[m]=level_0;
    }
    // WE LOOK CURRENT POINT NEIGHBORHOOD STARTING BY THE LAST NEIGHBORHOOD DIRECTION
    for(int k=0;k<Nd;k++){
      int k1=(k0+k)%Nd;
      for(int n=0;n<(int)x[k1].size();n++){
        int n1;
        if(n%2==0) n1=(n0+n/2)%x[k1].size();
        else n1=(n0-(n+1)/2)%x[k1].size();
        int i3=i2+x[k1][n1];
        int j3=j2+y[k1][n1];
        if(i3<0 || i3>(height2-1) || j3<0 || j3>(width2-1)) continue;
        int m=i3*width2+j3;
        // WE CHECK IF THE NEW POINT IS IN THE CONTOUR
        if(level_set_image[m]==level_0
            && ( (abs(i3-i_3)+abs(j3-j_3))>1 )   && ( (abs(i3-i_2)+abs(j3-j_2))>1 ) ){
          i_3=i_2; j_3=j_2; i_2=i_1; j_2=j_1; i_1=i2; j_1=j2;
          i2=i3; j2=j3; // WE UPDATE THE CURRENT POINT
          // WE REMOVE LAST POTENCIAL CONTOUR POINTS
          if(i_2>0 && i_2<(height2-1) && j_2>0 && j_2<(width2-1)){
            for(int p=-1;p<=1;p++)
              for(int q=-1;q<=1;q++)
                level_set_image[(i_2+p)*width2+j_2+q]=level_1;
          }
          new_point=true;
          c2.push_back(point2d<T>((T) j0+j2,(T) i0+i2));
          level_set_image[m]=level_1;
          k=Nd;
          k0=k1;
          if(k0>1) k0=0;
          n0=n1;
          n=x[k1].size();
        }
      }
    }
    // IN THE CASE WE HAVE NOT FOUND NEW POINT WE TRY TO GET ANOTHER POINT IN THE CONTOUR
    // LOOKING A BIGGER NEIGHBORHOOD
    if(new_point==true) continue;
    // WE SET INITIAL POINT TO IMAGE SNAKE CONTOUR POINT
    if(c2.size()==4){
      int m= ((int) (c2[0].y-i0))*width2+ (int) (c2[0].x-j0);
      level_set_image[m]=level_0;
    }
    for(int p=0;p<(int)neigh.size();p++){
      for(int q=0;q<(int)neigh[p].size();q++){
        int i4=i2+neigh[p][q].y;
        int j4=j2+neigh[p][q].x;
        if(i4<0 || i4>(height2-1) || j4<0 || j4>(width2-1) ) continue;
        int m=i4*width2+j4;
        // WE CHECK IF THE NEW POINT IS IN THE CONTOUR
        if(level_set_image[m]==level_0){
          level_set_image[m]=level_1;
          i_3=0; j_3=0; i_2=0; j_2=0; i_1=0; j_1=0;
          // WE INCLUDE NEW POINTS IN THE CONTOUR
          c2.pop_back();
          c2.add_segment(point2d<T>((T) j0+j2,(T) i0+i2),point2d<T>((T) j0+j4,(T) i0+i4));
          i2=i4; j2=j4;
          new_point=true;
          p=neigh.size();
          break;
        }
      }
    }
    if(new_point==true) continue;
    if(i2==0 || j2==0 || i2==(height2-1) || j2==(width2-1)){
      int i_min,j_min;
      float min_d=1e30;
      for(unsigned k=0,k_end=b.size();k!=k_end;++k){
        if(level_set_image[b[k][0]]==level_0){
          int i4=b[k][0]/width2;
          int j4=b[k][0]%width2;
          float paso=(i4-i2)*(i4-i2)+(j4-j2)*(j4-j2);
          if(paso<min_d){
            min_d=paso;
            i_min=i4;
            j_min=j4;
          }
        }
      }
      if(min_d<1e30){
        level_set_image[i_min*width2+j_min]=level_1;
        i_3=0; j_3=0; i_2=0; j_2=0; i_1=0; j_1=0;
        // WE INCLUDE NEW POINTS IN THE CONTOUR
        c2.pop_back();
        c2.add_segment(point2d<T>((T) j0+j2,(T) i0+i2),point2d<T>((T) j0+j_min,(T) i0+i_min));
        i2=i_min;
        j2=j_min;
        new_point=true;
      }
    }
  }

  // WE SAVE INPUT LEVEL SET IMAGE AND CONTOURS
  #ifdef LEVEL_SETS_DEBUG
  {
    static int cont_debug2=1000;
    ami::image<unsigned char> img_debug(width2,height2,3,255);
    int size2=width2*height2;
    for(int m=0;m!=size2;++m){
      //img_debug[m]=img_debug[m+size2]=img_debug[m+2*size2]=img[m];
    }
    for(int m=0;m<c2.size();m++){
      int n=(c2[m].y-i0)*width2+c2[m].x-j0;
      img_debug[n+size2]=255;
      img_debug[n]=0;
      img_debug[n+2*size2]=0;
    }
    std::stringstream ss;//create a stringstream
    ss << "construct_contours_level_set_contours" << cont_debug2++ <<".jpg";
    img_debug.write(ss.str());
  }
  #endif

  // WE CHECK IF THE CONTOUR HAS AT LESS 4 POINTS AND IT IS CLOSED
  if(c2.size()<4) return(-3);
  double norm=(c2[0]-c2[c2.size()-1]).norm();
  if(norm>1.5){
    point2d<T> p2d_paso=c2[c2.size()-1];
    c2.pop_back();
    c2.add_segment(p2d_paso,c2[0]);
    c=c2;
    if(norm>3) return(-4);
    return(0);
  }

  // WE UPDATE THE CONTOUR
  c=c2;
  return(0);

}



#endif
