#include "Operations3D.h"

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

Operation3D::Operation3D()
{
    init = false;
    context = false;
    cMultiplier = 3;      // TODO: allow changes (by config file?)
}

void Operation3D::set ( int _x1, int _y1, int _z1, int _x2, int _y2, int _z2, int _channel1, int _channel2, int _featType )
{
    channel1 = _channel1;
    channel2 = _channel2;
    featType = _featType;
    init = true;

    unsigned short multi = 1;
    if (context)
        multi = cMultiplier;

    x1 = _x1 * multi;
    y1 = _y1 * multi;
    z1 = _z1 * multi;
    x2 = _x2 * multi;
    y2 = _y2 * multi;
    z2 = _z2 * multi;

}

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

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

void Operation3D::setWSize ( int _wsize )
{
    if (context)
        wsize = _wsize * cMultiplier;
    else
        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 (context)
            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 mode;
    is >> mode;

    if ( mode >= 0 )
    {
        if ( mode == RAWFEAT || mode == 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);
    unsigned short 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 ), channel1);

    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 ), channel1);

    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 ), channel1);

    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;
}


//######################## local statistical moments ###########################


double LocalMean3D::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 - w, y - w, z - w, x + w, y + w, z + w, channel1 );
}

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

    double mean = 0.0, var = 0.0;
    const unsigned short w = (int)floor(wsize/2.0);
    const unsigned short order = (abs(x1) % 3) + 2;
    mean = feats.feats->getIntegralValue( x - w, y - w, z - w, x + w, y + w, z + w, channel1 );

    unsigned int count = 0;
    for ( short int i = -w; i <= w ; i++ )
        for ( short int j = -w; j <= w ; j++ )
            for ( short int k = -w; k <= w ; k++, count++ )
            {
                double val = intToRaw( feats,
                                       BOUND( x+i, 0, xsize-1 ),
                                       BOUND( y+j, 0, ysize-1 ),
                                       BOUND( z+k, 0, zsize-1 ),
                                       channel1 );
                var += pow((val-mean),(double)order);
            }

    var /= (double)count;

    return var;
}


//########################### rectangle operations #############################


void IntegralOps3D::set ( int _x1, int _y1, int _z1, int _x2, int _y2, int _z2, int _channel1, int _channel2, int _featType )
{
    channel1 = _channel1;
    channel2 = _channel2;
    featType = _featType;
    init = true;

    unsigned short multi = 1;
    if (context)
        multi = cMultiplier;

    x1 = std::min ( _x1, _x2 ) * multi;
    y1 = std::min ( _y1, _y2 ) * multi;
    z1 = std::min ( _z1, _z2 ) * multi;
    x2 = std::max ( _x1, _x2 ) * multi;
    y2 = std::max ( _y1, _y2 ) * multi;
    z2 = std::max ( _z1, _z2 ) * multi;

}

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 )
{
    channel1 = _channel1;
    channel2 = _channel2;
    featType = _featType;
    init = true;

    unsigned short multi = 1;
    if (context)
        multi = cMultiplier;

    x1 = abs ( _x1 ) * multi;
    y1 = abs ( _y1 ) * multi;
    z1 = abs ( _z1 ) * multi;
    x2 = abs ( _x2 ) * multi;
    y2 = abs ( _y2 ) * multi;
    z2 = abs ( _z2 ) * multi;

}

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 )
{
    channel1 = _channel1;
    channel2 = _channel2;
    featType = _featType;
    init = true;

    unsigned short multi = 1;
    if (context)
        multi = cMultiplier;

    x1 = std::min ( abs ( _x1 ), abs ( _x2 ) ) * multi;
    y1 = std::min ( abs ( _y1 ), abs ( _y2 ) ) * multi;
    z1 = std::min ( abs ( _z1 ), abs ( _z2 ) ) * multi;
    x2 = std::max ( abs ( _x1 ), abs ( _x2 ) ) * multi;
    y2 = std::max ( abs ( _y1 ), abs ( _y2 ) ) * multi;
    z2 = std::max ( abs ( _z1 ), abs ( _z2 ) ) * multi;
}

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);
}