#include "Operations3D.h"

using namespace OBJREC;
using namespace std;
using namespace NICE;

Operation3D::Operation3D()
{
  init = false;
  context = false;
}

void Operation3D::set ( int _x1, int _y1, int _z1, int _x2, int _y2, int _z2, int _channel1, int _channel2, int _featType )
{
  x1 = _x1;
  y1 = _y1;
	z1 = _z1;
  x2 = _x2;
  y2 = _y2;
	z2 = _z2;
  channel1 = _channel1;
  channel2 = _channel2;
  featType = _featType;
  init = true;
}

void Operation3D::setContext ( bool _context )
{
  context = _context;
}

bool Operation3D::getContext()
{
  return context;
}

void Operation3D::setWSize ( int _wsize )
{
  wsize = _wsize;
}

int Operation3D::getFeatType()
{
  return featType;
}

void Operation3D::getXYZ ( const Features &feats, int &xsize, int &ysize, int &zsize )
{
  xsize = feats.feats->width();
  ysize = feats.feats->height();
	zsize = feats.feats->depth();
}

double Operation3D::intToRaw(const Features &feats, const int &posX, const int &posY, const int &posZ, int &channel)
{

    if (posX == 0){
        if (posY == 0){
            if (posZ == 0){
              return feats.feats->get (posX, posY, posZ, channel);
            }
            else{
              return feats.feats->get (posX, posY, posZ, channel) - feats.feats->get (posX, posY, posZ-1, channel);
            }
        }
        else{
            if (posZ == 0){
              return feats.feats->get (posX, posY, posZ, channel) - feats.feats->get (posX, posY-1, posZ, channel);
            }
            else{
              return feats.feats->get (posX, posY, posZ, channel) - feats.feats->get (posX, posY, posZ-1, channel) - feats.feats->get (posX, posY-1, posZ, channel) + feats.feats->get (posX, posY-1, posZ-1, channel);
            }
        }
    }
    else {
        if (posY == 0){
            if (posZ == 0){
              return feats.feats->get (posX, posY, posZ, channel) - feats.feats->get (posX-1, posY, posZ, channel);
            }
            else{
              return feats.feats->get (posX, posY, posZ, channel) - feats.feats->get (posX, posY, posZ-1, channel) - feats.feats->get (posX-1, posY, posZ, channel) + feats.feats->get (posX-1, posY, posZ-1, channel);
            }
        }
        else{
            if (posZ == 0){
              return feats.feats->get (posX, posY, posZ, channel) - feats.feats->get (posX-1, posY, posZ, channel) - feats.feats->get (posX, posY-1, posZ, channel) + feats.feats->get (posX-1, posY-1, posZ, channel);
            }
            else{
              return feats.feats->get (posX, posY, posZ, channel) - feats.feats->get (posX, posY, posZ-1, channel) - feats.feats->get (posX, posY-1, posZ, channel) - feats.feats->get (posX-1, posY, posZ, channel) + feats.feats->get (posX-1, posY-1, posZ, channel) + feats.feats->get (posX-1, posY, posZ-1, channel) + feats.feats->get (posX, posY-1, posZ-1, channel) - feats.feats->get (posX-1, posY-1, posZ-1, channel);
            }
        }
    }

}


void Operation3D::store ( std::ostream & os )
{
  os << x1 << " " << x2 << " " << y1 << " " << y2 << " " << z1 << " " << z2 << " " << channel1 << " " << channel2 << " " << featType << std::endl;
  if ( !init )
    os << -1 << std::endl;
  else
  {
    if (featType == 3 || featType == 4)
      os << CONTEXT << std::endl;
    else
      os << RAWFEAT << std::endl;
  }
}

void Operation3D::restore ( std::istream &is )
{
  is >> x1;
  is >> x2;
  is >> y1;
  is >> y2;
  is >> z1;
  is >> z2;
  is >> channel1;
  is >> channel2;
  is >> featType;

  int tmp;
  is >> tmp;

  if ( tmp >= 0 )
  {
    if ( tmp == RAWFEAT || tmp == CONTEXT )
    {
      init = true;
    }
    else
    {
      throw ( "no valid ValueAccess" );
    }
  }
  else
  {
    init = false;
  }
}

std::string Operation3D::writeInfos()
{
  std::stringstream ss;
  ss << " x1: " << x1 << " y1: " << y1 << " z1: " << z1 << " x2: " << x2 << " y2: " << y2 << " z2: " << z2 <<  " c1: " << channel1 << " c2: " << channel2;
  return ss.str();
}




//############################## region feature ###############################


// returns probability of a randomly chosen class for the region which
// the pixel (x,y,z) belongs to
double RegionFeat3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
    int currentRegion = (*feats.feats)(x,y,z,channel1);
    int chosenClass = channel2;
    return (*feats.rProbs)[currentRegion][chosenClass];
}




//############################# two gray values ###############################


double Minus3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int xsize, ysize, zsize;
  getXYZ ( feats, xsize, ysize, zsize );

  double v1 = intToRaw(feats, BOUND ( x + x1, 0, xsize - 1 ), BOUND ( y + y1, 0, ysize - 1 ), BOUND ( z + z1, 0, zsize - 1 ), channel1);
  double v2 = intToRaw(feats, BOUND ( x + x2, 0, xsize - 1 ), BOUND ( y + y2, 0, ysize - 1 ), BOUND ( z + z2, 0, zsize - 1 ), channel2);

  return v1-v2;
}

double MinusAbs3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int xsize, ysize, zsize;
  getXYZ ( feats, xsize, ysize, zsize );

  double v1 = intToRaw(feats, BOUND ( x + x1, 0, xsize - 1 ), BOUND ( y + y1, 0, ysize - 1 ), BOUND ( z + z1, 0, zsize - 1 ), channel1);
  double v2 = intToRaw(feats, BOUND ( x + x2, 0, xsize - 1 ), BOUND ( y + y2, 0, ysize - 1 ), BOUND ( z + z2, 0, zsize - 1 ), channel2);

  return abs(v1-v2);
}

double Addition3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int xsize, ysize, zsize;
  getXYZ ( feats, xsize, ysize, zsize );

  double v1 = intToRaw(feats, BOUND ( x + x1, 0, xsize - 1 ), BOUND ( y + y1, 0, ysize - 1 ), BOUND ( z + z1, 0, zsize - 1 ), channel1);
  double v2 = intToRaw(feats, BOUND ( x + x2, 0, xsize - 1 ), BOUND ( y + y2, 0, ysize - 1 ), BOUND ( z + z2, 0, zsize - 1 ), channel2);

  return v1+v2;
}




//############################## one gray value ###############################


double Only13D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int xsize, ysize, zsize;
  getXYZ ( feats, xsize, ysize, zsize );

  return intToRaw(feats, BOUND ( x + x1, 0, xsize - 1 ), BOUND ( y + y1, 0, ysize - 1 ), BOUND ( z + z1, 0, zsize - 1 ), channel1);
}




//############################ relative positions #############################


double RelativeXPosition3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int xsize, ysize, zsize;
  getXYZ ( feats, xsize, ysize, zsize );
  return ( double ) x / ( double ) xsize;
}

double RelativeYPosition3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int xsize, ysize, zsize;
  getXYZ ( feats, xsize, ysize, zsize );
  return ( double ) y / ( double ) ysize;
}

double RelativeZPosition3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int xsize, ysize, zsize;
  getXYZ ( feats, xsize, ysize, zsize );
  return ( double ) z / ( double ) zsize;
}





//########################### integral operations #############################



double GlobalFeats3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int xsize, ysize, zsize;
  getXYZ ( feats, xsize, ysize, zsize );

  return feats.feats->getIntegralValue( 0, 0, 0, xsize - 1, ysize - 1, zsize - 1, channel1 );
}

void IntegralOps3D::set ( int _x1, int _y1, int _z1, int _x2, int _y2, int _z2, int _channel1, int _channel2, int _featType )
{
  x1 = std::min ( _x1, _x2 );
  y1 = std::min ( _y1, _y2 );
  z1 = std::min ( _z1, _z2 );
  x2 = std::max ( _x1, _x2 );
  y2 = std::max ( _y1, _y2 );
  z2 = std::max ( _z1, _z2 );
  channel1 = _channel1;
  channel2 = _channel2;
  featType = _featType;
  init = true;
}

double IntegralOps3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  return feats.feats->getIntegralValue(x + x1, y + y1, z + z1, x + x2, y + y2, z + z2, channel1);
}

double BiIntegralOps3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int w = (int)floor(wsize/2.0);
  return feats.feats->getIntegralValue(x + x1, y + y1, z + z1, x + x2, y + y2, z + z2, channel1) - feats.feats->getIntegralValue( x-w, y-w, z-w, x+w, y+w, z+w, channel1 );
}

void IntegralCenteredOps3D::set ( int _x1, int _y1, int _z1, int _x2, int _y2, int _z2, int _channel1, int _channel2, int _featType )
{
  x1 = abs ( _x1 );
  y1 = abs ( _y1 );
  z1 = abs ( _z1 );
  x2 = abs ( _x2 );
  y2 = abs ( _y2 );
  z2 = abs ( _z2 );
  channel1 = _channel1;
  channel2 = _channel2;
  featType = _featType;
  init = true;
}

double IntegralCenteredOps3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  return feats.feats->getIntegralValue(x - x1, y - y1, z - z1, x + x1, y + y1, z + z1, channel1);
}

void BiIntegralCenteredOps3D::set ( int _x1, int _y1, int _z1, int _x2, int _y2, int _z2, int _channel1, int _channel2, int _featType )
{
  x1 = std::min ( abs ( _x1 ), abs ( _x2 ) );
  y1 = std::min ( abs ( _y1 ), abs ( _y2 ) );
  z1 = std::min ( abs ( _z1 ), abs ( _z2 ) );
  x2 = std::max ( abs ( _x1 ), abs ( _x2 ) );
  y2 = std::max ( abs ( _y1 ), abs ( _y2 ) );
  z2 = std::max ( abs ( _z1 ), abs ( _z2 ) );
  channel1 = _channel1;
  channel2 = _channel2;
  featType = _featType;
  init = true;
}

double BiIntegralCenteredOps3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  return feats.feats->getIntegralValue(x - x1, y - y1, z - z1, x + x1, y + y1, z + z1, channel1 ) - feats.feats->getIntegralValue(x - x2, y - y2, z - z2, x + x2, y + y2, z + z2, channel1);
}






//############################ Haar-like features #############################



double HaarHorizontal3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int tlx = x - x1;
  int tly = y - y1;
  int tlz = z - z1;
  int lrx = x + x1;
  int lry = y + y1;
  int lrz = z + z1;

  return feats.feats->getIntegralValue(tlx, tly, tlz, lrx, y, lrz, channel1 ) - feats.feats->getIntegralValue(tlx, y, tlz, lrx, lry, lrz, channel1);
}

double HaarVertical3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int tlx = x - x1;
  int tly = y - y1;
  int tlz = z - z1;
  int lrx = x + x1;
  int lry = y + y1;
  int lrz = z + z1;

  return feats.feats->getIntegralValue(tlx, tly, tlz, x, lry, lrz, channel1) - feats.feats->getIntegralValue(x, tly, tlz, lrx, lry, lrz, channel1);
}

double HaarStacked3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int tlx = x - x1;
  int tly = y - y1;
  int tlz = z - z1;
  int lrx = x + x1;
  int lry = y + y1;
  int lrz = z + z1;

  return feats.feats->getIntegralValue(tlx, tly, tlz, lrx, lry, z, channel1) - feats.feats->getIntegralValue(tlx, tly, z, lrx, lry, lrz, channel1);
}

double HaarDiagXY3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int tlx = x - x1;
  int tly = y - y1;
  int tlz = z - z1;
  int lrx = x + x1;
  int lry = y + y1;
  int lrz = z + z1;

  return feats.feats->getIntegralValue(tlx, tly, tlz, x, y, lrz, channel1) + feats.feats->getIntegralValue(x, y, tlz, lrx, lry, lrz, channel1) - feats.feats->getIntegralValue(tlx, y, tlz, x, lry, lrz, channel1) - feats.feats->getIntegralValue(x, tly, tlz, lrx, y, lrz, channel1);
}

double HaarDiagXZ3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int tlx = x - x1;
  int tly = y - y1;
  int tlz = z - z1;
  int lrx = x + x1;
  int lry = y + y1;
  int lrz = z + z1;

  return feats.feats->getIntegralValue(tlx, tly, tlz, x, lry, z, channel1) + feats.feats->getIntegralValue(x, tly, z, lrx, lry, lrz, channel1) - feats.feats->getIntegralValue(tlx, tly, z, x, lry, lrz, channel1) - feats.feats->getIntegralValue(x, tly, tlz, lrx, lry, z, channel1);
}

double HaarDiagYZ3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int tlx = x - x1;
  int tly = y - y1;
  int tlz = z - z1;
  int lrx = x + x1;
  int lry = y + y1;
  int lrz = z + z1;

  return feats.feats->getIntegralValue(tlx, tly, tlz, lrx, y, z, channel1) + feats.feats->getIntegralValue(tlx, y, z, lrx, lry, lrz, channel1) - feats.feats->getIntegralValue(tlx, tly, z, lrx, y, lrz, channel1) - feats.feats->getIntegralValue(tlx, y, tlz, lrx, lry, z, channel1);
}

double Haar3Horiz3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int tlx = x - x2;
  int tly = y - y2;
  int tlz = z - z2;
  int mtly = y - y1;
  int mlry = y + y1;
  int lrx = x + x2;
  int lry = y + y2;
  int lrz = z - z2;

  return feats.feats->getIntegralValue(tlx, tly, tlz, lrx, mtly, lrz, channel1) - feats.feats->getIntegralValue(tlx, mtly, tlz, lrx, mlry, lrz, channel1) + feats.feats->getIntegralValue(tlx, mlry, tlz, lrx, lry, lrz, channel1);
}

double Haar3Vert3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int tlx = x - x2;
  int tly = y - y2;
  int tlz = z - z2;
  int mtlx = x - x1;
  int mlrx = x + x1;
  int lrx = x + x2;
  int lry = y + y2;
  int lrz = z + z2;

  return feats.feats->getIntegralValue(tlx, tly, tlz, mtlx, lry, lrz, channel1) - feats.feats->getIntegralValue(mtlx, tly, tlz, mlrx, lry, lrz, channel1) + feats.feats->getIntegralValue(mlrx, tly, tlz, lrx, lry, lrz, channel1);
}

double Haar3Stack3D::getVal ( const Features &feats, const int &x, const int &y, const int &z )
{
  int tlx = x - x2;
  int tly = y - y2;
  int tlz = z - z2;
  int mtlz = z - z1;
  int mlrz = z + z1;
  int lrx = x + x2;
  int lry = y + y2;
  int lrz = z + z2;

  return feats.feats->getIntegralValue(tlx, tly, tlz, lrx, lry, mtlz, channel1) - feats.feats->getIntegralValue(tlx, tly, mtlz, lrx, lry, mlrz, channel1) + feats.feats->getIntegralValue(tlx, tly, mlrz, lrx, lry, lrz, channel1);
}




//############################### Ray Features ################################
double RayDiff3D::getVal( const Features &feats, const int &x, const int &y, const int &z )
{

  double dist1 = feats.feats->get( x, y, z, feats.feats->channels()-24+z1 );
  double dist2 = feats.feats->get( x, y, z, feats.feats->channels()-24+z2 );

  if (dist1 != 0)
    return (dist1-dist2)/dist1;
  else
    return 0.0;
}

double RayDist3D::getVal( const Features &feats, const int &x, const int &y, const int &z )
{
  return feats.feats->get( x, y, z, feats.feats->channels()-24+z1 );
}

double RayNorm3D::getVal( const Features &feats, const int &x, const int &y, const int &z )
{
  return feats.feats->get( x, y, z, feats.feats->channels()-16+z1 );
}

double RayOrient3D::getVal( const Features &feats, const int &x, const int &y, const int &z )
{
  return feats.feats->get( x, y, z, feats.feats->channels()-8+z1 );
}