#include "texture.h"

unsigned int texture::generateid( int v1, int d )
{
  if( v1 < 0 || v1 >= histbins )
    std::cout << "ERROR bins1\n";
  if( d < 0 || d >= distbins )
    std::cout << "ERROR bins2\n";
  return (unsigned int) (v1 * distbins + d);
}

texture::texture()
{
  histbins = 18;
  distbins = 4;
}

texture::~texture()
{
}

int texture::print()
{
  std::cout << "c(";
  for( unsigned i = 0; i < desvec.size(); i++ )
    std::cout << desvec[i].v1 << " ";
  std::cout << ")\nc(";
  for( unsigned i = 0; i < desvec.size(); i++ )
    std::cout << desvec[i].v2 << " ";
  std::cout << ")\n";
  return 0;
}

int texture::quantize( std::vector< gaussianmodel* > gmodels )
{
  desc.clear();
  for( unsigned int i = 0; i < gmodels.size(); i++ )
    desc.push_back( 0 );
  long double contrib[100];
  long double ttt;
  matrix mt;
  mt.init( 2, 1 );
  for( unsigned int i = 0; i < desvec.size(); i++ )
    {
      ttt = 0;
      mt.set( desvec[i].v1, 0, 0 );
      mt.set( desvec[i].v2, 1, 0 );
      for( unsigned j = 0; j < gmodels.size(); j++ )
        {
          contrib[j] = gmodels[j]->prob(mt);
          ttt = ttt + contrib[j];
        }
      for( unsigned j = 0; j < gmodels.size(); j++ )
        {
          contrib[j] = contrib[j]/ttt;
          desc[j] = desc[j] + contrib[j]*desvec[i].w;
        }
    }

  return 0;
}

int texture::getvec( double *t )
{
  for( unsigned i = 0 ;i < desc.size(); i++ )
    t[i] = desc[i];
  return 0;
}

double **texture::getemdata( int &ndpts, int &nparams )
{
  double **data;
  ndpts = (int)desvec.size();
  data = new double *[ndpts];
  nparams = 2;
  for( int i = 0; i < ndpts; i++ )
    {
      data[i] = new double[nparams];
      data[i][0] = desvec[i].v1;
      data[i][1] = desvec[i].v2;
    }

  return data;
}

int texture::addvalue( double v1, double v2, double w )
{
  descriptor ds;
  ds.v1 = v1;
  ds.v2 = v2;
  ds.w = w;
  desvec.push_back( ds );
  return 0;
}

int texture::add( texture &tx )
{
  for( unsigned i = 0; i < tx.desvec.size(); i++ )
    desvec.push_back( tx.desvec[i] );
  return 0;
}

int texture::clear()
{
  desvec.clear();
  desc.clear();
  return 0;
}

int texture::init( int x, int y, int r,
                   image2d &vec, image2d &mag )
{
  xcoor = x;
  ycoor = y;
  int sx, sy, ex, ey;
  sx = x - r;
  sy = y - r;
  ex = x + r;
  ey = y + r;
  if( sx < 0 )
    sx = 0;
  if( sy < 0 )
    sy = 0;
  if( ex > vec.getheight() )
    ex = vec.getheight();
  if( ey > vec.getwidth() )
    ey = vec.getwidth();
  double mr;
  mr = sqrt( pow(sx-ex, 2) + pow(sy-ey, 2) );
  for( int x1 = sx; x1 < ex; x1++ )
    {
      for( int y1 = sy; y1 < ey; y1++ )
        {
          for( int x2 = sx; x2 < ex; x2++ )
            {
              for( int y2 = sy; y2 < ey; y2++ )
                {
                  double degree, val, dist;
                  dist = sqrt( pow(x1-x2, 2) + pow(y1-y2, 2) ); 
                  if( dist > 0 )
                    {
                      dist = dist / ( mr );
                      degree = cos( vec.getpixel(x1, y1) -
                                    vec.getpixel(x2, y2) );
                      val = acos( degree );
                      if( isnan(val) )
                        {
                          std::cout << "ERROR\n";
                    }
                      
                      addvalue( val, dist,
                                mag.getpixel(x1,y1)*
                                mag.getpixel(x2, y2) );
                    }
                }
            }
        }
    }

  return 0;
}
