#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/types_c.h>

#include <iostream>
#include <vector>

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <arpa/inet.h>
#include <unistd.h>

using namespace cv;

int toInt(char* b, int offset){
  int v = 0;
  v += ((int)b[offset+0] & 0x000000FF) << 24;
  v += ((int)b[offset+1] & 0x000000FF) << 16;
  v += ((int)b[offset+2] & 0x000000FF) <<  8;
  v += ((int)b[offset+3] & 0x000000FF) <<  0;
  return v;
}

int main(int argc, char** argv) {
    int sock, numrcv;
    struct sockaddr_in addr;    
    sock = socket(AF_INET, SOCK_DGRAM, 0);
    addr.sin_family = AF_INET;
    addr.sin_port = htons(16384);
    addr.sin_addr.s_addr = INADDR_ANY;
    bind(sock, (struct sockaddr *)&addr, sizeof(addr));

    cvNamedWindow("Receive", CV_WINDOW_AUTOSIZE);

    //cv::Mat image = cv::Mat(480,640,CV_8UC3);
    static const int receiveSize = 65500;

    char buf[2048];

    char raw[640*480*2];
    unsigned char rgb[640*480*3];

    CascadeClassifier cascade;
    cascade.load(argv[1]);
    vector<Rect> faces;

    for(;;){
      
      for(int i = 0; i < 480; i++){
	numrcv = recv(sock, buf, 2048, 0);
	int y = toInt(buf, 0);
	for(int j = 0; j < 640 * 2; j++){
	  raw[y * 640 * 2 + j] = buf[j + 4];
	}
      }
      
      int r, g, b, y0, y1, cb, cr;
      for(int i = 0; i < 320; i++){
	for(int j = 0; j < 480; j++){
	  y0 = ((int)raw[j*640*2+4*i+0]) & 0x000000FF;
	  cb = ((int)raw[j*640*2+4*i+1]) & 0x000000FF;
	  y1 = ((int)raw[j*640*2+4*i+2]) & 0x000000FF;
	  cr = ((int)raw[j*640*2+4*i+3]) & 0x000000FF;
	  
	  r = (int)(y0                        + 1.40200 * (cr - 128));
	  g = (int)(y0 - 0.34414 * (cb - 128) - 0.71414 * (cr - 128));
	  b = (int)(y0 + 1.77200 * (cb - 128));
	  
	  r = r < 0 ? 0 : r; r = r > 255 ? 255 : r;
	  g = g < 0 ? 0 : g; g = g > 255 ? 255 : g;
	  b = b < 0 ? 0 : b; b = b > 255 ? 255 : b;
	  
	  rgb[3*((2*i)+640*j)+0] = b;
	  rgb[3*((2*i)+640*j)+1] = g;
	  rgb[3*((2*i)+640*j)+2] = r;
	  
	  r = (int)(y1                        + 1.40200 * (cr - 128));
	  g = (int)(y1 - 0.34414 * (cb - 128) - 0.71414 * (cr - 128));
	  b = (int)(y1 + 1.77200 * (cb - 128));
	  
	  r = r < 0 ? 0 : r; r = r > 255 ? 255 : r;
	  g = g < 0 ? 0 : g; g = g > 255 ? 255 : g;
	  b = b < 0 ? 0 : b; b = b > 255 ? 255 : b;
	  rgb[3*((2*i+1)+640*j)+0] = b;
	  rgb[3*((2*i+1)+640*j)+1] = g;
	  rgb[3*((2*i+1)+640*j)+2] = r;
	}
      }

      cv::Mat image = cv::Mat(Size(640, 480), CV_8UC3, rgb);
      cv::imshow("Receive", image);

      cv::Mat gray_image;
      cv::cvtColor(image, gray_image, CV_BGR2GRAY, 0);
      cv::imshow("Gray", gray_image);

      cv::Mat sobel_x, sobel_y, sobel;
      Sobel(gray_image, sobel_x, CV_32F, 1, 0);
      Sobel(gray_image, sobel_y, CV_32F, 0, 1);
      sobel = (abs(sobel_x) + abs(sobel_y)) / 2.0;
      cv::threshold(sobel, sobel, 100, 255, THRESH_BINARY_INV);
      cv::imshow("Sobel", sobel);

      faces.clear();
      cascade.detectMultiScale(image, faces, 1.1, 2, 0, Size(4, 4));
      for (int i = 0; i < faces.size(); i++){
	rectangle(image,
		  Point(faces[i].x,faces[i].y),
		  Point(faces[i].x + faces[i].width,faces[i].y + faces[i].height),
		  Scalar(0, 200, 0),
		  3,
		  CV_AA);
      }
      cv::imshow("Face", image);
      
      cvWaitKey(10);

    }

    close(sock);

    return 0;
}