PRT_TYPE * PRT_CALL_CONV PrtMkSeqType(_In_ PRT_TYPE *innerType)
{
PrtAssert(PrtIsValidType(innerType), "Invalid type expression");
PRT_TYPE *type = (PRT_TYPE *)PrtMalloc(sizeof(PRT_TYPE));
PRT_SEQTYPE *seq = (PRT_SEQTYPE *)PrtMalloc(sizeof(PRT_SEQTYPE));
type->typeKind = PRT_KIND_SEQ;
type->typeUnion.seq = seq;
seq->innerType = PrtCloneType(innerType);
return type;
}
PRT_TYPE * PRT_CALL_CONV PrtMkTupType(_In_ PRT_UINT32 arity)
{
PrtAssert(arity > 0, "Invalid tuple arity");
PRT_TYPE *type = (PRT_TYPE *)PrtMalloc(sizeof(PRT_TYPE));
PRT_TUPTYPE *tup = (PRT_TUPTYPE *)PrtMalloc(sizeof(PRT_TUPTYPE));
type->typeKind = PRT_KIND_TUPLE;
type->typeUnion.tuple = tup;
tup->arity = arity;
tup->fieldTypes = (PRT_TYPE **)PrtCalloc((size_t)arity, sizeof(PRT_TYPE *));
return type;
}
PRT_TYPE * PRT_CALL_CONV PrtMkNmdTupType(_In_ PRT_UINT32 arity)
{
PrtAssert(arity > 0, "Invalid tuple arity");
PRT_TYPE *type = (PRT_TYPE *)PrtMalloc(sizeof(PRT_TYPE));
PRT_NMDTUPTYPE *nmdTup = (PRT_NMDTUPTYPE *)PrtMalloc(sizeof(PRT_NMDTUPTYPE));
type->typeKind = PRT_KIND_NMDTUP;
type->typeUnion.nmTuple = nmdTup;
nmdTup->arity = arity;
nmdTup->fieldNames = (PRT_STRING *)PrtCalloc((size_t)arity, sizeof(PRT_STRING));
nmdTup->fieldTypes = (PRT_TYPE **)PrtCalloc((size_t)arity, sizeof(PRT_TYPE *));
return type;
}
PRT_TYPE * PRT_CALL_CONV PrtMkMapType(_In_ PRT_TYPE *domType, _In_ PRT_TYPE *codType)
{
PrtAssert(PrtIsValidType(domType), "Invalid type expression");
PrtAssert(PrtIsValidType(codType), "Invalid type expression");
PRT_TYPE *type = (PRT_TYPE *)PrtMalloc(sizeof(PRT_TYPE));
PRT_MAPTYPE *map = (PRT_MAPTYPE *)PrtMalloc(sizeof(PRT_MAPTYPE));
type->typeKind = PRT_KIND_MAP;
type->typeUnion.map = map;
map->domType = PrtCloneType(domType);
map->codType = PrtCloneType(codType);
return type;
}
void P_CTOR_Timer_IMPL(PRT_MACHINEINST *context, PRT_VALUE *value)
{
TimerContext *timerContext = (TimerContext *)PrtMalloc(sizeof(TimerContext));
timerContext->client = PrtCloneValue(value);
timerContext->started = PRT_FALSE;
context->extContext = timerContext;
}
PRT_TYPE * PRT_CALL_CONV PrtMkForeignType(_In_ PRT_UINT16 typeTag)
{
PrtAssert(typeTag < prtNumForeignTypeDecls, "Invalid type tag");
PRT_TYPE *type = (PRT_TYPE *)PrtMalloc(sizeof(PRT_TYPE));
type->typeKind = PRT_KIND_FORGN;
type->typeUnion.typeTag = typeTag;
return type;
}
void P_CTOR_Timer_IMPL(PRT_MACHINEINST *context, PRT_VALUE *value)
{
printf("Entering P_CTOR_Timer_IMPL\n");
TimerContext *timerContext = (TimerContext *)PrtMalloc(sizeof(TimerContext));
timerContext->client = PrtCloneValue(value);
timerContext->timer = CreateWaitableTimer(NULL, TRUE, NULL);
PrtAssert(timerContext->timer != NULL, "CreateWaitableTimer failed");
context->extContext = timerContext;
}
PRT_STRING UserToStringForeignValue(_In_ PRT_UINT16 typeTag, _In_ PRT_UINT64 frgnVal)
{
PRT_STRING str = PrtMalloc(sizeof(PRT_CHAR) * 100);
switch (typeTag)
{
case P_FORGN_TYPE_StringType:
sprintf_s(str, 100, "String : %s", frgnVal);
break;
default:
break;
};
return str;
}
PRT_UINT64 UserMkDefaultForeignValue(_In_ PRT_UINT16 typeTag)
{
PRT_STRING str = NULL;
switch (typeTag)
{
case P_FORGN_TYPE_StringType:
str = PrtMalloc(sizeof(PRT_CHAR) * 100);
sprintf_s(str, 100, "xyx$12");
return (PRT_UINT64)str;
default:
return (PRT_UINT64)NULL;
};
}
PRT_TYPE * PRT_CALL_CONV PrtMkPrimitiveType(_In_ PRT_TYPE_KIND primType)
{
PRT_TYPE *type = (PRT_TYPE *)PrtMalloc(sizeof(PRT_TYPE));
type->typeUnion.map = NULL;
switch (primType)
{
case PRT_KIND_ANY:
case PRT_KIND_BOOL:
case PRT_KIND_EVENT:
case PRT_KIND_MACHINE:
case PRT_KIND_INT:
case PRT_KIND_NULL:
{
type->typeKind = primType;
return type;
}
default:
{
PrtAssert(PRT_FALSE, "Expected a primitive type.");
type->typeKind = PRT_TYPE_KIND_CANARY;
return type;
}
}
}
PRT_MACHINEINST_PRIV *
PrtMkMachinePrivate(
_Inout_ PRT_PROCESS_PRIV *process,
_In_ PRT_UINT32 instanceOf,
_In_ PRT_VALUE *payload
)
{
PRT_UINT32 packSize;
PRT_UINT32 nVars;
PRT_UINT8 eQSize;
PRT_MACHINEINST_PRIV *context;
PRT_UINT32 i;
PrtLockMutex(process->processLock);
nVars = process->program->machines[instanceOf].nVars;
eQSize = PRT_QUEUE_LEN_DEFAULT;
//
// Allocate memory for state machine context
//
context = (PRT_MACHINEINST_PRIV*)PrtMalloc(sizeof(PRT_MACHINEINST_PRIV));
//
// Add it to the array of machines in the process
//
PRT_UINT32 numMachines = process->numMachines;
PRT_UINT32 machineCount = process->machineCount;
PRT_MACHINEINST **machines = process->machines;
if (machineCount == 0)
{
machines = (PRT_MACHINEINST **)PrtCalloc(1, sizeof(PRT_MACHINEINST *));
process->machines = machines;
process->machineCount = 1;
}
else if (machineCount == numMachines) {
PRT_MACHINEINST **newMachines = (PRT_MACHINEINST **)PrtCalloc(2 * machineCount, sizeof(PRT_MACHINEINST *));
for (PRT_UINT32 i = 0; i < machineCount; i++)
{
newMachines[i] = machines[i];
}
PrtFree(machines);
machines = newMachines;
process->machines = newMachines;
process->machineCount = 2 * machineCount;
}
machines[numMachines] = (PRT_MACHINEINST *)context;
process->numMachines++;
//
// Initialize Machine Identity
//
context->process = (PRT_PROCESS *)process;
context->instanceOf = instanceOf;
PRT_MACHINEID id;
id.machineId = process->numMachines; // index begins with 1 since 0 is reserved
id.processId = process->guid;
context->id = PrtMkMachineValue(id);
context->extContext = NULL;
context->isModel = PRT_FALSE;
//
// Initialize the map used in PrtDist, map from sender to the last seqnumber received
//
PRT_TYPE *domType = PrtMkPrimitiveType(PRT_KIND_MACHINE);
PRT_TYPE *codType = PrtMkPrimitiveType(PRT_KIND_INT);
PRT_TYPE *recvMapType = PrtMkMapType(domType, codType);
context->recvMap = PrtMkDefaultValue(recvMapType);
PrtFreeType(domType);
PrtFreeType(codType);
PrtFreeType(recvMapType);
// Initialize Machine Internal Variables
//
context->currentState = process->program->machines[context->instanceOf].initStateIndex;
context->isRunning = PRT_FALSE;
context->isHalted = PRT_FALSE;
context->lastOperation = ReturnStatement;
context->currentTrigger = PrtMkEventValue(PRT_SPECIAL_EVENT_NULL);
context->currentPayload = PrtCloneValue(payload);
//
// Allocate memory for local variables and initialize them
//
context->varValues = NULL;
if (nVars > 0)
{
context->varValues = PrtCalloc(nVars, sizeof(PRT_VALUE*));
for (i = 0; i < nVars; i++)
{
context->varValues[i] = PrtMkDefaultValue(process->program->machines[instanceOf].vars[i].type);
}
}
context->receive = NULL;
//
// Initialize various stacks
//
context->callStack.length = 0;
context->funStack.length = 0;
//
// Initialize event queue
//
context->eventQueue.eventsSize = eQSize;
context->eventQueue.events = (PRT_EVENT*)PrtCalloc(eQSize, sizeof(PRT_EVENT));
context->eventQueue.headIndex = 0;
context->eventQueue.tailIndex = 0;
context->eventQueue.size = 0;
packSize = PrtGetPackSize(context);
//
// Initialize Inherited Deferred Set
//
context->inheritedDeferredSetCompact = (PRT_UINT32*)PrtCalloc(packSize, sizeof(PRT_UINT32));
//
// Initialize the current deferred set
//
context->currentDeferredSetCompact = (PRT_UINT32*)PrtCalloc(packSize, sizeof(PRT_UINT32));
//
// Initialize actions
//
context->inheritedActionSetCompact = (PRT_UINT32*)PrtCalloc(packSize, sizeof(PRT_UINT32));
context->currentActionSetCompact = (PRT_UINT32*)PrtCalloc(packSize, sizeof(PRT_UINT32));
//
//Initialize state machine lock
//
context->stateMachineLock = PrtCreateMutex();
//
//Log
//
PrtLog(PRT_STEP_CREATE, context);
//
// Allocate external context Structure
//
process->program->machines[context->instanceOf].extCtorFun((PRT_MACHINEINST *)context, payload);
PrtUnlockMutex(process->processLock);
//
// Run the state machine
//
PrtLockMutex(context->stateMachineLock);
PrtRunStateMachine(context, PRT_FALSE);
return context;
}
PRT_UINT64 PRT_FORGN_CLONE_StringType_IMPL(PRT_UINT64 frgnVal)
{
PRT_STRING str = PrtMalloc(sizeof(PRT_CHAR) * 100);
sprintf_s(str, 100, (PRT_STRING)frgnVal);
return (PRT_UINT64)str;
}
PRT_UINT64 PRT_FORGN_MKDEF_StringType_IMPL(void)
{
PRT_STRING str = PrtMalloc(sizeof(PRT_CHAR) * 100);
sprintf_s(str, 100, "xyx$12");
return (PRT_UINT64)str;
}
PRT_STRING PRT_FORGN_TOSTRING_StringType_IMPL(PRT_UINT64 frgnVal)
{
PRT_STRING str = PrtMalloc(sizeof(PRT_CHAR) * 100);
sprintf_s(str, 100, "String : %lld", frgnVal);
return str;
}
int main(int argc, char *argv[])
{
#ifndef PRT_PLAT_WINUSER
init_ros("test_motion_planner", &argc, argv);
#endif
if (!ParseCommandLine(argc, argv))
{
PrintUsage();
return 1;
}
const char* motion_planner_delta_s = getenv("MOTION_PLANNER_DELTA");
if(motion_planner_delta_s) {
Delta = atoi(motion_planner_delta_s);
printf("Using MOTION_PLANNER_DELTA = %d\n", Delta);
}
printf("Press any key to start simulation\n");
getchar();
PRT_DBG_START_MEM_BALANCED_REGION
{
PRT_PROCESS *process;
PRT_GUID processGuid;
PRT_VALUE *payload;
//Initialize the workspace
WORKSPACE_INFO = ParseWorkspaceConfig(workspaceConfig);
#ifdef USE_DIJKSTRA_PRECOMPUTATION
WS_LocationsList ends;
ends.size = WORKSPACE_INFO->starts.size + WORKSPACE_INFO->ends.size;
ends.locations = malloc(sizeof(WS_Coord) * ends.size);
int count = 0;
for(int i=0; i < WORKSPACE_INFO->starts.size; i++) {
ends.locations[count++] = WORKSPACE_INFO->starts.locations[i];
}
for(int i=0; i < WORKSPACE_INFO->ends.size; i++) {
ends.locations[count++] = WORKSPACE_INFO->ends.locations[i];
}
PreComputeObstacleDistanceH(WORKSPACE_INFO->dimension, WORKSPACE_INFO->obstacles, ends);
#endif
processGuid.data1 = 1;
processGuid.data2 = 0;
processGuid.data3 = 0;
processGuid.data4 = 0;
process = PrtStartProcess(processGuid, &P_GEND_PROGRAM, ErrorHandler, Log);
if (cooperative)
{
PrtSetSchedulingPolicy(process, PRT_SCHEDULINGPOLICY_COOPERATIVE);
}
if (parg == NULL)
{
payload = PrtMkNullValue();
}
else
{
int i = atoi(parg);
payload = PrtMkIntValue(i);
}
PrtUpdateAssertFn(MyAssert);
PrtMkMachine(process, P_MACHINE_Main, payload);
if (cooperative)
{
// test some multithreading across state machines.
#if defined(PRT_PLAT_WINUSER)
HANDLE* threadsArr = (HANDLE*)PrtMalloc(threads*sizeof(HANDLE));
for (int i = 0; i < threads; i++)
{
DWORD threadId;
threadsArr[i] = CreateThread(NULL, 16000, (LPTHREAD_START_ROUTINE)RunToIdle, process, 0, &threadId);
}
WaitForMultipleObjects(threads, threadsArr, TRUE, INFINITE);
PrtFree(threadsArr);
#elif defined(PRT_PLAT_LINUXUSER)
typedef void *(*start_routine) (void *);
pthread_t tid[threads];
for (int i = 0; i < threads; i++)
{
pthread_create(&tid[i], NULL, (start_routine)RunToIdle, (void*)process);
}
for (int i = 0; i < threads; i++)
{
pthread_join(tid[i], NULL);
}
#else
#error Invalid Platform
#endif
}
PrtFreeValue(payload);
PrtStopProcess(process);
}
PRT_DBG_END_MEM_BALANCED_REGION
//_CrtSetReportMode(_CRT_ERROR, _CRTDBG_MODE_DEBUG);
//_CrtDumpMemoryLeaks();
}
void P_CTOR_Client_IMPL(PRT_MACHINEINST *context, PRT_VALUE *value)
{
ClientContext *clientContext = (ClientContext *)PrtMalloc(sizeof(ClientContext));
clientContext->client = PrtCloneValue(value);
context->extContext = clientContext;
}
