#include "texturefeature.h"

int texturefeature::init( image2d **myims, image2d *mymask )
{
  ims = myims;
  mask = mymask;
  calcgradiant();
  return 0;
}

int texturefeature::calcgradiant()
{
  int i, j;
  grad.create( ims[0]->getheight(), ims[0]->getwidth() );
  imdx.create( ims[0]->getheight(), ims[0]->getwidth() );
  imdy.create( ims[0]->getheight(), ims[0]->getwidth() );

  for( i = 0; i < ims[0]->getheight(); i++ )
    {
      for( j = 0; j < ims[0]->getwidth(); j++ )
        {
          int lowi, highi;
          int lowj, highj;
          lowi = i-1;
          highi = i+1;
          lowj = j-1;
          highj = j+1;
          if( lowi < 0 )
            lowi = 0;
          if( highi == ims[0]->getheight() )
            highi = ims[0]->getheight();
          if( lowj < 0 )
            lowj = 0;
          if( highj == ims[0]->getwidth() )
            highj = ims[0]->getwidth();
          double dx, dy, dz, z;
          dx = ims[1]->getpixel( highi, j ) -
            ims[1]->getpixel( lowi, j );
          dx = dx / ( (double)( highi - lowi ) );
          dy = ims[1]->getpixel( i, highj ) -
            ims[1]->getpixel( i, lowj );
          dy = dy / ( (double)( highj - lowj ) );
          dz = (ims[0]->getpixel( i, j ) - ims[2]->getpixel( i, j ))/2.0;
          z = ( pow(dx, 2) + pow(dy, 2) + pow(dz, 2 ) )/( 3.0 );
          if( (z == 0 || isnormal( z )) )
            {
              imdx.setpixel( i, j, dx );
              imdy.setpixel( i, j, dy );
              grad.setpixel( i, j, z );
            }
          else
            {
              std::cout << "ERROR z: " << dx << " " 
                        << dy << " " << dz << " " << z << " ("
                        << i << " " << j << ") [" << lowi 
                        << " " << highi << "] [" << lowj << " " 
                        << highj << "]\n";
              exit(0);
            }
        }
    }
  return 0;
}

int texturefeature::calc( int x, int y, double *feature)
{
  int lowx, lowy, highx, highy;
  lowx = x-5;
  highx = x+5;
  lowy = y-5;
  highy = y+5;
  for( int i = 0; i < 32+6; i++ )
    feature[i] = 0;
  if( lowx < 0 || lowy < 0 || highx > grad.getheight() 
      || highy > grad.getwidth() )
    return -1;
  else
    {
      double vecx, vecy, dirmag;
      vecx = vecy = 0;
      for( int i = lowx; i < highx; i++ )
        {
          for( int j = lowy; j < highy; j++ )
            {
              double f, ip, fp;
              if( mask->getpixel(i, j) < 0.5 )
                return -2;
              for( int z = 0; z < 3; z++ )
                {
                  f = ims[z]->getpixel(i,j) + 15;
                  if( f < 0 )
                    f = 0;
                  else if( f > 31 )
                    f = 31;

                  fp = modf(f, &ip );

                  feature[ (int)ip ] = feature[(int)ip] + (1.0-fp);
                  if( (int)(ip+1) < 32 )
                    feature[ (int)(ip + 1) ] = feature[ (int)(ip + 1) ] + fp;
                }
              vecx = vecx + imdx.getpixel(i, j );
              vecy = vecy + imdy.getpixel(i, j );
            }
        }
      dirmag = sqrt( vecx*vecx + vecy*vecy );
      double angle, a;
      for( int i = lowx; i < highx; i++ )
        {
          for( int j = lowy; j < highy; j++ )
            {
              double x, y, m, ip, fp;
              x = imdx.getpixel(i,j);
              y = imdy.getpixel(i,j);
              m = sqrt( x*x + y*y );
              a = vecx*x + vecy*y;
              a = a / ( m*dirmag );
              if( fabs( a - 1.0 ) < 0.00001 )
                angle = 0;
              else if( fabs( a + 1 ) < 0.00001 )
                angle = PI;
              else
                angle = acos( a );
              if( isnan(angle) )
                {
                  std::cout << "ERROR a: "
                            << angle << " " << a << "\n";
                  exit(0);
                }
              angle = angle / PI * 5;
              fp = modf( angle, &ip );
              if( ip > 5 )
                {
                  std::cout << "ERROR angle over 5\n";
                  std::cout << "angle: " << angle 
                            << " ip: " << ip << " fp: " << fp << "\n";
                  exit(0);
                }
              feature[(int)(32+ip) ] = feature[(int)(32+ip) ] + (1.0-fp)*m;
              if( ip+1 < 6 )
                feature[(int)(32+ip+1) ] = feature[(int)(32+ip+1) ] + (fp)*m;
            }
        }
    }
  return 0;
}
