/***************************************************************************
 * $Id: projectimg.cc 81 2009-01-29 11:25:22Z kaori $
 *
 *  Copyright Kaori Hagihara
 *  Email: kaori.hagihara@uclouvain.be
 ****************************************************************************/
#include <iostream>
#include <fstream>
#include <cv.h>
#include <highgui.h>

using namespace std;

void load_asciiMap( const char *fname, float *data);

int main(int argc,char **argv){
  
  if( argc<5){
    cerr << "Usage: input image file" << endl
	 << "       reference xcoordinate file" << endl
	 << "       reference ycoordinate file" << endl
	 << "       output image file"          << endl;
    return -1;
  }

  IplImage *img, *undstimg;
  uchar *_img, *_undstimg;
  int step;
  int W, H;
  int nChannels;
  float *xcoord, *ycoord;
  double x0, y0;
  double dx, dy;
  int u, v;
  int idx1, idx2, idx3, idx4;
  double a1, a2, a3, a4;
  
  cerr << "loading image " << argv[1] << endl;

  img = cvLoadImage( argv[1], CV_LOAD_IMAGE_UNCHANGED);

  W = img->width; 
  H = img->height;
  nChannels = img->nChannels;

  undstimg = cvCreateImage( cvGetSize( img), img->depth, nChannels);
  cvSetZero( undstimg);

  xcoord = new float [ W*H];
  ycoord = new float [ W*H];

  load_asciiMap( argv[2], xcoord);
  load_asciiMap( argv[3], ycoord);


  cvGetRawData( img, &_img, &step);
  cvGetRawData( undstimg, &_undstimg, &step);
  step /= sizeof( _img[0]);

  for( int i=0; i<W*H; i++){
    
    x0 = xcoord[ i];
    y0 = ycoord[ i];

    if( x0!=0 || y0!=0){
      u = (int)floor(y0);
      v = (int)floor(x0);

      idx1 = u*W+v;
      idx2 = (u+1)*W+v; 
      idx3 = (u+1)*W+v+1; 
      idx4 = u*W+v+1;
	
      dx = x0-v;
      dy = y0-u;
	
      a1 = (1-dx)*(1-dy);
      a2 = (1-dx)*dy;	
      a3 = dx*dy;	
      a4 = dx*(1-dy);
   
      for( int c=0; c<nChannels; c++)
	_undstimg[i*nChannels+c] = _img[(idx1)*nChannels+c]*a1 + _img[idx2*nChannels+c]*a2 + _img[idx3*nChannels+c]*a3 + _img[idx4*nChannels+c]*a4;
    }
  }

  cerr << "saveing undistorted image to " << argv[4] << endl;

  cvSaveImage( argv[4], undstimg);

  cvReleaseImage( &img);
  cvReleaseImage( &undstimg);

  delete [] xcoord;
  delete [] ycoord;

  return 0;
}

void load_asciiMap( const char *fname, float *data)
{

  cerr << "reading file " << fname << endl;

  ifstream inf( fname);
  if(!inf){
    cerr << "Failed to open " << fname << endl;
    exit(-1);
  }
  
  int i=0;
  do{
    inf >> data[i++];
  }while( !inf.eof());
  inf.close();
}
