// ------------------------------------------------------------------------
  // This program is complementary material for the book:
  //
  // Frank Nielsen
  //
  // Visual Computing: Geometry, Graphics, and Vision
  //
  // ISBN: 1-58450-427-7
  //
  // Charles River Media, Inc.
  //
  //
  // All programs are available at www.charlesriver.com/visualcomputing/
  //
  // You may use this program for ACADEMIC and PERSONAL purposes ONLY. 
  //
  //
  // The use of this program in a commercial product requires EXPLICITLY
  // written permission from the author. The author is NOT responsible or 
  // liable for damage or loss that may be caused by the use of this program. 
  //
  // Copyright (c) 2005. Frank Nielsen. All rights reserved.
  // ------------------------------------------------------------------------
   
  // ------------------------------------------------------------------------
  // File: unperspective.cpp
  // 
  // Description: Use homographies to rectify images so that they look as if 
  // taken perpendicular to some planar surface
  // ------------------------------------------------------------------------
  
  include <stdafx.h>
  include <fstream>
  include <math.h>
  
  using namespace std;
  
  class Point{
  public:
          double x,y,z;
  
          Point(){x=y=z=0.0;}
          Point (double xx,double yy, double zz) {x=xx;y=yy;z=zz;}
  
  //
  // Cross product of two vectors. 
  // Returns a vector perpendicular to the plane spanned by the two vectors
  //
  inline void Point::CrossProduct(Point v1,Point v2) 
  { 
                  x=v1.y*v2.z-v1.z*v2.y;
          y=v1.z*v2.x-v1.x*v2.z;
          z=v1.x*v2.y-v1.y*v2.x;
  }
  
  inline void   Point::ScalarMultiply(Point v,double l)
  {
  x=l*v.x;
  y=l*v.y;
  z=l*v.z;
  }
  
  };
     
          
  
  inline double DotProduct(Point v1, Point v2)
  {
  double res;
  
  res=v1.x*v2.x+v1.y*v2.y+v1.z*v2.z;
  return res;
  }
  
  class Matrix{
  public: 
          double array[3][3];
  };
  
  void MatrixMultiply(Matrix m1,Matrix m2, Matrix &result)
  {
  int i,j,k;
  double res;
  
  for(i = 0; i < 3; i++)
   for(j = 0; j < 3; j++){
        res = 0.0; 
        for(k = 0; k < 3; k++) 
          res += (m1.array[i][k] * m2.array[k][j]);
        result.array[i][j] = res;
      }
  
  }
  
  //
  // Compute the homography by decomposing to the unit square
  // We can compute the inverse of a matrix as InvM=AdjunctM/detM. This implies mostly
  // multiplications. Also since it is a projective transformation we can discard the 
  // computation of det M
  //
  int ComputeHomography4Points(Point *l, Point *r, Matrix &H)
  {
  int i,j;
  Point cp[6],col[3],lig[3],pv[3];
  double det[6];
  Matrix H1inv,H2,tmp;
  
  cp[0].CrossProduct(r[2],r[3]);
  cp[1].CrossProduct(r[0],r[3]);
  cp[2].CrossProduct(r[2],r[0]);
  
  cp[3].CrossProduct(l[2],l[3]);
  cp[4].CrossProduct(l[0],l[3]);
  cp[5].CrossProduct(l[2],l[0]);
  
  det[0]=DotProduct(r[0],cp[0]);
  det[1]=DotProduct(r[1],cp[1]);
  det[2]=DotProduct(r[1],cp[2]);
  det[3]=DotProduct(l[0],cp[3]);
  det[4]=DotProduct(l[1],cp[4]);
  det[5]=DotProduct(l[1],cp[5]);
  
  col[0].ScalarMultiply(r[1],det[0]);
  col[1].ScalarMultiply(r[2],det[1]);
  col[2].ScalarMultiply(r[3],det[2]);
    
  pv[0].CrossProduct(l[2], l[3]);
  pv[1].CrossProduct(l[3], l[1]);
  pv[2].CrossProduct(l[1], l[2]);
  
  if ((det[0]!=0.0)&&(det[1]!=0.0)&&(det[2]!=0.0)\
          &&(det[3]!=0.0)&&(det[4]!=0.0)&&(det[5]!=0.0))
    {
          // Non-degenerate case
        lig[0].ScalarMultiply(pv[0],1.0/det[3]);
        lig[1].ScalarMultiply(pv[1],1.0/det[4]);
        lig[2].ScalarMultiply(pv[2],1.0/det[5]);
  
            H1inv.array[0][0]=lig[0].x;
            H1inv.array[0][1]=lig[0].y;
        H1inv.array[0][2]=lig[0].z;
  
            H1inv.array[1][0]=lig[1].x;
            H1inv.array[1][1]=lig[1].y;
        H1inv.array[1][2]=lig[1].z;
  
            H1inv.array[2][0]=lig[2].x;
            H1inv.array[2][1]=lig[2].y;
        H1inv.array[2][2]=lig[2].z;
  
            H2.array[0][0]=col[0].x;
            H2.array[1][0]=col[0].y;
            H2.array[2][0]=col[0].z;
  
            H2.array[0][1]=col[1].x;
            H2.array[1][1]=col[1].y;
            H2.array[2][1]=col[1].z;
  
            H2.array[0][2]=col[2].x;
            H2.array[1][2]=col[2].y;
            H2.array[2][2]=col[2].z;
  
     
        MatrixMultiply(H2,H1inv,tmp);
  
        // Normalize...
        
        for(i=0;i<3;i++)
                  {
                  for(j=0;j<3;j++)
                          H.array[i][j]=(tmp.array[i][j]/tmp.array[2][2]);
                  }
            return 1;
      }
  else 
    { 
      // Degenerate case: return Id matrix.
      for(i=0;i<3;i++)
                  {
                  for(j=0;j<3;j++) H.array[i][j]=((i==j)? 1.0 : 0.0);
                  }
          return 0;
          }  
  } 
  
  void inline MatrixPoint(Matrix H,double x,double y,int &xi, int &yi)
  {
  double xx,yy,zz;
  
  xx=H.array[0][0]*x+H.array[0][1]*y+H.array[0][2];
  yy=H.array[1][0]*x+H.array[1][1]*y+H.array[1][2];
  zz=H.array[2][0]*x+H.array[2][1]*y+H.array[2][2];
  
  xi=(int)(xx/zz+0.5);
  yi=(int)(yy/zz+0.5);
  }
  
  // Save a PPM Image
  void SaveImagePPM(unsigned char * data, int w, int h, char * file)
  {
          ofstream OUT(file, ios::binary);
          if (OUT){
      OUT << "P6" << endl << w << ' ' << h << endl << 255 << endl;
          OUT.write((char *)data, 3*w*h);
          OUT.close();}
  }
  
  // Load a PPM Image that does not contain comments.
  unsigned char * LoadImagePPM(char *ifile,  int &w, int &h)
  {
           char dummy1=0, dummy2=0; int maxc;
          unsigned char * img;
          ifstream IN(ifile, ios::binary);
  
          IN.get(dummy1);IN.get(dummy2);
          if ((dummy1!='P')&&(dummy2!='6')) {cerr<<"Not P6 PPM file"<<endl; return NULL;}
   
      IN >> w >> h;
      IN >> maxc;
      IN.get(dummy1);
          
      img=new  unsigned char[3*w*h];
          IN.read((char *)img, 3*w*h); 
          IN.close();
          return img;
  }
  
  int _tmain(int argc, _TCHAR* argv[])
  {
  char filename[]="newcourt.ppm";
  
  char filenameo[256];
  
  int w,h,wo,ho, i,j;
  unsigned char * img, *imgo;
  int ixorg, iyorg, index,indexo;
  Point l[4], r[4];
  Matrix H;
  
  cout<<"Visual Computing: Geometry, Graphics, and Vision (ISBN:1-58450-427-7)"<<endl;
  cout<<"Demo program\n\n"<<endl;
  
  cout << "Undo the perspective effect of a planar surface"<<endl;
  
  img=LoadImagePPM(filename,w,h);
  
  wo=300;
  ho=200;
  
  imgo=new unsigned char [wo*ho*3];
  
   // newcourt
  l[0]=Point(455,806,1.0); r[0]=Point(0,0,1);
  l[1]=Point(779,537,1.0);r[1]=Point(wo,0,1);
  l[2]=Point(803,202,1.0); r[2]=Point(wo,ho,1);
  l[3]=Point(440,332,1.0); r[3]=Point(0,ho,1);
  
  ComputeHomography4Points(r,l,H);
  
  // Should implement a better interpolator. Here we use nearest neighbor
  for(i=0;i<ho;i++)
  for(j=0;j<wo;j++)
  {
  MatrixPoint(H,j,ho-i,ixorg,iyorg);
  
  index=3*(iyorg*w+ixorg); 
  indexo=3*(i*wo+j);
  
  imgo[indexo]=img[index];
  imgo[indexo+1]=img[index+1];
  imgo[indexo+2]=img[index+2];
  }
  
  cout<<"Here is the computed homography used for rectifying the input image "<<filename<<endl;
  for(i=0;i<3;i++){
  
  for(j=0;j<3;j++)
  {
  cout <<H.array[i][j]<<" ";
  }
  cout <<endl;
  }
  
  sprintf(filenameo,"unperspective-\%s",filename);
  SaveImagePPM(imgo,wo,ho,filenameo);
  
  char line[256];
  cout<<"Press Return key"<<endl;
  gets(line);
  
  delete [] img;
  delete [] imgo;
  
          return 0;
  }