#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <cv.h>
#include <highgui.h>


int main(int argc, char *argv[])
{
  IplImage* img = 0;
  IplImage* world_img = 0;
  FILE* pfile;

  int i,j,k;
  double xi[4];
  double yi[4];
  double xw[4];
  double yw[4];


  if(argc<3){
    printf("Usage: main <image-file-name> <coordinates-file>\n\7");
    exit(0);
  }

  // load an image
  img=cvLoadImage(argv[1]);
  if(!img){
    printf("Could not load image file: %s\n",argv[1]);
    exit(0);
  }

  // read coordinates
  pfile = fopen(argv[2], "r");

  for (i=0;i<4;i++)
  {
      fscanf(pfile,"%lf",&xi[i]);
      fscanf(pfile,"%lf",&yi[i]);
  }
  for (i=0;i<4;i++)
  {
      fscanf(pfile,"%lf",&xw[i]);
      fscanf(pfile,"%lf",&yw[i]);
  }

    // fill up the matrix for world coordinates
    double wc[8];
    for (i=0;i<4;i++)
    {
    wc[2*i] = xw[i];
    wc[2*i+1] = yw[i];
    }

    CvMat WC;
    cvInitMatHeader(&WC, 8, 1, CV_64FC1, wc, CV_AUTOSTEP);

    // fill up the matrix equation
    double m[64];
    for (i=0;i<4;i++)
    {
    m[i*2*8+0] = xi[i];
    m[i*2*8+1] = yi[i];
    m[i*2*8+2] = 1;
    m[i*2*8+3] = 0;
    m[i*2*8+4] = 0;
    m[i*2*8+5] = 0;
    m[i*2*8+6] = -xw[i]*xi[i];
    m[i*2*8+7] = -xw[i]*yi[i];

    m[(i*2+1)*8+0] = 0;
    m[(i*2+1)*8+1] = 0;
    m[(i*2+1)*8+2] = 0;
    m[(i*2+1)*8+3] = xi[i];
    m[(i*2+1)*8+4] = yi[i];
    m[(i*2+1)*8+5] = 1;
    m[(i*2+1)*8+6] = -yw[i]*xi[i];
    m[(i*2+1)*8+7] = -yw[i]*yi[i];
    }

    CvMat M;
    cvInitMatHeader(&M, 8, 8, CV_64FC1, m, CV_AUTOSTEP);

    //solve the equation
    CvMat *ans = cvCreateMat(8, 1, CV_64FC1);
    cvSolve(&M, &WC, ans, CV_LU);

    //get H and H_inv
    CvMat *H = cvCreateMat(3, 3, CV_64FC1);
    CvMat *H_inv = cvCreateMat(3, 3, CV_64FC1);
    for (i=0;i<8;i++)
    {
    cvmSet(H_inv, i/3, i%3, cvmGet(ans, i, 0));
    }
    cvmSet(H_inv, 2, 2, 1.0);
    cvInvert(H_inv, H, CV_LU);


    // determine the world boundry
    double img_bound[] = {
    0, img->width-1, 0, img->width-1,
    0, 0, img->height-1, img->height-1,
    1,1,1,1};
    CvMat *m_imgb = cvCreateMat(3, 4, CV_64FC1);
    cvInitMatHeader(m_imgb, 3, 4, CV_64FC1, img_bound, CV_AUTOSTEP);

    CvMat *m_worldb = cvCreateMat(3, 4, CV_64FC1);
    cvMatMul(H_inv, m_imgb, m_worldb);

    double xmin = 10000;
    double xmax = 0;
    double ymin = 10000;
    double ymax = 0;

    for (i=0;i<4;i++)
    {
    double x = cvmGet(m_worldb, 0, i)/cvmGet(m_worldb, 2, i);
    double y = cvmGet(m_worldb, 1, i)/cvmGet(m_worldb, 2, i);
    if (x<xmin) xmin = x;
    if (x>xmax) xmax = x;
    if (y<ymin) ymin = y;
    if (y>ymax) ymax = y;
    }

    //printf("%f %f %f %f \n", xmin, xmax, ymin, ymax);

    //creat world image
    double scale = ((double)img->width)/(xmax - xmin);
    int world_height = (int) ((ymax - ymin) * scale);
    world_img = cvCreateImage(cvSize(img->width, world_height), IPL_DEPTH_8U, 3);
    cvZero(world_img);

    //generate the world image
    double step = 1.0/scale;
    CvMat* point_w = cvCreateMat(3,1, CV_64FC1);
    CvMat* point_i = cvCreateMat(3,1, CV_64FC1);
    cvmSet(point_w, 2, 0, 1.0);

    for (i=0; i<world_img->width; i++)
    {
    cvmSet(point_w, 0, 0, xmin+((double)i)*step);
    for(j=0; j<world_img->height; j++)
    {
        cvmSet(point_w, 1, 0, ymin+((double)j)*step);
        cvMatMul(H, point_w, point_i);
        double xi = cvmGet(point_i, 0, 0)/cvmGet(point_i, 2, 0);
        double yi = cvmGet(point_i, 1, 0)/cvmGet(point_i, 2, 0);

        //out of bound
        if (xi<0 || yi<0) continue;
        if (xi>=img->width-1 || yi>=img->height-1) continue;

        for (k=0;k<3;k++)
        {
        double value = ((uchar *)(img->imageData + img->widthStep*(int)yi))[(int)xi*3+k];
        ((uchar *)(world_img->imageData + world_img->widthStep*j))[i*3+k] = value;
        }
    }
    }

    //save world img
    char str[256];
    sprintf(str, "%s-world.png",argv[1]);
    cvSaveImage(str, world_img);

    // release the image and matrix
    cvReleaseImage(&img);
    cvReleaseImage(&world_img);
    cvReleaseMat(&ans);
    cvReleaseMat(&point_i);
    cvReleaseMat(&point_w);
    cvReleaseMat(&H);
    cvReleaseMat(&H_inv);
    cvReleaseMat(&m_worldb);
    cvReleaseMat(&m_imgb);
    return 0;
}