#include <stdio.h>
#include <iostream>
#include "Quaternion.h"
#include "struct.h"
#include "G4Trk.h"
#include "G4Hub.h"
#include <string.h>
#include <unistd.h>
#include <memory>
#include "clipp.h"
#include <ip/UdpSocket.h>
#include <osc/OscOutboundPacketStream.h>
#include <sstream>
#include <signal.h>
#include <chrono>
#include <thread>

using namespace clipp; using std::cout; using std::string;

#define OUTPUT_BUFFER_SIZE 512

int RunCollect=0;
std::unique_ptr<UdpTransmitSocket> transmitSocket;
bool use_text = false;
int pollPeriod;


void* Collect(REND_STRUCT* prs){
	int iter = 0;
	CG4Trk* pTrk = prs->pTrk;
	while (RunCollect){
		// periodically check for new hubs coming on line
		if(iter % 1000 == 0) {
			int numHubs;
			if (prs->tot_hubs_on_system!=prs->numHub){
				prs->numHub=numHubs=prs->tot_hubs_on_system=pTrk->UpdateHubs();
				int colorInd=0;
				if (prs->hubList) {
					delete[] prs->hubList;
				}
				prs->hubList=new CG4Hub[numHubs];
				int* hubIds=new int[numHubs];
				pTrk->GetHubList(hubIds,numHubs);
				for (int i=0;i<numHubs;i++){
					prs->hubList[i].SetId(hubIds[i]);
				}
				delete[] hubIds;
			}

		}
		iter++;
		prs->hubreadmap=0;
		prs->tot_hubs_on_system=prs->pTrk->GetHubsPno(prs);
		char buffer[OUTPUT_BUFFER_SIZE];

		for (int i=0;i<prs->numHub;i++){
			osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );
			if(!use_text) {
				p << osc::BeginBundleImmediate;
			}
			p << osc::BeginMessage( (string("/g4/hub/") + std::to_string(prs->hubList[i].GetId()) + string("/digio")).c_str());
			std::ostringstream ss;
			if(use_text) {
				ss << prs->hubList[i].GetDigIOData();
				auto res = ss.str();
				p << res.c_str();
			} else {
				p << osc::int64(prs->hubList[i].GetDigIOData());
			}
			p << osc::EndMessage;
			auto dat = prs->hubList[i].GetPnoData();
			for(int j=0;j<G4_SENSORS_PER_HUB;j++) {
				if(!prs->hubList[i].IsSenActive(j)){
					continue;
				}
				p << osc::BeginMessage( (string("/g4/hub/") + std::to_string(prs->hubList[i].GetId()) + string("/") + std::to_string(j)).c_str());
				std::ostringstream ss;
				if(use_text) {
					ss << "[" << dat[j][0] << "," << dat[j][1] << "," << dat[j][2] << "," << dat[j][3] << "," << dat[j][4] << "," << dat[j][5] << "]";
					auto res = ss.str();
					p << res.c_str();
				} else {
					p << dat[j][0] << dat[j][1] << dat[j][2] << dat[j][3] << dat[j][4] << dat[j][5];
				}
				p << osc::EndMessage;
			}
			if(!use_text) {
				p << osc::EndBundle;
			}
			transmitSocket->Send( p.Data(), p.Size() );
		}
		std::this_thread::sleep_for(std::chrono::milliseconds(pollPeriod));
	}

	g4_close_tracker();
	return NULL;
}

void sighandler(int s){
	printf("Caught signal %d\n",s);
	RunCollect=0;
}

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

  CG4Trk trk;
  REND_STRUCT rs;
  string cfgFile;
  string oscSinkAddr;
  int oscSinkPort;

  auto cli = (
	option("-t", "--text").set(use_text).doc("send strings in osc messages instead of a float bundle"),
        value("c4g configuration file", cfgFile),
        value("osc sink address", oscSinkAddr),
        integer("osc sink port", oscSinkPort),
        integer("polling period in ms (imprecise). You realistically won't need less than 8 ms.", pollPeriod)
  );

  if(!parse(argc, argv, cli) || pollPeriod <= 0) {
  	cout << make_man_page(cli, argv[0]);
	return 1;
  }
  transmitSocket = std::make_unique<UdpTransmitSocket>( IpEndpointName( oscSinkAddr.c_str(), oscSinkPort ) );
  memset(rs.viewTrans,0,sizeof(float)*3);
  rs.srcScale=rs.senScale=0.5;
  rs.counter=0;

  int resp=trk.Connect(const_cast<char*>(cfgFile.c_str())); // whaaaaa
  if (!resp){
    printf("Error Connecting to G4\n");
    return -1;
  }
 
  usleep(500000);
  rs.numSrc=trk.GetNumSrc();
  rs.srcList=new float[rs.numSrc][6];
  trk.GetSrcLoc(rs.srcList);

  rs.numHub=trk.UpdateHubs();
  rs.hubList=new CG4Hub[rs.numHub];
  int* hubIds=new int[rs.numHub];
  trk.GetHubList(hubIds,rs.numHub);
  for (int i=0;i<rs.numHub;i++){
    rs.hubList[i].SetId(hubIds[i]);
  }
  delete[] hubIds;

  rs.pTrk=&trk;
 
  RunCollect=1;

  // register signal handler
  struct sigaction sigIntHandler;

  sigIntHandler.sa_handler = sighandler;
  sigemptyset(&sigIntHandler.sa_mask);
  sigIntHandler.sa_flags = 0;

  sigaction(SIGINT, &sigIntHandler, NULL);

  Collect(&rs);

  return 0;
}