#include "SupervisorServer.h"
#include "Syslog.h"
#include "VehicleConfigurator.h"


SupervisorServer::SupervisorServer()
  : SupervisorIF_SK()
{
    _output = new SupervisorOutput( SharedData::Read, True );
    _command = new SupervisorCommand( MessageQueue::ReadWrite );

}

SupervisorServer::~SupervisorServer()
{
     VehicleConfigurator::clearPID();
     VehicleConfigurator::clearIFName();
     delete _output;
     delete _command;
}


void SupervisorServer::setMission(SupervisorIF::PlanName mission)
{
    SupervisorCommand::Command cmd( SupervisorCommand::SetMission, mission, "");
    _command->write( &cmd );
}

void SupervisorServer::setMission(SupervisorIF::PlanName mission,
				  SupervisorIF::PlanName abortplan)
{
    SupervisorCommand::Command cmd( SupervisorCommand::SetMission,
				    mission, abortplan);
    _command->write( &cmd );
}

void SupervisorServer::setFlags(Boolean simulated,
				Boolean realTailCone)
{
    SupervisorCommand::Command cmd( SupervisorCommand::SetFlags,
				    simulated,
				    realTailCone);
    _command->write( &cmd );
}

void SupervisorServer::setDriverCommand(SupervisorIF::DriverName dname, Boolean turnOn)
{
    SupervisorCommand::Command cmd(SupervisorCommand::SetDriverCommand, dname, turnOn);

    _command->write( &cmd );
}

void SupervisorServer::startMission()
{
    SupervisorCommand::Command cmd( SupervisorCommand::StartMission);
    _command->write( &cmd );
}

void SupervisorServer::abortMission()
{
    SupervisorCommand::Command cmd( SupervisorCommand::AbortMission);
    _command->write( &cmd );
}

void SupervisorServer::killMission()
{
    SupervisorCommand::Command cmd( SupervisorCommand::KillMission);
    _command->write( &cmd );
}

void SupervisorServer::stopSupervisor()
{
    SupervisorCommand::Command cmd( SupervisorCommand::StopSupervisor);
    _command->write( &cmd );
}

void SupervisorServer::missionCompleted()
{
    SupervisorCommand::Command cmd( SupervisorCommand::MissionCompleted);
    _command->write( &cmd );
}



void SupervisorServer::deviceFailed( char * devName )
{
    SupervisorCommand::Command cmd( SupervisorCommand::DeviceFailed, devName);
    _command->write( &cmd );
}

void SupervisorServer::getCurrentLogDirectoryName( char *directoryName)
{
	_output->read();
	strcpy(directoryName, _output->data.directoryName);
	return;
}


void SupervisorServer::reloadConfig()
{
	// reloadTables() just erased us from the config, and we
	// won't get restarted, so we have to do this by hand.
	// This is a HACK!
	fprintf(stderr, "SupervisorServer::reloadConfig() - Re-registering pid & interface.");
	VehicleConfigurator::setPID();
	VehicleConfigurator::setIFName( name() );

    fprintf(stderr, "SupervisorServer::reloadConfig() - Registering interface...");
    if ( VehicleConfigurator::registerInterface(SupervisorIFServerName) )
    	fprintf(stderr, "SupervisorServer::reloadConfig() - Problem registering interface.");

	// Okay, now we've prepared supervisor to restart stuff
	SupervisorCommand::Command cmd( SupervisorCommand::ReloadConfig);
	_command->write( &cmd );

}

Boolean SupervisorServer::isMissionRunning()
{
	_output->read();

	return _output->data.isMissionRunning;
}

short SupervisorServer::getCycleCount()
{
	_output->read();

	return _output->data.cycleCount;
}