#include <stdint.h>
#include <signal.h>
#include <sys/time.h>
#include <string.h>
#include <pthread.h>
#include <stdlib.h>
#include <iostream>
#include "elmo.h"
#include "control_thread.h"
#include "user_interface_thread.h"
#include "logging_thread.h"
#include "pressure_sensor_thread.h"
#include "accelerometer_sensor_thread.h"
#include "motor_interogator.h"

using namespace std;

// Connected to the Motion Control Moxa, usually 192.168.0.2
const uint16_t	WinchPortNumber 			= 4001;
const uint16_t	LevelWindPortNumber			= 4002;

// Connected to the Sensor Moxa, usually 192.168.0.3
const uint16_t	PressureSensorPortNumber	= 4001;
const uint16_t	AccelerometerPortNumber		= 4002;

// TODO: Probably should rename "Elmo" to "Conduit" or something that
// indicates its real use.

Elmo	winch(true);
Elmo	levelWind(false);

int main(int argc, char** argv)
{
	// Turn off SIGPIPE signals to prevent the profiler application
	// from terminating if the connection to the ship through the Freewave
	// modems is broken.

	sigignore(SIGPIPE);

	if (argc != 6) {
		cout << "usage: profiler <Telnet Port #> <Labview Port #> <IP address of Motion Control Moxa>";
		cout << "<IP Address of Sensor Moxa> <IP Address of Labjack>" << endl;
		exit(1);
	}

	uint16_t					telnetPortNumber = atoi(argv[1]);
	uint16_t					labviewPortNumber = atoi(argv[2]);

	string						MotionControlMoxa_IP_Address(argv[3]);
	string						SensorMoxa_IP_Address(argv[4]);

	sigset_t					signalsToBlock;
	ControlThread				espProfiler(winch, levelWind);
	UserInterfaceThread 		userInterface(telnetPortNumber, winch, levelWind, espProfiler);
	LoggingThread				logger(labviewPortNumber, espProfiler, winch, levelWind);
	PressureSensorThread		pressureSensor(espProfiler);
	AccelerometerSensorThread	accelerationSensor(espProfiler);

	// Connect to the Elmo Motion Controllers
	// winch.Connect(MotionControlMoxa_IP_Address, WinchPortNumber);
	// levelWind.Connect(MotionControlMoxa_IP_Address, LevelWindPortNumber);

	// Connect to pressure and acceleration sensors
	pressureSensor.Connect(SensorMoxa_IP_Address, PressureSensorPortNumber);
	accelerationSensor.Connect(SensorMoxa_IP_Address, AccelerometerPortNumber);

	sigemptyset(&signalsToBlock);
	sigaddset(&signalsToBlock,SIGALRM);
	pthread_sigmask(SIG_BLOCK,&signalsToBlock, NULL);
	espProfiler.Start((void *) argv[5]);

	// Start the threads, logging, control and user interface
	pressureSensor.SampleRate(100);
	pressureSensor.Start(NULL);
	// accelerationSensor.Start(NULL);

	logger.Start(NULL);

	userInterface.Start(NULL);

	// Wait until the user has had enough
	userInterface.Wait();
	cout << "That's All Folks" << endl;
}

