#include "som.h"

struct somxmldata
{
  std::vector< std::string > tmpstr;
  std::vector< std::string > tag;

  std::vector< int > cursomdim;
  int cursomid;

  int vectorsz;
  int curtime;
  int maxtime;

  std::vector< int > dimmult;
  std::vector< int > somdims;
  std::vector< std::vector<int> > excludes;
  std::vector< std::vector<int> > special;

  std::map< int, double* > *somgrid;

};

void somxmldata_handle(void *userData,
                       const XML_Char *s,
                       int len);

void somxmlstart_handle( void *userData,
                         const XML_Char *name,
                         const XML_Char **atts );

void somxmlend_handle( void *data, const char *el );

void somxmldata_handle(void *userData,
                       const XML_Char *s,
                       int len)
{
  char *buffer;
  buffer = new char[len+1];
  buffer[len] = 0;
  memcpy( buffer, s, len );
  somxmldata *x;
  x = (somxmldata*)userData;
  std::string ts;
  ts = buffer;
  x->tmpstr[ x->tmpstr.size()-1 ].append( ts );
  delete [] buffer;
}

void somxmlstart_handle( void *userData,
                      const XML_Char *name,
                      const XML_Char **atts )
{
  somxmldata *x;
  x = (somxmldata*)userData;
  x->tmpstr.push_back( "" );
  x->tag.push_back( name );
  if( strcmp(name, "somnode") == 0 )
    {
      int i;
      i = 0;
      while( atts[i] != NULL )
        {
          if( strcmp( atts[i], "coor" ) == 0 )
            {
              converttoint(atts[i+1], x->cursomdim);
            }
          else if( strcmp( atts[i], "id" ) == 0 )
            {
              x->cursomid = strtol( atts[i+1], NULL, 10 );
            }
          i = i + 2;
        }
    }
  else if( strcmp(name, "exclude" ) == 0 )
    {
      int i = 0;
      while( atts[i] != NULL )
        {
          if( strcmp( atts[i], "somid" ) == 0 )
            {
              std::cout << "excluding... " << atts[i+1] << "\n";
              std::vector< int > exclude;
              converttoint(atts[i+1], exclude);
              x->excludes.push_back( exclude );
            }
          i = i + 2;
        }
    }
  else if( strcmp(name, "special" ) == 0 )
    {
      int i = 0;
      while( atts[i] != NULL )
        {
          if( strcmp( atts[i], "somid" ) == 0 )
            {
              std::cout << "special... " << atts[i+1] << "\n";
              std::vector< int > e;
              converttoint(atts[i+1], e);
              x->special.push_back( e );
            }
          i = i + 2;
        }
    }
}

void somxmlend_handle( void *data, const char *el )
{
  somxmldata *x;
  x = (somxmldata*)data;
  // create rows
  if( strcmp( el, "dims" ) == 0 )
    {
      converttoint( x->tmpstr.back(), x->somdims );
      x->dimmult = x->somdims;
      x->dimmult[ x->somdims.size()-1 ] = 1;
      for( int i = (int) x->somdims.size()-2; i >= 0; i-- )
        x->dimmult[i] = x->dimmult[i+1]*x->somdims[i+1];
      for( unsigned i = 0; i < x->dimmult.size(); i++ )
        std::cout << x->dimmult[i] << " ";
      std::cout << "\n";
    }
  else if( strcmp( el, "sz" ) == 0 )
    {
      x->vectorsz = (int)strtol( x->tmpstr.back().c_str(), NULL, 10 );
    }
  else if( strcmp( el, "time" ) == 0 )
    {
      x->curtime = (int)strtol( x->tmpstr.back().c_str(), NULL, 10 );
    }
  else if( strcmp( el, "maxtime" ) == 0 )
    {
      x->maxtime = (int)strtol( x->tmpstr.back().c_str(), NULL, 10 );
    }
  else if( strcmp( el, "somnode" ) == 0 )
    {
      if( x->somgrid->count( x->cursomid ) != 0 )
        {
          std::cout << "ERROR dup somid: " << x->cursomid << "\n";
          exit(0);
        }
      double *f;
      f = new double[ x->vectorsz ];
      std::vector< double > td;
      stripstr( x->tmpstr.back() );
      converttodouble( x->tmpstr.back(), td );
      if( (int)td.size() != x->vectorsz )
        {
          std::cout << "vector size not match\n";
          std::cout << td.size() << " : " 
                    << x->vectorsz << "\n";
          std::cout << "\"" << x->tmpstr.back()
                    << "\"\n";
          exit(0);
        }
      for( int i = 0; i < x->vectorsz; i++ )
        f[i] = td[i];
      (*(x->somgrid))[ x->cursomid ] = f;
    }
  else if( strcmp( el, "som" ) == 0 )
    {
      std::cout << "finished som tag\n";
    }
  else if( strcmp( el, "somstate" ) == 0 )
    {
      std::cout << "finished somstate\n";
    }
  else if( strcmp(el, "exclude" ) == 0 )
    {
      std::cout << "finish exclude\n";
    }
  else if( strcmp(el, "special" ) == 0 )
    {
      std::cout << "finish special\n";
    }
  else
    {
      std::cout << "unexpected end tag\n"; 
      std::cout << el << "\n";
      exit(0);
    }
  x->tmpstr.pop_back();
  x->tag.pop_back();
}

som::som()
{
  dbf = NULL;
  somgridvec = NULL;
}

som::~som()
{
  clear();
  clearsomgridvec();
}

int som::clear()
{

  for( std::map<int, double*>::iterator it = somgrid.begin();
       it != somgrid.end(); it++ )
    {
      delete [] it->second;
    }
  somgrid.clear();
  somdims.clear();
  dimmult.clear();
  return 0;
}

int som::clearcache()
{
  for( unsigned i = 0; i < fcache.size(); i++ )
    {
      delete [] fcache[i];
    }
  fcache.clear();
  return 0;
}

int som::xmlout( std::ofstream &of )
{
  of << "<som>\n";
  std::vector< int > dims;
  for( std::map<int, double*>::iterator it = somgrid.begin();
       it != somgrid.end(); it++ )
    {
      of << "<somnode coor=\"";
      decodecoor( it->first, dims );
      for( int i = 0; i < (int) dims.size(); i++ )
        of << dims[i] << " ";
      of << " \" id=\"" << it->first;
      of << "\">\n" << std::setprecision(16);
      for( int i = 0; i < (int) vectorsz; i++ )
        of << it->second[i] << " ";
      of << "\n" << std::setprecision(6);
      of << "</somnode>\n";
    }
  of << "</som>\n";
  return 0;
}

int som::savestate( int time, int maxtime, std::string file )
{
  std::ofstream of;
  of.open( file.c_str() );
  of << "<somstate>\n";
  of << "<dims>";
  for( unsigned int i = 0; i < somdims.size(); i++ )
    of << somdims[i] << " ";
  of << "</dims>\n";
  of << "<sz>" << vectorsz << "</sz>\n";
  of << "<time>" << time << "</time>\n";
  of << "<maxtime>" << maxtime << "</maxtime>\n";
  xmlout( of );
  of << "</somstate>\n";
  of.close();
  return 0;
}

int som::init( dbtmpfeature *db, 
               std::string state)
{
  clear();
  dbf = db;
  somxmldata xmld;
  xmld.somgrid = &somgrid;
  XML_Parser p = XML_ParserCreate(NULL);

  XML_SetElementHandler(p, somxmlstart_handle,
                        somxmlend_handle);
  XML_SetCharacterDataHandler( p,
                               somxmldata_handle);
  FILE *fp;
  XML_SetUserData( p, &xmld );
  fp = fopen( state.c_str(), "r" );

  char buf[1024];
  while(1) 
    {
      int done;
      int len;
      
      len = fread(buf, 1, 1024, fp );
      done = feof(fp);
      
      if (! XML_Parse(p, buf, len, done)) {
        fprintf(stderr, "Parse error at line %d:\n%s\n",
                (int)XML_GetCurrentLineNumber(p),
                XML_ErrorString( XML_GetErrorCode(p)) );
        exit(-1);
      }

      if (done)
        break;
    }

  fclose(fp);
  XML_ParserFree( p );
  vectorsz = xmld.vectorsz;
  curtime = xmld.curtime;
  maxtime = xmld.maxtime;
  dimmult = xmld.dimmult;
  somdims = xmld.somdims;
  exclude = xmld.excludes;
  special = xmld.special;
  somdims_size = (int) somdims.size();
  std::cout << "dimentions " << somdims_size << "\n";
  initsomgridvec();
  return 0;
}

int som::init( dbtmpfeature *db, 
               int sz,
               std::vector< int > &dims )
{
  clear();
  curtime = 0;
  maxtime = 60000000;
  dbf = db;
  vectorsz = sz;
  somdims = dims;
  dimmult = dims;
  somdims_size = (int)dims.size();
  curtime = 0;
  //  vectorsz = (int)dims.size();
  dimmult[ dims.size()-1 ] = 1;
  for( int i = (int) dims.size()-2; i >= 0; i-- )
    dimmult[i] = dimmult[i+1]*dims[i+1];
  std::cout << "dims: ";
  for( unsigned i = 0; i < dims.size(); i++ )
    std::cout << somdims[i] << " ";
  std::cout << "\ndimmult: ";
  for( unsigned i = 0; i < dims.size(); i++ )
    std::cout << dimmult[i] << " ";
  std::cout << "\n";
  std::vector< int > cursom, tsom;
  cursom = dims;
  for( unsigned i = 0; i < dims.size(); i++ )
    cursom[i] = 0;
  while( cursom.back() < dims.back() )
    {
      int c;
      c = encodecoor( cursom );
      decodecoor( c, tsom );
      if( somgrid.count( c ) == 0 )
        {
          std::cout << "inserting ";
          for( int i = 0; i < (int)cursom.size(); i++ )
            std::cout << cursom[i] << " ";
          std::cout << " : " << c <<"\n";
          somgrid[c] = new double[vectorsz];
          for( int i = 0; i < vectorsz; i++ )
            somgrid[c][i] = ( (double)random() )/( (double)random() );
        }
      else
        {
          std::cout << "ERROR som dup stuff\n";
          std::cout << c << "\n";
          for( int i = 0; i < (int)cursom.size(); i++ )
            std::cout << cursom[i] << " ";
          std::cout << "\n";
          exit(0);
        }

      for( unsigned z = 0; z < tsom.size(); z++ )
        {
          if( cursom[z] != tsom[z] )
            {
              std::cout << "SOM encoding error\n";
              exit(0);
            }
        }
      cursom[0] = cursom[0] + 1;
      if( cursom[0] == dims[0] )
        {
          //carry
          int flag;
          flag = 1;
          int curdig;
          curdig = 0;
          while( flag == 1 && curdig < (int) dims.size()-1 )
            {
              cursom[curdig] = 0;
              cursom[curdig+1] = 
                cursom[curdig+1] + 1;
              if( cursom[curdig+1] < dims[curdig+1] )
                flag = 0;
              curdig = curdig + 1;
            }
        }
    }
  initsomgridvec();
  return 0;
}

int som::run( std::string file )
{
  unsigned int maxid;
  std::cout << "getting maxid\n";
  maxid = dbf->maxid();
  maxdis = 0;
  std::cout << "ids [1, " << maxid << "]\n";
  for( unsigned i = 0; i < somdims.size(); i++ )
    maxdis = maxdis + pow( somdims[i], 2 );
  maxdis = sqrt(maxdis);
  int nc;
  nc = curtime + 363499*50;
  srand( time(NULL) );
  clearcache();
  // loadcache( 3634993, 3634993+3634993+1634993 );
  loadcache( 2100000, 2100000+(1000000*2.5));

  // start threads
  std::cout << "cache loaded\n";
  // startthreads( NUMTHREADS );
  double abs;
  while( curtime < maxtime && curtime < nc )
    {
      abs = 0;
      timer mytimer;
      mytimer.reset();
      mytimer.start();
      std::random_shuffle( fcache.begin(), fcache.end() );
      for( unsigned myid = 0; myid < fcache.size(); myid = myid + 1 )
        {
          update( ( (double)curtime)/maxtime, fcache[myid] );
          abs = abs + curdis;
          curtime = curtime + 1;
        }
      mytimer.stop();
      std::cout << "cycle finished in: " 
                << mytimer.getseconds() << "s "
		<< mytimer.getcpu() << "cpu seconds\n";
      abs = abs / ( (double)fcache.size() );
      std::cout << "avgdis: " << abs << "\n" << std::flush;
    }
  std::cout << "\n";
  savestate( curtime, (int)maxtime, file);

  clearcache();
  // endthreads();

  return 0;
}

int som::endthreads()
{
  for( unsigned i = 0; i < threads.size(); i++ )
    {
      threads[i]->run = 0;
      updatethread[i]->run = 0;
    }
  pthread_barrier_wait( myupdatebar );
  pthread_barrier_wait( mybar );
  void *v;
  for( unsigned i = 0; i < threads.size(); i++ )
    {
      pthread_join( threads[i]->tid, &v );
      pthread_join( updatethread[i]->tid, &v );
      delete threads[i];
      delete updatethread[i];
    }
  threads.clear();
  updatethread.clear();
  pthread_barrier_destroy( mybar );
  pthread_barrier_destroy( myupdatebar );
  pthread_mutex_destroy( mylock );
  delete mylock;
  delete mybar;
  delete myupdatebar;

  return 0;
}

int som::startthreads( int numthreads )
{
  if( threads.size() != 0 )
    {
      std::cout << "ERROR threads not ended\n";
      exit(0);
    }
  threads.resize( numthreads );
  updatethread.resize( numthreads );
  mybar = new pthread_barrier_t;
  myupdatebar = new pthread_barrier_t;
  mylock = new pthread_mutex_t;
  pthread_mutex_init( mylock , NULL );
  pthread_barrier_init( mybar, NULL, numthreads+1 );
  pthread_barrier_init( myupdatebar,
                        NULL, numthreads+1 );
  for( int i = 0; i < numthreads; i++ )
    {
      threads[i] = new somclassify_td;
      threads[i]->f = &curfeature;
      threads[i]->run = 1;
      threads[i]->bar = mybar;
      threads[i]->lock = mylock;
      threads[i]->vectorsz = vectorsz;

      updatethread[i] = new somupdate_td;
      updatethread[i]->f = &curfeature;
      updatethread[i]->bar = myupdatebar;
      updatethread[i]->vectorsz = vectorsz;
      updatethread[i]->run = 1;
      updatethread[i]->mysom = this;
    }

  std::map<int, double*>::iterator it;
  it = somgrid.begin();
  int c;
  c = 0;
  while( it != somgrid.end() )
    {
      threads[ c%numthreads ]->todo[it->first] = it->second;
      updatethread[ c%numthreads ]->todo[it->first] = it->second;
      it++;
      c++;
    }
  for( int i = 0; i < numthreads; i++ )
    {
      pthread_create( &(threads[i]->tid), NULL,
                      classify_threaded_start, 
                      threads[i] );
      pthread_create( &(updatethread[i]->tid), NULL,
                      somupdate_start, 
                      updatethread[i] );
    }
  return 0;
}

int som::loadcache( unsigned int low, unsigned int high )
{
  std::cout << "loading cache " << low << " " << high << "\n";
  std::cout << "estsz: " << (high-low)*sizeof(double)*vectorsz/(1024*1024) << " mb\n";
  for( unsigned i = low; i < high; i++ )
    {
      double *f;
      f = new double[vectorsz];
      dbf->get( i, (char*)f );
      fcache.push_back( f );
      /*
      double s;
      s = 0;
      for( int z = 28; z < 32; z++ )
        s = s + f[z];
      for( int z = 0; z < 28; z++ )
        s = s - f[z];
      if( s > 50 )
        std::cout << 'x' << std::flush;
      */
      if( i % 100000 == 0 )
        std::cout << "." << std::flush;
    }
  std::cout << "\n";
  std::cout << "cache size: " << fcache.size()*sizeof(double)*vectorsz/(1024*1024) << " mb\n";
  return 0;
}

int som::update( double time, double *f )
{
  std::vector< int > gcoor, coor;
  int egcoor;
  egcoor = classify(f, gcoor );
  std::cout << ": " << egcoor << " | ";
  for( unsigned int i = 0; i < gcoor.size(); i++ )
    std::cout << gcoor[i] << " ";
  std::cout << "\n" << std::flush;
  double a, n, d;
  a = 0.75*( 1-time )+0.05;
  if( a < 0.025 )
    a = 0.025;
  n = maxdis*1.2*(1-time);
  if( n < 1.5 )
    n = 1.5;
  curfeature = f;
  /*
  for( std::map<int, double*>::iterator it = somgrid.begin();
       it != somgrid.end(); it++ )
  */
  for( int i = 0; i < numbernodes; i++ )
    {
      //update
      decodecoor( i, coor );
      d = distance( gcoor, coor );
      if( d < n )
        {
          double b;
          d = d/n;
          if( d > 0 )
            b = sin( d*PI )/( d*PI ) * a;
          else
            b = a;

          for( int z = 0; z < vectorsz; z++ )
            {
              somgridvec[i][z] = somgridvec[i][z] +
                b*( f[z] - somgridvec[i][z] );
#ifdef __SOMDEBUG__
              if( isnan( somgridvec[i][z] ) || 
                  isinf( somgridvec[i][z] ) )
                {
                  std::cout << "ERROR som update\n";
                  std::cout << somgridvec[i][z] << " " 
                            << b << " " << a << " "
                            << time  << " " << i 
                            << " " << z << "\n" 
                            << std::flush;
		  exit(0);
                }
#endif
              if( somgridvec[i][z] < 0 )
                somgridvec[i][z] = 0;
            }
        }
    }
  
  return 0;
}

int som::classify( double *f, 
                   std::vector< int > &coor )
{
  double bs;
  int bc;
  bc = 0;
  bs = -1;
  /*
  for( std::map<int, double*>::iterator it = somgrid.begin();
       it != somgrid.end(); it++ )
  */
  for( int i = 0; i < numbernodes; i++ )
    {
      double d;
      d = 0;
      for( int z = 0; z < vectorsz; z++ )
        d = d + pow( somgridvec[i][z] - f[z], 2 );
      if( d < bs || bs < 0)
        {
          bs = d;
          bc = i;
        }
    }
  curdis = bs;
  decodecoor( bc, coor );
  return bc;
}

int som::checkspecial( std::vector< int > &node )
{
 
  for( unsigned int i = 0; i < exclude.size(); i++ )
    {
      double d;
      d = distance( exclude[i], node );
      if( d < 0.1 )
        return -1;
    }
  for( unsigned int i = 0; i < special.size(); i++ )
    {
      double d;
      d = distance( special[i], node );
      if( d < 0.1 )
        return 1;
    }
  
  return 0;
}

int som::initsomgridvec()
{
  clearsomgridvec();
  numbernodes = (int) somgrid.size();
  std::cout << "initializing som grid: " << numbernodes << "\n"
            << std::flush;
  somgridvec = new double* [ numbernodes ];
  for( int i = 0; i < numbernodes; i++ )
    somgridvec[i] = NULL;
  for( std::map<int, double*>::iterator it = somgrid.begin();
       it != somgrid.end(); it++ )
    {
      if( somgridvec[ it->first ] == NULL )
        somgridvec[ it->first ] = it->second;
      else
        {
          std::cout << "ERROR som grid vec\n";
          exit(0);
        }
    }
  for( int i = 0; i < numbernodes; i++ )
    {
      if( somgridvec[i] == NULL )
        {
          std::cout << "ERROR som grid vec 1\n";
          exit(0);
        }
    }
  return 0;
}

int som::clearsomgridvec()
{
  if( somgridvec != NULL )
    delete [] somgridvec;
  somgridvec = NULL;
  return 0;
}

int som::classifyt( double *f, 
                    std::vector< int > &coor )
{
  // set the work
  curfeature = f;
  // signal thread to start
  pthread_barrier_wait( mybar );
  // wait for results
  pthread_barrier_wait( mybar );
  double bs;
  int bc;
  bs = -1;
  bc = 0;
  for( unsigned int i = 0; i < threads.size(); i++ )
    {
      if( threads[i]->bs < bs || bs < 0)
        {
          bs = threads[i]->bs;
          bc = threads[i]->bid;
        }
    }

#ifdef __DEBUG_SOMTHREAD__
  std::cout << "best score: " << bs << " " << bc << "\n";
#endif
  curdis = bs;
  decodecoor( bc, coor );
  return bc;
}

void *classify_threaded_start( void *p )
{
  somclassify_td *d;
  d = (somclassify_td *) p;
  classify_threaded( d );
  return NULL;
}

int classify_threaded( somclassify_td *d )
{
  // main thread signal to run first
  pthread_barrier_wait( d->bar );
  while( d->run == 1 )
    {
      double *myf;
      myf = *(d->f);

#ifdef __DEBUG_SOMTHREAD__
      pthread_mutex_lock( d->lock );
      std::cout << "thread: " << d->tid << " processing " 
                << std::hex << myf << std::dec << "\n";
      pthread_mutex_unlock( d->lock );
#endif

      d->bs = -2;
      d->bid = 0;
      for( std::map<int, double*>::iterator it = d->todo.begin();
           it != d->todo.end(); it++ )
        {
          double s;
          s = 0;
          for( int z = 0; z < d->vectorsz; z++ )
            {
              s = s + pow( myf[z] - it->second[z], 2 );
            }
          if( s < d->bs || d->bs < -1 )
            {
              d->bs = s;
              d->bid = it->first;
            }
        }

#ifdef __DEBUG_SOMTHREAD__
      pthread_mutex_lock( d->lock );
      std::cout << "thread: " << d->tid << " best score " << d->bs << " " << d->bid << "\n";
      pthread_mutex_unlock( d->lock );
#endif

      pthread_barrier_wait( d->bar );
      // main thread process results

      // main thread signal to run
      pthread_barrier_wait( d->bar );
    }
  return 0;
}

void *somupdate_start( void *data )
{
  somupdate_td *d;
  d = (somupdate_td *) data;
  somupdate_threaded( d );
  return NULL;
}

int somupdate_threaded( somupdate_td *data )
{
  double a, n, d;
  pthread_barrier_wait( data->bar );
  std::vector< int > coor;
  std::map< int, double > distances;
  while( data->run == 1 )
    {
      a = (data->a);
      n = (data->n);
      int z;
      z = data->gcoorid * 4096;
      for( std::map<int, double*>::iterator it = data->todo.begin();
           it != data->todo.end(); it++ )
        {
          //update
          int mydid;
          mydid = z + it->first;
          if( distances.count( mydid ) == 0 )
            {
              data->mysom->decodecoor( it->first, coor );
              d = data->mysom->distance( *(data->gcoor), coor );
              distances[mydid] = d;
            }
          else
            {
              d = distances[mydid];
            }
          if( d < n )
            {
              double b;
              d = d/n;
              if( d > 0 )
                b = sin( d*PI )/( d*PI );
              else
                b = 1;
              b = b * a;
              double *f;
              f = *(data->f);
              for( int z = 0; z < data->vectorsz; z++ )
                {
                  it->second[z] = it->second[z] +
                    b*( f[z] - it->second[z] );
#ifdef __SOMDEBUG__
                  if( isnan( it->second[z] ) || 
                      isinf( it->second[z] ) )
                    {
                      std::cout << "ERROR som update\n";
                      std::cout << it->second[z] << " " 
                                << b << " " << a << " "
                                << it->first 
                                << " " << z << "\n";
                      std::cout << "som:";
                      for( int i = 0; i < data->vectorsz; i++ )
                        std::cout << it->second[i] << " ";
                      std::cout << "\n";
                      std::cout << "feature:";
                      for( int i = 0; i < data->vectorsz; i++ )
                        std::cout << f[i] << " ";
                      std::cout << "\n";
                      exit(0);
                    }
#endif
                  if( it->second[z] < 0 )
                    it->second[z] = 0;
                }
            }
        }
      pthread_barrier_wait( data->bar );
      // main thread process results

      // main thread signal to run
      pthread_barrier_wait( data->bar );
    }
  return 0;
}
