/*
 *  ALEXEEV (c) 2012 compilated from mxc_fb_test and some other source (don't remember now)
 *
 *  This program can be used and distributed without restrictions.
 */

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

#include <fcntl.h>        /* low-level i/o */
#include <unistd.h>
#include <errno.h>
#include <malloc.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>

#include <asm/types.h>    /* for videodev2.h */

#include <linux/videodev2.h>

#include <linux/fb.h>

/* Standard Include Files */
#include <unistd.h>
#include <linux/mxcfb.h>
#include <math.h>

#define TFAIL -1
#define TPASS 0

int fd_fb = 0;
unsigned short * fb;
int g_fb_size;

  char *dev_name = "/dev/video0";


#define CLEAR(x) memset (&(x), 0, sizeof (x))

struct buffer {
  void *start;
  size_t length;
};

static int fd = -1;
struct buffer *buffers = NULL;
static unsigned int n_buffers = 0;

static void errno_exit(const char *s) {
  fprintf (stderr, "%s error %d, %s\n", s, errno, strerror (errno));

  exit (EXIT_FAILURE);
}

static int xioctl(int fd, int request, void * arg) {
  int r;

  do r = ioctl (fd, request, arg);
  while (-1 == r && EINTR == errno);

  return r;
}

//---------------------------------------------------------------
void saveToFile(const void *p, size_t length) {
  //fputc ('.', stdout);

  printf ("saving image to file, length=%d\n", length);
  char s[4096];
  int i;
  sprintf (s,"");
FILE *f;

f = fopen ("output.bin", "wb");
fwrite (p, sizeof(char), length, f);
fclose(f);

/*
  char *t = (char *)p;
  char w[10];
  for (i=0; i<64; i++)
	{
	sprintf (w, "%02X ", *t);
	strcat(s,w);
	t++;
	}
  strcat(s,"\n");
  fprintf (stderr, s);
*/

}
//---------------------------------------------------------------

void YUV444toRGB888 (char y, char u, char v, char *r, char *g, char *b)
{
*r = 1.164*(y - 16) + 1.596*(v - 128);
*g = 1.164*(y - 16) - 0.813*(v - 128) - 0.391*(u - 128);
*b = 1.164*(y - 16) + 2.018*(u - 128);
}

//---------------------------------------------------------------

    char clamp (int x)
    {
    if (x>255) return 255;
    if (x<0) return 0;
    return x;
    }


void showFrame(const void *p, size_t length)
{
printf ("show frame called\n");

	// -------- working with display --------------------



  printf ("image received, length=%d\n", length);
  char s[4096];
  sprintf (s,"");


	/* saving to file
FILE *f;
f = fopen ("output.bin", "wb");
fwrite (p, sizeof(char), length, f);
fclose(f);
*/

printf ("now displaying...\n");


int width=640;
int height=480;
long size = width*height;

// pixel format = 24bit
char i420[length];

typedef struct {
	char y, u, v;
	} Tyuv ;
typedef struct  {
	char r;
	char g;
	char b;
	}  Trgb;
Tyuv yuv[size];
Trgb rgb[size];
//int i;
printf ("copying buffer to i420 array\n");
   int i;

  for(i = 0; i < length; i++ )
  {
       i420[i] = *(char *)p++;
  }


// Переписали все компоненты яркости, в данном формате их по каждой на пиксел
printf ("copying Y\n");

  for(i = 0; i < width*height; i++ )
  {
       yuv[i].y = i420[i];
  }

//saveToFile(i420, length);

  int Counter = 0;

// Теперь компоненты цветоразностей, они одинаковы в каждом макропикселе, который представляет собой квадрат размером 2х2 пиксела
printf ("copying U and V\n");
  for(i = 0; i < width*height/4; i++ )
  {
  //printf ("counter=%d\n", Counter);
       yuv[Counter].u                  =
       yuv[Counter + 1].u              =
       yuv[Counter + width].u     =
       yuv[Counter + width + 1].u = i420[width*height + i];

       yuv[Counter].v                  =
       yuv[Counter + 1].v              =
       yuv[Counter + width].v     =
       yuv[Counter + width + 1].v = i420[width*height + i + width*height/4];

       Counter += 2;
       if( Counter%width == 0 )
           if( (Counter/width)%2 != 0 )
                Counter += width;
  }

 //saveToFile(&yuv[0], size*3);//  -- OK WORK!



printf ("converting yuv to rgb\n");


// теперь конфертнем из YUV в RGB
  for( i = 0; i < size; i++ )
  {
  //printf ("i=%d\n", i);
  /*
       rgb[i].b = 1.164*(yuv[i].y - 16)                   + 2.018*(yuv[i].u - 128);
       rgb[i].g = 1.164*(yuv[i].y - 16) - 0.813*(yuv[i].v - 128) - 0.391*(yuv[i].u - 128);
       rgb[i].r = 1.164*(yuv[i].y- 16) + 1.596*(yuv[i].v - 128);
       */
       int C,D,E;
       int Y= yuv[i].y;
       int U= yuv[i].u;
       int V= yuv[i].v;
  C = Y - 16;
    D = U - 128;
    E = V - 128;


    char R = clamp((int) ( 298 * C + 409 * E + 128) >> 8);
    char G = clamp((int) ( 298 * C - 100 * D - 208 * E + 128) >> 8);
    char B = clamp((int) ( 298 * C + 516 * D + 128) >> 8);
       rgb[i].r = R;
       rgb[i].g = G;
       rgb[i].b = B;

  }

//saveToFile(&rgb[0], size*3);



      int c=0;
      int x,y;
printf ("displaying rgb to screen\n");

int screenwidth=800;
int screenheight = 480;

  for( y = 0; y < screenheight; y++ )
  {
      for(x = 0; x < screenwidth; x++)
      {
      if ( (x<width) && (y<height) )
	{
                ((__u8*)fb)[c++] = rgb[y*width + x].b;       // Blue
                ((__u8*)fb)[c++] = rgb[y*width + x].g;       // Green
                ((__u8*)fb)[c++] = rgb[y*width + x].r;       // Red
	}
      else c+=3;
      }
  }


        return ;

}



//-----------------------------------------------------------------------------
static int read_frame(void) {
  struct v4l2_buffer buf;
  unsigned int i;

fprintf (stderr, "VERSION 2.1 --- read frame called\n");

  if (-1 == read (fd, buffers[0].start, buffers[0].length))
	{
	printf ("ERROR!\n");

	    switch (errno) {
	      case EAGAIN:
		{
		printf ("EAGAIN\n");
		  return 0;
		  }

	    case EIO:
	      /* Could ignore EIO, see spec. */
	      /* fall through */
	      printf ("EIO\n");

	    default:
		printf ("default\n");
	      errno_exit ("read");
	    }

	  }
printf ("OK passed reading!\n");
  showFrame (buffers[0].start, buffers[0].length);

  return 1;
}

//-----------------------------------------------------------------------------

int InitFrameBuffer()
{
        int retval = TPASS;
        struct fb_var_screeninfo screen_info;
        struct fb_fix_screeninfo fb_fix;
        u_int32_t screensize = 0;

        if ((fd_fb = open("/dev/fb0", O_RDWR, 0)) < 0)
        {
                printf("Unable to open /dev/fb0\n");
                retval = TFAIL;
                return 1;
        }

        retval = ioctl(fd_fb, FBIOGET_VSCREENINFO, &screen_info);
        if (retval < 0)
        {
                return 1;
        }
        printf("Set the background to 16-bpp\n");
        screen_info.bits_per_pixel = 16;
        screen_info.yoffset = 0;
        retval = ioctl(fd_fb, FBIOPUT_VSCREENINFO, &screen_info);
        if (retval < 0)
        {
                return 1;
        }
	printf ("screen info xres=%d, yres=%d, bits per pixel =%d\n", screen_info.xres , screen_info.yres , screen_info.bits_per_pixel );
	screensize = screen_info.xres * screen_info.yres * screen_info.bits_per_pixel / 8;
        g_fb_size = screen_info.xres * screen_info.yres_virtual * screen_info.bits_per_pixel / 8;

        printf("\n Screen size = %u \n", screensize);

        /* Map the device to memory*/
        fb = (unsigned short *)mmap(0, g_fb_size,PROT_READ | PROT_WRITE, MAP_SHARED, fd_fb, 0);
        if ((int)fb <= 0)
        {
                printf("\nError: failed to map framebuffer device 0 to memory.\n");
                return 1;
        }


        //__u32 screensize;
        //int retval;
        //struct fb_var_screeninfo screen_info;

        retval = ioctl(fd_fb, FBIOGET_VSCREENINFO, &screen_info);
        if (retval < 0)
        {
	printf ("FBIOGET_VSCREENINFO failed\n");
                return;
        }

        printf("Set the BG to 24-bpp\n");
        screen_info.bits_per_pixel = 24;
        retval = ioctl(fd_fb, FBIOPUT_VSCREENINFO, &screen_info);
        if (retval < 0)
        {
                return;
        }
        printf("fb_test: xres_virtual = %d\n", screen_info.xres_virtual);
        screensize = screen_info.xres * screen_info.yres * screen_info.bits_per_pixel / 8;


return 0;
}



//-------------------------------------------------------------------------
static void singleloop(void) {
  unsigned int count;

//fprintf (stderr, "main loop started\n");
  count = 10000;

  while (count-- > 0) {
    for (;;) {
      fd_set fds;
      struct timeval tv;
      int r;

      FD_ZERO (&fds);
      FD_SET (fd, &fds);

      /* Timeout. */
      tv.tv_sec = 2;
      tv.tv_usec = 0;

      r = select (fd + 1, &fds, NULL, NULL, &tv);

      if (-1 == r) {
        if (EINTR == errno)
          continue;

        errno_exit ("select");
      }

      if (0 == r) {
        fprintf (stderr, "select timeout\n");
        exit (EXIT_FAILURE);
      }
/*
      if (read_frame ())
            break;
	    */
      read_frame ();


//fprintf (stderr, "main loop finished. \n");
      return;


      /* EAGAIN - continue select loop. */
    }
  }
}

static void uninit_device(void) {
  unsigned int i;

  free (buffers[0].start);
  free (buffers);
}

static void init_read(unsigned int buffer_size) {
  buffers = calloc (1, sizeof (*buffers));

  if (!buffers) {
    fprintf (stderr, "Out of memory\n");
    exit (EXIT_FAILURE);
  }

  buffers[0].length = buffer_size;
  buffers[0].start = malloc (buffer_size);

  if (!buffers[0].start) {
        fprintf (stderr, "Out of memory\n");
        exit (EXIT_FAILURE);
  }
}

//-----------------------------------------------------------------------

static void init_device(char *dev_name) {
  struct v4l2_capability cap;
  struct v4l2_cropcap cropcap;
  struct v4l2_crop crop;
  struct v4l2_format fmt;
  unsigned int min;

  //---- inserted by Alexeev -----------

        //struct v4l2_format fmt;
	//struct v4l2_crop crop;
        struct v4l2_control ctrl;
        struct v4l2_streamparm parm;
        //int fd = 0;
	//struct v4l2_mxc_offset off;
	struct v4l2_dbg_chip_ident chip;
	struct v4l2_frmsizeenum fsize;
	struct v4l2_fmtdesc ffmt;
//-----------end of paste-----------------


  if (-1 == xioctl (fd, VIDIOC_QUERYCAP, &cap)) {
    if (EINVAL == errno) {
      fprintf (stderr, "%s is no V4L2 device\n",
         dev_name);
      exit (EXIT_FAILURE);
    } else {
      errno_exit ("VIDIOC_QUERYCAP");
    }
  }

  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
    fprintf (stderr, "%s is no video capture device\n",
       dev_name);
    exit (EXIT_FAILURE);
  }

  if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
    fprintf (stderr, "%s does not support read i/o\n",
       dev_name);
    exit (EXIT_FAILURE);
  }

  /* Select video input, video standard and tune here. */

  CLEAR (cropcap);

  cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (0 == xioctl (fd, VIDIOC_CROPCAP, &cropcap)) {
    crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    crop.c = cropcap.defrect; /* reset to default */

    if (-1 == xioctl (fd, VIDIOC_S_CROP, &crop)) {
      switch (errno) {
      case EINVAL:
        /* Cropping not supported. */
        break;
      default:
        /* Errors ignored. */
        break;
      }
    }
  } else {
    /* Errors ignored. */
  }


  CLEAR (fmt);

  fmt.type    = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  fmt.fmt.pix.width       = 640; //640;//1280; //2592;//640;
  fmt.fmt.pix.height      = 480; //480; //720; // 1944; //480

// This worked with my capture card, but bombed with
// "VIDIOC_S_FMT error 22, Invalid argument" on my Logitech QuickCam Pro 4000
// fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
// This worked on the logitech:
  fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
  fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;

  if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt))
    errno_exit("VIDIOC_S_FMT");


    //---------------------Inserted by Alexeev -----------------------------

////	ov5642_mode_QSXGA_2592_1944 = 6,
int g_capture_mode=0;//2; //high resolution

/*
	ov5642_mode_MIN = 0,
	ov5642_mode_VGA_640_480 = 0,
	ov5642_mode_QVGA_320_240 = 1,
	ov5642_mode_NTSC_720_480 = 2,
	ov5642_mode_PAL_720_576 = 3,
	ov5642_mode_720P_1280_720 = 4,
	ov5642_mode_1080P_1920_1080 = 5,
	ov5642_mode_QSXGA_2592_1944 = 6,
	ov5642_mode_QCIF_176_144 = 7,
	ov5642_mode_MAX = 7
*/


	fsize.index = g_capture_mode;
	if (ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &fsize) < 0)
	{
	        printf("VIDIOC_ENUM_FRAMESIZES failed\n");
	        return;
	}

	printf("sensor frame size is %dx%d\n", fsize.discrete.width,
					       fsize.discrete.height);
	ffmt.index = g_capture_mode;
	if (ioctl(fd, VIDIOC_ENUM_FMT, &ffmt) < 0)
	{
	        printf("VIDIOC_ENUM_FMT failed\n");
	        return;
	}
	if (ffmt.pixelformat == V4L2_PIX_FMT_YUYV)
		printf("sensor frame format is YUYV\n");
	else if (ffmt.pixelformat == V4L2_PIX_FMT_UYVY)
		printf("sensor frame format is UYVY\n");

	parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; // even if we make G_PARM the type of parm we must set
	if (ioctl(fd, VIDIOC_G_PARM, &parm) < 0)
	{
	        printf("VIDIOC_G_PARM failed (parm type was %d) \n", parm.type);
	        //return;
	}
	/*
	printf ("G_PARM:\n");
	printf ("capability = %d\n", parm.parm.capture.capability);
	printf ("time per frame: denom=%d, numer=%d\n", parm.parm.capture.timeperframe.denominator, parm.parm.capture.timeperframe.numerator);
	printf ("capturemode = %d\n", parm.parm.capture.capturemode);
	*/

	parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	parm.parm.capture.timeperframe.numerator = 1;  //tgt_fps = timeperframe->denominator /  timeperframe->numerator;
	parm.parm.capture.timeperframe.denominator = 30;
	parm.parm.capture.capturemode = g_capture_mode;

/*
	printf ("S_PARM:\n");
	printf ("capability = %d\n", parm.parm.capture.capability);
	printf ("time per frame: denom=%d, numer=%d\n", parm.parm.capture.timeperframe.denominator, parm.parm.capture.timeperframe.numerator);
	printf ("capturemode = %d\n", parm.parm.capture.capturemode);
	*/


/* CANCELLED

	if (ioctl(fd, VIDIOC_S_PARM, &parm) < 0)
	{
	        printf("VIDIOC_S_PARM failed\n");
	        return;
	}

	printf ("VIDIOC_S_PARM succeeded\n");
	*/


// --------------------- end of paste---------------------------



  /* Note VIDIOC_S_FMT may change width and height. */

  /* Buggy driver paranoia. */
  min = fmt.fmt.pix.width * 2;
  if (fmt.fmt.pix.bytesperline < min)
    fmt.fmt.pix.bytesperline = min;
  min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
  if (fmt.fmt.pix.sizeimage < min)
    fmt.fmt.pix.sizeimage = min;

  init_read (fmt.fmt.pix.sizeimage);
}

static void close_device(void) {
  if (-1 == close (fd))
    errno_exit ("close");

  fd = -1;
}

static void open_device(char *dev_name) {
  struct stat st;

  if (-1 == stat (dev_name, &st)) {
    fprintf (stderr, "Cannot identify '%s': %d, %s\n",
       dev_name, errno, strerror (errno));
    exit (EXIT_FAILURE);
  }

  if (!S_ISCHR (st.st_mode)) {
    fprintf (stderr, "%s is no device\n", dev_name);
    exit (EXIT_FAILURE);
  }

  fd = open (dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);

  if (-1 == fd) {
    fprintf (stderr, "Cannot open '%s': %d, %s\n",
       dev_name, errno, strerror (errno));
    exit (EXIT_FAILURE);
  }
}


int main(int argc, char **argv) {


  if (InitFrameBuffer())
	{
	printf ("Error. Failed\n");
	return;
	}

while (1)
	{
  open_device(dev_name);
  init_device(dev_name);


  singleloop();

  uninit_device();
  close_device();

  //break;
	}

  close(fd_fb);


  exit(EXIT_SUCCESS);
  return 0;
}