/*
    Robot server - 2005 Jason Hunt
    
    This program is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License
    as published by the Free Software Foundation; either version 2
    of the License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

    Contact: huntjas2@msu.edu
*/

#include <linux/videodev.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <signal.h>

#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <pthread.h>
#include <memory.h>
#include <errno.h>


#include "server.h"
#include "camera.h"
#include "compress.h"

int camera_server_sd;  
volatile sig_atomic_t camera_client_connected = 0;

video_mbuf mbuf;
video_mmap vid_buffer;
unsigned char *vid_map = NULL;
int vid_fd = -1;


unsigned char encode_buff[max_x_size * max_y_size * 3];



#define  max   4000000
unsigned char compressed[max];   // compression output buffer

int image_x = 0, image_y = 0;


/* open and init the video device */
int setup_camera( void )
{
    // open video device
    if ((vid_fd = open("/dev/video", O_RDWR)) == -1) 
    {
        printf("[Camera] Unable to open /dev/video0\n");
        return(1);
    }

    video_capability capability;


    // get capability
    if (ioctl(vid_fd, VIDIOCGCAP,&capability) == -1)
    {
        printf("[Camera] VIDIOCGCAP failed\n");
        return(1);
    }

    printf("Device information:\n");
    printf("  Name: %s\n", capability.name);
    printf("  Channels : %d\n", capability.channels);
    printf("  Audios : %d\n", capability.audios);
    printf("  Size (%dx%d) to (%dx%d)\n", capability.minwidth, capability.minheight, 
                                          capability.maxwidth, capability.maxheight);

    if (capability.type & VID_TYPE_CAPTURE)     printf("  Can capture to memory\n");
    if (capability.type & VID_TYPE_TUNER )      printf("  Has a tuner of some form\n");
    if (capability.type & VID_TYPE_TELETEXT)    printf("  Has teletext capability\n");
    if (capability.type & VID_TYPE_OVERLAY )    printf("  Can overlay its image onto the frame buffer\n");
    if (capability.type & VID_TYPE_CHROMAKEY)   printf("  Overlay is Chromakeyed\n");
    if (capability.type & VID_TYPE_CLIPPING)    printf("  Overlay clipping is supported\n");
    if (capability.type & VID_TYPE_FRAMERAM)    printf("  Overlay overwrites frame buffer memory\n");
    if (capability.type & VID_TYPE_SCALES)      printf("  The hardware supports image scaling\n");
    if (capability.type & VID_TYPE_MONOCHROME)  printf("  Image capture is grey scale only\n");
    if (capability.type & VID_TYPE_SUBCAPTURE)  printf("  Capture can be of only part of the image\n");
    if (capability.type & VID_TYPE_MPEG_ENCODER)  printf("  Can encode to mpeg\n");
    if (capability.type & VID_TYPE_MJPEG_ENCODER)  printf("  Can encode to mjpeg\n");



/*    struct video_channel  vid_ch;
    vid_ch.channel = 0;
    vid_ch.type = VIDEO_TYPE_CAMERA;
    vid_ch.norm = 0;
    // change to webcam channel
    if(ioctl(vid_fd, VIDIOCSCHAN, &vid_ch) < 0)
    {
        printf("[Camera] VIDIOCSCHAN failed\n");
        return(1);
    }*/

    struct video_picture vid_pict;
    // get picture information
    if (ioctl(vid_fd, VIDIOCGPICT, &vid_pict) == -1)
    {
        printf("[Camera] VIDIOCGPICT failed\n");
        return(1);        
    }   
    
    printf("  Brightness %d\n", vid_pict.brightness);
    printf("  Hue %d\n", vid_pict.hue);
    printf("  Colour %d\n", vid_pict.colour);
    printf("  Contrast %d\n", vid_pict.contrast);
    printf("  Depth %d\n", vid_pict.depth);
    printf("  Palette:  ");
    
    if (vid_pict.palette == VIDEO_PALETTE_GREY)      printf("  VIDEO_PALETTE_GREY\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_HI240)     printf("  VIDEO_PALETTE_HI240\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_RGB565)    printf("  VIDEO_PALETTE_RGB565\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_RGB555)    printf("  VIDEO_PALETTE_RGB555\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_RGB24)     printf("  VIDEO_PALETTE_RGB24\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_RGB32)     printf("  VIDEO_PALETTE_RGB32\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_YUV422)    printf("  VIDEO_PALETTE_YUV422\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_YUYV)      printf("  VIDEO_PALETTE_YUYV\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_UYVY)      printf("  VIDEO_PALETTE_UYVY\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_YUV420)    printf("  VIDEO_PALETTE_YUV420\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_YUV411)    printf("  VIDEO_PALETTE_YUV411\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_RAW)       printf("  VIDEO_PALETTE_RAW\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_YUV422P)   printf("  VIDEO_PALETTE_YUV422P\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_YUV411P)   printf("  VIDEO_PALETTE_YUV411P\n"); else 
    if (vid_pict.palette == VIDEO_PALETTE_YUV420P)   printf("  VIDEO_PALETTE_YUV420P\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_YUV410P)   printf("  VIDEO_PALETTE_YUV410P\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_PLANAR)    printf("  VIDEO_PALETTE_PLANAR\n"); else
    if (vid_pict.palette == VIDEO_PALETTE_COMPONENT) printf("  VIDEO_PALETTE_COMPONENT\n");

//    vid_pict.palette = VIDEO_PALETTE_RGB565;
//    vid_pict.palette = VIDEO_PALETTE_YUV420P;
//    vid_pict.depth = 24;

    if (ioctl(vid_fd, VIDIOCSPICT, &vid_pict) == -1)
    {
        printf("[Camera] VIDIOCSPICT failed\n");
        return(1);
    }

    struct video_window vid_win;

    // get capture window information
    if (ioctl(vid_fd, VIDIOCGWIN, &vid_win) == -1)
    {
        printf("[Camera] VIDIOCGWIN failed\n");
        return(1);
    }

    printf("Capture (%dx%d)\n", vid_win.width, vid_win.height);

    image_x = vid_win.width;
    image_y = vid_win.height;

    if (ioctl(vid_fd, VIDIOCGMBUF, &mbuf) == -1)
    {
        printf("[Camera] VIDIOCGMBUF failed\n");
        return(1);        
    }
    
    vid_buffer.frame = 0;
    vid_buffer.width  = image_x;
    vid_buffer.height = image_y;
    vid_buffer.format = vid_pict.palette;

    printf("buffer size: %d ", mbuf.size);
    printf("with %d frames\n", mbuf.frames);
    
    // map the buffer
    vid_map = (unsigned char *) mmap(0, mbuf.size, PROT_READ | PROT_WRITE, MAP_SHARED, vid_fd, 0); 

    if (vid_map == NULL) 
    {
        printf("[Camera] mmap failed\n");
    	return(1);
    }

    // start capture of frame zero
    if (ioctl(vid_fd, VIDIOCMCAPTURE, &vid_buffer) == -1) 
    {
	    printf("[Camera] Unable to capture\n"); 
        return(1);
  	}

    return(0);
}
/* end of setup_camera */


/* return a pointer to the cutternt image frame */
void get_frame( void )
{
    // support for single frame buffer 
    if (mbuf.frames == 1)
    {
        // start capture of frame one
        if (ioctl(vid_fd, VIDIOCMCAPTURE, &vid_buffer) == -1) 
        {
    	    printf("[Camera] Unable to capture\n"); 
            return;
      	}

        // sync to frame zero
        int val = 0;
        if (ioctl(vid_fd, VIDIOCSYNC, &val) == -1) 
        {
            printf("[Camera] Unable to sync\n"); 
            return;
        }    
        return;
    }


    // normal double buffering
    if (vid_buffer.frame == 0)
    {
        vid_buffer.frame  = 1;

        // start capture of frame one
        if (ioctl(vid_fd, VIDIOCMCAPTURE, &vid_buffer) == -1) 
        {
    	    printf("[Camera] Unable to capture\n"); 
            return;
      	}

        vid_buffer.frame  = 0;
        // sync to frame zero
        int val = 0;
        if (ioctl(vid_fd, VIDIOCSYNC, &val) == -1) 
        {
            printf("[Camera] Unable to sync\n"); 
            return;
        }
        vid_buffer.frame  = 1; // next frame will be one       
    }
    else
    {
        vid_buffer.frame  = 0;

        // start capture of frame 
        if (ioctl(vid_fd, VIDIOCMCAPTURE, &vid_buffer) == -1) 
        {
    	    printf("[Camera] Unable to capture\n"); 
            return;
      	}

         vid_buffer.frame  = 1;
        // sync to frame one
        int val = 1;
        if (ioctl(vid_fd, VIDIOCSYNC, &val) == -1) 
        {
            printf("[Camera] Unable to sync\n"); 
            return;
        }
        
         vid_buffer.frame  = 0; // next frame will be zero
    }
}
/* end of get frame */





/* converyt the buffer to rbg */
// found on internet, unknown author
void convert_to_rgb(unsigned char *in, unsigned char *out, unsigned int x_size, unsigned int y_size)
{
    int tmp;
    unsigned char *y, *u, *v, *r, *g, *b;
    r=out+2;  g=out+1;
    b=out;    y=in;
    u=y+x_size*y_size;
    v=y+x_size*y_size+(x_size*y_size)/4;

    for (int i=0; i<x_size*y_size; i++) 
    {
        tmp=(((int)*y-16)*76284 + 104595*((int)*u-128)) >>16;
        if (tmp<0) tmp=0; if (tmp>255) tmp=255;
        *b=tmp;
        tmp=(((int)*y-16)*76284 - 53281*((int)*u-128) - 25625*((int)*v-128)) >>16;
        if (tmp<0) tmp=0; if (tmp>255) tmp=255;
        *g=tmp;
        tmp=(((int)*y-16)*76284 + 132252*((int)*v-128)) >>16;
        if (tmp<0) tmp=0; if (tmp>255) tmp=255;
        *r=tmp;
        if (i & 1) {u++;v++;}
        if (i%(x_size*2)==y_size-1) {	u-=x_size/2; v-=x_size/2;  }
        r+=3;    g+=3;
        b+=3;    y++;
    }
}
/* end of convert_to_rgb */


/* send camera image */
int send_camera_image( int client_sd )
{
    get_frame();
    
    if (vid_map == NULL)
    {
        printf("[Camera] Unable to get camera frame\n");
        return(0);
    }

    unsigned int out_size = 0;

    // convert to rgb before compression
    if (vid_buffer.format == VIDEO_PALETTE_YUV420P)
    {
        convert_to_rgb(vid_map, encode_buff, image_x, image_y);
        compress_frame(encode_buff, compressed, max, &out_size, image_x, image_y);
    }
    else
        compress_frame(vid_map, compressed, max, &out_size, image_x, image_y);

    // create the packet      
    char comm[] = { 0x40,               // send compressed image
                    image_x >> 8,    // x_size high byte
                    image_x & 0xff,  // x_size low byte
                    image_y >> 8,    // y_size high byte
                    image_y & 0xff,  // y_size low byte
       
                    out_size >> 24,     // high     set the data _size
                    out_size >> 16,     //
                    out_size >> 8,      // 
                    out_size & 0xff};   // low
 
    // send information packet
    if (send(client_sd, comm, 9, 0) == -1)
    {
        printf("[Camera] Send error\n");
        camera_client_connected = 0;
        return(1);
    }
    
    // send compressed frame
    if (send(client_sd, compressed, out_size, 0) == -1)
    {
        printf("[Camera] Send error\n");
        camera_client_connected = 0;
        return(1);
    }
    
/*    FILE *f = fopen("test.jpeg", "wb");
    fwrite(compressed, out_size, 1, f);
    fclose(f);*/
        
    
    return(0);
}
/* end of send camera image */


/* gee data from the client */
int camera_get_data(int sd, char *data)
{
    return(recv(sd, data, REC_BUFFER, 0));
}
/* end of get_data */


/* catch network error signal */
void catch_camera_neterror( int sig )
{
    printf("[Camera] Unable to send data\n");

    camera_client_connected = 0;

    signal (sig, catch_camera_neterror);
}
/* end of catch network error */


/* init the network */
int setup_camera_server()
{
    signal (SIGPIPE, catch_camera_neterror);
    signal (SIGURG, catch_camera_neterror);

    // setup the camera first
    setup_camera();

    camera_server_sd = socket(AF_INET, SOCK_STREAM, 0);

    if(camera_server_sd < 0) 
    {
        printf("[Camera] Unable to open socket\n");
        return(1);
    }

    sockaddr_in server_addr; 

    memset(&server_addr, 0, sizeof(server_addr));

    server_addr.sin_family = AF_INET;
    server_addr.sin_addr.s_addr = INADDR_ANY;
    server_addr.sin_port = htons(CAMERA_PORT);
  
      // set socket to resuse
    int reuse = 1;
    int reuse_len = sizeof(int);

    if (setsockopt(camera_server_sd, SOL_SOCKET, SO_REUSEADDR, &reuse, reuse_len) == -1)
    {
        printf("[Camera] Unable to set REUSE\n");
        return(1);
    }
   
    if(bind(camera_server_sd, (struct sockaddr *) &server_addr, sizeof(server_addr)) == -1) 
    {
        printf("[Camera] Unable to bind\n");
        return(1);
    }
    
    if(listen(camera_server_sd, 1) == -1)  // listen for one only client
    {
        printf("[Camera] Unable to listen\n");
        return(1);
    }
 
    return(0);   
}
/* end of setup network */


/* wait for a client to connect */
int camera_wait_for_client()
{
    printf("[Camera] Waiting for connection\n");

    sockaddr_in client_addr;
    socklen_t client_len = sizeof(sockaddr_in);

    return(accept(camera_server_sd, (struct sockaddr *) &client_addr, &client_len));
}
/* end of wait for client */


/* core loop for clients */
void camera_network_loop(int client_sd)
{
    camera_client_connected = 1;    // we are now connected

    char data[REC_BUFFER];
        
    while(!program_done && camera_client_connected) // n == 0, client disconnect
    {
        int n = recv(client_sd, data, REC_BUFFER, 0);

        if (n < 1) // , no data
        {
            printf("[Camera] Unable to get data\n");
            camera_client_connected = 0;
        } else

        {
            // parse client commands
            if (data[0] == 0x40) // send image
            {
            //    printf("Send request\n");      
                if (send_camera_image(client_sd))
                    camera_client_connected = 0;
            }
        } 
    }

    close(client_sd);               

    camera_client_connected = 0;    // no longer connected
}
/* end of camera_client_loop */


/* core network thread */
void *camera_server(void *threadid)
{
    printf("[Camera] Server started\n");

    // core server loop
    while(!program_done) 
    {
        // client socket
        int client_sd = camera_wait_for_client();

        if (client_sd < 0)
        {
            printf("[Camera] Unable to accept\n");
            break;
        }

        printf("[Camera] Client connected\n");

        // core loop
        // we will be creating a new process here for 
        // multiple clients
        camera_network_loop(client_sd);

        printf("[Camera] client disconnect\n");
    }

    close(camera_server_sd);
  
    close(vid_fd);

    printf("[Camera] Server shutdown\n");

    pthread_exit(NULL); // exit thread
}
/* end of handle_network */



