/*************************************************************/
/* takepic.c                                                 */
/*                                                           */
/* This program is for taking pictures with the camera on    */
/* the Eyebot, and then uploading them to the PC             */
/*                                                           */
/* Andrew Berry                                              */
/* 18/10/99                                                  */
/*************************************************************/

#include "eyebot.h"

#include <stdlib.h>
#include <stdio.h>

#define MAXPICS 15

/***********************************************************************/
/** Error message.
    Print error message and end program.

    @param str string, which should be printed
*/
/***********************************************************************/

void error(char *str)
{
  LCDPrintf("ERROR: %s\n", str);
  OSWait(200);
  exit(0);
}
           

/***********************************************************************/
/** Init camera.

    Called by main().
    @see  main()
*/
/***********************************************************************/

void InitCam()
{
  int camera;
  int bright, hue, sat;
  /*  int i,x,y;*/

  camera = CAMInit(WIDE);
  OSWait(10);
  camera = CAMInit(WIDE);

  if (camera < COLCAM)
    error("No colour camera!");
  else
  {
    if (camera == NOCAM)
      error("No camera!\n");
    else
      if (camera == INITERROR)
        error("CAMInit!\n");
  }

  /* set brightness and hue */
  CAMGet(&bright, &hue, &sat);
  CAMMode(NOAUTOBRIGHTNESS);
  CAMSet(160, 110, sat);

}

/***********************************************************************/
/** Change camera parameters.
    Changes brightness, hue and saturation in case default values
    aren't good enough (e.g. different light conditions). Function as
    in RoBiOS-Setup, but without camera-initialisation, works only for
    colour-camera.

    Called by main().
    @see  main()
*/
/***********************************************************************/

void set_cam_parameters ()
{
  int boc[] = {0, 0, 0, 0};
  int ind = 0;
  image greyimg;
  colimage img;
  
  int end_proc = FALSE;

  LCDClear();
  LCDMenu(" + "," - ","Nxt","END");
  LCDPutString("Camera param.:\n");

  while (!end_proc)
  {
    CAMGet(&boc[0], &boc[1], &boc[2]);
    
    if (boc[3] == 0) CAMMode(NOAUTOBRIGHTNESS);
    else CAMMode(AUTOBRIGHTNESS);
    
    LCDSetPos(2, 0);
    LCDPrintf("Brigh.:   %d \n", boc[0]);
    LCDPrintf("Hue   :   %d \n", boc[1]);
    LCDPrintf("Satur.:   %d \n", boc[2]);
    
    CAMGetColFrame(&img, FALSE);
    IPColor2Grey(&img, &greyimg);
    
    LCDPrintf("AutoBr.:  ");
    if (boc[3] == 0)
      LCDPrintf("OFF\n");
    else
      LCDPrintf("ON \n");
    LCDSetChar(2 + ind, 15, '*');
    
    LCDPutGraphic (&greyimg);
    
    switch (KEYRead())
    {
    case KEY1:      boc[ind] ++;
      if (ind == 0 || ind == 3)
      {
        if (ind == 0)
        {
          if (boc[ind] > 179) boc[ind] = 0;
        }
        else
          if (boc[ind] > 1) boc[ind] = 0;
      }
      else
        if (boc[ind] > 255) boc[ind] = 0;
      CAMSet(boc[0], boc[1], boc[2]);
      break;

    case KEY2:      boc[ind] --;
      if (ind == 0 || ind == 3)
      {
        if (ind == 0)
        {
          if (boc[ind] < 0) boc[ind] = 179;
        }
        else
          if (boc[ind] < 0) boc[ind] = 1;
      }
      else
        if (boc[ind] < 0) boc[ind] = 255;
      CAMSet(boc[0], boc[1], boc[2]);
      break;

    case KEY3:
      LCDSetChar(2 + ind, 15, ' ');
      ind ++;
      if (ind > 3) ind = 0;
      break;

    case KEY4:
      LCDClear(); end_proc = TRUE; break;

    default: break;
    }
  }
}




/*************************************************************/
/* TakePic                                                   */
/*                                                           */
/* Takes a picture                                           */
/*************************************************************/

int TakePic(colimage *pic)
{
  AUBeep();
  return CAMGetColFrame(pic,FALSE);
}

/*************************************************************
 * Accurate converting color image to grey scale image
 *************************************************************/
int
IP_Color2Grey(colimage *src, image *dest)
{
  int row, column;
  for (row = 1; row < imagerows-1; row++)
    for (column = 1; column < imagecolumns-1; column++)
      {
	int val;
	val  = ((*src)[row][column][0]
	  + (*src)[row][column][1]
	  + (*src)[row][column][2])/3;
	(*dest)[row][column] = val >> 4;
      }
  return 0;
}

/*************************************************************/
/* sendData                                                  */
/*                                                           */
/* Program to send the data to the PC                        */
/* Jesse Pepper 1999                                         */
/*************************************************************/

void sendString(int port, char* s )
{
  while (*s)
  {
    OSSendRS232( s, port );
    s++;
  }
}
                 
void sendRGBData(int port, BYTE *img )
{
  int i,j;
  char temp[100];
  
  sprintf(temp,
	  "P3\n"
	  "82 62\n"
	  "255\n");
  sendString(port, temp);
  for(i=0, j=0; i < imagerows*imagecolumns; i++, j+=3)
  {
    sprintf(temp,"%u %u %u \n",img[j],img[j+1],img[j+2]);
    if ((i % imagecolumns) == 0)
      {
	LCDClear();
	LCDPrintf(" %4d of %4d\n", i, imagerows*imagecolumns );
      }
    sendString(port, temp);
  }
}                     

void sendGreyData(int port, BYTE *img )
{
  int i,j;
  char temp[100];
  
  sprintf(temp,
	  "P2\n"
	  "82 62\n"
	  "15\n");
  sendString(port, temp);
  for(i=0, j=0; i < imagerows*imagecolumns; i++)
  {
    sprintf(temp,"%u\n",img[i]);
    if ((i % imagecolumns) == 0)
      {
	LCDClear();
	LCDPrintf(" %4d of %4d\n", i, imagerows*imagecolumns );
      }
    sendString(port, temp);
  }
}



/*************************************************************/
/* SendPic                                                   */
/*                                                           */
/* Takes a picture                                           */
/*************************************************************/

int SendPic(colimage *colimg)
{
  int port = SERIAL1;
  image img1, img2, img3;
  char *current = "none";
  int i;
  int convert = 0; /* 0 = none,
		      1 = col->grey,
		      2 = col->grey->dither,
		      3 = col->grey->laplace,
		      4 = col->grey->sobel,
		      5 = diff laplace|sobel. */
  AUBeep();
  LCDClear();
  LCDMenu("Nxt", " ", "Send", "END");
  LCDSetString(0,0, "Convert filter");
  LCDSetString(1,0, current);
 
  while (KEY3 != (i = KEYRead()))
    {
      if (KEY4 == i)
	return 0;
      if (KEY1 == i)
	{
	  convert = (convert+1) % 6;
	  switch (convert)
	    {
	    case 0:
	      current = "none          ";
	      break;
	    case 1:
	      IPColor2Grey(colimg, &img2);
	      current = "IPColor2Grey  ";
	      break;
	    case 2:
	      IPColor2Grey(colimg, &img1);
	      IPDither(&img1, &img2);
	      current = "IPDither      ";
	      break;
	    case 3:
	      IPColor2Grey(colimg, &img1);
	      IPSobel(&img1, &img2);
	      current = "IPSobel       ";
	      break;
	    case 4:
	      IPColor2Grey(colimg, &img1);
	      IPLaplace(&img1, &img2);
	      current = "IPLaplace     ";
	      break;
	    case 5:
	      IP_Color2Grey(colimg, &img1); /* Accurate */
	      IPColor2Grey(colimg, &img3);  /* Approximate */
	      IPDiffer(&img1, &img3, &img2);
	      current = "IPDiffer      ";
	      break;
	    }
	  LCDSetString(1,0, current);
	}
    }
 
  AUBeep();

  OSInitRS232( SER115200, NONE, port );

  if (convert)
    sendGreyData(port, (BYTE*)img2);
  else
    sendRGBData(port, (BYTE*)colimg);

  /* send RS232 termination character */
  sendString(port, "\x04\n");
  
  return TRUE;
}

/*************************************************************/
/* main                                                      */
/*************************************************************/
int main()
{
  int current_pic = 0;
  image greyimg;
  colimage images[MAXPICS],vf_img;
  int occupied[MAXPICS];
  int end_flag = FALSE;
  int i = 0;
  int key;
  char s[4];
  
  for(i=0;i<MAXPICS;i++)
    occupied[i] = FALSE;
  InitCam();
  set_cam_parameters ();

 
  while(!end_flag)
  {
    if (current_pic == -1)
    {
      LCDSetString(0,12,"vf  ");
      CAMGetColFrame(&vf_img,FALSE);
      IPColor2Grey(&vf_img, &greyimg);
      LCDPutGraphic (&greyimg);
    }
    else
    {
      sprintf(s,"%d ",current_pic);
      LCDSetString(0,12,s);
      if (occupied[current_pic] )
      {
	IPColor2Grey(&(images[current_pic]), &greyimg);
	LCDPutGraphic (&greyimg);
      }
      else
      {
	LCDSetString(0,0,"no pic\n");
      }
    }
    LCDMenu("Nxt ","Clk","Snd","Exit");
    key = KEYRead();
    switch(key)
    {
    case KEY1:
      LCDClear();
      if (current_pic == MAXPICS-1)
	current_pic = -1;
      else
	current_pic++;
      break;
    case KEY2:
      TakePic(&images[current_pic]);
      occupied[current_pic] = TRUE;
      break;
    case KEY3:
      SendPic(&images[current_pic]);
      LCDClear();
      break;
    case KEY4:
      end_flag = TRUE;
      break;
    }  
  }
  return 0;
}