#include "GLCP.h"

GLCP::GLCP()
{

}

GLCP::~GLCP()
{
  del();
}

int GLCP::del()
{
  for(std::map< std::pair< int, int >, double* >::iterator it = coocmatrix.begin(); it != coocmatrix.end(); it++)
    delete [] it->second;
  coocmatrix.clear();
  power.clear();
  entropy.clear();
  contrast.clear();
  correlation.clear();
  deltas.clear();
  angles.clear();
  return 0;
}

int GLCP::meanthetadelta( mask &fm, std::vector<double> &res )
{
  if( ( fm.entropy != 0 && fm.entropy != 1 ) || 
      ( fm.contrast != 0 && fm.contrast != 1 ) || 
      ( fm.correlation != 0 && fm.correlation != 1 ) || 
      ( fm.mean != 0 && fm.mean != 1 ) ||
      ( fm.power != 0 && fm.power != 1 ) )
    {
      std::cout << "mask error\n";
      exit(0);
    }
  res.clear();
  double tr[5];
  for( int i = 0; i < 5; i++ )
    tr[i] = 0;
  int c;
  c = 0;
  for( std::map< std::pair< int, int >, double >::iterator it = entropy.begin();
       it != entropy.end(); it++ )
    {
      c++;
      tr[0] = tr[0] + entropy[it->first];
      tr[1] = tr[1] + contrast[it->first];
      tr[2] = tr[2] + correlation[it->first];
      tr[3] = tr[3] + power[it->first];
    }
  tr[4] = mean;
  if( fm.entropy == 1 )
    res.push_back( tr[0] / ( (double) c ) );
  if( fm.contrast == 1 )
    res.push_back( tr[1] / ( (double) c ) );
  if( fm.correlation == 1 )
    res.push_back( tr[2] / ( (double) c ) );
  if( fm.power == 1 )
    res.push_back( tr[3] / ( (double) c ) );
  if( fm.mean == 1 )
    res.push_back( tr[4] );
  return 0;
}

int GLCP::varthetadelta( mask &fm, std::vector<double> &res )
{
  if( ( fm.entropy != 0 && fm.entropy != 1 ) || 
      ( fm.contrast != 0 && fm.contrast != 1 ) || 
      ( fm.correlation != 0 && fm.correlation != 1 ) || 
      ( fm.mean != 0 && fm.mean != 1 ) ||
      ( fm.power != 0 && fm.power != 1 ) )
    {
      std::cout << "mask error\n";
      exit(0);
    }
  res.clear();
  double tr[4];
  double zs[4];
  for( int i = 0; i < 4; i++ )
    tr[i] = zs[i] = 0;
  int c;
  c = 0;
  for( std::map< std::pair< int, int >, double >::iterator it = entropy.begin();
       it != entropy.end(); it++ )
    {
      c++;
      tr[0] = tr[0] + entropy[it->first];
      tr[1] = tr[1] + contrast[it->first];
      tr[2] = tr[2] + correlation[it->first];
      tr[3] = tr[3] + power[it->first];
    }
  for( int i = 0; i < 4; i++ )
    tr[i] = tr[i] / ( (double) c );

  for( std::map< std::pair< int, int >, double >::iterator it = entropy.begin();
       it != entropy.end(); it++ )
    {
      c++;
      zs[0] = zs[0] + pow( tr[0] - entropy[it->first], 2 );
      zs[1] = zs[1] + pow( tr[1] - contrast[it->first], 2 );
      zs[2] = zs[2] + pow( tr[2] - correlation[it->first], 2 );
      zs[3] = zs[3] + pow( tr[3] - power[it->first], 2 );
    }
  for( int i = 0; i < 4; i++ )
    zs[i] = zs[i]/( (double) (c-1) );

  if( fm.entropy == 1 )
    res.push_back( tr[0] / ( (double) c ) );
  if( fm.contrast == 1 )
    res.push_back( tr[1] / ( (double) c ) );
  if( fm.correlation == 1 )
    res.push_back( tr[2] / ( (double) c ) );
  if( fm.power == 1 )
    res.push_back( tr[3] / ( (double) c ) );
  
  return 0;
}

int GLCP::print()
{
  std::cout << "::" << centerx << " " << centery << ":: ";
  for( std::map< std::pair< int, int >, double >::iterator it = entropy.begin();
       it != entropy.end(); it++ )
    std::cout << entropy[it->first] << " " << contrast[it->first]
              << " " << correlation[it->first] << " " 
              << power[it->first] << " ";
  std::cout << "[" << size() << "]\n" << std::flush;
  return 0;
}

int GLCP::checkequal( GLCP &o )
{
  double diff;
  diff = 0;
  int exitflag;
  exitflag = 0;
  for( std::map< std::pair< int, int >, double >::iterator it = entropy.begin();
       it != entropy.end(); it++ )
    {
      if( fabs( entropy[ it->first ] - o.entropy[it->first] ) > 0.0001 )
        {
          std::cout << "[" << it->first.first << ", "
                    << it->first.second << "] "
                    << entropy[ it->first ] << " "
                    << o.entropy[ it->first ] << "\n";
          exitflag = 1;
        }
      if( fabs( contrast[ it->first ] - o.contrast[it->first] ) > 0.0001 )
        {
          std::cout << "[" << it->first.first << ", "
                    << it->first.second << "] "
                    << contrast[ it->first ] << " "
                    << o.contrast[ it->first ] << "\n";
          exitflag = 1;
        }
      if( fabs( correlation[ it->first ] - o.correlation[it->first] ) > 0.0001 )
        {
          std::cout << "[" << it->first.first << ", "
                    << it->first.second << "] "
                    << correlation[ it->first ] << " "
                    << o.correlation[ it->first ] << "\n";
          exitflag = 1;
        }
      if( fabs( power[ it->first ] - o.power[it->first] ) > 0.0001 )
        {
          std::cout << "[" << it->first.first << ", "
                    << it->first.second << "] "
                    << power[ it->first ] << " "
                    << o.power[ it->first ] << "\n";
          exitflag = 1;
        }
    }
  return diff;
}

int GLCP::distance( GLCP &o )
{
  double diff;
  diff = 0;
  for( std::map< std::pair< int, int >, double >::iterator it = entropy.begin();
       it != entropy.end(); it++ )
    {
      diff = diff + pow( entropy[ it->first ] - o.entropy[it->first], 2 );
      diff = diff + pow( contrast[ it->first ] - o.contrast[it->first], 2 );
      diff = diff + pow( correlation[ it->first ] - o.correlation[it->first], 2 );
      diff = diff + pow( power[ it->first ] - o.power[it->first], 2 );
    }
  return diff;
}

int GLCP::size()
{
  return entropy.size()*4+1;
}

int GLCP::getfeaturevector( std::vector< double > &f )
{
  for( std::map< std::pair< int, int >, double >::iterator it = entropy.begin();
       it != entropy.end(); it++ )
    {
      f.push_back( entropy[it->first] );
      f.push_back( contrast[it->first] );
      f.push_back( correlation[it->first]);
      f.push_back( power[it->first] );
    }
  f.push_back( mean );
  return 0;
}

int GLCP::getfeaturevector( double* f )
{
  int i;
  i = 0;
  for( std::map< std::pair< int, int >, double >::iterator it = entropy.begin();
       it != entropy.end(); it++ )
    {
      f[i] = entropy[it->first];
      i++;
      f[i] = contrast[it->first];
      i++;
      f[i] = correlation[it->first];
      i++;
      f[i] = power[it->first];
      i++;
    }
  f[i] = mean;
  i++;
  return 0;
}

int GLCP::getfeaturets( std::vector< double* > &f, int &sz )
{
  sz = (deltas.size()*4) + 1;
  for( int t = 0; t < 1800; t = t + 225 )
    {
      double *nf;
      int i;
      i = 0;
      nf =  new double[sz];
      for( std::set<int>::iterator it = deltas.begin(); it != deltas.end(); it++ )
        {
          std::pair< int, int > cur;
          cur.first = *it;
          cur.second = t;
          nf[i] = entropy[ cur ];
          i++;
          nf[i] = contrast[ cur ];
          i++;
          nf[i] = correlation[ cur ];
          i++;
          nf[i] = power[ cur ];
          i++;
        }
      nf[i] = mean;
      i++;
      f.push_back(nf);
      if( i != sz )
        {
          std::cout << "get featurets size check failed\n";
          exit(0);
        }
    }
  return 0;
}

int GLCP::getfeaturetsavg( std::vector< double* > &f, mask &fm, int &sz )
{
  if( ( fm.entropy != 0 && fm.entropy != 1 ) || 
      ( fm.contrast != 0 && fm.contrast != 1 ) || 
      ( fm.correlation != 0 && fm.correlation != 1 ) || 
      ( fm.mean != 0 && fm.mean != 1 ) ||
      ( fm.power != 0 && fm.power != 1 ) )
    {
      std::cout << "mask error\n";
      exit(0);
    }
  int fmsum;
  fmsum = fm.entropy + fm.contrast + fm.correlation + fm.power;
  sz = fmsum + fm.mean;

  for( int t = 0; t < 1800; t = t + 225 )
    {
      int tt;
      tt = f.size();
      f.push_back( new double[ sz ] );
      int i;
      i = 0;

      for( int j = 0; j < sz; j++ )
        f[tt][j] = 0;

      for( std::set<int>::iterator it = deltas.begin();
           it != deltas.end(); it++ )
        {
          std::pair< int, int > cur;
          cur.first = *it;
          cur.second = t;
#ifdef __DEBUG__
          if( entropy.count( cur ) != 1 )
            {
              std::cout << "ERROR entropy map does not contain "
                        << cur.first << " " << cur.second << "\n";
              exit(0);
            }
          if( contrast.count( cur ) != 1 )
            {
              std::cout << "ERROR contrast map does not contain "
                        << cur.first << " " << cur.second << "\n";
              exit(0);
            }
          if( correlation.count( cur ) != 1 )
            {
              std::cout << "ERROR correlation map does not contain "
                        << cur.first << " " << cur.second << "\n";
              exit(0);
            }
          if( power.count( cur ) != 1 )
            {
              std::cout << "ERROR power map does not contain "
                        << cur.first << " " << cur.second << "\n";
              exit(0);
            }
#endif
          if( fm.entropy == 1 )
            {
              f[tt][i] = f[tt][i] + entropy[ cur ];
              i++;
            }
          if( fm.contrast == 1 )
            {
              f[tt][i] = f[tt][i] + contrast[ cur ];
              i++;
            }
          if( fm.correlation == 1 )
            {
              f[tt][i] = f[tt][i] + correlation[ cur ];
              i++;
            }
          if( fm.power == 1 )
            {
              f[tt][i] = f[tt][i] + power[ cur ];
              i++;
            }
        }
      if( fm.mean == 1 )
        {
          f[tt][i] = f[tt][i] + mean;
          i++;
        }

      if( i != sz )
        {
          std::cout << "get featurets size check failed\n";
          std::cout << "expect " << sz << " got " << i << "\n";
          std::cout << "entropy: " << fm.entropy << "\ncontrast: "
                    << fm.contrast << "\ncorrelation: " << fm.correlation 
                    << "\npower: " << fm.power << "\nmean: " << fm.mean << "\n";
          std::cout << "num deltas: " << deltas.size() << "\n";
          exit(0);
        }
      for( int j = 0; j < sz-fm.mean; j++ )
        f[tt][j] = f[tt][j]/(deltas.size());
    }
  return 0;
}

int GLCP::getfeaturets( std::vector< double* > &f, mask &fm, int &sz )
{
  if( ( fm.entropy != 0 && fm.entropy != 1 ) || 
      ( fm.contrast != 0 && fm.contrast != 1 ) || 
      ( fm.correlation != 0 && fm.correlation != 1 ) || 
      ( fm.mean != 0 && fm.mean != 1 ) ||
      ( fm.power != 0 && fm.power != 1 ) )
    {
      std::cout << "mask error\n";
      exit(0);
    }
  int fmsum;
  fmsum = fm.entropy + fm.contrast + fm.correlation + fm.power;
  sz = (fmsum*deltas.size()) + fm.mean;
  for( int t = 0; t < 1800; t = t + 225 )
    {
      double *nf;
      int i;
      i = 0;
      nf =  new double[sz];
      for( std::set<int>::iterator it = deltas.begin(); it != deltas.end(); it++ )
        {
          std::pair< int, int > cur;
          cur.first = *it;
          cur.second = t;
          if( entropy.count( cur ) != 1 )
            {
              std::cout << "ERROR entropy map does not contain "
                        << cur.first << " " << cur.second << "\n";
              exit(0);
            }
          if( fm.entropy == 1 )
            {
              nf[i] = entropy[ cur ];
              i++;
            }
          if( contrast.count( cur ) != 1 )
            {
              std::cout << "ERROR contrast map does not contain "
                        << cur.first << " " << cur.second << "\n";
              exit(0);
            }
          if( fm.contrast == 1 )
            {
              nf[i] = contrast[ cur ];
              i++;
            }
          if( correlation.count( cur ) != 1 )
            {
              std::cout << "ERROR correlation map does not contain "
                        << cur.first << " " << cur.second << "\n";
              exit(0);
            }
          if( fm.correlation == 1 )
            {
              nf[i] = correlation[ cur ];
              i++;
            }
          if( power.count( cur ) != 1 )
            {
              std::cout << "ERROR power map does not contain "
                        << cur.first << " " << cur.second << "\n";
              exit(0);
            }
          if( fm.power == 1 )
            {
              nf[i] = power[ cur ];
              i++;
            }
        }
      if( fm.mean == 1 )
        {
          nf[i] = mean;
          i++;
        }
      f.push_back(nf);
      if( i != sz )
        {
          std::cout << "get featurets size check failed\n";
          std::cout << "expect " << sz << " got " << i << "\n";
          std::cout << "entropy: " << fm.entropy << "\ncontrast: "
                    << fm.contrast << "\ncorrelation: " << fm.correlation 
                    << "\npower: " << fm.power << "\nmean: " << fm.mean << "\n";
          std::cout << "num deltas: " << deltas.size() << "\n";
          exit(0);
        }
    }
  return 0;
}

int GLCP::getfeaturets( std::vector< matrix > &f, mask &fm, int &sz )
{
  if( ( fm.entropy != 0 && fm.entropy != 1 ) || 
      ( fm.contrast != 0 && fm.contrast != 1 ) || 
      ( fm.correlation != 0 && fm.correlation != 1 ) || 
      ( fm.mean != 0 && fm.mean != 1 ) ||
      ( fm.power != 0 && fm.power != 1 ) )
    {
      std::cout << "mask error\n";
      exit(0);
    }
  int fmsum;
  fmsum = fm.entropy + fm.contrast + fm.correlation + fm.power;
  sz = (fmsum*deltas.size()) + fm.mean;

  f.clear();
  for( int t = 0; t < 1800; t = t + 225 )
    {
      matrix nf;
      int i;
      i = 0;
      nf.init( sz, 1 );
      for( std::set<int>::iterator it = deltas.begin(); it != deltas.end(); it++ )
        {
          std::pair< int, int > cur;
          cur.first = *it;
          cur.second = t;
          if( entropy.count( cur ) != 1 )
            {
              std::cout << "ERROR entropy map does not contain "
                        << cur.first << " " << cur.second << "\n";
              exit(0);
            }
          if( fm.entropy == 1 )
            {
              nf.set(entropy[ cur ], i, 0 );
              i++;
            }
          if( contrast.count( cur ) != 1 )
            {
              std::cout << "ERROR contrast map does not contain "
                        << cur.first << " " << cur.second << "\n";
              exit(0);
            }
          if( fm.contrast == 1 )
            {
              nf.set( contrast[cur], i, 0 );
              i++;
            }
          if( correlation.count( cur ) != 1 )
            {
              std::cout << "ERROR correlation map does not contain "
                        << cur.first << " " << cur.second << "\n";
              exit(0);
            }
          if( fm.correlation == 1 )
            {
              nf.set( correlation[cur], i, 0);
              i++;
            }
          if( power.count( cur ) != 1 )
            {
              std::cout << "ERROR power map does not contain "
                        << cur.first << " " << cur.second << "\n";
              exit(0);
            }
          if( fm.power == 1 )
            {
              nf.set( power[ cur ], i, 0);
              i++;
            }
        }
      if( fm.mean == 1 )
        {
          nf.set( mean, i, 0 );
          i++;
        }
      f.push_back(nf);
      if( i != sz )
        {
          std::cout << "get featurets size check failed\n";
          std::cout << "expect " << sz << " got " << i << "\n";
          std::cout << "entropy: " << fm.entropy << "\ncontrast: "
                    << fm.contrast << "\ncorrelation: " << fm.correlation 
                    << "\npower: " << fm.power << "\nmean: " << fm.mean << "\n";
          std::cout << "num deltas: " << deltas.size() << "\n";
          exit(0);
        }
    }
  return 0;
}

int GLCP::serialize( databuffer &buf )
{
  buf.clear();
  int numangles, numdeltas;
  numangles = angles.size();
  numdeltas = deltas.size();

  buf.append( (char*)(&numangles), sizeof(int) );
  buf.append( (char*)(&numdeltas), sizeof(int) );
  buf.append( (char*)(&mean), sizeof(double) );

  for( std::set<int>::iterator dit = deltas.begin();
       dit != deltas.end(); dit++ )
    {
      for( std::set<int>::iterator ait = angles.begin();
           ait != angles.end(); ait++ )
        {
          int tmp;
          tmp = *dit;
          buf.append( (char*)(&tmp), sizeof(int) );
          tmp = *ait;
          buf.append( (char*)(&tmp), sizeof(int) );
          std::pair< int, int > p;

          p.first = *dit;
          p.second = *ait;

          double tz;
          tz = entropy[p];
          buf.append( (char*)(&tz), sizeof(double) );
          tz = contrast[p];
          buf.append( (char*)(&tz), sizeof(double) );
          tz = correlation[p];
          buf.append( (char*)(&tz), sizeof(double) );
          tz = power[p];
          buf.append( (char*)(&tz), sizeof(double) );
        }
    }

  buf.consolidate();
  return 0;
}

int GLCP::deserialize( char *buf, int sz )
{
  del();
  int numangles, numdeltas;
  memcpy(&numangles, buf, sizeof(int));
  memcpy(&numdeltas, buf+sizeof(int), sizeof(int));

  memcpy(&mean, buf+(2*sizeof(int)), sizeof(double));

  char *curpos;
  curpos = buf+(2*sizeof(int)+sizeof(double));

  for( int dit = 0; dit < numdeltas; dit++ )
    {
      for( int ait = 0; ait < numangles; ait++ )
        {
          int tmp, curt, curd;


          memcpy( &curd, curpos, sizeof(int) );
          curpos = curpos+sizeof(int);
          memcpy( &curt, curpos, sizeof(int) );
          curpos = curpos+sizeof(int);

          std::pair< int, int > p;
          p.first = curd;
          p.second = curt;
          angles.insert( curt );
          deltas.insert( curd );
          double tz;
          memcpy( &tz, curpos, sizeof(double) );
          curpos = curpos+sizeof(double);
          entropy[p] = tz;

          memcpy( &tz, curpos, sizeof(double) );
          contrast[p] = tz;
          curpos = curpos+sizeof(double);

          memcpy( &tz, curpos, sizeof(double) );
          correlation[p] = tz;
          curpos = curpos+sizeof(double);
          
          memcpy( &tz, curpos, sizeof(double) );
          power[p] = tz;
          curpos = curpos+sizeof(double);
        }
    }
#ifdef __DEBUG__
  std::cout << "loaded angles: ";
  for( std::set<int>::iterator a = angles.begin();
       a != angles.end(); a++ )
    std::cout << *a << " ";
  
  std::cout << "\nthetas: ";
  for( std::set<int>::iterator a = deltas.begin();
       a != deltas.end(); a++ )
    std::cout << *a << " ";
  std::cout << "\n";
  std::cout << "loaded " << power.size() << " components\n";
#endif
  char *chpos;
  chpos = buf + sz;
  if( chpos != curpos )
    {
      std::cout << "ERROR size does not match\n";
      exit(0);
    }
  return 0;
}

int GLCP::init( image2d *im, int cx, int cy, int w )
{
  myim = im;
  del();
  centerx = cx;
  centery = cy;
  window = w;
  int hw = window/2;
  lowx = cx - hw;
  highx = cx + hw;
  lowy = cy - hw;
  highy = cy + hw;
  if( lowx < 0 || highx >= im->getheight() ||
      lowy < 0 || highy >= im->getwidth() )
    return -1;

  for( int delta = 2; delta < w/2.0; delta = delta + 2 )
    {
      deltas.insert( delta );
      for( int theta = 0; theta < 1800; theta=theta+225)
        {
          angles.insert( theta );
          double *cf;
          std::pair< int, int > deltatheta;
          cf = new double[ (int)(GLCPGRAY*GLCPGRAY) ];
          calccomatrix( im, delta, ((double)theta)/10.0, cf );
          deltatheta.first = delta;
          deltatheta.second = theta;
          coocmatrix[ deltatheta ] = cf;
        }
    }

  calcentropy();
  calccontrast();
  calccorrelation();
  calcpower();
  calcmean();
  return 0;
}

int GLCP::calcentropy()
{
  for(std::map< std::pair< int, int >, double* >::iterator it = coocmatrix.begin(); it != coocmatrix.end(); it++)
    {
      double v;
      v = calcentropy( it->second );
      entropy[it->first] = v;
    }
  return 0;
}

double GLCP::calcentropy( double *p )
{
  double e;
  e = 0;
  for( int i = 0; i < GLCPGRAY*GLCPGRAY; i++ )  
    if( p[i] > 0 )
      e = e + p[i]*log( p[i] );
  e = -e;
  return e;
}

int GLCP::calccontrast()
{
  for(std::map< std::pair< int, int >, double* >::iterator it = coocmatrix.begin(); it != coocmatrix.end(); it++)
    {
      double v;
      v = calccontrast( it->second );
      contrast[it->first] = v;
    }
  return 0;
}

double GLCP::calccontrast( double *p )
{
  double e;
  e = 0;
  for( int i = 0; i < GLCPGRAY; i++ )
    for( int j = 0; j < GLCPGRAY; j++ )
      e = e + p[ i*(int)GLCPGRAY+j ]*pow( i - j, 2 );
  return e;
}

int GLCP::calcpower()
{
  for(std::map< std::pair< int, int >, double* >::iterator it = coocmatrix.begin(); it != coocmatrix.end(); it++)
    {
      double v;
      v = calcpower( it->second );
      power[it->first] = v;
    }
  return 0;
}

double GLCP::calcpower( double *p )
{
  double e;
  e = 0;
  for( int i = 0; i < GLCPGRAY; i++ )
    for( int j = 0; j < GLCPGRAY; j++ )
      e = e + pow( p[ i*(int)GLCPGRAY+j ], 2 );
  return e;
}

int GLCP::calccorrelation()
{
 for(std::map< std::pair< int, int >, double* >::iterator it = coocmatrix.begin(); it != coocmatrix.end(); it++)
   {
      double v;
      v = calccorrelation( it->second );
      correlation[it->first] = v;
   }
 return 0;
}

double GLCP::calccorrelation( double *p )
{
  double e;
  e = 0;
  double ui, uj, si, sj;
  double count;
  ui = uj = si = sj = 0;
  count = 0;
  for( int i = 0; i < GLCPGRAY; i++ )
    {
      for( int j = 0; j < GLCPGRAY; j++ )
        {
          ui = ui + p[i*(int)GLCPGRAY+j] * i;
          uj = uj + p[i*(int)GLCPGRAY+j] * j;
          count = count + p[i*(int)GLCPGRAY+j];
        }
    }
  if( fabs( count - 1.0 ) > 0.00001 )
    {
      std::cout << "ERROR joint cdf sum > 1\n";
      std::cout << count << "\n";
      exit(0);
    }
  for( int i = 0; i < GLCPGRAY; i++ )
    {
      for( int j = 0; j < GLCPGRAY; j++ )
        {
          si = si + p[i*(int)GLCPGRAY+j] * pow( ui - ((double)i), 2);
          sj = sj + p[i*(int)GLCPGRAY+j] * pow( uj - ((double)j), 2);
          count = count + p[i*(int)GLCPGRAY+j];
        }
    }  
  si = pow( si, 0.5 );
  sj = pow( sj, 0.5 );
  for( int i = 0; i < GLCPGRAY; i++ )
    for( int j = 0; j < GLCPGRAY; j++ )
      e = e + ( ((double)i-ui)*((double)j-uj)*p[i*(int)GLCPGRAY+j] )/( si*sj );
  
  return e;
}

int GLCP::calcmean()
{
  double res;
  int count;
  count = 0;
  res = 0;

  for( int i = lowx; i <= highx; i++ )
    {
      for( int j = lowy; j <= highy; j++ )
        {
          res = res + (GLCPGRAY-1.0)*myim->getpixel(i, j);
          count = count + 1;
        }
    }

  mean = res/( (double) count );
  return 0;
}

int GLCP::calccomatrix( image2d *im,  double delta, 
                        double theta, double *cf )
{
  int pxc;
  pxc = 0;
  double dx, dy;
  double radian_t;
  radian_t = (theta*2.0*PI)/360.0;
  double sr = sin(radian_t);
  double cr = cos(radian_t);
  dx = cr*delta;
  dy = sr*delta;

  for( int i = 0; i < GLCPGRAY*GLCPGRAY; i++ )
    cf[i] = 0;
 
  for( int i = lowx; i <= highx; i++ )
    {
      for( int j = lowy; j <= highy; j++ )
        {
          double di, dj;
          di = i + dx;
          dj = j + dy;
          if( lowx <= di && di <= highx &&
              lowy <= dj && dj <= highy )
            {
              double v1, v2;
              v1 = (GLCPGRAY-1.0)*im->getpixelb(i, j);
              v2 = (GLCPGRAY-1.0)*im->getpixelb(di, dj);
              if( isnan(v1) || isnan(v2) )
                {
                  std::cout << "v1 v2 error\n";
                  std::cout << v1 << " " << v2 << "\n";
                }
              addtoglcp( v1, v2, cf );
              addtoglcp( v2, v1, cf );
              pxc = pxc + 2;
            }
        }
    }

  for( int i = 0; i < GLCPGRAY*GLCPGRAY; i++ )
    {
      if( !isnormal( cf[i] ) && cf[i] != 0 )
        {
          std::cout << "ERROR glcp generation prenormalize\n";
          std::cout << cf[i] << "\n";
          exit(0);
        }
    }

  for( int i = 0; i < GLCPGRAY*GLCPGRAY; i++ )
    cf[i] = cf[i]/( (double)pxc );

  double fin;
  fin = 0;
  for( int i = 0; i < GLCPGRAY*GLCPGRAY; i++ )
    {
      if( !isnormal( cf[i] ) && cf[i] != 0 )
        {
          std::cout << "ERROR glcp generation\n";
          std::cout << cf[i] << "\n";
          std::cout << pxc << "\n";
          std::cout << "delta: " << delta << "\n";
          std::cout << "theta: " << theta << "\n";
          exit(0);
        }
      fin = fin + cf[i];
    }
  if( fabs( 1.0 - fin ) > 0.00001 )
    {
      std::cout << "ERROR glcp generation not normalized\n";
      std::cout << fin << "\n";
      exit(0);
    }
  return 0;
}

int GLCP::addtoglcp( double v1, double v2, double *cf )
{
  double fv1, fv2, iv1, iv2;
  fv1 = modf( v1, &iv1 );
  fv2 = modf( v2, &iv2 );

  cf[((int)iv1)*(int)GLCPGRAY+((int)iv2)] +=
                (1.0-fv1)*(1.0-fv2);

  if( ((int)v1) + 1 < GLCPGRAY )
    cf[((int)iv1+1)*(int)GLCPGRAY+((int)iv2)] +=
      (fv1)*(1.0-fv2);

  if( ((int)v2) + 1 < GLCPGRAY )
    cf[((int)iv1)*(int)GLCPGRAY+((int)iv2+1)] +=
      (1.0-fv1)*(fv2);

  if( ((int)v1) + 1 < GLCPGRAY &&
      ((int)v2) + 1 < GLCPGRAY )
    cf[((int)iv1+1)*(int)GLCPGRAY+((int)iv2+1)] +=
      fv1*fv2;

  return 0;
}
