#include<stdio.h>
#include<cv.h>

/* useful function for reading in ppm/pgm headers */
void getline(char *buf,FILE *fp)
  {  //read until end of line from fp
	  char *p=buf;
	  char c;
	  while ((c=fgetc(fp))!='\n')
	    {	   *p=c;
		    p++;
	    }
	  *p='\0';//terminate as string
  }

     CvSize imagesize;

void process(IplImage *in,IplImage *out)
{
  IplImage *eigImage, *tempImage; //needed for temporary	
  CvPoint2D32f corners[1000];
  
  int corner_count=100; //max number
  int i,j;
  unsigned char *p1,*p2;
  /* uses global imagesize set in main - bad programming!! */
     eigImage= cvCreateImage(imagesize,IPL_DEPTH_32F,1);
     tempImage= cvCreateImage(imagesize,IPL_DEPTH_32F,1);
     fprintf(stderr,"bpp of temp images %d\n",tempImage->nChannels*8);
     strncpy(tempImage->colorModel,"RGB\0",4);
     strncpy(tempImage->channelSeq,"BGR\0",4);
     strncpy(eigImage->colorModel,"RGB\0",4);
     strncpy(eigImage->channelSeq,"BGR\0",4);
     eigImage->roi=0;
     tempImage->roi=0;
     cvGoodFeaturesToTrack(in,eigImage,tempImage,corners,&corner_count,0.001,50);
       //use 0.5*eigenvalue when thresholding and make sure 5 pixels apart

     fprintf(stderr,"found %d corners\n",corner_count);
     /* colour red corners overlayed on output */

     //first copy over the pixels from grey to RGB colour
     for (i=0;i<in->height;i++)
	 {
           p1=in->imageData+i*(in->widthStep); 
	   p2=out->imageData+i*(out->widthStep);
	   for (j=0;j<in->width;j++)
	   {
              *(p2+2)=*(p2+1)=*p2=*p1;
	      p2+=3;
	      p1++;
	    }
	 }
     /* assume origin is top left corner and x-->
      *                      y
      *                      |
      *                      |
      *                      V
      */
     for (i=0;i<corner_count;i++)
     { fprintf(stderr,"corner at (%g,%g)\n",corners[i].x,corners[i].y);
        *((out->imageData)+
	   ( ((int) corners[i].y)*out->widthStep)+((int) corners[i].x)*3)=
		  (unsigned char)  255;
        *((out->imageData)+
	   ( ((int) corners[i].y)*out->widthStep)+((int) corners[i].x)*3+1)=
		   (unsigned char) 0;
        *((out->imageData)+
	   ( ((int) corners[i].y)*out->widthStep)+((int) corners[i].x)*3+2)=
		   (unsigned char) 0;
     }
}


/* some useful variables */
     int sizeX,sizeY;
     IplImage *ip,*ipout; 
     unsigned char *rawptr;
    char tstring[1000],inchar;
    FILE *fp;
    int maxval;
     char fname[100];




 int main(int argc,char **argv)
{
	int i;
	/* read in image */
     printf("name of input?\n");
     scanf("%s",fname);
    fp = fopen(fname, "rb");
    if (!fp)
    {
	fprintf(stderr, "Unable to open file `%s'\n", fname);
	exit(1);
    }
    getline(tstring,fp);
    fprintf(stderr,"scanned %s\n",tstring);
    if (! ((tstring[0]=='P') && (tstring[1]=='5')) )
        { 
         fprintf(stderr,"can only work with P5 versions of ppm images\n");
         exit(-1);
          }
         /*Check for comment and swallow whole line */
    getline(tstring,fp);
    while (tstring[0]=='#') getline(tstring,fp); //skip comments
    if (sscanf(tstring, "%d %d", &sizeX, &sizeY) != 2)
    {
	fprintf(stderr, "Error loading image `%s'\n", fname);
	exit(1);
    }
    getline(tstring,fp);
    while (tstring[0]=='#') getline(tstring,fp); //skip comments
     // throw away the maxval

    imagesize.width=sizeX; imagesize.height=sizeY;
     ip= cvCreateImage(imagesize,IPL_DEPTH_8U,1);// 3 channels RGB
                                                  //default is 4 bytes align
     fprintf(stderr,"colorModel %s\n",ip->colorModel);
     fprintf(stderr,"channelSeq %s\n",ip->channelSeq);
   ip->roi=0; // no region of interest  
     /* read image in one line at a time */
     rawptr= (unsigned char *) ip->imageData;
     for (i=0;i<sizeY;i++)
       {
       fread(rawptr,sizeX,1,fp);
       rawptr+= ip->widthStep; //increment padded line
       }
    fclose(fp);

    /* process image */
     ipout= cvCreateImage(imagesize,IPL_DEPTH_8U,3);// 3 channels RGB
     process(ip,ipout); //ipout with be ppm - with red squares where corners are
    /* output image */
     printf("name of output?\n");
     scanf("%s",fname);
    fp = fopen(fname, "wb");
    fprintf(fp,"P6\n%d %d\n255\n",ipout->width,ipout->height);
     /* write image in out line at a time */
     rawptr= (unsigned char *) ipout->imageData;
     for (i=0;i<sizeY;i++)
       {
       fwrite(rawptr,sizeX,3,fp);
       rawptr+= ipout->widthStep; //increment padded line
       }
    fclose(fp);

    }