/*
 */

/* 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_PATH	(2*MAX_PATH)
#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") };

/* 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 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, 36 };
const int IntensityOffsets[]  = {  6, 42 };
const int OccupancyOffsets[]  = { 21, 60 };

AimsunDS::AimsunDS() : DS()
{
	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;

	TCHAR * last;

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

	/* Determine the root directory of the simulator installation on
	   this machine. Start with the full path to the directory where
	   this executable resides. */
	//GetModuleFileName ( NULL, szBasePath, MAX_PATH );
	StringCbCopy ( szBasePath, MAX_PATH_BYTES, BASE_PATH );
	/* Now move up. The root of the installation is one directory above
	   the directory of this executable, but the current value of the
	   path contains also the executable name.
	   The first search for backslash removes the executable from the
	   path.*/
	//last = _tcsrchr ( szBasePath, '\\' );
	//*last = '\0';
	/* The second moves to the root of the installation. */
	//last = _tcsrchr ( szBasePath, '\\' );
	//*last = '\0';

	/* Create the path to DLL directory.
	   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, szBasePath );
	StringCbCat ( szDllPath, MAX_PATH_BYTES, TEXT("\\dlls") );

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

	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(" -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("\\zlicin_20071212_495_601.csv") );

	/* 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")
		);

  /* Description of the channels of the data vector. */
  Drv = RV ( "{"
    "495_VA  495_VB   495_VC   495_VD   495_VE   495_VF "
    "495iDVA 495iDVB  495iDVA1 495iDVB1 495iDVC  495iDVD  495iDVE  495iDVF  495iDVF1 "
    "495iS1  495iS2   495iS3   495iS4   495iS5   495iS5a "
    "495oDVA 495oDVB  495oDVA1 495oDVB1 495oDVC  495oDVD  495oDVE  495oDVF  495oDVF1 "
    "495oS1  495oS2   495oS3   495oS4   495oS5   495oS5a "
    "601_VA  601_VB   601_VC   601_VD   601_VE   601_SE "
    "601iDVA 601iDVAa 601iDVB  601iDVBa 601iDVB1 601iDVC  601iDVD  601iDVD1 601iDSE 601iDVE 601iDSE1 601iDVE1 "
    "601iS6  601iS6a  601iS7   601iS8   601iS9   601iS9a "
    "601oDVA 601oDVAa 601oDVB  601oDVBa 601oDVB1 601oDVC  601oDVD  601oDVD1 601oDSE 601oDVE 601oDSE1 601oDVE1 "
    "601oS6  601oS6a  601oS7   601oS8   601oS9   601oS9a "
    "}" );
  /* 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();

  set_log_level(0);
  /* Initialise the pointer to received data. */
  p_rsp = NULL;
}

void AimsunDS::initVGS (
	const vgs_headway_mode headway,
	const TCHAR * entrancePath,
	const TCHAR * sectionStatsPath,
	const TCHAR * globalStatsPath
	)
{
    int res;
	
	/* 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. */
    CT2A ep_c(entrancePath);
	vgs_generate_vehicles ( ep_c, 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. */
    CT2A ss_c(sectionStatsPath);
    CT2A gs_c(globalStatsPath);
    vgs_set_stats_file_names ( ss_c, gs_c );
    
    /* 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 measurement sets (currently we will deliver
	just the last one, but this will surely change. */
	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 intersections and fill in signal plan
		data. */
		for ( int j = 0; j < sp_ptr->count ; j++ )
		{
			sp_stattab * dat_ptr = sp_ptr->splans + j;
			int          offset  = SignalPlanOffsets[j];

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

			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 intersections and fill in the
		detector measurements. */
		for ( int j = 0 ; j < ds_ptr->count ; j++ )
		{
			det_aggreg_table * dat_ptr = ds_ptr->stats + j;
			int                ioffset  = IntensityOffsets[j];
			int                ooffset  = OccupancyOffsets[j];

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

			for ( int k = 0 ; k < dat_ptr->num_det ; k++ )
			{
				dt[k+ioffset] = dat_ptr->dt_intensity[k];
				dt[k+ooffset] = dat_ptr->dt_occupancy[k];
				printf ( "{%2d/%2d} ",
					dat_ptr->dt_intensity[k], dat_ptr->dt_occupancy[k] );
			}
			printf ( "\n" );
		}
	}
	printf ( "\tAimsunDS::getdata() finished\n" );
}


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