// Include header files
#include "cv.h"
#include "highgui.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include <float.h>
#include <limits.h>
#include <time.h>
#include <ctype.h>

#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>

// Create memory for calculations
static CvMemStorage *storage = 0;

// Create a new Haar classifier
static CvHaarClassifierCascade *cascade = 0;

// Function prototype for detecting and drawing an object from an image
void detect_and_draw(IplImage *image);

// Create a string that contains the cascade name
const char *cascade_name = "haarcascade_frontalface_alt.xml";
/*    "haarcascade_profileface.xml";*/
int opensocket();
void closesocket(int iSocket);
int g_iSocket = -1;

void SetSpeed()
{
   printf("Medium\n");
   write(g_iSocket, "v", 1);
}

void MoveUp()
{
   printf("Up\n");
   write(g_iSocket, "w", 1);
}

void MoveDown()
{
   printf("Down\n");
   write(g_iSocket, "s", 1);
}

void MoveLeft()
{
   printf("Left\n");
   write(g_iSocket, "a", 1);
}

void MoveRight()
{
   printf("Right\n");
   write(g_iSocket, "d", 1);
}

void MoveStop()
{
   write(g_iSocket, "q", 1);
   printf("Stop\n");
}

void MoveFire()
{
   write(g_iSocket, " ", 1);
   printf("Fire\n");
}

// Main function, defines the entry point for the program.
int main(int argc, char **argv)
{

   // Structure for getting video from camera or avi
   CvCapture *capture = 0;

   // Images to capture the frame from video or camera or from file
   IplImage *frame, *frame_copy = 0;

   // Used for calculations
   int optlen = strlen("--cascade=");

   // Input file name for avi or image file.
   const char *input_name;

   // Check for the correct usage of the command line
   if (argc > 1 && strncmp(argv[1], "--cascade=", optlen) == 0)
   {
      cascade_name = argv[1] + optlen;
      input_name = argc > 2 ? argv[2] : 0;
   }
   else
   {
      fprintf(stderr,
              "Usage: facedetect --cascade=\"<cascade_path>\" [filename|camera_index]\n");
      return -1;
      /*input_name = argc > 1 ? argv[1] : 0;*/
   }

   // try to open a socket
   g_iSocket = opensocket();

   if (g_iSocket < 0)
   {
      printf("Error connecting to turret\n");
      return -1;
   }
   SetSpeed();
   // Load the HaarClassifierCascade
   cascade = (CvHaarClassifierCascade *)cvLoad(cascade_name, 0, 0, 0);

   // Check whether the cascade has loaded successfully. Else report and error and quit
   if (!cascade)
   {
      fprintf(stderr, "ERROR: Could not load classifier cascade\n");
      return -1;
   }

   // Allocate the memory storage
   storage = cvCreateMemStorage(0);

   // Find whether to detect the object from file or from camera.
   if (!input_name || (isdigit(input_name[0]) && input_name[1] == '\0'))
      capture = cvCaptureFromCAM(!input_name ? 0 : input_name[0] - '0');
   else
      capture = cvCaptureFromAVI(input_name);

   // Create a new named window with title: result
   cvNamedWindow("result", 1);

   // Find if the capture is loaded successfully or not.

   // If loaded succesfully, then:
   if (capture)
   {
      // Capture from the camera.
      for (;;)
      {
         // Capture the frame and load it in IplImage
         if (!cvGrabFrame(capture))
            break;
         frame = cvRetrieveFrame(capture);

         /*
         if (frame->origin == IPL_ORIGIN_TL)
            cvCopy(frame, frame_copy, 0);
         // Else flip and copy the image
         else
            cvFlip(frame, frame_copy, 0);
         */
         // Call the function to detect and draw the face
         detect_and_draw(frame);

         // Wait for a while before proceeding to the next frame
         if (cvWaitKey(10) >= 0)
            break;
      }

      // Release the images, and capture memory
      cvReleaseImage(&frame);
      cvReleaseCapture(&capture);
   }

   // Destroy the window previously created with filename: "result"
   cvDestroyWindow("result");

   // return 0 to indicate successfull execution of the program
   return 0;
}

// Function to detect and draw any faces that is present in an image
void detect_and_draw(IplImage *imgIn)
{
   int scale = 1;

   // Create a new image based on the input image
   IplImage *temp = cvCreateImage(cvSize(320, 240), imgIn->depth, imgIn->nChannels);
   IplImage *imgPrc = cvCreateImage(cvSize(320, 240), imgIn->depth, 1);
   cvResize(imgIn, temp, CV_INTER_LINEAR);
   cvCvtColor(temp, imgPrc, CV_RGB2GRAY);

   // cvConvert(img,temp);

   // Create two points to represent the face locations
   CvPoint pt1, pt2;
   int i;

   // Clear the memory storage which was used before
   cvClearMemStorage(storage);

   // Find whether the cascade is loaded, to find the faces. If yes, then:
   if (cascade)
   {

      // There can be more than one face in an image. So create a growable sequence of faces.
      // Detect the objects and store them in the sequence
      //		CvSeq* faces = cvHaarDetectObjects(imgPrc, cascade, storage, 1.1, 2,
      //				CV_HAAR_DO_CANNY_PRUNING, cvSize(40, 40));
      CvSeq *faces = cvHaarDetectObjects(imgPrc, cascade, storage);

      // Loop the number of faces found.
      for (i = 0; i < (faces ? faces->total : 0); i++)
      {
         // Create a new rectangle for drawing the face
         CvRect *r = (CvRect *)cvGetSeqElem(faces, i);

         // Find the dimensions of the face,and scale it if necessary
         pt1.x = r->x * scale;
         pt2.x = (r->x + r->width) * scale;
         pt1.y = r->y * scale;
         pt2.y = (r->y + r->height) * scale;

         // Draw the rectangle in the input image
         cvRectangle(imgPrc, pt1, pt2, CV_RGB(255, 0, 0), 3, 8, 0);

         // move turret in face direction
         if (i == 0)
         {
            // move x deviation
            if (imgPrc->width / 2 > pt1.x && imgPrc->width / 2 < pt2.x)
            {
               // printf("Face in rect X Axis\n");
               if (imgPrc->height / 2 > pt1.y && imgPrc->height / 2 < pt2.y)
               {
                  // ready to fire
                  // printf("Face in rect Y Axis\n");
                  MoveFire();
               }
            }

            // move in x direction
            if (imgPrc->width / 2 < pt1.x)
            {
               MoveRight();
            }
            else if (imgPrc->width / 2 > pt2.x)
            {
               MoveLeft();
            }
            else
            {
               // move in y direction
               if (imgPrc->height / 2 < pt1.y)
               {
                  MoveDown();
               }
               else if (imgPrc->height / 2 > pt2.y)
               {
                  MoveUp();
               }
            }
         }
      }
      if (i == 0)
         MoveStop();
   }

   // Show the image in the window named "result"
   cvShowImage("result", imgPrc);

   // Release the temp image created.
   cvReleaseImage(&temp);
}

int opensocket()
{
   int sockfd, portno;
   struct sockaddr_in serv_addr;
   struct hostent *server;

   portno = 8088;
   sockfd = socket(AF_INET, SOCK_STREAM, 0);
   if (sockfd < 0)
      printf("ERROR opening socket\n");
   server = gethostbyname("localhost");

   if (server == NULL)
   {
      fprintf(stderr, "ERROR, no such host\n");
      exit(0);
   }

   bzero((char *)&serv_addr, sizeof(serv_addr));
   serv_addr.sin_family = AF_INET;
   bcopy((char *)server->h_addr,
         (char *)&serv_addr.sin_addr.s_addr,
         server->h_length);
   serv_addr.sin_port = htons(portno);

   if (connect(sockfd, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0)
      printf("ERROR connecting\n");

   // setup speed slow
   write(sockfd, "v", 1);

   return sockfd;
}

void closesocket(int iSocket)
{
   close(iSocket);
}