//////////////////////////////////////////////////////////////////////////////
// Example illustrating the use of GCoptimization.cpp
//
/////////////////////////////////////////////////////////////////////////////
//
//  Optimization problem:
//  is a set of sites (pixels) of width 10 and hight 5. Thus number of pixels is 50
//  grid neighborhood: each pixel has its left, right, up, and bottom pixels as neighbors
//  7 labels
//  Data costs: D(pixel,label) = 0 if pixel < 25 and label = 0
//            : D(pixel,label) = 10 if pixel < 25 and label is not  0
//            : D(pixel,label) = 0 if pixel >= 25 and label = 5
//            : D(pixel,label) = 10 if pixel >= 25 and label is not  5
// Smoothness costs: V(p1,p2,l1,l2) = min( (l1-l2)*(l1-l2) , 4 )
// Below in the main program, we illustrate different ways of setting data and smoothness costs
// that our interface allow and solve this optimizaiton problem

// For most of the examples, we use no spatially varying pixel dependent terms.
// For some examples, to demonstrate spatially varying terms we use
// V(p1,p2,l1,l2) = w_{p1,p2}*[min((l1-l2)*(l1-l2),4)], with
// w_{p1,p2} = p1+p2 if |p1-p2| == 1 and w_{p1,p2} = p1*p2 if |p1-p2| is not 1

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <time.h>
#include "GCoptimization.h"

using namespace OBJREC;


struct ForDataFn {
  int numLab;
  Graph::captype *data;
};


Graph::captype smoothFn(int p1, int p2, int l1, int l2)
{
  if ( (l1 - l2)*(l1 - l2) <= 4 ) return((l1 -l2)*(l1 - l2));
  else return(4);
}

Graph::captype dataFn(int p, int l, void *data)
{
  ForDataFn *myData = (ForDataFn *) data;
  int numLab = myData->numLab;

  return( myData->data[p*numLab+l] );
}



////////////////////////////////////////////////////////////////////////////////
// smoothness and data costs are set up one by one, individually
// grid neighborhood structure is assumed
//
void GridGraph_Individually(int width, int height, int num_pixels, int num_labels)
{

  int *result = new int[num_pixels];   // stores result of optimization



  try {
    GCoptimizationGridGraph *gc = new GCoptimizationGridGraph(width, height, num_labels);

    // first set up data costs individually
    for ( int i = 0; i < num_pixels; i++ )
      for (int l = 0; l < num_labels; l++ )
        if (i < 25 ) {
          if (  l == 0 ) gc->setDataCost(i, l, 0);
          else gc->setDataCost(i, l, 10);
        }
        else {
          if (  l == 5 ) gc->setDataCost(i, l, 0);
          else gc->setDataCost(i, l, 10);
        }

    // next set up smoothness costs individually
    for ( int l1 = 0; l1 < num_labels; l1++ )
      for (int l2 = 0; l2 < num_labels; l2++ ) {
        int cost = (l1 - l2) * (l1 - l2) <= 4  ? (l1 - l2) * (l1 - l2) : 4;
        gc->setSmoothCost(l1, l2, cost);
      }

    printf("\nBefore optimization energy is %d", gc->compute_energy());
    gc->expansion(2);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
    printf("\nAfter optimization energy is %d", gc->compute_energy());

    for ( int  i = 0; i < num_pixels; i++ )
      result[i] = gc->whatLabel(i);

    delete gc;
  }
  catch (GCException e) {
    e.Report();
  }

  delete [] result;
}

////////////////////////////////////////////////////////////////////////////////
// in this version, set data and smoothness terms using arrays
// grid neighborhood structure is assumed
//
void GridGraph_DArraySArray(int width, int height, int num_pixels, int num_labels)
{

  int *result = new int[num_pixels];   // stores result of optimization

  // first set up the array for data costs
  Graph::captype *data = new Graph::captype[num_pixels*num_labels];
  for ( int i = 0; i < num_pixels; i++ )
    for (int l = 0; l < num_labels; l++ )
      if (i < 25 ) {
        if (  l == 0 ) data[i*num_labels+l] = 0;
        else data[i*num_labels+l] = 10;
      }
      else {
        if (  l == 5 ) data[i*num_labels+l] = 0;
        else data[i*num_labels+l] = 10;
      }
  // next set up the array for smooth costs
  Graph::captype *smooth = new Graph::captype[num_labels*num_labels];
  for ( int l1 = 0; l1 < num_labels; l1++ )
    for (int l2 = 0; l2 < num_labels; l2++ )
      smooth[l1+l2*num_labels] = (l1 - l2) * (l1 - l2) <= 4  ? (l1 - l2) * (l1 - l2) : 4;


  try {
    GCoptimizationGridGraph *gc = new GCoptimizationGridGraph(width, height, num_labels);
    gc->setDataCost(data);
    gc->setSmoothCost(smooth);
    printf("\nBefore optimization energy is %d", gc->compute_energy());
    gc->expansion(2);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
    printf("\nAfter optimization energy is %d", gc->compute_energy());

    for ( int  i = 0; i < num_pixels; i++ )
      result[i] = gc->whatLabel(i);

    delete gc;
  }
  catch (GCException e) {
    e.Report();
  }

  delete [] result;
  delete [] smooth;
  delete [] data;

}
////////////////////////////////////////////////////////////////////////////////
// in this version, set data and smoothness terms using arrays
// grid neighborhood structure is assumed
//
void GridGraph_DfnSfn(int width, int height, int num_pixels, int num_labels)
{

  int *result = new int[num_pixels];   // stores result of optimization

  // first set up the array for data costs
  Graph::captype *data = new Graph::captype[num_pixels*num_labels];
  for ( int i = 0; i < num_pixels; i++ )
    for (int l = 0; l < num_labels; l++ )
      if (i < 25 ) {
        if (  l == 0 ) data[i*num_labels+l] = 0;
        else data[i*num_labels+l] = 10;
      }
      else {
        if (  l == 5 ) data[i*num_labels+l] = 0;
        else data[i*num_labels+l] = 10;
      }


  try {
    GCoptimizationGridGraph *gc = new GCoptimizationGridGraph(width, height, num_labels);

    // set up the needed data to pass to function for the data costs
    ForDataFn toFn;
    toFn.data = data;
    toFn.numLab = num_labels;

    gc->setDataCost(&dataFn, &toFn);

    // smoothness comes from function pointer
    gc->setSmoothCost(&smoothFn);

    printf("\nBefore optimization energy is %d", gc->compute_energy());
    gc->expansion(2);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
    printf("\nAfter optimization energy is %d", gc->compute_energy());

    for ( int  i = 0; i < num_pixels; i++ )
      result[i] = gc->whatLabel(i);

    delete gc;
  }
  catch (GCException e) {
    e.Report();
  }

  delete [] result;
  delete [] data;

}
////////////////////////////////////////////////////////////////////////////////
// Uses spatially varying smoothness terms. That is
// V(p1,p2,l1,l2) = w_{p1,p2}*[min((l1-l2)*(l1-l2),4)], with
// w_{p1,p2} = p1+p2 if |p1-p2| == 1 and w_{p1,p2} = p1*p2 if |p1-p2| is not 1
void GridGraph_DArraySArraySpatVarying(int width, int height, int num_pixels, int num_labels)
{
  int *result = new int[num_pixels];   // stores result of optimization

  // first set up the array for data costs
  Graph::captype *data = new Graph::captype[num_pixels*num_labels];
  for ( int i = 0; i < num_pixels; i++ )
    for (int l = 0; l < num_labels; l++ )
      if (i < 25 ) {
        if (  l == 0 ) data[i*num_labels+l] = 0;
        else data[i*num_labels+l] = 10;
      }
      else {
        if (  l == 5 ) data[i*num_labels+l] = 0;
        else data[i*num_labels+l] = 10;
      }
  // next set up the array for smooth costs
  Graph::captype *smooth = new Graph::captype[num_labels*num_labels];
  for ( int l1 = 0; l1 < num_labels; l1++ )
    for (int l2 = 0; l2 < num_labels; l2++ )
      smooth[l1+l2*num_labels] = (l1 - l2) * (l1 - l2) <= 4  ? (l1 - l2) * (l1 - l2) : 4;

  // next set up spatially varying arrays V and H

  Graph::captype *V = new Graph::captype[num_pixels];
  Graph::captype *H = new Graph::captype[num_pixels];


  for ( int i = 0; i < num_pixels; i++ ) {
    H[i] = i + (i + 1) % 3;
    V[i] = i * (i + width) % 7;
  }


  try {
    GCoptimizationGridGraph *gc = new GCoptimizationGridGraph(width, height, num_labels);
    gc->setDataCost(data);
    gc->setSmoothCostVH(smooth, V, H);
    printf("\nBefore optimization energy is %d", gc->compute_energy());
    gc->expansion(2);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
    printf("\nAfter optimization energy is %d", gc->compute_energy());

    for ( int  i = 0; i < num_pixels; i++ )
      result[i] = gc->whatLabel(i);

    delete gc;
  }
  catch (GCException e) {
    e.Report();
  }

  delete [] result;
  delete [] smooth;
  delete [] data;


}

////////////////////////////////////////////////////////////////////////////////
// in this version, set data and smoothness terms using arrays
// grid neighborhood is set up "manually"
//
void GeneralGraph_DArraySArray(int width, int height, int num_pixels, int num_labels)
{

  int *result = new int[num_pixels];   // stores result of optimization

  // first set up the array for data costs
  Graph::captype *data = new Graph::captype[num_pixels*num_labels];
  for ( int i = 0; i < num_pixels; i++ )
    for (int l = 0; l < num_labels; l++ )
      if (i < 25 ) {
        if (  l == 0 ) data[i*num_labels+l] = 0;
        else data[i*num_labels+l] = 10;
      }
      else {
        if (  l == 5 ) data[i*num_labels+l] = 0;
        else data[i*num_labels+l] = 10;
      }
  // next set up the array for smooth costs
  Graph::captype *smooth = new Graph::captype[num_labels*num_labels];
  for ( int l1 = 0; l1 < num_labels; l1++ )
    for (int l2 = 0; l2 < num_labels; l2++ )
      smooth[l1+l2*num_labels] = (l1 - l2) * (l1 - l2) <= 4  ? (l1 - l2) * (l1 - l2) : 4;


  try {
    GCoptimizationGeneralGraph *gc = new GCoptimizationGeneralGraph(num_pixels, num_labels);
    gc->setDataCost(data);
    gc->setSmoothCost(smooth);

    // now set up a grid neighborhood system
    // first set up horizontal neighbors
    for (int y = 0; y < height; y++ )
      for (int  x = 1; x < width; x++ )
        gc->setNeighbors(x + y*width, x - 1 + y*width);

    // next set up vertical neighbors
    for (int y = 1; y < height; y++ )
      for (int  x = 0; x < width; x++ )
        gc->setNeighbors(x + y*width, x + (y - 1)*width);

    printf("\nBefore optimization energy is %d", gc->compute_energy());
    gc->expansion(2);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
    printf("\nAfter optimization energy is %d", gc->compute_energy());

    for ( int  i = 0; i < num_pixels; i++ )
      result[i] = gc->whatLabel(i);

    delete gc;
  }
  catch (GCException e) {
    e.Report();
  }

  delete [] result;
  delete [] smooth;
  delete [] data;

}
////////////////////////////////////////////////////////////////////////////////
// in this version, set data and smoothness terms using arrays
// grid neighborhood is set up "manually". Uses spatially varying terms. Namely
// V(p1,p2,l1,l2) = w_{p1,p2}*[min((l1-l2)*(l1-l2),4)], with
// w_{p1,p2} = p1+p2 if |p1-p2| == 1 and w_{p1,p2} = p1*p2 if |p1-p2| is not 1

void GeneralGraph_DArraySArraySpatVarying(int width, int height, int num_pixels, int num_labels)
{
  int *result = new int[num_pixels];   // stores result of optimization

  // first set up the array for data costs
  Graph::captype *data = new Graph::captype[num_pixels*num_labels];
  for ( int i = 0; i < num_pixels; i++ )
    for (int l = 0; l < num_labels; l++ )
      if (i < 25 ) {
        if (  l == 0 ) data[i*num_labels+l] = 0;
        else data[i*num_labels+l] = 10;
      }
      else {
        if (  l == 5 ) data[i*num_labels+l] = 0;
        else data[i*num_labels+l] = 10;
      }
  // next set up the array for smooth costs
  Graph::captype *smooth = new Graph::captype[num_labels*num_labels];
  for ( int l1 = 0; l1 < num_labels; l1++ )
    for (int l2 = 0; l2 < num_labels; l2++ )
      smooth[l1+l2*num_labels] = (l1 - l2) * (l1 - l2) <= 4  ? (l1 - l2) * (l1 - l2) : 4;


  try {
    GCoptimizationGeneralGraph *gc = new GCoptimizationGeneralGraph(num_pixels, num_labels);
    gc->setDataCost(data);
    gc->setSmoothCost(smooth);

    // now set up a grid neighborhood system
    // first set up horizontal neighbors
    for (int y = 0; y < height; y++ )
      for (int  x = 1; x < width; x++ ) {
        int p1 = x - 1 + y * width;
        int p2 = x + y * width;
        gc->setNeighbors(p1, p2, p1 + p2);
      }

    // next set up vertical neighbors
    for (int y = 1; y < height; y++ )
      for (int  x = 0; x < width; x++ ) {
        int p1 = x + (y - 1) * width;
        int p2 = x + y * width;
        gc->setNeighbors(p1, p2, p1*p2);
      }

    printf("\nBefore optimization energy is %d", gc->compute_energy());
    gc->expansion(2);// run expansion for 2 iterations. For swap use gc->swap(num_iterations);
    printf("\nAfter optimization energy is %d", gc->compute_energy());

    for ( int  i = 0; i < num_pixels; i++ )
      result[i] = gc->whatLabel(i);

    delete gc;
  }
  catch (GCException e) {
    e.Report();
  }

  delete [] result;
  delete [] smooth;
  delete [] data;


}
////////////////////////////////////////////////////////////////////////////////

int main(int argc, char **argv)
{
  int width = 10;
  int height = 5;
  int num_pixels = width * height;
  int num_labels = 7;


  // smoothness and data costs are set up one by one, individually
  GridGraph_Individually(width, height, num_pixels, num_labels);

  // smoothness and data costs are set up using arrays
  GridGraph_DArraySArray(width, height, num_pixels, num_labels);

  // smoothness and data costs are set up using functions
  GridGraph_DfnSfn(width, height, num_pixels, num_labels);

  // smoothness and data costs are set up using arrays.
  // spatially varying terms are present
  GridGraph_DArraySArraySpatVarying(width, height, num_pixels, num_labels);

  //Will pretend our graph is
  //general, and set up a neighborhood system
  // which actually is a grid
  GeneralGraph_DArraySArray(width, height, num_pixels, num_labels);

  //Will pretend our graph is general, and set up a neighborhood system
  // which actually is a grid. Also uses spatially varying terms
  GeneralGraph_DArraySArraySpatVarying(width, height, num_pixels, num_labels);

  printf("\n  Finished %d (%d) clock per sec %d", clock() / CLOCKS_PER_SEC, clock(), CLOCKS_PER_SEC);

  return 0;
}

/////////////////////////////////////////////////////////////////////////////////