/*
opti.c  serve data from OptiTrack motion capture system
*/

#pragma comment(lib, "Ws2_32") 

#define WIN32_LEAN_AND_MEAN
#define _CRT_SECURE_NO_WARNINGS

#include <windows.h>
#include <conio.h>

#include <iostream>
#include <stdio.h>
#include <sys/timeb.h>
#include <sys/types.h>

#include <errno.h>
#include <string.h>

#include <winsock2.h>
#include <ws2tcpip.h>

#include "inttypes.h"
#include "stdint.h"

#include <tchar.h>
#include "cameralibrary.h"
namespace cl = CameraLibrary;

#define MIN(X, Y)  ((X) < (Y) ? (X) : (Y))
//#define NAN 0.0
#ifdef WIN32
  #ifndef NAN
    static const unsigned long __nan[2] = {0xffffffff, 0x7fffffff};
    #define NAN (*(const float *) __nan)
  #endif
#endif

#define PORT "43949"   // port we're listening on
#define FPS 250        // camera frames per second
#define CMDLEN  4      // length of commands is 4 bytes, i.e. 1 long int
#define DATLEN  4      // length of one unit of data is 4 bytes, i.e. 1 float
#define NUMDAT  4      // count of data per object
#define MAXCAMS 12     // maximum number of cameras
#define MAXOBJS 99     // maximum number of objects per camera
#define NAMELEN 256    // length of data name
// maximum possible length of info
// header, name length & name for each object in each camera
#define MAXINFO ( CMDLEN + MAXCAMS * MAXOBJS * ( CMDLEN + NAMELEN ) ) 

//#define USECAMS false
#define USECAMS true

// commands for communication over socket
#define QUERY 1
#define INFO  2
#define START 3
#define STOP  4
#define DATA  5

enum ERR {ERRNAMES=1, ERRSIZEOF, ERRADDRINFO, ERRBIND, ERRCAMINIT, ERRLISTEN, ERRSELECT, ERRNOCAMS, ERRSYNC};

//enum DAT {DATX, DATY, DATA, DATR};

// get sockaddr, IPv4 or IPv6:
void *get_addr( struct sockaddr *sa )
{
  if (sa->sa_family == AF_INET) {
    return &(((struct sockaddr_in*)sa)->sin_addr);
  }

  return &(((struct sockaddr_in6*)sa)->sin6_addr);
}

void add_name( uint32_t * N, char names[][NAMELEN], char * name ) {
  if ( *N >= MAXOBJS ) {
    fprintf(stderr, "opti: ERROR too many objects, %d > %d; increase MAXOBJS\n", (*N)+1, MAXOBJS);
    exit(ERRNAMES);
  }

  memcpy( names[*N], name, strlen(name)+1 );
  (*N)++;
}

// initialize data names
void fill_names( uint32_t * N, int ncams, int nobjs,
                 char names[][NAMELEN], int camids[MAXCAMS], char dat[NUMDAT] ) {
  char name[NAMELEN], fmt[] = "dat='%c', cam=%d, obj=%d, units='px'";

  sprintf(name, "fps=%d, units='sec'", FPS);
  add_name( N, names, name );
  for ( int c=0; c < ncams; c++ ) {
    for ( int j=0; j < nobjs; j++ ) {
      for ( int d=0; d < NUMDAT; d++ ){
        sprintf(name, fmt, dat[d], camids[c], j);
        add_name( N, names, name );
      }
    }
  }
}

// fill info buffer
size_t fill_info( uint32_t * N, char names[][NAMELEN], char * info ) {
  uint32_t n;
  size_t i=2*CMDLEN, len;
  n = htonl(INFO);
  memcpy( &info[0 * CMDLEN], &n, CMDLEN );
  n = htonl(*N);
  memcpy( &info[1 * CMDLEN], &n, CMDLEN );
  for ( n = 0; n < *N; n++ ) {
    len = htonl(strlen(names[n]));
    memcpy( &info[i], &len, CMDLEN );
    memcpy( &info[i + CMDLEN], &names[n][0], strlen(names[n]) );
    i += CMDLEN + strlen(names[n]);
  }

  return i;
}

// fill data buffer
// hdr, N, t, x_1, y_1, a_1, r_1, ...
void fill_data( uint32_t * N, char * data, int ncams, int nobjs, cl::FrameGroup * fg ) {
  cl::Frame   * fr;
  cl::cObject * obj;

  uint32_t n;
  int d=0;
  float t, x, y, a, r;

  //printf("opti: filling data\n");

  // hdr
  n = htonl(DATA);
  memcpy( &data[d], &n, CMDLEN ); d += CMDLEN;
  // N
  n = htonl(*N);
  memcpy( &data[d], &n, CMDLEN ); d += CMDLEN;

  // inconsistent number of entries in data
  if ( (*N) != ncams*nobjs*NUMDAT + 1 ) {
    printf("opti: inconsistent data: N == %d != %d == ncams*nobjs + 1\n", *N, ncams*nobjs + 1);
    t = NAN;
    for ( n=0; n < (*N); n++ ) {
      memcpy( &data[d], &t, DATLEN ); d += DATLEN;
    }
    return;
  }

  // synchronized set of images
  //fg = sync->GetFrameGroup();
  if ( fg ) {
    t = (float) fg->TimeStamp();
    memcpy( &data[d], &t, DATLEN ); d += DATLEN;
    //printf("opti: frame group, ncams = %d, mode = %d, spread = %f\n", sync->CameraCount(), sync->LastFrameGroupMode(), sync->LastFrameGroupSpread());
  } else {
    //printf("opti: no frame group, ncams = %d, mode = %d, spread = %f\n", sync->CameraCount(), sync->LastFrameGroupMode(), sync->LastFrameGroupSpread());
    t = NAN;
    for ( n=0; n < (*N); n++ ) {
      memcpy( &data[d], &t, DATLEN ); d += DATLEN;
    }
    return;
  }
  
  // loop through frames
  for ( int f=0; f < ncams; f++ ) {
    if ( fg && ( f < fg->Count() ) ) {
      fr = fg->GetFrame(f);
    } else {
      fr = NULL;
    }
    // loop through objects
    for ( int j=0; j < nobjs; j++ ) {
      if ( fr && ( j < fr->ObjectCount() ) ) {
        obj = fr->Object(j);
        x = obj->X(); y = obj->Y(); a = obj->Area(); r = obj->Radius();
      } else {
        x = NAN; y = NAN; a = NAN; r = NAN;
      }

      // x_j, y_j, a_j, r_j
      memcpy( &data[d], &x, DATLEN ); d += DATLEN;
      memcpy( &data[d], &y, DATLEN ); d += DATLEN;
      memcpy( &data[d], &a, DATLEN ); d += DATLEN;
      memcpy( &data[d], &r, DATLEN ); d += DATLEN;
    } 
    if ( fr ) {
      fr->Release(); fr = NULL;
    }
  }

  if ( fg ) {
    // release frame group
    //fg->Release(); fg = NULL;
  }
}

class SyncListener : public cl::cModuleSyncListener {
  public:
    cl::cModuleSync * _sync;

    SyncListener(cl::cModuleSync * sync) {
      _sync = sync;
    };

    ~SyncListener() {};

    void FrameGroupAvailable() {
      printf("opti: FrameGroupAvailable()\n");
      cl::FrameGroup * fg = _sync->GetFrameGroup();
      fg->Release(); fg = NULL;
    }
};

int main(int argc, char *argv[]) {
  fd_set master_fds;// socket set master
  fd_set read_fds;  // socket set for select()
  fd_set data_fds;  // socket set subscribed to data stream
  int fdmax;        // maximum file descriptor number
  int nconn=0;        // number of active connections
  struct timeval timeout;

  int listener; // listening socket descriptor
  int newfd;    // newly accept()ed socket descriptor
  struct sockaddr_storage remoteaddr; // client address
  socklen_t addrlen;

  uint32_t cmd; // command buffer
  int nbytes;

  char remote[INET6_ADDRSTRLEN];

  uint32_t N=0;
  char names[MAXCAMS*MAXOBJS][NAMELEN], info[MAXINFO];
  char dat[NUMDAT+1]="xyar";
  int  nobjs=0, ncams=0, camids[MAXCAMS];
  char * data=NULL;

  size_t info_len, data_len;

  int yes=1;        // for setsockopt() SO_REUSEADDR, below
  int i, rv;

  struct addrinfo hints, *ai, *p;

  WSADATA wsaData;   // if this doesn't work

  cl::CameraList cams;
  //cl::cModuleSync sync;
  cl::cModuleSync * sync = NULL;
  cl::Camera  * cam = NULL;
  cl::Frame   * fr  = NULL;
  cl::cObject * obj = NULL;
  cl::FrameGroup * fg = NULL;

  if ( sizeof(float) != DATLEN ) {
    fprintf(stderr, "opti: ERROR sizeof float == %d != %d == DATLEN\n", sizeof(float), DATLEN);
    exit(ERRSIZEOF);
  }
  printf("opti: correct sizeof float\n");

  if (argc != 2) {
    fprintf(stderr,"usage: opti.exe hostname\n");
    std::cin.get();
    return 1;
  }
  printf("opti: correct commandline arguments\n");

  // windows-specific socket setup
  if (WSAStartup(MAKEWORD(2,0), &wsaData) != 0) {
    fprintf(stderr, "opti: ERROR WSAStartup failed.\n");
    std::cin.get();
    return 1;
  }
  printf("opti: WSAStartup succeeded\n");

  // initialize socket sets
  FD_ZERO(&master_fds);    
  FD_ZERO(&read_fds);
  FD_ZERO(&data_fds);
  printf("opti: initialized socket sets\n");

  // set timeout for socket listener
  timeout.tv_sec = 0;
  //timeout.tv_usec = 500000; // .5 sec
  timeout.tv_usec = 1500; // measured in microsec

  // get us a socket and bind it
  memset(&hints, 0, sizeof hints);
  hints.ai_family = AF_UNSPEC;
  hints.ai_socktype = SOCK_STREAM;
  hints.ai_flags = AI_PASSIVE;
  if ((rv = getaddrinfo(argv[1], PORT, &hints, &ai)) != 0) {
    fprintf(stderr, "opti: ERROR (getaddrinfo) %s\n", gai_strerror(rv));
    exit(ERRADDRINFO);
  }
  
  // bind listener to socket
  for(p = ai; p != NULL; p = p->ai_next) {
    listener = socket(p->ai_family, p->ai_socktype, p->ai_protocol);
    if (listener < 0) { 
      continue;
    }
    
    // lose the pesky "address already in use" error message
    setsockopt(listener, SOL_SOCKET, SO_REUSEADDR, (char *)&yes, sizeof(int));

    if (bind(listener, p->ai_addr, p->ai_addrlen) < 0) {
      closesocket(listener);
      continue;
    }

    break;
  }

  // socket failed to bind
  if (p == NULL) {
    fprintf(stderr, "opti: socket failed to bind\n");
    exit(ERRBIND);
  }
  // advertise listener
  printf("opti: listening for connections on %s:%s\n", p->ai_canonname, PORT);

  freeaddrinfo(ai); // done with local address info

  if ( USECAMS ) {
    // initialize OptiTrack cameras
    printf("opti: initializing cameras (wait 7.5 sec)\n");
    CameraLibrary_EnableDevelopment();
    cl::CameraManager::X().WaitForInitialization();
    Sleep( 7500 );
    
    if ( cl::CameraManager::X().AreCamerasInitialized() ) {
      printf("opti: cameras initialized\n");
    } else {
      fprintf( stderr, "opti: ERROR camera initialization failed\n" );
      exit(ERRCAMINIT);
    }

    cl::CameraManager::X().GetCameraList(cams);
    ncams = cams.Count();

    if ( ncams == 0 ) {
      fprintf(stderr, "opti: no cameras\n");
      exit(ERRNOCAMS);
    }
    
    sync  = new cl::cModuleSync();

    // synchronize cameras
    for ( int c=0; c < ncams; c++ ) {
      cam = cl::CameraManager::X().GetCamera(cams[c].UID());
      printf("opti: cam %d (%s : %d) %dx%d\n", c, cam->Model(), cam->Serial(), cam->Width(), cam->Height());
      camids[c] = cam->Serial();
      sync->AddCamera(cam);
    }

    // start cameras
    for ( int c=0; c < ncams; c++ ) {
      cam = cl::CameraManager::X().GetCamera(cams[c].UID());
      cam->SetVideoType(cl::ObjectMode);
      cam->SetFrameRate(FPS);
      cam->Start();
    }

    // allow cameras to sync
    //Sleep(2000);
  }

  // listen for first connection
  if (listen(listener, 10) == -1) {
    perror("listen");
    exit(ERRLISTEN);
  }
  printf("opti: acquired first listener\n");

  // add the listener to the master set
  FD_SET(listener, &master_fds);

  // biggest file descriptor
  fdmax = listener;

  // main loop
  while ( !_kbhit() ) {
    // copy socket set
    read_fds = master_fds; 
    // listen for incoming packets
    if ( select(fdmax+1, &read_fds, NULL, NULL, &timeout ) == -1) {
        perror("select");
        exit(ERRSELECT);
    }

    // loop through connections
    for ( i = 0; i <= fdmax; i++ ) {
      // read
      if ( FD_ISSET(i, &read_fds) ) {
        // new connection
        if ( i == listener ) {
          addrlen = sizeof remoteaddr;
          newfd = accept(listener, (struct sockaddr *)&remoteaddr, &addrlen);

          // accept failed
          if ( newfd == -1 ) {
              perror("accept");
          } 
          // accept succeeded
          else {
            // add to socket set
            FD_SET(newfd, &master_fds);
            if ( newfd > fdmax ) {
              fdmax = newfd;
            }
            printf("opti: (%d) connect from %s\n", newfd,
              inet_ntop(remoteaddr.ss_family, get_addr((struct sockaddr*)&remoteaddr),
                remote, INET6_ADDRSTRLEN));
          }
        }
        // client command
        else {
          // no command
          if ((nbytes = recv(i, (char *)&cmd, sizeof cmd, 0)) <= 0) {
            // connection closed by client
            if (nbytes == 0) {
              printf("opti: (%d) disconnect\n", i);
            } 
            // recv failed
            else {
              perror("opti: recv");
            }
            // close socket and remove from socket set
            closesocket(i);
            FD_CLR(i, &master_fds);
            if ( FD_ISSET(i, &data_fds) ) {
              FD_CLR(i, &data_fds);
              // decrease connection count
              nconn--;
            }
          }
          // command
          else {
            cmd = ntohl( cmd );

            // QUERY
            if ( cmd == QUERY ) {

              printf("opti: (%d) recv'd QUERY\n", i);

              // if this is the first connection
              if ( nconn == 0 ) {
                // build INFO
                printf("opti: (%d) building INFO\n", i);

                if ( USECAMS ) {
                  // count objects in cameras
                  fg = sync->GetFrameGroup();
                  if ( fg ) {
                    for ( int f=0; f < fg->Count(); f++ ) {
                      fr = fg->GetFrame(f);
                      obj = NULL;
                      if ( fr ) {
                        printf("opti: %d objects found in cam %d\n", fr->ObjectCount(), f);
                        // record maximum number of objects
                        if ( fr->ObjectCount() > nobjs ) {
                          nobjs = fr->ObjectCount();
                        }

                        for ( int j=0; j < fr->ObjectCount(); j++ ) {
                          obj = fr->Object(j);
                          
                          //printf("obj %d : (x=%0.2f,y=%0.2f,a=%0.2f,r=%0.2f) \n", j, obj->X(), obj->Y(), obj->Area(), obj->Radius());
                        }

                        fr->Release(); fr = NULL;
                      } else {
                        fprintf(stderr, "opti: failed to get frame %d\n", f);
                      }
                    }
                    // release frame group
                    fg->Release(); fg = NULL;
                  } else {
                    fprintf(stderr, "opti: failed to synchronize cameras\n");
                  }
                }

                // initialize data names
                N = 0;
                fill_names( &N, ncams, nobjs, names, camids, dat );

                //for ( n = 0; n < N; n++ ) {
                //  printf("opti: name[%2d] = %s\n", n, names[n]);
                //}

                // initialize info
                info_len = fill_info( &N, names, info );
                printf("opti: filled info buffer\n");
 
                // create data buffer
                // hdr, N, t, x_1, y_1, a_1, r_1, ...
                data_len = 2 * CMDLEN + N * DATLEN;
                data = (char *) malloc( data_len );
                printf("opti: created data buffer\n");
              }

              // send INFO
              printf("opti: (%d) send'ing INFO\n", i);
              if (send(i, info, info_len, 0) == -1) {
                perror("send INFO");
              }
            }
            // START
            else if ( cmd == START ) {
              if ( !FD_ISSET(i, &data_fds) ) {
                printf("opti: (%d) recv'd START, starting DATA stream\n", i);
                FD_SET(i, &data_fds);
                // increase connection count
                nconn++;
              } else {
                printf("opti: (%d) recv'd START, but DATA already streaming\n", i);
              }
            }
            // STOP
            else if ( cmd == STOP ) {
              if ( FD_ISSET(i, &data_fds) ) {
                printf("opti: (%d) recv'd STOP, stopping DATA stream\n", i);
                FD_CLR(i, &data_fds);
                // decrease connection count
                nconn--;
              } else {
                printf("opti: (%d) recv'd STOP, but DATA not streaming\n", i);
              }
            }
            // unknown
            else {
              printf("opti: (%d) recv'd unknown command %u\n", i, cmd);
            }
          }
        } // client command
      } // read

    } // loop through connections

    // if any DATA subscriptions
    if ( 1 && ( nconn > 0 ) ) {

      // get data from OptiTrack cameras
      fg = sync->GetFrameGroup();
      if ( fg ) {
        fill_data( &N, data, ncams, nobjs, fg );

        // loop through connections
        for ( i = 0; i <= fdmax; i++ ) {
          // if subscribed to data stream
          if ( FD_ISSET(i, &data_fds) ) {
            // send DATA
            if (send(i, data, data_len, 0) == -1) {
              perror("send DATA");
            }
            //// DBG unsubscribe
            //FD_CLR(i, &data_fds);
            //nconn--;
          } // if subscribed to data stream
        } // loop through connections

        fg->Release();
      }
    } // if any DATA subscriptions

    // allow threads to process
    Sleep(2);

  } // while ( !_kbhit() )

  if ( USECAMS ) {
    // free camera synchronization
    if ( sync ) {
      sync->RemoveAllCameras();
      delete sync;
    }

    // free cameras
    for ( int c=0; c < ncams; c++ ) {
      cam = cl::CameraManager::X().GetCamera(cams[c].UID());
      cam->Release(); cam = NULL;
    }

    // shut down camera library
    printf("opti: camera shutdown\n");
    cl::CameraManager::X().Shutdown();
  }

  // free data buffer
  free( data );
  data = NULL;

  WSACleanup();
  
  return 0;
}