#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>

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;


void* Collect(REND_STRUCT* prs){
	int i = 0;
	CG4Trk* pTrk = prs->pTrk;
	while (RunCollect){
		// periodically check for new hubs coming on line
		if(i % 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;
			}

		}
		i++;
		prs->hubreadmap=0;
		prs->tot_hubs_on_system=prs->pTrk->GetHubsPno(prs);
		char buffer[OUTPUT_BUFFER_SIZE];
		osc::OutboundPacketStream p( buffer, OUTPUT_BUFFER_SIZE );

		if(!use_text) {
			p << osc::BeginBundleImmediate;
		}
		for (int i=0;i<prs->numHub;i++){
			auto dat = prs->hubList[i].GetPnoData()[0];
			//printf("sns: %i: [%f;%f;%f],[%f,%f,%f]\n",i, dat[0],dat[1],dat[2], dat[3],dat[4],dat[5]);
			p << osc::BeginMessage( (string("/hub/") + std::to_string(i)).c_str() );
			std::ostringstream ss;
			if(use_text) {
				ss << "[" << dat[0] << "," << dat[1] << "," << dat[2] << "," << dat[3] << "," << dat[4] << "," << dat[5] << "]";
				auto res = ss.str();
				p << res.c_str();
			} else {
				p << dat[0] << dat[1] << dat[2] << dat[3] << dat[4] << dat[5];
			}
			p << osc::EndMessage;
		}
		if(!use_text) {
			p << osc::EndBundle;
		}
		transmitSocket->Send( p.Data(), p.Size() );
		usleep(15000);
		// usleep(8000);
	}

	return NULL;
}

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)
  );

  if(!parse(argc, argv, cli)) {
  	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;

  cfgFile="config.g4c";
  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;

  Collect(&rs);

  delete[] rs.srcList;
  delete[] rs.hubList;

  return 0;
}