/*
 */

/* Class definition */
#include "aimsun_ds.h"
#include "tools.h"

// TODO: reference additional headers your program requires here
#include <process.h>
#include <strsafe.h>
#include <shlwapi.h>
#include <io.h>

//#include <atlbase.h>

#define MAX_PATH_BYTES	(MAX_PATH*sizeof(TCHAR))
#define MAX_EXE_BYTES	(MAX_EXE_PATH*sizeof(TCHAR))

/* Contoller list is hardwired for Aimsun scenario `zlicin_495_601.sce` */
#define NUM_CONTROLLERS	2
const TCHAR * ControllerList[NUM_CONTROLLERS] = { TEXT("K_5_495"), TEXT("K_5_601") };
const TCHAR * WindowStr[NUM_CONTROLLERS]      = { TEXT("els3@5.495"), TEXT("els3@5.601") };
const int IntersectionIDs[] = { 495, 601 };
const int NumLanes[] = { 6, 6 };
const int NumIntersections = sizeof ( IntersectionIDs )/sizeof(int);

/* Entrance sections are hardwired for Aimsun scenario `zlicin_495_601.sce` */
const int EntranceSections[] = { 1, 42, 45, 287, 26 };
const int NumEntranceSections = sizeof(EntranceSections)/sizeof(int);

/* Identifiers of sections that Aimsun shall collect statistics for are also
   hardwired for the scenario `zlicin_495_601.sce`. */
const int StatIds[] = {
	1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20,
	26, 27, 35, 28, 29, 30, 31, 32, 33, 34, 39, 36, 37, 38, 21, 22, 23, 24, 25, 40,
	42, 43, 44, 45, 49, 46, 47, 50, 51, 287, 288, 54, 55, 56, 57, 58, 59, 60, 61,
	62, 63, 64, 65, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77 };
const int NumStatIds = sizeof(StatIds)/sizeof(int);

const int LaneQueuesSectIds[] = {
	1,	2,	3,	6,	7,
	3,	4,	5,
	35,	28,	32,	33,	34,
	26,	27,	35,	28,	29,	30,	31,
	16,	17,	18,	19,	20,	14,	15,
	16,	17,	18,	19,	20,	12,	13,
	21,	22,	23,	24,	25,	40,
	21,	22,	23,	24,	25,	40,
	61,	62,	63,	64,	65,
	287,288,54,	55,	56,	57,	58,	59,	60,	61,	62,	63,	64,	65,
	45,	49,	51,
	45,	49,	50,
	43,	44,
	43,	44,	42 };
const int NumLaneQueuesSectIds = sizeof(LaneQueuesSectIds)/sizeof(int);

/* This array contains offsets of particular lanes stored in LaneQueuesSectIds,
   plus the would-be offset of the next non-existent lane (this is used to
   set the limits of the index when extracting statistical data from the
   section_stats structure). */
int LaneQueueOffsets[] = {
	0, 5, 8, 13, 20, 27,
	34, 40, 46, 51, 65, 68, 70, 72, NumLaneQueuesSectIds+1 };

/* The position of the statistics is given by the indices in the StatIds
   array. In order to parse the statistics correctly we have to create
   a copy of LaneQueuesSectIds holding indices to the StatIds array instead
   of the original section ids. */
int * LaneQueuesStatPos;

const int LaneQueuesLaneIds[] = {
	1,	1,	2,	1,	1,
	1,	1,	1,
	2,	2,	1,	1,	1,
	1,	1,	1,	1,	1,	1,	1,
	2,	2,	2,	2,	2,	1,	1,
	1,	1,	1,	1,	1,	1,	1,
	2,	2,	2,	2,	2,	2,
	1,	1,	1,	1,	1,	1,
	2,	2,	2,	2,	2,
	1,	1,	1,	1,	1,	1,	1,	1,	1,	1,	1,	1,	1,	1,
	2,	2,	1,
	1,	1,	1,
	2,	2,
	1,	1,	1 };


const TCHAR * PatternsELS3[] = { TEXT("GetramELS3.dll"), TEXT("GetramELS3d.dll") };
const TCHAR * PatternsVGS[] = { TEXT("GetramVGS.dll"), TEXT("GetramVGSd.dll") };

/* Offsets of `dt` components (dt is the vector returned by AimsunDS, it contains composite
   values of all measurements for all intersections in the system). */
const int SignalPlanOffsets[]  = {  0, 42 };
const int QueueLengthOffsets[] = {  6, 48 };
const int MeasurementOffsets[] = { 12, 54 };

AimsunDS::AimsunDS () : DS()
{
  /* Description of the channels of the data vector. */
  Drv = RV ( "{"
    "495_VA  495_VB   495_VC   495_VD   495_VE   495_VF "
    "495_QA  495_QB   495_QC   495_QD   495_QE   495_QF "
    "495_DVA 495_DVB  495_DVA1 495_DVB1 495_DVC  495_DVD  495_DVE  495_DVF  495_DVF1 "
    "495_S1  495_S2   495_S3   495_S4   495_S5   495_S5a "
    "601_VA  601_VB   601_VC   601_VD   601_VE   601_SE "
    "601_QA  601_QB   601_QC   601_QD   601_QE   601_QSE "
	"601_DVA 601_DVAa 601_DVB  601_DVBa 601_DVB1 601_DVC  601_DVD  601_DVD1 601_DSE 601_DVE 601_DSE1 601_DVE1 "
    "601_S6  601_S6a  601_S7   601_S8   601_S9   601_S9a "
    "}",
	"    1,      1,      1,      1,      1,       1,      "
	"    1,      1,      1,      1,      1,       1,      "
	"    2,      2,      2,      2,      2,       2,      2,      2,      2,"
	"    2,      2,      2,      2,      2,       2,"
	"    1,      1,      1,      1,      1,       1,      "
	"    1,      1,      1,      1,      1,       1,      "
	"    2,      2,      2,      2,      2,       2,       2,       2,       2,       2,       2,       2,"
	"    2,      2,      2,      2,      2,       2"
	);
  /* Remember the size of the data vector. */
  dtsize = Drv._dsize();

  /* Description of the channels of the input vector. */
  Urv = RV ( "{"
	  "Tc"
	  "495_offset"
	  "495_VA  495_VB   495_VC   495_VD   495_VE   495_VF "
	  "601_offset"
	  "601_VA  601_VB   601_VC   601_VD   601_VE   601_SE "
	  "}" );
  /* Remember the size of the input vector. */
  utsize = Urv._dsize();

  /* Initialise the pointer to received data. */
  p_rsp = NULL;
}

void AimsunDS::from_setting ( const Setting &cfg )
{
	/* Check the `stop time` field in configuration. */
	UI::get ( stopTime, cfg, "stop_time" );

	/* Query the configuration file for the name of the file containsing
	   vehicle entrances. */
	UI::get ( entranceFileName, cfg, "entrances" );
}

void AimsunDS::validate ()
{
	HKEY  hKey;
	TCHAR szBasePath[MAX_PATH];		/**< Base path of the whole application, root of the tree. */
	TCHAR szDllPath[MAX_PATH];		/**< Directory where all DLLs reside. Will be added to PATH. */
	TCHAR szSceDllPath[MAX_PATH];	/**< Temporary storage for DLL path rewritten in the scenario file. */
	TCHAR szSceDir[MAX_PATH];		/**< Directory of the simulated scenario. */
	TCHAR szScePath[MAX_PATH];		/**< Full path to the Aimsun scenario file. The file resides in `szSceDir`. */
	TCHAR szScePathA[MAX_PATH];
	TCHAR szScePathB[MAX_PATH];
	TCHAR szScePathC[MAX_PATH];
	TCHAR szNetPath[MAX_PATH];
	TCHAR szHomeDir[MAX_PATH];		/**< Installation directory of Aimsun 4.2.x */
	TCHAR szELS3Path[MAX_PATH];		/**< Full path of the simulator of ELS3 controller. */
	TCHAR szExePath[MAX_EXE_PATH];	/**< Command line when staring ELS3 controllers. */
	TCHAR szEntrancePath[MAX_PATH];	/**< Points to the CSV file with vehicle entrances for the simulation. */
	DWORD dwBufLen = MAX_PATH_BYTES;
	LONG  res;

	intptr_t hPid;

	int verIndex = 1; //@TODO@: 0 is for release, 1 for debug build of the DLL libraries.

	/* The root directory of the simulator tree is defined externally
	   in "CMakeLists.txt". */
	StringCbCopy ( szBasePath, MAX_PATH_BYTES, BASE_PATH );

	/* Create the path to DLL directory. The path is defined externally
	   in  "CMakeLists.txt".
	   The path will be used to inject the appropriate location of
	   Getram extensions into the selected Aimsun scenario and also
	   to modify PATH variable of executed processes in order to
	   provide access to API DLLs. */
	StringCbCopy ( szDllPath, MAX_PATH_BYTES, DLL_PATH );

	/* Add the DLL directory to the PATH used by this process.
	   The PATH will be passed to all other processes started
	   by this one and hence they will be able to find API
	   DLLs. */
	_addpath ( szDllPath );

#ifdef LATER_OR_NEVER
	GetPrivateProfileString (
		TEXT("paths"),
		TEXT("base"),
		NULL,
		szBasePath,
		MAX_PATH,
		TEXT(".\\simulate.ini")
		);
#endif

	/* Find the location of Aimsun executable. It is stored in registry tree as
	   \\HKEY_LOCAL_MACHINE\SOFTWARE\TSS-Transport Simulation Systems\GETRAM\4.2.0 */
	res = RegOpenKeyEx (
		HKEY_LOCAL_MACHINE,
		TEXT("SOFTWARE\\TSS-Transport Simulation Systems\\GETRAM\\4.2.0"),
		0,
		KEY_QUERY_VALUE,
		&hKey
		);
	if ( res != ERROR_SUCCESS )
	{
		perror ( "Cannot get handle to Aimsun registry entry" );
		return;
	}

	/* The executable location is `HomeDir` key. */
	res = RegQueryValueEx (
		hKey,
		TEXT("HomeDir"),
        NULL, NULL,
		(LPBYTE) szHomeDir,
		&dwBufLen
		);
	RegCloseKey( hKey );
    if (( res != ERROR_SUCCESS ) || ( dwBufLen > MAX_PATH_BYTES ))
	{
		perror ( "Cannot read the Aimsun home directory from registry" );
        return;
	}

	/* Concatenate the home path with the executable name. */
	StringCbCat ( szHomeDir, MAX_PATH_BYTES, TEXT("\\aimsun42.exe") );

	/* Create the version of home path including the quotation marks. */
	StringCbCopy ( szExePath, MAX_EXE_PATH, TEXT("\"") );
	StringCbCat ( szExePath, MAX_EXE_PATH, szHomeDir );
	StringCbCat ( szExePath, MAX_EXE_PATH, TEXT("\"") );

	/* Create the path to ELS3 executable. */
	StringCbCopy ( szELS3Path, MAX_PATH_BYTES, TEXT("\"") );
	StringCbCat ( szELS3Path, MAX_PATH_BYTES, szBasePath );
	StringCbCat ( szELS3Path, MAX_PATH_BYTES, TEXT("\\els3\\els3sample.exe\"") );

	/* Aimsun scenario modification.
	   We have to take care of items that are specified by absolute
	   paths in the scenario file. These items include:
	   - network path (section #NETWORK),
	   - ELS3 Getram extension DLL (section #EXTENSIONS),
	   - VGS Getram extension DLL (section #EXTENSIONS).

	   We will start with constructing the new network path.*/
	StringCbCopy ( szSceDir, MAX_EXE_PATH, szBasePath );
	StringCbCat ( szSceDir, MAX_EXE_PATH, TEXT("\\scenarios\\zlicin_495_601") );
	StringCbCopy ( szNetPath, MAX_EXE_PATH, szSceDir );
	StringCbCat ( szNetPath, MAX_EXE_PATH, TEXT("\\zlicin_495_601") );

	/* Our convention is that the scenario file name corresponds
	   to the network path name. */
	StringCbCopy ( szScePath, MAX_EXE_PATH, szNetPath );
	StringCbCat ( szScePath, MAX_EXE_PATH, TEXT(".sce") );

	/* First modification step: Replace the location of ELS3 Getram
	   extension.
	   Start with constructing full path to the extension DLL.*/
	StringCbCopy ( szSceDllPath, MAX_EXE_PATH, szDllPath );
	StringCbCat ( szSceDllPath, MAX_EXE_PATH, TEXT("\\") );
	StringCbCat ( szSceDllPath, MAX_EXE_PATH, PatternsELS3[verIndex] );
	/* We are not allowed to modify scenario in place (it is part
	   of SVN snapshot and we do not want our local changes to
	   propagate to the trunk with every commit). Therefore we
	   need an alternative scenario name that will be constructed
	   now. */
	StringCbCopy ( szScePathA, MAX_EXE_PATH, szScePath );
	StringCbCat ( szScePathA, MAX_EXE_PATH, TEXT(".a") );
	/* And do the replacements. */
	replace_in_scenario (
		szScePath, szScePathA,
		TEXT("#EXTENSIONS"),
		PatternsELS3,
		2,
		szSceDllPath
		);

	/* Second modification step.
	   Replace the location of VGS Getram extension. */
	StringCbCopy ( szSceDllPath, MAX_EXE_PATH, szDllPath );
	StringCbCat ( szSceDllPath, MAX_EXE_PATH, TEXT("\\") );
	StringCbCat ( szSceDllPath, MAX_EXE_PATH, PatternsVGS[verIndex] );
	StringCbCopy ( szScePathB, MAX_EXE_PATH, szScePath );
	StringCbCat ( szScePathB, MAX_EXE_PATH, TEXT(".b") );
	replace_in_scenario (
		szScePathA, szScePathB,
		TEXT("#EXTENSIONS"),
		PatternsVGS,
		2,
		szSceDllPath
		);

	const TCHAR * pattern_net[] = { TEXT("zlicin_495_601") };

	/* Third modification step.
	   Replace the network path. */
	StringCbCopy ( szScePathC, MAX_EXE_PATH, szScePath );
	StringCbCat ( szScePathC, MAX_EXE_PATH, TEXT(".c") );
	replace_in_scenario (
		szScePathB, szScePathC,
		TEXT("#NETWORK"),
		pattern_net,
		1,
		szNetPath
		);

	/* Fourth modification step.
	   Replace the `stop time` field of RunTime information. */
	replace_stoptime (
		szNetPath,
		stopTime.c_str()
		);

	//???
	//StringCbCat ( szScePath, MAX_EXE_PATH, TEXT("\"") );
	//StringCbCat ( szScePath, MAX_EXE_PATH, TEXT("\"") );

	/* Spawn the process. */
	hPid = _tspawnl (
		_P_NOWAIT,
		szHomeDir,
		szExePath,
		TEXT("-run"),
		szScePathC,
		NULL
		);

	if ( hPid < 0 )
	{
		perror ( "Cannot start Aimsun by _spawnl()" );
		return;
	}

	/* Now that Aimsun is running, we need to start also the controllers. */
	for ( int c = 0 ; c < NUM_CONTROLLERS ; c++ )
	{
		TCHAR szELS3Config[MAX_PATH];
		szELS3Config[0] = 0;
		StringCbCat ( szELS3Config, MAX_PATH_BYTES, TEXT("\"") );
		StringCbCat ( szELS3Config, MAX_PATH_BYTES, szBasePath );
		StringCbCat ( szELS3Config, MAX_PATH_BYTES, TEXT("\\els3\\configs\\") );
		StringCbCat ( szELS3Config, MAX_PATH_BYTES, ControllerList[c] );
		StringCbCat ( szELS3Config, MAX_PATH_BYTES, TEXT(".ini\"") );

		/* Create the command line for the controller. */
		szExePath[0] = 0;
		StringCbCat ( szExePath, MAX_EXE_PATH, TEXT("start \"") );
		StringCbCat ( szExePath, MAX_EXE_PATH, WindowStr[c] );
		StringCbCat ( szExePath, MAX_EXE_PATH, TEXT("\" ") );
		StringCbCat ( szExePath, MAX_EXE_PATH, szELS3Path );
		StringCbCat ( szExePath, MAX_EXE_PATH, TEXT(" -a") );
		StringCbCat ( szExePath, MAX_EXE_PATH, TEXT(" -l frm,det,spl") );
		StringCbCat ( szExePath, MAX_EXE_PATH, TEXT(" -c 12.12.2007 00:00:00") );
		StringCbCat ( szExePath, MAX_EXE_PATH, TEXT(" -f ") );
		StringCbCat ( szExePath, MAX_EXE_PATH, szELS3Config );

		/* Spawn the process. */
		_tsystem ( szExePath );
	}

	/* Aaaaggggrrrrrhhhh!
	   C++ is much much faster than Matlab and we have a synchronisation
	   issue with els3 controllers starting too late. */
	Sleep ( 5000 );


	/* Prepare full path for vehicle input data. */
	StringCbCopy ( szEntrancePath, MAX_EXE_PATH, szSceDir );
	StringCbCat  ( szEntrancePath, MAX_EXE_PATH, TEXT("\\") );
	StringCbCat  ( szEntrancePath, MAX_EXE_PATH, entranceFileName.c_str() );

	/* Initialise and start the vehicle input.
	   The function call refers to delay-loaded DLL, so expect possible
	   complainst in case that the DLL canot be found in PATH. */
	initVGS (
		HEADWAY_UNIFORM,
		szEntrancePath,
		TEXT("sect_stats.csv"),
		TEXT("glob_stats.csv")
		);

	/* Create LaneQueuesStatPos holding indices to the statistical data for the
	   sections listed in LaneQueuesSectIds in the same order. */
	LaneQueuesStatPos = (int *) calloc ( NumLaneQueuesSectIds, sizeof(int));
	for ( int i=0 ; i < NumLaneQueuesSectIds ; i++ )
	{
		int p = _search_index ( StatIds, NumStatIds, LaneQueuesSectIds[i] );
		if ( p >= 0 )
		{
			LaneQueuesStatPos[i] = p;
		}
		else
		{
			fprintf ( stderr, "Cannot find index  LaneQueuesStatIds[%d]=%d in StatIds!\n",
				i, LaneQueuesSectIds[i] );
		    exit(1);
		}
	}

}

void AimsunDS::initVGS (
	const vgs_headway_mode headway,
	const string &entrancePath,
	const string &sectionStatsPath,
	const string &globalStatsPath
	)
{
    int res;
	
	/* Allocate space for the section statistics.
	   TODO: This is ugly. This should be part of VGS API. */
	sections_stats.queues = (int*) calloc ( MAX_SECTS*MAX_LANES, sizeof(int) );
	sections_stats.stats  = (vgs_se_stat_entry*) calloc ( MAX_SECTS, sizeof(vgs_se_stat_entry) );

	/* Pass information about entrance sections to VGS API (and so to the
       GetramVGS extension that will be generating vehicles for these
	   entrances). */
    vgs_set_entrance_sections ( EntranceSections, NumEntranceSections );
    
    /* Check the validity of the headway mode. */
    res = vgs_is_valid_headway_mode ( headway );
    if ( !res )
	{
        fprintf ( stderr, "Invalid headway mode '%d'\n", headway );
		exit ( -1 );
	}
    
    /* Specify where to find the CSV with vehicle entrances and which
	   headway mode to use for generating vehicle flows. */
	vgs_generate_vehicles ( entrancePath.c_str(), headway );
    
    /* Pass to VGS API information about section identifiers we want to
       collect statistics for (and VGS will pass it to the GetramVGS
       extension). */ 
	vgs_set_stats_sections ( StatIds, NumStatIds );
    
    /* Specify where to store CSV files with statistical data. */
	vgs_set_stats_file_names ( sectionStatsPath.c_str(), globalStatsPath.c_str() );
    
    /* Specify the maximum queuing speed in kmph. */
    vgs_set_max_qspeed ( 3.6f );
    
    /* Unlock the semaphore blocking Aimsun from execution. */
    vgs_unlock_aimsun();
}

void AimsunDS::getdata ( vec &dt ) const
{
	/* Return the last data. */
	printf ( "\n\tAimsunDS::getdata() starting\n" );

	if ( p_rsp == NULL )
	{
		fprintf ( stderr, "ERROR: AimsunDS::getdata() - no data available yet!\n" );
		return;
	}

	/* Loop over all controllers in the measurement set. */
	for ( int i = 0; i < p_rsp->count; i++)
	{
		det_stats * ds_ptr  = p_rsp->dets + i;
		sp_stats *  sp_ptr  = p_rsp->sps  + i;

		printf ( "\tRealised signal plans:\n" );

		/* Loop over all measurement sets. We will deliver just
		   the last one. */
		for ( int j = 0; j < sp_ptr->count ; j++ )
		{
			sp_stattab * dat_ptr = sp_ptr->splans + j;
			int          pos     = _search_index ( IntersectionIDs, NumIntersections, dat_ptr->its_id );
			int          offset  = SignalPlanOffsets[pos];

			printf ( "\t[%3d] t=%6ds ", j, dat_ptr->global_time );

			/* Copy particular signals. */
			for ( int k = 0 ; k < dat_ptr->num_siggroups ; k++ )
			{
				dt[k+offset] = dat_ptr->green_time[k];
				printf ( "%3d ", dat_ptr->green_time[k] );
			}
			printf ( "\n" );
		}

		printf ( "\n\tDetector data:\n" );

		/* Loop over all mesurement records with intensities and
		   occupancies. */
		for ( int j = 0 ; j < ds_ptr->count ; j++ )
		{
			det_aggreg_table * dat_ptr = ds_ptr->stats + j;
			int  pos     = _search_index ( IntersectionIDs, NumIntersections, dat_ptr->its_id );
			int  moffset = MeasurementOffsets[pos];

			printf ( "\t[%3d] t=%6ds ", j, dat_ptr->global_time );

			for ( int k = 0 ; k < dat_ptr->num_det ; k++ )
			{
				dt[2*k+moffset]   = dat_ptr->dt_intensity[k];
				dt[2*k+moffset+1] = dat_ptr->dt_occupancy[k];
				printf ( "{%2d/%2d} ",
					dat_ptr->dt_intensity[k], dat_ptr->dt_occupancy[k] );
			}
			printf ( "\n" );
		}
	}

	/* Loop over the statistical data. We will extract the queue length for
	   every intersection and store it into the data vector. */
	int oLanes = 0; // lane offset for the actual intersection
	int oLpos  = 0;
	for ( int i = 0 ; i < NumIntersections ; i++ )
	{
		/* Every intersection has a certain number of lanes that (roughly)
		   correspond to the number of signal groups. */
		int nLanes  = NumLanes[i];
		int qoffset = QueueLengthOffsets[i];
		for ( int j = 0 ; j < nLanes ; j++ )
		{
			/* Initial queue length for this lane is zero. */
			int qLen = 0;
			/* Every lane has one or more section identifiers at certain
			   positions in LaneQueuesStatIds. The sequence of lanes
			   is given by offsets in LaneQueueOffsets, the position of
			   the statistical information is stored in LaneQueuesStatPos. */
			for ( int k = oLpos ; k < LaneQueueOffsets[oLanes+j+1] ; k++ )
			{
				/* Get the index into statistical information records. */
				int pi = LaneQueuesStatPos[k];
				int pl = LaneQueuesLaneIds[k]-1; // indices are not zero-based
				/* Now query the last queue length (section with more than
				   one lane holds several queue lengths) and add it to the
				   acumulated queue length value. */
				qLen += sections_stats.queues[pi*sections_stats.max_lanes+pl];
			}
			/* Move the lane position offset. */
			oLpos = LaneQueueOffsets[oLanes+j+1];
			/* Update the data vector. */
			dt[qoffset+j] = qLen;
		}
		oLanes = oLanes + nLanes;
	}

	printf ( "\tAimsunDS::getdata() finished\n" );
	cout << dt ;
}


void AimsunDS::write ( vec &ut )
{
    eh_res     sp_res;   /* Result code from write operation. */
	eh_hrdels3 hrdsp[2]; /* Signal plan for two intersections. */
    
    /* Check that the length of the vector is correct. */
    if ( ut.size() != utsize )
    {
      fprintf ( stderr, "Incorrect length of data vector (should be %d)", utsize );
      exit(-1);
    }
    /* Check that the phase lengths sum to correct cycle time. */
    /*
	if ( ut[0]+ut[1]+ut[2] != 80 )
    {
      fprintf ( stderr, "Intersection 495: F1+F2+F3 != Tc (should be 80)" );
      exit(-1);
    }
    if ( ut[3]+ut[4]+ut[5] != 80 )
    {
      fprintf ( stderr, "Intersection 601: F1+F2+F3 != Tc (should be 80)" );
      exit(-1);
    }*/

    hrdsp[0].id = 495;
    hrdsp[0].sp.cycle_time   = int ( ut[0] );
	hrdsp[0].sp.offset       = int ( ut[1] );
	hrdsp[0].sp.phase_len[0] = int ( ut[2] );
    hrdsp[0].sp.phase_len[1] = int ( ut[3] );
    hrdsp[0].sp.phase_len[2] = int ( ut[4] );
    hrdsp[0].sp.phase_len[3] = -1;

    hrdsp[1].id = 601;
    hrdsp[1].sp.cycle_time   = int ( ut[ 0] );
    hrdsp[1].sp.offset       = int ( ut[ 8] );
    hrdsp[1].sp.phase_len[0] = int ( ut[ 9] );
    hrdsp[1].sp.phase_len[1] = int ( ut[10] );
    hrdsp[1].sp.phase_len[2] = int ( ut[11] );
    hrdsp[1].sp.phase_len[3] = -1;

	/* Write new signal plan to the shared memory of active controllers. */
    sp_res = eh_write_hrd_data( hrdsp, 2 );

	if ( sp_res != EH_RES_OK )
	{
		printf ( "eh_write_hrd_data(): " );
		if ( sp_res == EH_RES_WCNT )
		{
			printf ( "Wrong count of ELS3...\n" );
		}
		else if ( sp_res == EH_RES_WRID )
		{
			printf ( "Wrong intersection ID...\n" );
		}
		else
		{
			printf ( "Unknown error [%d] ...\n", sp_res );
		}
	}
}


void AimsunDS::step()
{
	eh_wres      wsp_res;   /* Result code of the wait operation. */
    int          count;		/* Count of measurement sets. */
    
    /* Wait for 100 seconds until data from all ELS3 controllers arrive. */
	wsp_res = eh_wait_on_rsp_complete( 100000 );

	if ( wsp_res != EH_WRES_OK )
	{
		printf ( "eh_wait_on_rsp_complete(): " );

		if ( wsp_res == EH_WRES_TIMEOUT )
		{
			printf ( "Timeout elapsed ...\n" );
		}
		else if ( wsp_res == EH_WRES_EXIT )
		{
			printf ( "Some ELS3 has exited ...\n" );
		}
		else
		{
			printf ( "Unknown error [%d] ...\n", wsp_res );
		}
		exit(-1);
	}

	/* This will copy the detector data out. It is needed by the
	   Matlab HRDS version due to its internal structure - realised
	   signal plans are read asynchronously and later. The value
       of `count` reports the number of data sets that will be
       returned by the call to `eh_read_realised_sp()`. This coount
       will be at least 1 but it may be higher in cases where the
       controller reports measurements in short intervals but we are
       reading them less frequently. */
	count = eh_copy_out_measurements ();

	printf ( "got %d measurements from controllers ...", count );

	/* Read realised signal plan from shared memory. */
	if ( eh_read_realised_sp( &p_rsp ) == EH_RES_OK )
	{
		printf("\nsuccesfully read data from controllers\n" );
	}
	else
	{
		printf(" but got no data ???\n");
	}

	/* Finally we will also read the last statistical information
	   from the VGS interface. */
	vgs_get_last_stats ( &sections_stats );
}