#include <stdio.h>
#ifndef WIN32
#include <unistd.h>
#endif

#ifdef WIN32
#define _USE_MATH_DEFINES
#endif
#include <math.h>

#include <cv.h>
#include <highgui.h>

#include "../Common/sharedmem.h"

#ifdef WIN32
#include "../Common/Linux4Win.h"
#endif

#include "define.h"


#define MAP_WINDOW "ceil map"
//#define BIG_WINDOW "Localization"


static void on_mouse(int event, int x, int y, int flags, void* param);

static robot_status *rstat;

int main (int argc, char **argv )
{
#ifdef BIG_WINDOW
  double scale(3);
#endif
  int key = 0;
  IplImage *map_img;
  IplImage *output_img;
  IplImage *capture_img;
  IplImage *capture_tmp;
#ifdef SAVE_TRACK      
  char fname[256];
  unsigned int count = 0;
#endif
  CvRect roi;

  rstat = (robot_status *)InitSharedMem(SHM_KEY, sizeof(robot_status));

  map_img = cvLoadImage (MAP_SHOW, CV_LOAD_IMAGE_COLOR /* CV_LOAD_IMAGE_GRAYSCALE */);
  if (!map_img)
    {
      fprintf(stderr, "%s: LOAD ERROR\n", MAP_SHOW);
      return 1;
    }
  
  printf("mapsize: x=%3d y=%3d\n",map_img->width,map_img->height);

  output_img = cvCreateImage( cvSize(map_img->width, map_img->height), IPL_DEPTH_8U, 3);
  capture_img = cvCreateImage( cvSize(TEMPLATE_SIZE, TEMPLATE_SIZE), IPL_DEPTH_8U, 1);
  capture_tmp = cvCreateImage( cvSize(TEMPLATE_SIZE, TEMPLATE_SIZE), IPL_DEPTH_8U, 3);
  //IplImage *capture_img = cvCreateImage( cvSize(TEMPLATE_SIZE,TEMPLATE_SIZE), IPL_DEPTH_8U, 3);

#ifdef BIG_WINDOW
  IplImage *big_img = cvCreateImage (cvSize ((int)(output_img->width*scale),(int)(output_img->height*scale)), output_img->depth, output_img->nChannels);
  cvNamedWindow (BIG_WINDOW, CV_WINDOW_AUTOSIZE);
#endif

  cvNamedWindow (MAP_WINDOW, CV_WINDOW_AUTOSIZE);
cvNamedWindow("CameraWindow", 0);
  cvSetMouseCallback(MAP_WINDOW, on_mouse, 0);

  while(1)
    {

      cvCopy (map_img, output_img,NULL);
      memcpy (capture_img->imageData, rstat->buf, sizeof(unsigned char)*TEMPLATE_SIZE*TEMPLATE_SIZE);
      //      cvConvertImage( capture_img, output_img, 0);
      cvConvertImage( capture_img, capture_tmp, 0);
      
      roi = cvRect (0, 0, capture_img->width, capture_img->height);
      cvSetImageROI (output_img, roi);
      cvCopy (capture_tmp, output_img, 0);
      cvResetImageROI(output_img);
      cvRectangle ( output_img, cvPoint(roi.x,roi.y), cvPoint(roi.x+roi.width,roi.y+roi.height), CV_RGB(255,0,0), 1, 4, 0 );      

#ifdef _DEBUG
      printf("%d, %d, %f ( %d )\n", rstat->x, rstat->y, rstat->r, rstat->s);
#endif
      // robot point
      cvCircle ( map_img, 
		 cvPoint( (int)(rstat->x+TEMPLATE_SIZE/2),
			  (int)(rstat->y+TEMPLATE_SIZE/2) ),
		 1, CV_RGB(255,0,0), -1, 4, 0 );

      // deg
      cvLine ( output_img,
	       cvPoint( (int)(rstat->x+TEMPLATE_SIZE/2),
			(int)(rstat->y+TEMPLATE_SIZE/2) ),
	       cvPoint( (int)(rstat->x+TEMPLATE_SIZE/2+sin(rstat->r)*10),
			(int)(rstat->y+TEMPLATE_SIZE/2+cos(rstat->r)*10) ),
	       CV_RGB(255,140,0), 1, CV_AA, 0 );

      // detect rect
      cvLine ( output_img,
	       cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(cos(-rstat->r) - sin(-rstat->r)) ),
			(int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(sin(-rstat->r) + cos(-rstat->r)) ) ),
	       cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-cos(-rstat->r) - sin(-rstat->r)) ),
			(int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-sin(-rstat->r) + cos(-rstat->r)) ) ),
	       CV_RGB(255,255,0), 1, CV_AA, 0 );
      cvLine ( output_img,
	       cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(cos(-rstat->r) - sin(-rstat->r)) ),
			(int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(sin(-rstat->r) + cos(-rstat->r)) ) ),
	       cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(cos(-rstat->r) + sin(-rstat->r)) ),
			(int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(sin(-rstat->r) - cos(-rstat->r)) ) ),
	       CV_RGB(255,255,0), 1, CV_AA, 0 );
      cvLine ( output_img,
	       cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(cos(-rstat->r) + sin(-rstat->r)) ),
 			(int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(sin(-rstat->r) - cos(-rstat->r)) ) ),
	       cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-cos(-rstat->r) + sin(-rstat->r)) ),
			(int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-sin(-rstat->r) - cos(-rstat->r)) ) ),
	       CV_RGB(255,255,0), 1, CV_AA, 0 );
      cvLine ( output_img,
	       cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-cos(-rstat->r) - sin(-rstat->r)) ),
			(int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-sin(-rstat->r) + cos(-rstat->r)) ) ),
	       cvPoint( (int)(rstat->x + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-cos(-rstat->r) + sin(-rstat->r)) ),
			(int)(rstat->y + TEMPLATE_SIZE/2 + TEMPLATE_SIZE/2*(-sin(-rstat->r) - cos(-rstat->r)) ) ),
	       CV_RGB(255,255,0), 1, CV_AA, 0 );
      
      cvShowImage(MAP_WINDOW, output_img);
	cvShowImage("CameraWindow",capture_img);

#ifdef BIG_WINDOW
      cvResize (output_img, big_img, CV_INTER_NN);
      cvShowImage(BIG_WINDOW, big_img);
#endif

#ifdef SAVE_TRACK      
      sprintf( fname, SAVE_TRACK"map_%05d.png", count++ );
//      cvSaveImage( fname, output_img );
#endif
      //cvSaveImage( "test.png", output_img );

      //usleep(2000);

      key = cvWaitKey(100);
      //key = cvWaitKey(33);
      switch (key)
        {
        case 0x1b: // exit
          goto exit;
        case 'q':  // exit
          goto exit;
        case 'r':  // reset
	  map_img = cvLoadImage (MAP_SHOW, CV_LOAD_IMAGE_COLOR /* CV_LOAD_IMAGE_GRAYSCALE */);
	  printf("Map reset!");
          break;
        }
    }

 exit:;


#ifdef BIG_WINDOW
  cvDestroyWindow (BIG_WINDOW);
#endif
  cvDestroyWindow (MAP_WINDOW);
  //cvReleaseImage(&dst);


  return 0;
}

static CvPoint lbuttondown_pt;

void on_mouse(int event, int x, int y, int flags, void* param)
{
  switch (event)
    {
    case CV_EVENT_LBUTTONDOWN:
      lbuttondown_pt.x = x;
      lbuttondown_pt.y = y;
      rstat->x = x-TEMPLATE_SIZE/2;
      rstat->y = y-TEMPLATE_SIZE/2;
      break;
    case CV_EVENT_LBUTTONUP:
      rstat->r = atan2(x-lbuttondown_pt.x,y-lbuttondown_pt.y);
      printf("rstat->(x,y,r)=(%4d,%4d, %3.3f )\n", lbuttondown_pt.x-TEMPLATE_SIZE/2, lbuttondown_pt.y-TEMPLATE_SIZE/2, rstat->r );
      break;
    }
}
