//
// Date init       14.12.2004
//
// Revision date   $Date: 24-06-09 8:53 $
//
// Filename        $Workfile:: c_cmd.c                                       $
//
// Version         $Revision: 14 $
//
// Archive         $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_cmd. $
//
// Platform        C
//

//
// File Description:
// This file contains the virtual machine implementation to run bytecode
// programs compatible with LEGO MINDSTORMS NXT Software 2.0.
//
// This module (c_cmd) is also responsible for reading the system timer
// (d_timer) and returning on 1 ms timer boundaries.
//

#include "stdconst.h"
#include "modules.h"

#include "c_cmd.iom"
#include "c_output.iom"
#include "c_input.iom"
#include "c_loader.iom"
#include "c_ui.iom"
#include "c_sound.iom"
#include "c_button.iom"
#include "c_display.iom"
#include "c_comm.iom"
#include "c_lowspeed.iom"
#include  "m_sched.h"

#include "c_cmd.h"
#include "c_cmd_bytecodes.h"
#include "d_timer.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h> // for sqrt, abs, and trig stuff

#define VMProfilingCode 0

static  IOMAPCMD  IOMapCmd;
static  VARSCMD   VarsCmd;
static  HEADER    **pHeaders;
static  ULONG     gInstrsToExecute;
static  SLONG gPCDelta;
#define NUM_INTERP_FUNCS 16
#define NUM_SHORT_INTERP_FUNCS 8
#define VAR_INSTR_SIZE 0xE
// important to cast since most args are assigned from signed value, and locals may be ULONG
#define GetDataArg(arg) ((UWORD)(arg))
#if VMProfilingCode
static  ULONG     ExecutedInstrs= 0, CmdCtrlTime= 0, OverheadTime= 0, CmdCtrlCalls= 0, LeaveTime= 0, NotFirstCall= 0, LastAvgCount= 0;
static ULONG CmdCtrlClumpTime[256];
typedef struct  {
  ULONG Time;
  ULONG Count;
  ULONG Avg;
  ULONG Max;
} VMInstrProfileInfo;
static VMInstrProfileInfo InstrProfile[OPCODE_COUNT];
static VMInstrProfileInfo SysCallProfile[SYSCALL_COUNT];
static VMInstrProfileInfo InterpFuncProfile[NUM_INTERP_FUNCS];
static VMInstrProfileInfo ShortInstrProfile[NUM_SHORT_OPCODE_COUNT];
#endif

#define cCmdDSType(Arg) (VarsCmd.pDataspaceTOC[(Arg)].TypeCode)
#define cCmdDSScalarPtr(DSElementID, Offset) (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset + Offset)
#define cCmdSizeOf(TC) (TC_Size_Table[(TC)])

#define scalarBinopDispatchMask 0x1
#define scalarUnop2DispatchMask 0x2

const     HEADER  cCmd =
{
  0x00010001L,
  "Command",
  cCmdInit,
  cCmdCtrl,
  cCmdExit,
  (void *)&IOMapCmd,
  (void *)&VarsCmd,
  (UWORD)sizeof(IOMapCmd),
  (UWORD)sizeof(VarsCmd),
  0x0000                      //Code size - not used so far
};

#if ENABLE_VM

// c_cmd_drawing.inc is just another C source file
// (the graphics implementation was split off for practical file management reasons)
#include "c_cmd_drawing.inc"

//
//Function pointers to sub-interpreters
//This table is indexed by instr size
//Unary operations can have arity of 1 or 2 (some need a destination)
//All instructions taking 4 or more operands are handled as "Other"
// Table uses NoArg for illegal instr sizes such as zero and odd sizes
//
static pInterp InterpFuncs[NUM_INTERP_FUNCS] =
{
  cCmdInterpNoArg,
  cCmdInterpNoArg,
  cCmdInterpNoArg, // size 2
  cCmdInterpNoArg,
  cCmdInterpUnop1, // size 4
  cCmdInterpNoArg,
  cCmdInterpUnop2, // size 6 general poly is cCmdInterpUnop2, scalar is cCmdInterpScalarUnop2
  cCmdInterpNoArg,
  cCmdInterpBinop, // size 8, general poly is cCmdInterpBinop, scalar is cCmdInterpScalarBinop
  cCmdInterpNoArg,
  cCmdInterpOther, // size 10
  cCmdInterpNoArg,
  cCmdInterpOther, // size 12
  cCmdInterpNoArg,
  cCmdInterpOther, // size 14
  cCmdInterpNoArg
};

static pInterpShort ShortInterpFuncs[NUM_SHORT_INTERP_FUNCS] =
{
  cCmdInterpShortMove,
  cCmdInterpShortAcquire,
  cCmdInterpShortRelease,
  cCmdInterpShortSubCall,
  cCmdInterpShortError,
  cCmdInterpShortError,
  cCmdInterpShortError,
  cCmdInterpShortError
};

ULONG TC_Size_Table[]= {
  0, // void
  SIZE_UBYTE,
  SIZE_SBYTE,
  SIZE_UWORD,
  SIZE_SWORD,
  SIZE_ULONG,
  SIZE_SLONG,
  SIZE_UWORD, // array
  0, // cluster
  SIZE_MUTEX,
  SIZE_FLOAT
};


//
//Function pointers to SysCall implementations
//See interpreter for OP_SYSCALL
//
static pSysCall SysCallFuncs[SYSCALL_COUNT] =
{
  cCmdWrapFileOpenRead,
  cCmdWrapFileOpenWrite,
  cCmdWrapFileOpenAppend,
  cCmdWrapFileRead,
  cCmdWrapFileWrite,
  cCmdWrapFileClose, // 5
  cCmdWrapFileResolveHandle,
  cCmdWrapFileRename,
  cCmdWrapFileDelete,
  cCmdWrapSoundPlayFile,
  cCmdWrapSoundPlayTone, // 10
  cCmdWrapSoundGetState,
  cCmdWrapSoundSetState,
  cCmdWrapDrawText,
  cCmdWrapDrawPoint,
  cCmdWrapDrawLine, // 15
  cCmdWrapDrawCircle,
  cCmdWrapDrawRect,
  cCmdWrapDrawPicture,
  cCmdWrapSetScreenMode,
  cCmdWrapReadButton, // 20
  cCmdWrapCommLSWrite,
  cCmdWrapCommLSRead,
  cCmdWrapCommLSCheckStatus,
  cCmdWrapRandomNumber,
  cCmdWrapGetStartTick, // 25
  cCmdWrapMessageWrite,
  cCmdWrapMessageRead,
  cCmdWrapCommBTCheckStatus,
  cCmdWrapCommBTWrite,
  cCmdWrapCommBTRead, // 30
  cCmdWrapKeepAlive,
  cCmdWrapIOMapRead,
  cCmdWrapIOMapWrite,
  cCmdWrapColorSensorRead,
  cCmdWrapCommBTOnOff, // 35
  cCmdWrapCommBTConnection,
  cCmdWrapCommHSWrite,
  cCmdWrapCommHSRead,
  cCmdWrapCommHSCheckStatus,
  cCmdWrapReadSemData, //40
  cCmdWrapWriteSemData,
  cCmdWrapComputeCalibValue,
  cCmdWrapUpdateCalibCacheInfo,
  cCmdWrapDatalogWrite,
  cCmdWrapDatalogGetTimes,  //45
  cCmdWrapSetSleepTimeout,
  cCmdWrapListFiles //47

  // don't forget to update SYSCALL_COUNT in c_cmd.h
};

//
//Next set of arrays are lookup tables for IOM access bytecodes
//
TYPE_CODE IO_TYPES_IN[IO_IN_FIELD_COUNT] =
{
  //IO_IN0
  TC_UBYTE, //IO_IN_TYPE
  TC_UBYTE, //IO_IN_MODE
  TC_UWORD, //IO_IN_ADRAW
  TC_UWORD, //IO_IN_NORMRAW
  TC_SWORD, //IO_IN_SCALED_VAL
  TC_UBYTE, //IO_IN_INVALID_DATA

  //IO_IN1
  TC_UBYTE, //IO_IN_TYPE
  TC_UBYTE, //IO_IN_MODE
  TC_UWORD, //IO_IN_ADRAW
  TC_UWORD, //IO_IN_NORMRAW
  TC_SWORD, //IO_IN_SCALED_VAL
  TC_UBYTE, //IO_IN_INVALID_DATA

  //IO_IN2
  TC_UBYTE, //IO_IN_TYPE
  TC_UBYTE, //IO_IN_MODE
  TC_UWORD, //IO_IN_ADRAW
  TC_UWORD, //IO_IN_NORMRAW
  TC_SWORD, //IO_IN_SCALED_VAL
  TC_UBYTE, //IO_IN_INVALID_DATA

  //IO_IN3
  TC_UBYTE, //IO_IN_TYPE
  TC_UBYTE, //IO_IN_MODE
  TC_UWORD, //IO_IN_ADRAW
  TC_UWORD, //IO_IN_NORMRAW
  TC_SWORD, //IO_IN_SCALED_VAL
  TC_UBYTE, //IO_IN_INVALID_DATA
};

TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] =
{
  //IO_OUT0
  TC_UBYTE, //IO_OUT_FLAGS
  TC_UBYTE, //IO_OUT_MODE
  TC_SBYTE, //IO_OUT_SPEED
  TC_SBYTE, //IO_OUT_ACTUAL_SPEED
  TC_SLONG, //IO_OUT_TACH_COUNT
  TC_ULONG, //IO_OUT_TACH_LIMIT
  TC_UBYTE, //IO_OUT_RUN_STATE
  TC_SBYTE, //IO_OUT_TURN_RATIO
  TC_UBYTE, //IO_OUT_REG_MODE
  TC_UBYTE, //IO_OUT_OVERLOAD
  TC_UBYTE, //IO_OUT_REG_P_VAL
  TC_UBYTE, //IO_OUT_REG_I_VAL
  TC_UBYTE, //IO_OUT_REG_D_VAL
  TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
  TC_SLONG, //IO_OUT_ROTATION_COUNT
  TC_UBYTE, //IO_OUT_OPTIONS
  TC_SBYTE, //IO_OUT_MAX_SPEED
  TC_SBYTE, //IO_OUT_MAX_ACCELERATION

  //IO_OUT1
  TC_UBYTE, //IO_OUT_FLAGS
  TC_UBYTE, //IO_OUT_MODE
  TC_SBYTE, //IO_OUT_SPEED
  TC_SBYTE, //IO_OUT_ACTUAL_SPEED
  TC_SLONG, //IO_OUT_TACH_COUNT
  TC_ULONG, //IO_OUT_TACH_LIMIT
  TC_UBYTE, //IO_OUT_RUN_STATE
  TC_SBYTE, //IO_OUT_TURN_RATIO
  TC_UBYTE, //IO_OUT_REG_MODE
  TC_UBYTE, //IO_OUT_OVERLOAD
  TC_UBYTE, //IO_OUT_REG_P_VAL
  TC_UBYTE, //IO_OUT_REG_I_VAL
  TC_UBYTE, //IO_OUT_REG_D_VAL
  TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
  TC_SLONG, //IO_OUT_ROTATION_COUNT
  TC_UBYTE, //IO_OUT_OPTIONS
  TC_SBYTE, //IO_OUT_MAX_SPEED
  TC_SBYTE, //IO_OUT_MAX_ACCELERATION

  //IO_OUT2
  TC_UBYTE, //IO_OUT_FLAGS
  TC_UBYTE, //IO_OUT_MODE
  TC_SBYTE, //IO_OUT_SPEED
  TC_SBYTE, //IO_OUT_ACTUAL_SPEED
  TC_SLONG, //IO_OUT_TACH_COUNT
  TC_ULONG, //IO_OUT_TACH_LIMIT
  TC_UBYTE, //IO_OUT_RUN_STATE
  TC_SBYTE, //IO_OUT_TURN_RATIO
  TC_UBYTE, //IO_OUT_REG_MODE
  TC_UBYTE, //IO_OUT_OVERLOAD
  TC_UBYTE, //IO_OUT_REG_P_VAL
  TC_UBYTE, //IO_OUT_REG_I_VAL
  TC_UBYTE, //IO_OUT_REG_D_VAL
  TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT
  TC_SLONG, //IO_OUT_ROTATION_COUNT
  TC_UBYTE, //IO_OUT_OPTIONS
  TC_SBYTE, //IO_OUT_MAX_SPEED
  TC_SBYTE, //IO_OUT_MAX_ACCELERATION
};


TYPE_CODE * IO_TYPES[2] =
{
  IO_TYPES_IN,
  IO_TYPES_OUT
};

//Actual pointers filled in during cCmdInit()
void * IO_PTRS_IN[IO_IN_FIELD_COUNT];
void * IO_PTRS_OUT[IO_OUT_FIELD_COUNT];

void ** IO_PTRS[2] =
{
  IO_PTRS_IN,
  IO_PTRS_OUT
};

// Data used to indicate usage of motor ports, or usage requests
UBYTE gUsageSemData, gRequestSemData;

UBYTE     cCmdBTGetDeviceType(UBYTE *pCOD)
{
  ULONG   COD;
  UBYTE   Result;
  UBYTE   Tmp;

  COD      = 0;
  for (Tmp = 0;Tmp < SIZE_OF_CLASS_OF_DEVICE;Tmp++)
  {
    COD  <<= 8;
    COD   |= (ULONG)*pCOD;
    pCOD++;
  }

  Result   = DEVICETYPE_UNKNOWN;
  if ((COD & 0x00001FFF) == 0x00000804)
  {
    Result = DEVICETYPE_NXT;
  }
  if ((COD & 0x00001F00) == 0x00000200)
  {
    Result = DEVICETYPE_PHONE;
  }
  if ((COD & 0x00001F00) == 0x00000100)
  {
    Result = DEVICETYPE_PC;
  }

  return (Result);
}

//cCmdHandleRemoteCommands is the registered handler for "direct" command protocol packets
//It is only intended to be called via c_comm's main protocol handler
UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen)
{
  NXT_STATUS RCStatus = NO_ERR;
  //Response packet length.  Always includes RCStatus byte.
  ULONG ResponseLen = 1;
  //Boolean flag to send a response.  TRUE unless overridden below.
  ULONG SendResponse = TRUE;
  //Boolean flag if we are handling a reply telegram.  FALSE unless overridden.
  ULONG IncomingReply = FALSE;
  ULONG i, FirstPort, LastPort;
  UWORD LStatus;
  UWORD Count, QueueID;
  UBYTE * pData;

  //Illegal call, give up
  if (pInBuf == NULL || pLen == NULL)
  {
    NXT_BREAK;
    return (0xFFFF);
  }

  //No output buffer provided, so skip any work related to returning a response
  if (pOutBuf == NULL)
    SendResponse = FALSE;

  //If first byte identifies this as a reply telegram, we have different work to do.
  if (pInBuf[0] == 0x02)
  {
    IncomingReply = TRUE;
    //Reply telegrams never get responses, even if caller provided a buffer.
    SendResponse = FALSE;
  }

  //Advance pInBuf past command type byte
  pInBuf++;

  if (!IncomingReply)
  {
    switch(pInBuf[0])
    {
      case RC_START_PROGRAM:
      {
        //Check that file exists.  If not, return error
        //!!! Should return standard loader file error in cases like this??
        //!!! Proper solution would also check file mode to avoid confusing errors
        if (LOADER_ERR(LStatus = pMapLoader->pFunc(FINDFIRST, (&pInBuf[1]), NULL, NULL)) != SUCCESS)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        //Close file handle returned by FINDFIRST
        pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(LStatus), NULL, NULL);

        //File must exist, so inform UI to attempt execution in the usual way (enables consistent feedback)
        pMapUi->Flags |= UI_EXECUTE_LMS_FILE;
        strncpy((PSZ)(pMapUi->LMSfilename), (PSZ)(&pInBuf[1]), FILENAME_LENGTH + 1);
      }
      break;

      case RC_STOP_PROGRAM:
      {
        if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
        {
          RCStatus = ERR_NO_PROG;
          break;
        }

        IOMapCmd.DeactivateFlag = TRUE;
      }
      break;

      case RC_PLAY_SOUND_FILE:
      {
        if (LOADER_ERR(LStatus = pMapLoader->pFunc(FINDFIRST, (&pInBuf[2]), NULL, NULL)) != SUCCESS)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        //Close file handle returned by FINDFIRST
        pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(LStatus), NULL, NULL);

        if (pInBuf[1] == FALSE)
          pMapSound->Mode = SOUND_ONCE;
        else //Any non-zero value treated as TRUE
          pMapSound->Mode = SOUND_LOOP;

        strncpy((PSZ)pMapSound->SoundFilename, (PSZ)(&pInBuf[2]), FILENAME_LENGTH + 1);
        pMapSound->Flags |= SOUND_UPDATE;
      }
      break;

      case RC_PLAY_TONE:
      {
        pMapSound->Mode = SOUND_TONE;
        //!!! Range check valid values?
        memcpy((PSZ)(&(pMapSound->Freq)), (PSZ)(&pInBuf[1]), 2);
        memcpy((PSZ)(&(pMapSound->Duration)), (PSZ)(&pInBuf[3]), 2);

        pMapSound->Flags |= SOUND_UPDATE;
      }
      break;

      case RC_SET_OUT_STATE:
      {
        UBYTE Port = pInBuf[1];
        //Don't do anything if illegal port specification is made
        if (Port >= NO_OF_OUTPUTS && Port != 0xFF)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        //0xFF is protocol defined to mean "all ports".
        if (Port == 0xFF)
        {
          FirstPort = 0;
          LastPort = NO_OF_OUTPUTS - 1;
        }
        else
          FirstPort = LastPort = Port;

        for (i = FirstPort; i <= LastPort; i++)
        {
          OUTPUT * pOut = &(pMapOutPut->Outputs[i]);
          pOut->Speed             = pInBuf[2];
          pOut->Mode              = pInBuf[3];
          pOut->RegMode           = pInBuf[4];
          pOut->SyncTurnParameter = pInBuf[5];
          pOut->RunState          = pInBuf[6];
          pOut->Options           = pOut->Mode & REG_METHOD;
          memcpy((PSZ)(&(pOut->TachoLimit)), (PSZ)(&pInBuf[7]), 4);

          pOut->Flags |= UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT;
        }
      }
      break;

      case RC_SET_IN_MODE:
      {
        i = pInBuf[1];

        //Don't do anything if illegal port specification is made
        //!!! Should check against legal Types and Modes? (bitmask for Modes?)
        if (i >= NO_OF_INPUTS)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }
        INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);

        pIn->SensorType = pInBuf[2];
        pIn->SensorMode = pInBuf[3];

        //Set InvalidData flag automatically since type may have changed
        pIn->InvalidData = TRUE;
      }
      break;

      case RC_GET_OUT_STATE:
      {
        if (SendResponse == TRUE)
        {
          i = pInBuf[1];

          //Return error and all zeros if illegal port specification is made
          if (i >= NO_OF_OUTPUTS)
          {
            RCStatus = ERR_RC_ILLEGAL_VAL;
            memset(&(pOutBuf[ResponseLen]), 0, 22);
            ResponseLen += 22;
            break;
          }
          OUTPUT * pOut = &(pMapOutPut->Outputs[i]);

          //Echo port
          pOutBuf[ResponseLen] = i;
          ResponseLen++;

          //Power
          pOutBuf[ResponseLen] = pOut->Speed;
          ResponseLen++;

          //Mode
          pOutBuf[ResponseLen] = pOut->Mode;
          ResponseLen++;

          //RegMode
          pOutBuf[ResponseLen] = pOut->RegMode;
          ResponseLen++;

          //TurnRatio
          pOutBuf[ResponseLen] = pOut->SyncTurnParameter;
          ResponseLen++;

          //RunState
          pOutBuf[ResponseLen] = pOut->RunState;
          ResponseLen++;

          //TachoLimit ULONG
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->TachoLimit)), 4);
          ResponseLen += 4;

          //TachoCount SLONG
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->TachoCnt)), 4);
          ResponseLen += 4;

          //BlockTachoCount SLONG
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->BlockTachoCount)), 4);
          ResponseLen += 4;

          //RotationCount SLONG
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->RotationCount)), 4);
          ResponseLen += 4;

          NXT_ASSERT(ResponseLen == 23);
        }
      }
      break;

      case RC_GET_IN_VALS:
      {
        if (SendResponse == TRUE)
        {
          i = pInBuf[1];

          //Return error and all zeros if illegal port specification is made
          if (i >= NO_OF_INPUTS)
          {
            RCStatus = ERR_RC_ILLEGAL_VAL;
            memset(&(pOutBuf[ResponseLen]), 0, 13);
            ResponseLen += 13;
            break;
          }

          //Echo port
          pOutBuf[ResponseLen] = i;
          ResponseLen++;

          INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);

          //Set "Valid?" boolean
          if (pIn->InvalidData)
            pOutBuf[ResponseLen] = FALSE;
          else
            pOutBuf[ResponseLen] = TRUE;

          ResponseLen++;

          //Set "Calibrated?" boolean
          //!!! "Calibrated?" is a placeholder in the protocol.  Always FALSE for now.
          pOutBuf[ResponseLen] = FALSE;
          ResponseLen++;

          pOutBuf[ResponseLen] = pIn->SensorType;
          ResponseLen++;

          pOutBuf[ResponseLen] = pIn->SensorMode;
          ResponseLen++;

          //Set Raw, Normalized, and Scaled values
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->ADRaw)), 2);
          ResponseLen += 2;

          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorRaw)), 2);
          ResponseLen += 2;

          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorValue)), 2);
          ResponseLen += 2;

          //!!! Return normalized raw value in place of calibrated value for now -- see comment above
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorRaw)), 2);
          ResponseLen += 2;

          NXT_ASSERT(ResponseLen == 14);
        }
      }
      break;

      case RC_RESET_IN_VAL:
      {
        i = pInBuf[1];

        //Don't do anything if illegal port specification is made
        if (i >= NO_OF_INPUTS)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        //Clear SensorValue to zero.  Leave Raw and Normalized as-is, since they never accumulate running values.
        pMapInput->Inputs[i].SensorValue = 0;
      }
      break;

      case RC_MESSAGE_WRITE:
      {
        QueueID = pInBuf[1];
        Count = pInBuf[2];
        pData = &(pInBuf[3]);

        //If Count is illegal or MsgData is not null-terminated,
        // we can't accept it as a valid string
        if (Count == 0 || Count > MAX_MESSAGE_SIZE || pData[Count - 1] != 0x00)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        RCStatus = cCmdMessageWrite(QueueID, pData, Count);

        //ERR_MEM here means we must compact the dataspace and retry message write
        if (RCStatus == ERR_MEM)
        {
          cCmdDSCompact();
          RCStatus = cCmdMessageWrite(QueueID, pData, Count);
        }
      }
      break;

      case RC_RESET_POSITION:
      {
        i = pInBuf[1];

        //Don't do anything if illegal port specification is made
        if (i >= NO_OF_OUTPUTS)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        //pInBuf[2] is a selector
        //FALSE: Position relative to start of last program
        //TRUE: Position relative to start of last motor control block
        pMapOutPut->Outputs[i].Flags |= (pInBuf[2] ? UPDATE_RESET_BLOCK_COUNT : UPDATE_RESET_ROTATION_COUNT);
      }
      break;

      case RC_GET_BATT_LVL:
      {
        if (SendResponse == TRUE)
        {
          //Return BatteryVoltage directly from IOMapUI, in mV
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&(pMapUi->BatteryVoltage), 2);
          ResponseLen += 2;
        }
      }
      break;

      case RC_STOP_SOUND:
      {
        //Tell sound module to stop playback, no questions asked
        pMapSound->State = SOUND_STOP;
      }
      break;

      case RC_KEEP_ALIVE:
      {
        pMapUi->Flags |= UI_RESET_SLEEP_TIMER;

        if (SendResponse == TRUE)
        {
          //Convert to milliseconds to match external conventions
          i = (pMapUi->SleepTimeout * 60 * 1000);
          memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&i, 4);
          ResponseLen += 4;
        }
      }
      break;

      case RC_LS_GET_STATUS:
      {
        if (SendResponse == TRUE)
        {
          i = pInBuf[1];

          //Don't do anything if illegal port specification is made
          if (i >= NO_OF_LOWSPEED_COM_CHANNEL)
          {
            RCStatus = ERR_RC_ILLEGAL_VAL;
            break;
          }

          RCStatus = cCmdLSCheckStatus(i);

          pOutBuf[ResponseLen] = cCmdLSCalcBytesReady(i);
          ResponseLen++;
        }
      }
      break;

      case RC_LS_WRITE:
      {
        i = pInBuf[1];
        Count = pInBuf[2];

        //Don't do anything if illegal port specification is made
        if (i >= NO_OF_LOWSPEED_COM_CHANNEL)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        RCStatus = cCmdLSWrite(i, Count, &(pInBuf[4]), pInBuf[3]);
      }
      break;

      case RC_LS_READ:
      {
        if (SendResponse == TRUE)
        {
          i = pInBuf[1];

          //Don't do anything if illegal port specification is made
          if (i >= NO_OF_LOWSPEED_COM_CHANNEL)
          {
            RCStatus = ERR_RC_ILLEGAL_VAL;
            break;
          }

          //Get channel status and number of bytes available to read
          RCStatus = cCmdLSCheckStatus(i);
          Count = cCmdLSCalcBytesReady(i);

          pOutBuf[ResponseLen] = (UBYTE)Count;
          ResponseLen++;

          //If channel is ready and has data ready for us, put the data into outgoing buffer
          if (!IS_ERR(RCStatus) && Count > 0)
          {
            RCStatus = cCmdLSRead(i, (UBYTE)Count, &(pOutBuf[ResponseLen]));
            ResponseLen += Count;
          }

          //Pad remaining data bytes with zeroes
          Count = 16 - Count;
          memset(&(pOutBuf[ResponseLen]), 0, Count);
          ResponseLen += Count;
        }
      }
      break;

      case RC_GET_CURR_PROGRAM:
      {
        if (SendResponse == TRUE)
        {
          //If there's no active program, return error and empty name buffer
          if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
          {
            RCStatus = ERR_NO_PROG;
            memset(&(pOutBuf[ResponseLen]), 0, FILENAME_LENGTH + 1);
          }
          //Else, copy out stashed program name
          else
          {
            strncpy((PSZ)(&(pOutBuf[ResponseLen])), (PSZ)(VarsCmd.ActiveProgName), FILENAME_LENGTH + 1);
          }

          //Regardless, we've copied out a filename's worth of bytes...
          ResponseLen += FILENAME_LENGTH + 1;
        }
      }
      break;

      case RC_MESSAGE_READ:
      {
        if (SendResponse == TRUE)
        {
          QueueID = pInBuf[1];

          //Fill in response with remote mailbox number so remote device knows where to store this message.
          pOutBuf[ResponseLen] = pInBuf[2];
          ResponseLen++;

          RCStatus = cCmdMessageGetSize(QueueID, &Count);
          pOutBuf[ResponseLen] = Count;
          ResponseLen++;

          if (!IS_ERR(RCStatus) && Count > 0)
          {
            pData = &(pOutBuf[ResponseLen]);
            RCStatus = cCmdMessageRead(QueueID, pData, Count, (pInBuf[3]));
            //If cCmdMessageRead encountered an error, there is no real data in the buffer, so clear it out (below)
            if (IS_ERR(RCStatus))
              Count = 0;
            else
              ResponseLen += Count;
          }

          //Pad remaining data bytes with zeroes
          Count = MAX_MESSAGE_SIZE - Count;
          memset(&(pOutBuf[ResponseLen]), 0, Count);
          ResponseLen += Count;
        }
      }
      break;

      // remote-only command to read from datalog buffer
      // pInBuf[1] = Remove? (bool)
      case RC_DATALOG_READ:
      {
        if (SendResponse == TRUE)
        {
          RCStatus = cCmdDatalogGetSize(&Count);
          pOutBuf[ResponseLen] = Count;
          ResponseLen++;

          if (!IS_ERR(RCStatus) && Count > 0)
          {
            pData = &(pOutBuf[ResponseLen]);
            RCStatus = cCmdDatalogRead(pData, Count, (pInBuf[1]));
            //If cCmdDatalogRead encountered an error, there is no real data in the buffer, so clear it out (below)
            if (IS_ERR(RCStatus))
              Count = 0;
            else
              ResponseLen += Count;
          }

          //Pad remaining data bytes with zeroes
          Count = MAX_DATALOG_SIZE - Count;
          memset(&(pOutBuf[ResponseLen]), 0, Count);
          ResponseLen += Count;
        }
      }
      break;
      case RC_DATALOG_SET_TIMES:
      {
        //SyncTime SLONG
        memcpy((PSZ)&IOMapCmd.SyncTime, (PSZ)&(pInBuf[1]), 4);
        IOMapCmd.SyncTick= dTimerReadNoPoll();
      }
      break;

      case RC_BT_GET_CONN_COUNT:
        if (SendResponse == TRUE) {
          pOutBuf[ResponseLen]= SIZE_OF_BT_CONNECT_TABLE;
          ResponseLen++;
          }
      break;
      case RC_BT_GET_CONN_NAME: // param in is index, param out is name
        if (SendResponse == TRUE) { // get index from inbuf
          i = pInBuf[1];
          if(i < SIZE_OF_BT_CONNECT_TABLE) { // unsigned, so guaranteed >= 0
            pOutBuf[ResponseLen] = cCmdBTGetDeviceType(pMapComm->BtConnectTable[i].ClassOfDevice);
            memcpy((PSZ)(&(pOutBuf[ResponseLen+1])), (PSZ)(pMapComm->BtConnectTable[i].Name), SIZE_OF_BT_NAME + 1);
            ResponseLen += SIZE_OF_BT_NAME + 2;
          }
          else {
            pOutBuf[ResponseLen] = 0;
            ResponseLen += SIZE_OF_BT_NAME + 2;
          }
        }
      break;
      case RC_BT_GET_CONTACT_COUNT:
        if (SendResponse == TRUE) {
          pOutBuf[ResponseLen]= SIZE_OF_BT_DEVICE_TABLE;
          ResponseLen++;
          }
      break;
      case RC_BT_GET_CONTACT_NAME:
        if (SendResponse == TRUE) { // get index from inbuf
          i = pInBuf[1];
          if(i < SIZE_OF_BT_DEVICE_TABLE && (pMapComm->BtDeviceTable[i].DeviceStatus & BT_DEVICE_KNOWN)) {  // unsigned, so guaranteed >= 0
            (pOutBuf[ResponseLen])= cCmdBTGetDeviceType(pMapComm->BtDeviceTable[i].ClassOfDevice);
            memcpy((PSZ)(&(pOutBuf[ResponseLen+1])), (PSZ)(pMapComm->BtDeviceTable[i].Name), SIZE_OF_BT_NAME + 1);
            ResponseLen += SIZE_OF_BT_NAME + 2;
          }
          else
          {
            pOutBuf[ResponseLen] = 0;
            memset((PSZ)(&(pOutBuf[ResponseLen+1])), 0, SIZE_OF_BT_NAME + 1);
            ResponseLen += SIZE_OF_BT_NAME + 2;
          }
        }
      break;
    case RC_SET_PROPERTY: // label/value pairs
        i = pInBuf[1];
        switch(i) {
          case RC_PROP_BTONOFF: {
            UWORD retVal, status;
            if(pInBuf[2])
              status= pMapComm->pFunc(BTON, 0, 0, 0, NULL, &retVal);
            else
              status= pMapComm->pFunc(BTOFF, 0, 0, 0, NULL, &retVal);

            RCStatus= (status == SUCCESS) ? retVal : status;
          }
          break;
          case RC_PROP_SOUND_LEVEL: {
            UBYTE volume= pInBuf[2];
            if(volume > 4)
              volume= 4;
            pMapSound->Volume= volume; // apparently stored in two places
            pMapUi->Volume= volume;
          }
          break;
          case RC_PROP_SLEEP_TIMEOUT: { // ulong millisecs to sleep
            ULONG value;
            memcpy((PSZ)&value, (PSZ)&(pInBuf[2]), 4);
            pMapUi->SleepTimeout= value / 60000;
          }
          break;
          default:
            //Unknown property -- still inform client to not expect any response bytes
            NXT_BREAK;
            RCStatus = ERR_RC_UNKNOWN_CMD;
          break;
        }
      break;
    case RC_GET_PROPERTY: // label/value pairs
        if (SendResponse == TRUE) { // get index from inbuf
          i = pInBuf[1];
          switch(i) {
            case RC_PROP_BTONOFF:
              pOutBuf[ResponseLen]= pMapUi->BluetoothState != BT_STATE_OFF;
              ResponseLen++;
            break;
            case RC_PROP_SOUND_LEVEL: {
              pOutBuf[ResponseLen]= pMapSound->Volume;
              ResponseLen++;
              }
            break;
            case RC_PROP_SLEEP_TIMEOUT: {
              ULONG value= (pMapUi->SleepTimeout * 60 * 1000);
              memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&value, 4);
              ResponseLen += 4;
            }
            break;
            default:
              //Unknown property -- still inform client to not expect any response bytes
              NXT_BREAK;
              RCStatus = ERR_RC_UNKNOWN_CMD;
            break;
          }
        }
      break;
    case RC_UPDATE_RESET_COUNT:
      {
        i = pInBuf[1];

        //Don't do anything if illegal port specification is made
        if (i >= NO_OF_OUTPUTS)
        {
          RCStatus = ERR_RC_ILLEGAL_VAL;
          break;
        }

        pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_COUNT;
      }
      break;
      default:
      {
        //Unknown remote command -- still inform client to not expect any response bytes
        NXT_BREAK;
        RCStatus = ERR_RC_UNKNOWN_CMD;
      }
      break;
    };
  }
  //Handle reply telegrams
  else
  {
    switch(pInBuf[0])
    {
      case RC_MESSAGE_READ:
      {
        QueueID = pInBuf[2];
        Count   = pInBuf[3];
        pData = &(pInBuf[4]);

        //This is a response to our request to read a message from a remote mailbox.
        //If telegram looks valid, write the resulting message into our local mailbox.
        //(If MsgData is not null-terminated, we can't accept it as a valid string.)
        if (!IS_ERR((SBYTE)(pInBuf[1]))
         && Count > 0
         && Count <= MAX_MESSAGE_SIZE
         && pData[Count - 1] == 0x00)
        {
          RCStatus = cCmdMessageWrite(QueueID, pData, Count);

          //ERR_MEM here means we must compact the dataspace
          if (RCStatus == ERR_MEM)
          {
            cCmdDSCompact();
            RCStatus = cCmdMessageWrite(QueueID, pData, Count);
          }
        }

        //If telegram doesn't check out, do nothing.  No errors are ever returned for reply telegrams.
      }
      break;

      default:
      {
        //Unhandled reply telegram.  Do nothing.
        //!!! Could/should stash unhandled/all replies somewhere so a syscall could read them
      }
      break;
    };
  }

  if (SendResponse == TRUE)
  {
    //Return response length (pointer checked above)
    *pLen = (UBYTE)ResponseLen;
    //Fill in status byte
    pOutBuf[0] = (UBYTE)(RCStatus);
  }
  else
    *pLen = 0;

  return (0);
}


//
// Standard interface functions
//

void      cCmdInit(void* pHeader)
{
  ULONG i;

  pHeaders        = pHeader;

  IOMapCmd.pRCHandler = &cCmdHandleRemoteCommands;

#if defined(ARM_DEBUG)
  //Init run-time assert tracking variables
  VarsCmd.AssertFlag = FALSE;
  VarsCmd.AssertLine = 0;
#endif

  //Initialize IO_PTRS_OUT
  for (i = 0; i < NO_OF_OUTPUTS; i++)
  {
    OUTPUT * pOut = &(pMapOutPut->Outputs[i]);
    IO_PTRS_OUT[IO_OUT_FLAGS            + i * IO_OUT_FPP] = (void*)&(pOut->Flags);
    IO_PTRS_OUT[IO_OUT_MODE             + i * IO_OUT_FPP] = (void*)&(pOut->Mode);
    IO_PTRS_OUT[IO_OUT_SPEED            + i * IO_OUT_FPP] = (void*)&(pOut->Speed);
    IO_PTRS_OUT[IO_OUT_ACTUAL_SPEED     + i * IO_OUT_FPP] = (void*)&(pOut->ActualSpeed);
    IO_PTRS_OUT[IO_OUT_TACH_COUNT       + i * IO_OUT_FPP] = (void*)&(pOut->TachoCnt);
    IO_PTRS_OUT[IO_OUT_TACH_LIMIT       + i * IO_OUT_FPP] = (void*)&(pOut->TachoLimit);
    IO_PTRS_OUT[IO_OUT_RUN_STATE        + i * IO_OUT_FPP] = (void*)&(pOut->RunState);
    IO_PTRS_OUT[IO_OUT_TURN_RATIO       + i * IO_OUT_FPP] = (void*)&(pOut->SyncTurnParameter);
    IO_PTRS_OUT[IO_OUT_REG_MODE         + i * IO_OUT_FPP] = (void*)&(pOut->RegMode);
    IO_PTRS_OUT[IO_OUT_OVERLOAD         + i * IO_OUT_FPP] = (void*)&(pOut->Overloaded);
    IO_PTRS_OUT[IO_OUT_REG_P_VAL        + i * IO_OUT_FPP] = (void*)&(pOut->RegPParameter);
    IO_PTRS_OUT[IO_OUT_REG_I_VAL        + i * IO_OUT_FPP] = (void*)&(pOut->RegIParameter);
    IO_PTRS_OUT[IO_OUT_REG_D_VAL        + i * IO_OUT_FPP] = (void*)&(pOut->RegDParameter);
    IO_PTRS_OUT[IO_OUT_BLOCK_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pOut->BlockTachoCount);
    IO_PTRS_OUT[IO_OUT_ROTATION_COUNT   + i * IO_OUT_FPP] = (void*)&(pOut->RotationCount);
    IO_PTRS_OUT[IO_OUT_OPTIONS          + i * IO_OUT_FPP] = (void*)&(pOut->Options);
    IO_PTRS_OUT[IO_OUT_MAX_SPEED        + i * IO_OUT_FPP] = (void*)&(pOut->MaxSpeed);
    IO_PTRS_OUT[IO_OUT_MAX_ACCELERATION + i * IO_OUT_FPP] = (void*)&(pOut->MaxAcceleration);
  }

  //Initialize IO_PTRS_IN
  for (i = 0; i < NO_OF_INPUTS; i++)
  {
    INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);
    IO_PTRS_IN[IO_IN_TYPE         + i * IO_IN_FPP] = (void*)&(pIn->SensorType);
    IO_PTRS_IN[IO_IN_MODE         + i * IO_IN_FPP] = (void*)&(pIn->SensorMode);
    IO_PTRS_IN[IO_IN_ADRAW        + i * IO_IN_FPP] = (void*)&(pIn->ADRaw);
    IO_PTRS_IN[IO_IN_NORMRAW      + i * IO_IN_FPP] = (void*)&(pIn->SensorRaw);
    IO_PTRS_IN[IO_IN_SCALEDVAL    + i * IO_IN_FPP] = (void*)&(pIn->SensorValue);
    IO_PTRS_IN[IO_IN_INVALID_DATA + i * IO_IN_FPP] = (void*)&(pIn->InvalidData);
  }

  //Clear memory pool and initialize VarsCmd (cCmdDeactivateProgram effectively re-inits VarsCmd)
  cCmdInitPool();
  cCmdDeactivateProgram();

  //Global state variables for BlueTooth communication.
  VarsCmd.CommStat      = (SWORD)SUCCESS;
  VarsCmd.CommStatReset = (SWORD)BTBUSY;
  VarsCmd.CommCurrConnection = 1;

  //Global flags for various reset and bookkeeping scenarios
  VarsCmd.DirtyComm = FALSE;
  VarsCmd.DirtyDisplay = FALSE;

  VarsCmd.VMState = VM_IDLE;

#if defined (ARM_NXT)
  //Make sure Pool is long-aligned
  NXT_ASSERT(!((ULONG)(POOL_START) % SIZE_SLONG));
#endif

  IOMapCmd.ProgStatus = PROG_IDLE;
  IOMapCmd.ActivateFlag = FALSE;
  IOMapCmd.Awake  = TRUE;

  //Default offsets explicitly chosen to cause an error if used with IOMAPREAD/IOMAPWRITE
  //Real values will be set when programs run and/or the DS is re-arranged.
  IOMapCmd.OffsetDVA = 0xFFFF;
  IOMapCmd.OffsetDS = 0xFFFF;

  //Initialize format string and clear out FileName string
  strncpy((PSZ)(IOMapCmd.FormatString), VM_FORMAT_STRING, VM_FORMAT_STRING_SIZE);
  memset(IOMapCmd.FileName, 0, sizeof(IOMapCmd.FileName));

  dTimerInit();
  IOMapCmd.Tick = dTimerRead();
  IOMapCmd.SyncTime= 0;
  IOMapCmd.SyncTick= 0;

  return;
}


void cCmdCtrl(void)
{
  NXT_STATUS Status = NO_ERR;

  switch (VarsCmd.VMState)
  {
    case VM_RUN_FREE:
    case VM_RUN_SINGLE:
    {
 #if VMProfilingCode
    ULONG EnterTime= dTimerReadHiRes(), FinishTime;
    CmdCtrlCalls ++;
#endif
    ULONG Continue;

#if VM_BENCHMARK
      //IOMapCmd.Tick currently holds the tick from the end of last cCmdCtrl call.
      //If we don't come back here before dTimerRead() increments, the m_sched loop has taken *at least* 1 ms.
      if (IOMapCmd.Tick != dTimerRead())
      {
        VarsCmd.OverTimeCount++;
        //Record maximum magnitude of schedule loop overage, in millisecs
        if (dTimerRead() - IOMapCmd.Tick > VarsCmd.MaxOverTimeLength)
          VarsCmd.MaxOverTimeLength = dTimerRead() - IOMapCmd.Tick;
      }
      VarsCmd.CmdCtrlCount++;
#endif
      //Abort current program if cancel button is pressed
      if (IOMapCmd.DeactivateFlag == TRUE || pMapButton->State[BTN1] & PRESSED_EV)
      {
        IOMapCmd.DeactivateFlag = FALSE;

        //Clear pressed event so it doesn't get double-counted by UI
        pMapButton->State[BTN1] &= ~PRESSED_EV;

        //Go to VM_RESET1 state and report abort
        VarsCmd.VMState = VM_RESET1;
        IOMapCmd.ProgStatus = PROG_ABORT;
        break;
      }

      //Assert that we have an active program
      NXT_ASSERT(VarsCmd.ActiveProgHandle != NOT_A_HANDLE);

      //Handle any resting clumps that are ready to awaken
      cCmdCheckRestQ(IOMapCmd.Tick); // not using result, yet
      //Execute from at least one clump
      do
      {
        //Execute instructions from a clump up to INSTR_MAX, to end of millisec,
        //Finishing/suspending a clump, BREAKOUT_REQ, or any errors will cause a return
#if VMProfilingCode
          ULONG ClumpEnterTime= dTimerReadHiRes();
          CLUMP_ID clump= VarsCmd.RunQ.Head;
#endif
          Status = cCmdInterpFromClump();
#if VMProfilingCode
          CmdCtrlClumpTime[clump] += dTimerReadHiRes() - ClumpEnterTime;
#endif

        //If RunQ and RestQ are empty, program is done, or wacko
        if (!cCmdIsClumpIDSane(VarsCmd.RunQ.Head)) {
          Continue = FALSE;
          if(!cCmdIsClumpIDSane(VarsCmd.RestQ.Head)) {
            VarsCmd.VMState = VM_RESET1;
            IOMapCmd.ProgStatus = PROG_OK;
          }
        }
        else if (Status == CLUMP_SUSPEND || Status == CLUMP_DONE)
          Continue = TRUE; // queue isn't empty, didn't timeout
          //Only rotate RunQ on a "normal" finish, i.e. no error, clump end, or breakout request
        else if (Status == ROTATE_QUEUE) { // done and suspend do their own
          cCmdRotateQ();
          Continue= TRUE;
        }
       else if (Status == TIMES_UP) {
          cCmdRotateQ();
          Continue = FALSE;
        }
        else if (IS_ERR(Status)) // mem error is handled in InterpFromClump if possible
        {
          Continue = FALSE;
          VarsCmd.VMState = VM_RESET1;
          IOMapCmd.ProgStatus = PROG_ERROR;
        }
        else if (Status == STOP_REQ)
        {
          Continue = FALSE;
          VarsCmd.VMState = VM_RESET1;
          IOMapCmd.ProgStatus = PROG_OK;
        }
        else if (Status == BREAKOUT_REQ)
        {
          Continue = FALSE;
        }
      } while (Continue == TRUE);
#if VMProfilingCode
      FinishTime= dTimerReadHiRes();
      if(NotFirstCall)
        OverheadTime += EnterTime - LeaveTime;
      else
        NotFirstCall= 1;
      CmdCtrlTime += FinishTime - EnterTime;
      LeaveTime= FinishTime;
#endif
      // May busy wait to postpone to 1ms schedule
      while (IOMapCmd.Tick == dTimerRead());
    }
    break;
    case VM_IDLE:
    {
      //If there's a new program to activate...
      if (IOMapCmd.ActivateFlag == TRUE)
      {
        //Clear flag so we only activate once per new file
        IOMapCmd.ActivateFlag = FALSE;

        Status = cCmdActivateProgram(IOMapCmd.FileName);

        //If we hit an activation error:
        //1. Set PROG_ERROR status
        //2. Proceed to VM_RESET1 (some unneeded work, yes, but preserves contract with UI
        if (IS_ERR(Status))
        {
          IOMapCmd.ProgStatus = PROG_ERROR;
          VarsCmd.VMState = VM_RESET1;
        }
        //Else start running program
        else
        {
          VarsCmd.VMState = VM_RUN_FREE;
          IOMapCmd.ProgStatus = PROG_RUNNING;
          VarsCmd.StartTick = IOMapCmd.Tick;
          if(VarsCmd.VMState == VM_RUN_FREE)
           gInstrsToExecute = 20;
          else
            gInstrsToExecute= 1;

#if VM_BENCHMARK
          //Re-init benchmark
          VarsCmd.InstrCount         = 0;
          VarsCmd.Average            = 0;
          VarsCmd.OverTimeCount      = 0;
          VarsCmd.MaxOverTimeLength  = 0;
          VarsCmd.CmdCtrlCount       = 0;
          VarsCmd.CompactionCount    = 0;
          VarsCmd.LastCompactionTick = 0;
          VarsCmd.MaxCompactionTime  = 0;
          memset(VarsCmd.OpcodeBenchmarks, 0, sizeof(VarsCmd.OpcodeBenchmarks));
          memset(VarsCmd.SyscallBenchmarks, 0, sizeof(VarsCmd.SyscallBenchmarks));
#endif
          //Reset devices to a known state before we begin running
          cCmdResetDevices();

          pMapUi->Flags |= (UI_DISABLE_LEFT_RIGHT_ENTER | UI_DISABLE_EXIT);
        }
      }
    while (IOMapCmd.Tick == dTimerRead()); // delay until scheduled time
    }
    break;

    //Initialize VM internal state data and devices which must respond immediately to program ending
    case VM_RESET1:
    {
      //If we aborted a program, reset devices (specifically, motors) immediately
      //Otherwise, wait for UI to put us into PROG_RESET (gives motors a chance to brake before setting to coast)
      //!!! This means cCmdResetDevices will get called twice on abort.  Should not be a big deal.
      if (IOMapCmd.ProgStatus == PROG_ABORT)
        cCmdResetDevices();

      //Reenable UI access to buttons
      pMapUi->Flags &= ~(UI_DISABLE_LEFT_RIGHT_ENTER | UI_DISABLE_EXIT);

#if VM_BENCHMARK
      if (IOMapCmd.Tick != VarsCmd.StartTick)
        VarsCmd.Average = VarsCmd.InstrCount / (IOMapCmd.Tick - VarsCmd.StartTick);
      else
        //It appears that we finished in 0 milliseconds.  Very unlikely on ARM, so set a flag value.
        VarsCmd.Average = 0xFFFFFFFF;

      cCmdWriteBenchmarkFile();
#endif

      //Re-initialize program state data (contents of memory pool preserved)
      //!!! Skip this step in simulator builds so helper access methods still work
#ifndef SIM_NXT
      cCmdDeactivateProgram();
#endif //SIM_NXT

      //If this program has taken over the display, reset it for the UI
      cCmdRestoreDefaultScreen();

      //Stop any currently playing sound and re-init volume according to UI prefs
      pMapSound->State = SOUND_STOP;
      pMapSound->Volume = pMapUi->Volume;

      //Artificially set CommStatReset to BTBUSY to force at least one SETCMDMODE call (see VM_RESET2 case)
      VarsCmd.CommStatReset = (SWORD)BTBUSY;

      VarsCmd.VMState = VM_RESET2;
      while (IOMapCmd.Tick == dTimerRead()); // delay until scheduled time
    }
    break;

    case VM_RESET2:
    {
      //Reset BlueCore into "command mode" (close any open streams)
      //Since SETCMDMODE subject to BTBUSY, we may need to make multiple calls
      //Any CommStatReset value other than BTBUSY means our request was accepted
      //Assumptions:
      //Process should never take longer than UI timeout (see below), but if it does,
      // we could be left with the stream open to an NXT peer and block out the PC.
      //Also assuming that once SETCMDMODE request is accepted, it never fails.
      if (VarsCmd.CommStatReset == (SWORD)BTBUSY && VarsCmd.DirtyComm == TRUE)
        pMapComm->pFunc(SETCMDMODE, 0, 0, 0, NULL, (UWORD*)&(VarsCmd.CommStatReset));

      //If UI is done displaying ending program status, move on.
      if (IOMapCmd.ProgStatus == PROG_RESET)
      {
        //Reset devices whenever a program ends for any reason
        cCmdResetDevices();

        VarsCmd.DirtyComm = FALSE;

        //Go to VM_IDLE state
        VarsCmd.VMState = VM_IDLE;
        IOMapCmd.ProgStatus = PROG_IDLE;
      }
    while (IOMapCmd.Tick == dTimerRead()); // delay until scheduled time
    }
      break;
  }//END state machine switch

  //Set tick to new value for next time 'round
  IOMapCmd.Tick = dTimerReadNoPoll();

  return;
}


void cCmdExit(void)
{
  dTimerExit();

  return;
}


NXT_STATUS cCmdReadFileHeader(UBYTE* pData, ULONG DataSize,
            PROG_FILE_OFFSETS* pFileOffsets)
{
  ULONG i;
  UBYTE * pCursor;
  UWORD CurrOffset = 0;
  UBYTE DepCount;
  UWORD DopeVectorOffset;
  UWORD FileClumpCount;
  UBYTE FileMajor, FileMinor,
        CompatibleMinor, CompatibleMajor,
        CurrentMajor;

  NXT_ASSERT(pData != NULL);

  if (strncmp((PSZ)pData, "NXTBINARY", VM_FORMAT_STRING_SIZE) == 0)
  {
    ULONG NativeOffset;
    pCursor = (pData + 12);
    NativeOffset = (ULONG)(*pCursor);
    void (*native)(ULONG, ULONG) = (void (*)(ULONG, ULONG))(pData + NativeOffset);
    (*native)((ULONG)pData, DataSize);
    NXT_BREAK;
    return (ERR_VER);
  }
  //Assign pCursor to point to version word inside file header
  pCursor = (pData + VM_FORMAT_STRING_SIZE - 2);

  //Decode version numbers into comparable bytes
  FileMajor = *pCursor;
  FileMinor = *(pCursor + 1);
  CompatibleMajor = (UBYTE)(VM_OLDEST_COMPATIBLE_VERSION >> 8);
  CompatibleMinor = (UBYTE)(VM_OLDEST_COMPATIBLE_VERSION);
  CurrentMajor = (UBYTE)(FIRMWAREVERSION >> 8);
  //CurrentMinor = (UBYTE)(FIRMWAREVERSION);

  //Return ERR_VER if file lacks proper format string or version number
  //!!! Only checking major version recommended for future development
  if (strncmp((PSZ)pData, VM_FORMAT_STRING, VM_FORMAT_STRING_SIZE)
   || FileMajor < CompatibleMajor || FileMinor < CompatibleMinor
   || FileMajor > CurrentMajor)
  {
    NXT_BREAK;
    return (ERR_VER);
  }

  //Advance CurrOffset past header information
  CurrOffset += VM_FORMAT_STRING_SIZE;

  //
  //Initialize bookkeeping variables
  //
  VarsCmd.DataspaceCount = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  VarsCmd.DataspaceSize = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  VarsCmd.DSStaticSize = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  pFileOffsets->DSDefaultsSize = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  pFileOffsets->DynamicDefaults = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  pFileOffsets->DynamicDefaultsSize = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  VarsCmd.MemMgr.Head = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  VarsCmd.MemMgr.Tail = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  DopeVectorOffset = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  //!!! Odd code here to deal with type mismatch between file format and CLUMP_ID typedef.
  //Neither is trivial to change, so it's best to just check the data for consistency here.
  FileClumpCount = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  //Must have at least one clump and count can't exceed the NOT_A_CLUMP sentinel
  if (FileClumpCount == 0 || FileClumpCount >= NOT_A_CLUMP)
    return (ERR_FILE);
  else
    VarsCmd.AllClumpsCount = (CLUMP_ID)FileClumpCount;

  VarsCmd.CodespaceCount = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  //Can't have a valid program with no code
  if (VarsCmd.CodespaceCount == 0)
    return (ERR_FILE);

  //
  // Now, calculate offsets for each data segment in the file
  //

  CurrOffset += CurrOffset % 2;
  pFileOffsets->DSTOC = CurrOffset;
  CurrOffset += VarsCmd.DataspaceCount * sizeof(DS_TOC_ENTRY);

  CurrOffset += CurrOffset % 2;
  pFileOffsets->DSDefaults = CurrOffset;
  CurrOffset += pFileOffsets->DSDefaultsSize;

  //ClumpRecs must be aligned on even boundaries
  CurrOffset += CurrOffset % 2;
  pFileOffsets->Clumps = CurrOffset;

  //Set cursor to start of clump records
  pCursor = pData + CurrOffset;

  //Set CurrOffset to start of dependent lists
  CurrOffset += VarsCmd.AllClumpsCount * VM_FILE_CLUMP_REC_SIZE;

  //Read dependent count from each clump record, advancing CurrOffset accordingly
  for (i = 0; i < VarsCmd.AllClumpsCount; i++)
  {
    DepCount = *(pCursor + 1);
    CurrOffset += DepCount;
    pCursor += VM_FILE_CLUMP_REC_SIZE;
  }

  //Codespace must be aligned on even boundary
  CurrOffset += CurrOffset % 2;
  pFileOffsets->Codespace = CurrOffset;

  //No need to read through codespace, but make sure CurrOffset ended up sane
  //If not, something went wrong reading the header information
  if (CurrOffset != (DataSize - VarsCmd.CodespaceCount * 2))
  {
    NXT_BREAK;
    return (ERR_FILE);
  }

  //
  // Finally, update VarsCmd fields
  //

  VarsCmd.RunQ.Head = NOT_A_CLUMP;
  VarsCmd.RunQ.Tail = NOT_A_CLUMP;
  VarsCmd.RestQ.Head = NOT_A_CLUMP;
  VarsCmd.RestQ.Tail  = NOT_A_CLUMP;

  //Reset codespace pointer
  VarsCmd.pCodespace = (CODE_WORD*)(pData + pFileOffsets->Codespace);

  //...placing clump records first...
  VarsCmd.pAllClumps = (CLUMP_REC*)(VarsCmd.Pool + VarsCmd.PoolSize);
  VarsCmd.PoolSize += VarsCmd.AllClumpsCount * sizeof(CLUMP_REC);

  //...then DSTOC...
  VarsCmd.pDataspaceTOC = (DS_TOC_ENTRY*)(pData + pFileOffsets->DSTOC);

  //...then the dataspace itself
  ALIGN_TO_MOD(VarsCmd.PoolSize, POOL_ALIGN);
  VarsCmd.pDataspace = (VarsCmd.Pool + VarsCmd.PoolSize);
  IOMapCmd.OffsetDS = (UWORD)((ULONG)(VarsCmd.pDataspace) - (ULONG)&(IOMapCmd));
  VarsCmd.PoolSize += VarsCmd.DataspaceSize;

  //init rest of MemMgr
  VarsCmd.MemMgr.pDopeVectorArray = (DOPE_VECTOR *)(VarsCmd.pDataspace + DopeVectorOffset);
  IOMapCmd.OffsetDVA = (UWORD)((ULONG)(VarsCmd.MemMgr.pDopeVectorArray) - (ULONG)&(IOMapCmd));
  VarsCmd.MemMgr.FreeHead = NOT_A_DS_ID;


  if (VarsCmd.PoolSize > POOL_MAX_SIZE)
  {
    NXT_BREAK;
    return (ERR_FILE);
  }

  return (NO_ERR);
}


//!!! Recursive function
NXT_STATUS cCmdInflateDSDefaults(UBYTE* pDSDefaults, UWORD *pDefaultsOffset, DS_ELEMENT_ID DSElementID)
{
  NXT_STATUS Status = NO_ERR;
  TYPE_CODE TypeCode;
  UWORD i, Count;
  UBYTE *pVal;

  NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));

  TypeCode = cCmdDSType(DSElementID);

  if (TypeCode > TC_LAST_VALID)
    return ERR_INSTR;
  else if (TypeCode == TC_CLUSTER)
  {
    Count = cCmdClusterCount(DSElementID);
    //Advance DSElementID to sub-type
    DSElementID = INC_ID(DSElementID);
    //Loop through sub-types, inflate recursively
    for (i = 0; i < Count; i++)
    {
      Status = cCmdInflateDSDefaults(pDSDefaults, pDefaultsOffset, DSElementID);
      if (IS_ERR(Status))
        return Status;
      DSElementID = cCmdNextDSElement(DSElementID);
    }
  }
  else
  {
    if (TypeCode == TC_ARRAY)
    {
      //Resolve pointer to DVIndex
      pVal = VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset;
    }
    else
    {
      pVal = cCmdResolveDataArg(DSElementID, 0, NULL);
    }

    //Check if the element has the "default default"
    if (VarsCmd.pDataspaceTOC[DSElementID].Flags & DS_DEFAULT_DEFAULT)
    {
      //Fill element with the "default default" of zero
      memset(pVal, 0, cCmdSizeOf(TypeCode));
    }
    else
    {
      //Get default from stream
      memmove(pVal, pDSDefaults + *pDefaultsOffset, cCmdSizeOf(TypeCode));
      *pDefaultsOffset += cCmdSizeOf(TypeCode);
    }
  }

  //!!! Currently will always return NO_ERR
  return Status;
}

void cCmdRefreshActiveClump(CLUMP_ID CurrID)
{
  CLUMP_REC *  clumpRecPtr= &(VarsCmd.pAllClumps[CurrID]);

  if(clumpRecPtr->clumpScalarDispatchHints & scalarBinopDispatchMask)
    InterpFuncs[8]= cCmdInterpScalarBinop;
  else
    InterpFuncs[8]= cCmdInterpBinop;
  if(clumpRecPtr->clumpScalarDispatchHints & scalarUnop2DispatchMask)
    InterpFuncs[6]= cCmdInterpScalarUnop2;
  else
    InterpFuncs[6]= cCmdInterpUnop2;
}

NXT_STATUS cCmdActivateProgram(UBYTE * pFileName)
{
  UWORD i, j;
  UBYTE * pCursor;

  NXT_STATUS Status = NO_ERR;
  PROG_FILE_OFFSETS FileOffsets;

  LOADER_STATUS LStatus;
  ULONG DataSize;
  UBYTE * pData;
  ULONG pDataHolder;
  UWORD DefaultsOffset;

  LStatus = pMapLoader->pFunc(OPENREADLINEAR, pFileName, (UBYTE*)(&pDataHolder), &DataSize);
  pData = (UBYTE*)(pDataHolder);

  //If Loader returned error or bad file pointer, bail out
  if (LOADER_ERR(LStatus) != SUCCESS || pData == NULL || DataSize == 0)
    return (ERR_FILE);

  //Deactivate current program and re-initialize memory pool
  cCmdDeactivateProgram();
  cCmdInitPool();

  //Stash this program's handle since we hold it open while running
  VarsCmd.ActiveProgHandle = LOADER_HANDLE(LStatus);

  //Stash this program's name for easy reference later
  strncpy((PSZ)(VarsCmd.ActiveProgName), (PSZ)(pFileName), FILENAME_LENGTH + 1);

  //Consume activation record data stream.
  //See TargettingVIs/NXT.PackAR.vi for data stream packing details

  //Read header portion of the file, calculating offsets and initializing VarsCmd
  Status = cCmdReadFileHeader(pData, DataSize, &FileOffsets);
  if (IS_ERR(Status))
    return Status;

  //Do some spot checks to make sure bad file contents didn't leave us with obviously insane VarsCmd contents
  //!!! Should add alignment checks on these pointers to avoid data abort exceptions later
  if (((UBYTE*)(VarsCmd.pCodespace) < pData)
   || ((UBYTE*)(VarsCmd.pCodespace) >= (pData + DataSize))
   || ((UBYTE*)(VarsCmd.pAllClumps) < POOL_START)
   || ((UBYTE*)(VarsCmd.pAllClumps) >= POOL_SENTINEL)
   || ((UBYTE*)(VarsCmd.pDataspace) < POOL_START)
   || ((UBYTE*)(VarsCmd.pDataspace) >= POOL_SENTINEL)
   || (VarsCmd.DataspaceSize == 0) )
  {
    NXT_BREAK;
    return ERR_FILE;
  }

  //Initialize CLUMP_RECs as contiguous list in RAM
  pCursor = (pData + FileOffsets.Clumps);
  for (i = 0; i < VarsCmd.AllClumpsCount; i++)
  {
    CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[i];
    clumpPtr->InitFireCount  = *(UBYTE*)(pCursor + i * VM_FILE_CLUMP_REC_SIZE);
    clumpPtr->DependentCount = *(UBYTE*)(pCursor + (i * VM_FILE_CLUMP_REC_SIZE) + 1);
    clumpPtr->CodeStart      = *(UWORD*)(pCursor + (i * VM_FILE_CLUMP_REC_SIZE) + 2) + VarsCmd.pCodespace;

    //Initialize remaining CLUMP_REC fields
    clumpPtr->PC = clumpPtr->CodeStart;
    clumpPtr->Link = NOT_A_CLUMP;

    //Activate any clumps with CurrFireCount of 0
    clumpPtr->CurrFireCount = clumpPtr->InitFireCount;
    if (clumpPtr->CurrFireCount == 0)
      cCmdEnQClump(&(VarsCmd.RunQ), (CLUMP_ID)i);
  }

  //Patch up dependents in separate pass (reuse of pCursor)
  pCursor += VarsCmd.AllClumpsCount * VM_FILE_CLUMP_REC_SIZE;
  for (i = 0; i < VarsCmd.AllClumpsCount; i++)
  {
    CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[i];
    if (clumpPtr->DependentCount > 0)
    {
      clumpPtr->pDependents = (CLUMP_ID*)(pCursor);

      pCursor += (clumpPtr->DependentCount * sizeof(CLUMP_ID));
    }
    else
      clumpPtr->pDependents = NULL;

    //Patch up CodeEnd value based on CodeStart of next clump or last overall codeword
    if (i < (VarsCmd.AllClumpsCount - 1))
      clumpPtr->CodeEnd = (clumpPtr+1)->CodeStart - 1;
    else
      clumpPtr->CodeEnd = VarsCmd.CodespaceCount - 1 + VarsCmd.pCodespace;

    //Test for empty/insane clump code definitions
    NXT_ASSERT(clumpPtr->CodeStart < clumpPtr->CodeEnd);
  }

  // Check if the instructions within a clump are polymorphic and mark which table to dispatch from
  for (i = 0; i < VarsCmd.AllClumpsCount; i++)
  { // Check type on Boolean, math, ArrInit and ArrIndex, ingore GetSet I/O as these are always scalar
    // do we need to check for DataArg encodings to I/O map??? GM
    // Get Opcode and size of each instr, if ^^, check Arg types for Array or Cluster
    CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[i];
    CODE_WORD *pInstr = clumpPtr->CodeStart, *lastPC = clumpPtr->CodeEnd;
    ULONG InstrSize, opCode, shortOp, isT2Agg, isT3Agg, isScalarBinop= TRUE, isScalarUnop2= TRUE;
    TYPE_CODE t1, t2, t3;
    ULONG instrWord;
   do
    {
      instrWord= *(UWORD*)pInstr;
      opCode= OP_CODE(pInstr);
      shortOp= (instrWord>>8) & 0x0F;
      InstrSize = INSTR_SIZE(instrWord);
      if (InstrSize == VAR_INSTR_SIZE)
        InstrSize = ((UWORD*)pInstr)[1];
      if(shortOp <= 7) // no shorts are binOps
      {
        t2= cCmdDSType(pInstr[2]);
        isT2Agg= IS_AGGREGATE_TYPE(t2);
        if(InstrSize == 8) {
        t3= cCmdDSType(pInstr[3]);
        isT3Agg= IS_AGGREGATE_TYPE(t3);
          if(isT2Agg || isT3Agg) {
            if(opCode == OP_CMP) {
              UBYTE isString2, isString3;
              isString2= (t2  == TC_ARRAY) && cCmdDSType(INC_ID(pInstr[2])) == TC_UBYTE;
              isString3= (t3  == TC_ARRAY) && cCmdDSType(INC_ID(pInstr[3])) == TC_UBYTE;
              t1= cCmdDSType(pInstr[1]);
              if((!isString2 || !isString3) || t1 == TC_ARRAY) // allow strings to go scalar, don't let through element compares of bytes or Bools
                isScalarBinop= FALSE;
            }
            else if(opCode == OP_BRCMP)
              isScalarBinop= FALSE;
          }
        }
        else if(InstrSize == 6 && isT2Agg && (opCode == OP_NOT || opCode == OP_BRTST))
          isScalarUnop2= FALSE;
      }
      pInstr += InstrSize/2;
    } while((isScalarBinop || isScalarUnop2) && pInstr < lastPC);
    if(isScalarBinop)
      clumpPtr->clumpScalarDispatchHints |= scalarBinopDispatchMask;
    else
      clumpPtr->clumpScalarDispatchHints &= ~scalarBinopDispatchMask;

    if(isScalarUnop2)
      clumpPtr->clumpScalarDispatchHints |= scalarUnop2DispatchMask;
    else
      clumpPtr->clumpScalarDispatchHints &= ~scalarUnop2DispatchMask;

  }
  //Programs with no active clumps constitutes an activation error
  if (VarsCmd.RunQ.Head == NOT_A_CLUMP)
    return (ERR_FILE);
  else
  {
    // now that we know which clumps are scalar and poly, refresh dispatch table to match head
    cCmdRefreshActiveClump(VarsCmd.RunQ.Head);

  }

  //Initialize dataspace with default values from file
  //!!! This would be a good place to enforce check against potentially
  //    unsafe nested types (deeply nested types mean deep recursive calls)
  DefaultsOffset = 0;
  for (i = 0; i != NOT_A_DS_ID; i = cCmdNextDSElement(i))
  {

    Status = cCmdInflateDSDefaults(pData + FileOffsets.DSDefaults, &DefaultsOffset, i);
    if (IS_ERR(Status))
      return Status;
  }

  if ((DefaultsOffset != FileOffsets.DynamicDefaults)
   || (DefaultsOffset + FileOffsets.DynamicDefaultsSize != FileOffsets.DSDefaultsSize))
  {
    NXT_BREAK;
    return (ERR_FILE);
  }

  //Copy Dynamic defaults from file
  memmove(VarsCmd.pDataspace + VarsCmd.DSStaticSize, pData + FileOffsets.DSDefaults + FileOffsets.DynamicDefaults, FileOffsets.DynamicDefaultsSize);

  // fix memmgr links. old files contain unused backPtrs, we now use these to store backLink
  DV_INDEX prev= NOT_A_DS_ID;
  for (i = VarsCmd.MemMgr.Head; i != NOT_A_DS_ID; i = DV_ARRAY[i].Link) {
    DV_ARRAY[i].BackLink= prev;
    prev= i;
  }

  //Verify the MemMgr ended up where we said it would
  if ((UBYTE *)VarsCmd.MemMgr.pDopeVectorArray != VarsCmd.pDataspace + DV_ARRAY[0].Offset)
  {
    NXT_BREAK;
    return (ERR_FILE);
  }

  //Initialize message queues
  for (i = 0; i < MESSAGE_QUEUE_COUNT; i++)
  {
    VarsCmd.MessageQueues[i].ReadIndex = 0;
    VarsCmd.MessageQueues[i].WriteIndex = 0;

    for (j = 0; j < MESSAGES_PER_QUEUE; j++)
    {
      VarsCmd.MessageQueues[i].Messages[j] = NOT_A_DS_ID;
    }
  }

  //Initialize datalog queue
  VarsCmd.DatalogBuffer.ReadIndex = 0;
  VarsCmd.DatalogBuffer.WriteIndex = 0;
  for (j = 0; j < DATALOG_QUEUE_DEPTH; j++)
  {
    VarsCmd.DatalogBuffer.Datalogs[j] = NOT_A_DS_ID;
  }

  // now that we've loaded program, prime memmgr dopevectors based upon number of handles in ds.
  ULONG numHandles= DV_ARRAY[0].Count/2;
  if(numHandles > 200)
    numHandles= 200;
  Status = cCmdGrowDopeVectorArray(numHandles);

  if (cCmdVerifyMemMgr() != TRUE)
    return (ERR_FILE);

  gUsageSemData= 0;
  gRequestSemData= 0;
  // preload all calibration coefficients into mem
  cCmdLoadCalibrationFiles();
  return (Status);
}


void cCmdDeactivateProgram()
{
  UBYTE i, tmp;

  //Wipe away all references into the pool and clear all run-time data
  VarsCmd.pCodespace = NULL;
  VarsCmd.CodespaceCount = 0;

  VarsCmd.pAllClumps = NULL;
  VarsCmd.AllClumpsCount = 0;

  VarsCmd.DataspaceCount = 0;
  VarsCmd.pDataspaceTOC = NULL;
  VarsCmd.pDataspace = NULL;
  VarsCmd.DataspaceSize = 0;
  VarsCmd.DSStaticSize = 0;

  VarsCmd.MemMgr.Head = NOT_A_DS_ID;
  VarsCmd.MemMgr.Tail = NOT_A_DS_ID;
  VarsCmd.MemMgr.FreeHead = NOT_A_DS_ID;
  VarsCmd.MemMgr.pDopeVectorArray = NULL;

  VarsCmd.RunQ.Head = NOT_A_CLUMP;
  VarsCmd.RunQ.Tail = NOT_A_CLUMP;

  if (VarsCmd.ActiveProgHandle != NOT_A_HANDLE)
  {
    //Close handle that we've kept open for this program
    pMapLoader->pFunc(CLOSE, &(VarsCmd.ActiveProgHandle), NULL, NULL);
    VarsCmd.ActiveProgHandle = NOT_A_HANDLE;

    //Clear internal stashed name
    memset(VarsCmd.ActiveProgName, 0, FILENAME_LENGTH + 1);
  }

  //Close any files we had opened programatically
  for (i = 0; i < MAX_HANDLES; i++)
  {
    //Copy i to tmp, because we pass a pointer to it to pFunc
    tmp = i;
    //Close file
    if (*(VarsCmd.FileHandleTable[i]) != 0)
      pMapLoader->pFunc(CROPDATAFILE, &tmp, NULL, NULL);
  }

  //Clear FileHandleTable
  memset(VarsCmd.FileHandleTable, 0, sizeof(VarsCmd.FileHandleTable));

  return;
}


void cCmdResetDevices(void)
{
  UBYTE i;

  //Clear NXT button counts so 'bumped' will work on first run
  for (i = 0; i < NO_OF_BTNS; i++)
  {
    pMapButton->BtnCnt[i].RelCnt = 0;
    //Need to clear short and long counts too, because RelCnt depends on them.  No known side effects.
    pMapButton->BtnCnt[i].ShortRelCnt = 0;
    pMapButton->BtnCnt[i].LongRelCnt = 0;
  }

  for (i = 0; i < NO_OF_INPUTS; i++)
  {
    INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);
    //Clear type and mode to defaults
    pIn->SensorType = NO_SENSOR;
    pIn->SensorMode = RAWMODE;

    //Reset input values to 0 prior to running (clear things like stale rotation counts)
    pIn->ADRaw       = 0;
    pIn->SensorRaw   = 0;
    pIn->SensorValue = 0;

    //Assert invalid data flag so future code is aware of these changes
    pIn->InvalidData = TRUE;
  }

  for (i = 0; i < NO_OF_OUTPUTS; i++)
  {
    //Coast and reset all motor parameters
    OUTPUT * pOut = &(pMapOutPut->Outputs[i]);
    pOut->Mode = 0;
    pOut->RegMode = REGULATION_MODE_IDLE;
    pOut->RunState = MOTOR_RUN_STATE_IDLE;
    pOut->Speed = 0;
    pOut->TachoLimit = 0;
    pOut->SyncTurnParameter = 0;
    pOut->Flags = UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT | UPDATE_RESET_COUNT | UPDATE_RESET_BLOCK_COUNT | UPDATE_RESET_ROTATION_COUNT;
  }

  //Lowspeed init, INSERT CODE !!!
  for (i = 0; i < NO_OF_LOWSPEED_COM_CHANNEL; i++)
  {
    pMapLowSpeed->InBuf[i].InPtr = 0;
    pMapLowSpeed->InBuf[i].OutPtr = 0;
    pMapLowSpeed->InBuf[i].BytesToRx = 0;
    pMapLowSpeed->OutBuf[i].InPtr = 0;
    pMapLowSpeed->OutBuf[i].OutPtr = 0;
    if (pMapLowSpeed->ChannelState[i] != LOWSPEED_IDLE)
    {
      pMapLowSpeed->ChannelState[i] = LOWSPEED_DONE;
      pMapLowSpeed->State |= (0x01<<i);
    }
  }

}


//Add NewClump to end, updating Queue's head/tail as needed
void cCmdEnQClump(CLUMP_Q * Queue, CLUMP_ID NewClump)
{
  //Make sure NewClump's ID is valid and not already on Q
  NXT_ASSERT(cCmdIsClumpIDSane(NewClump));
  NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);
  NXT_ASSERT(!cCmdIsClumpOnQ(Queue, NewClump));

  VarsCmd.pAllClumps[NewClump].Link = NOT_A_CLUMP;

  //If queue is empty, NewClump becomes both head and tail
  if (Queue->Head == NOT_A_CLUMP)
  {
    NXT_ASSERT(Queue->Tail == NOT_A_CLUMP);

    Queue->Head = NewClump;
    Queue->Tail = NewClump;
    if(Queue == &(VarsCmd.RunQ))
      cCmdRefreshActiveClump(NewClump);
  }
  //Otherwise, tack onto the end
  else
  {
    VarsCmd.pAllClumps[Queue->Tail].Link = NewClump;
    Queue->Tail = NewClump;
  }

  return;
}

//Dequeue specified clump
//Normal usage is to dequeue only from the head (i.e. pass Queue.Head as arg)
void cCmdDeQClump(CLUMP_Q * Queue, CLUMP_ID Clump)
{
  CLUMP_ID CurrID, LinkID;

  //Make sure Clump's ID is valid and is already on Queue
  NXT_ASSERT(cCmdIsClumpIDSane(Clump));
  NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);
  NXT_ASSERT(cCmdIsClumpOnQ(Queue, Clump));

  CurrID = Queue->Head;

  //If our clump is the head, move up the next and disconnect
  if (CurrID == Clump)
  {
    Queue->Head = VarsCmd.pAllClumps[Clump].Link;
    VarsCmd.pAllClumps[Clump].Link = NOT_A_CLUMP;

    //If we just removed the last clump, patch up the queue's tail
    if (Queue->Head == NOT_A_CLUMP)
      Queue->Tail = NOT_A_CLUMP;
    else if(Queue == &(VarsCmd.RunQ))
      cCmdRefreshActiveClump(Queue->Head);
  }
  //Else, look through rest of list looking for a link to our clump
  else
  {
    do
    {
      CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[CurrID];
      LinkID = clumpPtr->Link;

      //If we find a link to our clump, patch up predecessor's link
      if (clumpPtr->Link == Clump)
      {
        clumpPtr->Link = VarsCmd.pAllClumps[Clump].Link;
        VarsCmd.pAllClumps[Clump].Link = NOT_A_CLUMP;

        //If we just removed the tail, patch tail
        if (Clump == Queue->Tail)
          Queue->Tail = CurrID;
      }

      CurrID = LinkID;
    } while (CurrID != NOT_A_CLUMP);
  }

  return;
}


//Rotate head to tail and advance head for given Queue
void cCmdRotateQ()
{
  CLUMP_ID CurrID;
  CLUMP_REC * pClumpRec;
  CLUMP_Q * Queue = &VarsCmd.RunQ;

  //Make sure Queue is sane
  NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);

  //If queue has at least two clumps
  if (Queue->Head != Queue->Tail)
  {
    CurrID = Queue->Head;
    pClumpRec = &(VarsCmd.pAllClumps[CurrID]);

    //Disconnect head
    Queue->Head = pClumpRec->Link;
    pClumpRec->Link = NOT_A_CLUMP;

    //Reconnect head as tail
    pClumpRec = &(VarsCmd.pAllClumps[Queue->Tail]);
    pClumpRec->Link = CurrID;
    Queue->Tail = CurrID;

    // reinit clump info
    CurrID= Queue->Head;
    cCmdRefreshActiveClump(Queue->Head);

    //Make sure we didn't make any really stupid mistakes
    NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);
  }

  return;
}


UBYTE cCmdIsClumpOnQ(CLUMP_Q * Queue, CLUMP_ID Clump)
{
  CLUMP_ID CurrID;

  //Make sure Clump's ID is valid and is already on Queue
  NXT_ASSERT(cCmdIsClumpIDSane(Clump));
  NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);

  CurrID = Queue->Head;

  while (CurrID != NOT_A_CLUMP)
  {
    if (CurrID == Clump)
      return TRUE;

    CurrID = VarsCmd.pAllClumps[CurrID].Link;
  }

  return FALSE;
}


UBYTE cCmdIsQSane(CLUMP_Q * Queue)
{
  CLUMP_ID Head, Tail;
  CLUMP_REC * pHead;

  if (Queue == NULL)
  {
    NXT_BREAK;
    return FALSE;
  }

  Head = Queue->Head;
  Tail = Queue->Tail;

  if (Head == NOT_A_CLUMP && cCmdIsClumpIDSane(Tail))
    return FALSE;

  if (cCmdIsClumpIDSane(Head) && Tail == NOT_A_CLUMP)
    return FALSE;

  if (cCmdIsClumpIDSane(Head) && cCmdIsClumpIDSane(Tail))
  {
    pHead = &(VarsCmd.pAllClumps[Head]);

    //!!! More comprehensive queue tests could go here

    //Check for mislinked head if there are at least two queue members
    if (Head != Tail && pHead->Link == NOT_A_CLUMP)
      return FALSE;
  }

    return TRUE;
}

//
// Mutex queuing functions
//

NXT_STATUS cCmdAcquireMutex(MUTEX_Q * Mutex)
{
  NXT_STATUS Status = NO_ERR;
  CLUMP_ID Clump= VarsCmd.RunQ.Head; // save off before queue changes below

  NXT_ASSERT(Mutex != NULL && cCmdIsClumpIDSane(Clump));

  if (Mutex->Owner == NOT_A_CLUMP)
  {
    //Mutex is open, so just take it
    Mutex->Owner = Clump;

    NXT_ASSERT(Mutex->WaitQ.Head == NOT_A_CLUMP && Mutex->WaitQ.Tail == NOT_A_CLUMP);
  }
  else
  {
    //Mutex is reserved by someone else, take self off RunQ and add to WaitQ
    cCmdDeQClump(&(VarsCmd.RunQ), Clump);
    cCmdEnQClump(&(Mutex->WaitQ), Clump);
    Status = CLUMP_SUSPEND;
  }

  NXT_ASSERT(cCmdIsQSane(&(Mutex->WaitQ)));

  return (Status);
}


NXT_STATUS cCmdReleaseMutex(MUTEX_Q * Mutex)
{
#if defined(WIN_DEBUG) || defined(ARM_DEBUG)
  CLUMP_ID Clump= VarsCmd.RunQ.Head;
#endif
  NXT_ASSERT(Mutex != NULL);
  //!!! don't actually need to pass in Owner clump, but provides nice error checking for now
  // Might want to return an error/warning if we see a Release on an free mutex, though...
  NXT_ASSERT(Clump != NOT_A_CLUMP && Mutex->Owner == Clump);

  //Always set new Owner to WaitQ's Head, since NOT_A_CLUMP means mutex is free
  Mutex->Owner = Mutex->WaitQ.Head;

  if (Mutex->Owner != NOT_A_CLUMP)
  {
    cCmdDeQClump(&(Mutex->WaitQ), Mutex->Owner);
    cCmdEnQClump(&(VarsCmd.RunQ), Mutex->Owner);
  }

  NXT_ASSERT(cCmdIsQSane(&(Mutex->WaitQ)));
  NXT_ASSERT(cCmdIsQSane(&(VarsCmd.RunQ)));

  return (NO_ERR);
}

// No instruction to do this yet, but put current clump to sleep until awakeTime occurs
NXT_STATUS cCmdSleepClump(ULONG time)
{
  CLUMP_ID Clump= VarsCmd.RunQ.Head; // save off before queue changes below
  CLUMP_REC * pClump = &(VarsCmd.pAllClumps[Clump]);
  cCmdDeQClump(&(VarsCmd.RunQ), Clump);
  cCmdEnQClump(&(VarsCmd.RestQ), Clump);
  pClump->awakenTime= time;
  return CLUMP_SUSPEND;
}

UBYTE cCmdCheckRestQ(ULONG currTime)
{
  UBYTE awakened= FALSE;
  CLUMP_ID curr, next;
  CLUMP_REC * pClump;
  curr= VarsCmd.RestQ.Head;
  while(curr != NOT_A_CLUMP) {
    pClump= &(VarsCmd.pAllClumps[curr]);
    next= pClump->Link;
    if(pClump->awakenTime <= currTime) {
      pClump->awakenTime= 0; // not necessary, but for debugging identification
      cCmdDeQClump(&(VarsCmd.RestQ), curr);
      cCmdEnQClump(&(VarsCmd.RunQ), curr);
      awakened= TRUE;
    }
    curr= next;
  }
  return awakened;
}

NXT_STATUS cCmdSchedDependents(CLUMP_ID Clump, SWORD Begin, SWORD End)
{
  CLUMP_ID CurrDepClumpID;
  SWORD i;

  //Begin and End specify range of CLUMP_IDs in dependent list to schedule
  //If either equals -1, both should equal -1, and no dependents will be scheduled
  //Else schedule specified subset offset from pDependents

  //Check for valid args
  NXT_ASSERT(cCmdIsClumpIDSane(Clump));
  NXT_ASSERT((Begin >= 0 && End >= 0 && End < VarsCmd.pAllClumps[Clump].DependentCount)
          || (Begin == -1 && End == -1));

  //If non-empty range
  if (Begin != -1 || End != -1)
  {
    //update dependents, scheduling if their CurrFireCount reaches 0
    for (i = Begin; i <= End; i++)
    {
      CurrDepClumpID = VarsCmd.pAllClumps[Clump].pDependents[i];

      NXT_ASSERT(cCmdIsClumpIDSane(CurrDepClumpID));

      VarsCmd.pAllClumps[CurrDepClumpID].CurrFireCount--;

      if (VarsCmd.pAllClumps[CurrDepClumpID].CurrFireCount == 0)
        cCmdEnQClump(&(VarsCmd.RunQ), CurrDepClumpID);
    }
  }

  return (NO_ERR);
}


NXT_STATUS cCmdSchedDependent(CLUMP_ID Clump, CLUMP_ID TargetClump)
{
  //TargetClump specifies the clump number of the target to schedule explicitly.

  //Check for valid args
  NXT_ASSERT(cCmdIsClumpIDSane(Clump));
  NXT_ASSERT(cCmdIsClumpIDSane(TargetClump));

  CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[TargetClump];
  clumpPtr->CurrFireCount--;
  if (clumpPtr->CurrFireCount == 0)
    cCmdEnQClump(&(VarsCmd.RunQ), TargetClump);

  return (NO_ERR);
}


UBYTE cCmdIsClumpIDSane(CLUMP_ID Clump)
{
  if (Clump < VarsCmd.AllClumpsCount)
    return TRUE;
  else
    return FALSE;
}


//
// Memory pool management functions
//
void cCmdInitPool(void)
{
  ULONG i;
  ULONG *poolPtr;

  //VarsCmd.Pool is a UBYTE pointer to ULONG array
  //This was done to enforce portable alignment.
  VarsCmd.Pool = (UBYTE*)(IOMapCmd.MemoryPool);

  for (i = (POOL_MAX_SIZE / 4), poolPtr= (ULONG*)&(POOL_START)[0]; i>0; i--, poolPtr++)
    *poolPtr = 0xDEADBEEF;

  VarsCmd.PoolSize = 0;
}


#if VMProfilingCode
ULONG memMgrTime= 0;
#endif
NXT_STATUS cCmdDSArrayAlloc(DS_ELEMENT_ID DSElementID, UWORD Offset, UWORD NewCount)
{
  NXT_STATUS Status = NO_ERR;
  UWORD DVIndex;
  UWORD OldCount;
  UWORD i;
#if VMProfilingCode
  ULONG enterTime= dTimerReadHiRes();
#endif
  NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));

  //Only arrays are valid here
  //!!! Recommended to upgrade NXT_ASSERT to ERR_INSTR return
  NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY);

  DVIndex = cCmdGetDVIndex(DSElementID, Offset);
  OldCount = DV_ARRAY[DVIndex].Count;

  if(OldCount == NewCount)
    goto allocExit;
  Status = cCmdDVArrayAlloc(DVIndex, NewCount);

  if (Status < NO_ERR)
    goto allocExit;

  if(!IS_AGGREGATE_TYPE(cCmdDSType(INC_ID(DSElementID))))
    goto allocExit;

  if (OldCount > NewCount)
  {
    //Free dope vectors for sub-arrays.
    for (i = NewCount; i < OldCount; i++)
    {
      Status = cCmdFreeSubArrayDopeVectors(INC_ID(DSElementID), ARRAY_ELEM_OFFSET(DVIndex, i));
      if (IS_ERR(Status))
        goto allocExit;
    }
  }
  else if (OldCount < NewCount)
  {
    //Alloc dope vectors for sub-arrays. Set up DVIndexes
    for (i = OldCount; i < NewCount;  i++)
    {
      Status = cCmdAllocSubArrayDopeVectors(INC_ID(DSElementID), ARRAY_ELEM_OFFSET(DVIndex, i));
      if (IS_ERR(Status))
        goto allocExit;
    }
  }

  NXT_ASSERT(cCmdVerifyMemMgr());
allocExit:
#if VMProfilingCode
  memMgrTime += dTimerReadHiRes() - enterTime;
#endif
  return Status;
}

NXT_STATUS cCmdDVArrayAlloc(DV_INDEX DVIndex, UWORD NewCount)
{
  NXT_STATUS Status = NO_ERR;
  UBYTE *pData;
  UWORD ArraySize, InplaceSize;
  UWORD NextDVIndex;
  UWORD OldCount;

  OldCount = DV_ARRAY[DVIndex].Count;

  if (OldCount == NewCount)
  {
    //Nothing to alloc. Return.
    return Status;
  }
  else if (OldCount > NewCount)
  {
    //Already have the space. Shrink inplace.
    DV_ARRAY[DVIndex].Count = NewCount;
        return Status;
  }
  else // need to grow array
  {
    //Calculate new array size
    ArraySize = NewCount * DV_ARRAY[DVIndex].ElemSize;

    //Try growing inplace
    // If the Offset == NOT_AN_OFFSET then the array has never been allocated and can't grow inplace.
    if (DV_ARRAY[DVIndex].Offset != NOT_AN_OFFSET)
    {
      //Get pointer to next dope vector in dataspace
      if (DV_ARRAY[DVIndex].Link != NOT_A_DS_ID)
      {
        NextDVIndex = DV_ARRAY[DVIndex].Link;
        InplaceSize = DV_ARRAY[NextDVIndex].Offset - DV_ARRAY[DVIndex].Offset;
      }
      else
      {
        //Last element in dataspace.
        NXT_ASSERT(DVIndex == VarsCmd.MemMgr.Tail);
        InplaceSize = VarsCmd.DataspaceSize - DV_ARRAY[DVIndex].Offset;
      }

      if (ArraySize <= InplaceSize)
      {
        DV_ARRAY[DVIndex].Count = NewCount;
            return Status;
      }
    }

    //Can't grow inplace, have to allocate new space

    //Make sure we properly align for type
    //!!! This could also overflow memory (make PoolSize > POOL_MAX_SIZE) if we're within 3 bytes of the end.
    //     I don't think it matters because if it does happend, we'll trigger the ERR_MEM below and compact.
    //     During compaction, we'll reclaim these unused bytes.
    //!!! Aligning beginning of ALL arrays to 4 byte address
    ALIGN_TO_MOD(VarsCmd.PoolSize, SIZE_ULONG);
    ALIGN_TO_MOD(VarsCmd.DataspaceSize, SIZE_ULONG);

    if (VarsCmd.PoolSize + ArraySize >= POOL_MAX_SIZE)
    {
      //Not enough memory available
      return ERR_MEM;
    }

    //Get data from end of pool
    pData = VarsCmd.Pool + VarsCmd.PoolSize;
    //Grow pool and dataspace
    VarsCmd.PoolSize += ArraySize;
    VarsCmd.DataspaceSize += ArraySize;

    //Move old Array Data to new allocation
    if(OldCount)
      memmove(pData, VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset, (UWORD)(DV_ARRAY[DVIndex].ElemSize * OldCount));
    //!!! Clear mem so old mem doesn't contain stale data. Not strictly needed.
#if defined(WIN_DEBUG) || defined(ARM_DEBUG)
    memset(VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset, 0xFF, (UWORD)(DV_ARRAY[DVIndex].ElemSize * OldCount));
#endif
    //Update dope vector
    DV_ARRAY[DVIndex].Offset = pData - VarsCmd.pDataspace;
    DV_ARRAY[DVIndex].Count = NewCount;

    //Move dope vector to end of MemMgr list
    Status = cCmdMemMgrMoveToTail(DVIndex);
    if (IS_ERR(Status))
      return Status;

    NXT_ASSERT(cCmdVerifyMemMgr());
  }

  return Status;
}


//!!! Recursive function
NXT_STATUS cCmdAllocSubArrayDopeVectors(DS_ELEMENT_ID DSElementID, UWORD Offset)
{
  // Walks a single array element to see if it contains arrays
  // For any array it finds, a dope vector is allocated and the DVIndex is placed in the dataspace for the parent array.
  // This is a non-recursive function. It only walks the immediate array element.
  // DSElementID - ID of array sub-entry
  // Offset - offset to array element in dataspace
  NXT_STATUS Status = NO_ERR;
  TYPE_CODE TypeCode;
  DV_INDEX DVIndex;
  UWORD i;
  UWORD DVIndexOffset; //Offset to DVIndex field that points to the DopeVector from pDataspace
  UWORD LoopCount = 1;
  UWORD ElemSize;

  for (i = 0; i < LoopCount; i++)
  {
    TypeCode = cCmdDSType((DS_ELEMENT_ID)(DSElementID + i));
    if (TypeCode == TC_CLUSTER)
    {
      LoopCount += cCmdClusterCount(DSElementID);
    }
    else if (TypeCode == TC_ARRAY)
    {
      //!!! ElemSize is a static value, but we don't have anywhere we put it (another TOC sub-entry?)
      //     It'd be nice to not have to recalculate it.
      ElemSize = cCmdCalcArrayElemSize((DS_ELEMENT_ID)(DSElementID + i));
      DVIndexOffset = VarsCmd.pDataspaceTOC[DSElementID + i].DSOffset + Offset;
      Status = cCmdAllocDopeVector(&DVIndex, ElemSize);
      if (IS_ERR(Status))
        return Status;

      *((UWORD *)(VarsCmd.pDataspace + DVIndexOffset)) = DVIndex;
    }
  }

  return Status;
}


//!!! Recursive function
NXT_STATUS cCmdFreeSubArrayDopeVectors(DS_ELEMENT_ID DSElementID, UWORD Offset)
{
  // Walks a single array element to see if it contains arrays
  // Frees all dope vectors associated with the array element.
  // Recursively deletes sub-arrays.
  // DSElementID - ID of array sub-entry
  // Offset - offset to array element in dataspace
  NXT_STATUS Status = NO_ERR;
  TYPE_CODE TypeCode;
  DV_INDEX DVIndex;
  UWORD i, Count;

  TypeCode = cCmdDSType(DSElementID);

  if (TypeCode == TC_ARRAY)
  {
    DVIndex = cCmdGetDVIndex(DSElementID, Offset);

    NXT_ASSERT(DVIndex < DV_ARRAY[0].Count);

    Count = DV_ARRAY[DVIndex].Count;
    //Recur on sub-elements
    for (i = 0; i < Count; i++)
    {
      Status = cCmdFreeSubArrayDopeVectors(INC_ID(DSElementID), ARRAY_ELEM_OFFSET(DVIndex, i));
      if (IS_ERR(Status))
        return Status;
    }

    //Free Dope Vector
    Status = cCmdFreeDopeVector(DVIndex);
  }
  else if (TypeCode == TC_CLUSTER)
  {
    Count = cCmdClusterCount(DSElementID);
    DSElementID = INC_ID(DSElementID);
    //Recur on sub-elements
    for (i = 0; i < Count; i++)
    {
      Status = cCmdFreeSubArrayDopeVectors((DS_ELEMENT_ID)(DSElementID + i), Offset);
      if (IS_ERR(Status))
        return Status;
    }
  }

  return Status;
}


NXT_STATUS cCmdAllocDopeVector(DV_INDEX *pIndex, UWORD ElemSize)
{
  NXT_STATUS Status = NO_ERR;

  if (VarsCmd.MemMgr.FreeHead == NOT_A_DS_ID)
  {
    //No free DVs. Need to grow DopeVector array.
    Status = cCmdGrowDopeVectorArray(DV_ARRAY_GROWTH_COUNT);
    if (IS_ERR(Status))
      return Status;
  }

  if(VarsCmd.MemMgr.FreeHead == NOT_A_DS_ID)
    return ERR_MEM;

  //Remove DV from free list
  *pIndex = VarsCmd.MemMgr.FreeHead;
  VarsCmd.MemMgr.FreeHead = DV_ARRAY[*pIndex].Link;
  if(VarsCmd.MemMgr.FreeHead != NOT_A_DS_ID)
    DV_ARRAY[VarsCmd.MemMgr.FreeHead].BackLink= NOT_A_DS_ID;
  //Add DV to tail of MemMgr list
  Status = cCmdMemMgrInsertAtTail(*pIndex);

  //Initialize values
  DV_ARRAY[*pIndex].Offset = NOT_AN_OFFSET;
  DV_ARRAY[*pIndex].ElemSize = ElemSize;
  DV_ARRAY[*pIndex].Count = 0;

  NXT_ASSERT(cCmdVerifyMemMgr());

  return Status;
}

//
//cCmdFreeDopeVector() - Open up a spot in the DopeVectorArray for future use
// The DopeVectorArray doesn't shrink when arrays (and their dope vectors) are deleted.
// Instead they're pushed on the free list and the array stays the same size.
// Future allocations check the free list before resorting to cCmdGrowDopeVectorArray()
//
NXT_STATUS cCmdFreeDopeVector(DV_INDEX DVIndex)
{
  NXT_STATUS Status = NO_ERR;
  DV_INDEX prev, post;

  //Bounds check
  NXT_ASSERT(DVIndex < DV_ARRAY[0].Count);

  //Reset dope vector fields
  DV_ARRAY[DVIndex].Count = 0;
  DV_ARRAY[DVIndex].ElemSize = 0;
  DV_ARRAY[DVIndex].Offset = NOT_AN_OFFSET;

  //Remove from MemMgr list
  if (DVIndex == VarsCmd.MemMgr.Head)
  {
    VarsCmd.MemMgr.Head = DV_ARRAY[DVIndex].Link;
    if(VarsCmd.MemMgr.Head != NOT_A_DS_ID)
      DV_ARRAY[VarsCmd.MemMgr.Head].BackLink= NOT_A_DS_ID;
  }
  else
  {
    // patchup middle or end of list.
    prev= DV_ARRAY[DVIndex].BackLink;
    post= DV_ARRAY[DVIndex].Link;
    NXT_ASSERT(prev != NOT_A_DS_ID);

    DV_ARRAY[prev].Link = post;
    if(post != NOT_A_DS_ID)
      DV_ARRAY[post].BackLink= prev;
    if (DVIndex == VarsCmd.MemMgr.Tail)
      VarsCmd.MemMgr.Tail = prev;
    //Make sure we found the previous DV, otherwise this DV was not in the the list (already freed?)
  }

  //Push onto free list
  DV_ARRAY[DVIndex].Link = VarsCmd.MemMgr.FreeHead;
  DV_ARRAY[DVIndex].BackLink = NOT_A_DS_ID;
  DV_ARRAY[VarsCmd.MemMgr.FreeHead].BackLink= DVIndex;
  VarsCmd.MemMgr.FreeHead = DVIndex;

  NXT_ASSERT(cCmdVerifyMemMgr());

  return Status;
}

//
//cCmdGrowDopeVectorArray() - expand DopeVectorArray to be able to track more dataspace arrays
//
NXT_STATUS cCmdGrowDopeVectorArray(UWORD NewNodesCount)
{
  NXT_STATUS Status = NO_ERR;
  UWORD ArraySize;
  UWORD OldCount, NewCount, i;
  UBYTE * pData;

  NXT_ASSERT(cCmdVerifyMemMgr());

  OldCount = DV_ARRAY[0].Count;
  NewCount = OldCount + NewNodesCount;
  NXT_ASSERT(NewCount > OldCount);

  ArraySize = DV_ARRAY[0].ElemSize * NewCount;

  //!!! Aligning beginning of ALL arrays to 4 byte address
  ALIGN_TO_MOD(VarsCmd.PoolSize, SIZE_ULONG);
  ALIGN_TO_MOD(VarsCmd.DataspaceSize, SIZE_ULONG);

  if (VarsCmd.PoolSize + ArraySize >= POOL_MAX_SIZE)
  {
    //Not enough memory available
    return ERR_MEM;
  }

  //Get data from end of pool
  pData = VarsCmd.Pool + VarsCmd.PoolSize;
  //Grow pool and dataspace
  VarsCmd.PoolSize += ArraySize;
  VarsCmd.DataspaceSize += ArraySize;

  //Move DopeVector Array
  memmove(pData, (UBYTE *)VarsCmd.MemMgr.pDopeVectorArray, (UWORD)(DV_ARRAY[0].ElemSize * DV_ARRAY[0].Count));

  //Update MemMgr pointer
  VarsCmd.MemMgr.pDopeVectorArray = (DOPE_VECTOR *)pData;
  IOMapCmd.OffsetDVA = (UWORD)((ULONG)(VarsCmd.MemMgr.pDopeVectorArray) - (ULONG)&(IOMapCmd));

  //Update dope vector
  DV_ARRAY[0].Offset = pData - VarsCmd.pDataspace;
  DV_ARRAY[0].Count = NewCount;

  //Add new DopeVectors to free list
  //Push in reverse order so they get popped in order (mostly for ease of debugging)
  for (i = NewCount - 1; i >= OldCount; i--)
  {
    DV_ARRAY[i].Offset = 0xFFFF;
    DV_ARRAY[i].ElemSize = 0;
    DV_ARRAY[i].Count = 0;
    DV_ARRAY[i].BackLink = NOT_A_DS_ID;
    if(VarsCmd.MemMgr.FreeHead != NOT_A_DS_ID)
      DV_ARRAY[VarsCmd.MemMgr.FreeHead].BackLink = i;
    DV_ARRAY[i].Link = VarsCmd.MemMgr.FreeHead;
    VarsCmd.MemMgr.FreeHead = i;
  }

  //Move dope vector to end of MemMgr list
  Status = cCmdMemMgrMoveToTail(0);

  NXT_ASSERT(cCmdVerifyMemMgr());

  return Status;
}


UWORD cCmdCalcArrayElemSize(DS_ELEMENT_ID DSElementID)
{
  TYPE_CODE TypeCode;
  UWORD SizeOfType;
  UWORD i;
  UWORD LoopCount = 1;
  UWORD Size = 0;
  UWORD Alignment = 0;

  NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY);

  DSElementID = INC_ID(DSElementID);
  for (i = 0; i < LoopCount; i++)
  {
    TypeCode = cCmdDSType((DS_ELEMENT_ID)(DSElementID + i));
    if (TypeCode == TC_CLUSTER)
    {
      LoopCount += cCmdClusterCount((DS_ELEMENT_ID)(DSElementID + i));
    }
    else
    {
      SizeOfType = cCmdSizeOf(TypeCode);
      ALIGN_TO_MOD(Size, SizeOfType);
      Size += SizeOfType;
      if (SizeOfType > Alignment)
        Alignment = SizeOfType;
    }
  }
  ALIGN_TO_MOD(Size, Alignment);

  return Size;
}


NXT_STATUS cCmdMemMgrMoveToTail(DV_INDEX DVIndex)
{
  DV_INDEX prev, post;

  //Bounds check
  NXT_ASSERT(DVIndex < DV_ARRAY[0].Count);

  //Short circut if its already at the tail
  if (DVIndex == VarsCmd.MemMgr.Tail)
    return NO_ERR;

  if (DVIndex == VarsCmd.MemMgr.Head) {
    VarsCmd.MemMgr.Head = DV_ARRAY[DVIndex].Link;
    DV_ARRAY[VarsCmd.MemMgr.Head].BackLink= NOT_A_DS_ID;
  }
  else
  {
    // connect to middle or end of list.
    prev= DV_ARRAY[DVIndex].BackLink;
    post= DV_ARRAY[DVIndex].Link;
    NXT_ASSERT(prev != NOT_A_DS_ID);
    DV_ARRAY[prev].Link = post;
    if(post != NOT_A_DS_ID)
      DV_ARRAY[post].BackLink= prev;
  }

  DV_ARRAY[DVIndex].Link = NOT_A_DS_ID;
  DV_ARRAY[DVIndex].BackLink = VarsCmd.MemMgr.Tail;
  if(VarsCmd.MemMgr.Tail != NOT_A_DS_ID)
    DV_ARRAY[VarsCmd.MemMgr.Tail].Link = DVIndex;
  VarsCmd.MemMgr.Tail = DVIndex;

  NXT_ASSERT(cCmdVerifyMemMgr());

  return NO_ERR;
}


NXT_STATUS cCmdMemMgrInsertAtTail(DV_INDEX DVIndex)
{
  //Bounds check
  NXT_ASSERT(DVIndex < DV_ARRAY[0].Count);

  DV_ARRAY[VarsCmd.MemMgr.Tail].Link = DVIndex;
  DV_ARRAY[DVIndex].BackLink= VarsCmd.MemMgr.Tail;
  DV_ARRAY[DVIndex].Link = NOT_A_DS_ID;
  VarsCmd.MemMgr.Tail = DVIndex;

  NXT_ASSERT(cCmdVerifyMemMgr());

  return NO_ERR;
}


UBYTE cCmdVerifyMemMgr()
{
  DV_INDEX i, prev, post;
  UWORD CurrOffset = 0;
  UWORD PrevOffset = 0;
  UWORD DVCount = 0;

  //Make sure the MemMgr list is properly sorted in ascending offset order
  for (i = VarsCmd.MemMgr.Head; i != NOT_A_DS_ID; i = DV_ARRAY[i].Link)
  {
    CurrOffset = DV_ARRAY[i].Offset;

    if (CurrOffset != 0xFFFF)
    {
      if (PrevOffset > CurrOffset)
        return FALSE;

      PrevOffset = CurrOffset;
    }

    prev= DV_ARRAY[i].BackLink;
    post= DV_ARRAY[i].Link;
    if (post == NOT_A_DS_ID && i != VarsCmd.MemMgr.Tail)
      return FALSE;
    else if(prev == NOT_A_DS_ID && i != VarsCmd.MemMgr.Head)
      return FALSE;
    else if(prev != NOT_A_DS_ID && DV_ARRAY[prev].Link != i)
      return FALSE;
    else if(post != NOT_A_DS_ID && DV_ARRAY[post].BackLink != i)
      return FALSE;

    DVCount++;
  }

  // could check link and backlinks too
  for (i = VarsCmd.MemMgr.FreeHead; i != NOT_A_DS_ID; i = DV_ARRAY[i].Link)
  {
    DVCount++;
  }

  //Make sure the # of dope vectors = # used + # free
  if (DVCount != DV_ARRAY[0].Count)
    return FALSE;

  return TRUE;
}


NXT_STATUS cCmdDSCompact(void)
{
  NXT_STATUS Status = NO_ERR;

  DV_INDEX CurrIndex;
  UWORD NewOffset;
  UWORD CurrOffset;
  UWORD Size;
  UWORD DeltaDSSize;
  UWORD TempOffset, TempSize;

#if VM_BENCHMARK
  ULONG StartTime, TotalTime;

  VarsCmd.CompactionCount++;
  VarsCmd.LastCompactionTick = IOMapCmd.Tick - VarsCmd.StartTick;

  StartTime = dTimerRead();
#endif

  NXT_ASSERT(cCmdVerifyMemMgr());

  NewOffset = VarsCmd.DSStaticSize;
  for (CurrIndex = VarsCmd.MemMgr.Head; CurrIndex != NOT_A_DS_ID; CurrIndex = DV_ARRAY[CurrIndex].Link)
  {
    //Align NewOffset for array to 4 byte address.
    ALIGN_TO_MOD(NewOffset, SIZE_ULONG);

    CurrOffset = DV_ARRAY[CurrIndex].Offset;
    if (CurrOffset != NOT_AN_OFFSET)
    {
      Size = DV_ARRAY[CurrIndex].ElemSize * DV_ARRAY[CurrIndex].Count;
      if (CurrOffset != NewOffset)
      {
        NXT_ASSERT(NewOffset < CurrOffset);
        memmove(VarsCmd.pDataspace + NewOffset, VarsCmd.pDataspace + CurrOffset, Size);

        //  Clear mem to make stale data references more obvious while debugging.
        //  Correct for overlapping memory regions (make sure we don't clear what we just moved).
        //!!! Clearing step not strictly necessary, so it could be optimized out
        if (NewOffset + Size > CurrOffset)
        {
          TempOffset = NewOffset + Size;
          TempSize = Size - (TempOffset - CurrOffset);
        }
        else
        {
          TempOffset = CurrOffset;
          TempSize = Size;
        }
        memset(VarsCmd.pDataspace + TempOffset, 0xFF, TempSize);

        //Update pDopeVectorArray if we move the dope vector array
        if (CurrIndex == 0)
        {
          VarsCmd.MemMgr.pDopeVectorArray = (DOPE_VECTOR *)(VarsCmd.pDataspace + NewOffset);
          IOMapCmd.OffsetDVA = (UWORD)((ULONG)(VarsCmd.MemMgr.pDopeVectorArray) - (ULONG)&(IOMapCmd));
        }

        //Update offset in DV Array
        DV_ARRAY[CurrIndex].Offset = NewOffset;
      }

      NewOffset += Size;
    }
  }

  DeltaDSSize = VarsCmd.DataspaceSize - NewOffset;

  VarsCmd.PoolSize -= DeltaDSSize;
  VarsCmd.DataspaceSize -= DeltaDSSize;

  NXT_ASSERT(cCmdVerifyMemMgr());

#if VM_BENCHMARK
  TotalTime = dTimerRead() - StartTime;

  if (TotalTime > VarsCmd.MaxCompactionTime)
    VarsCmd.MaxCompactionTime = TotalTime;
#endif

  return Status;
}


//
// Message Queue functions
//

NXT_STATUS cCmdMessageWrite(UWORD QueueID, UBYTE * pData, UWORD Length)
{
  NXT_STATUS Status = NO_ERR;

  if (pData == NULL)
    return ERR_ARG;

  if (QueueID >= MESSAGE_QUEUE_COUNT)
    return ERR_INVALID_QUEUE;

  if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
    return ERR_NO_PROG;

  //Can't accept oversize messages because we treat them as strings (truncation would remove null termination)
  if (Length > MAX_MESSAGE_SIZE)
    return ERR_INVALID_SIZE;

  if (IS_DV_INDEX_SANE(GET_WRITE_MSG(QueueID)))
  {
    //A message is already there, the queue is full
    NXT_ASSERT(VarsCmd.MessageQueues[QueueID].WriteIndex == VarsCmd.MessageQueues[QueueID].ReadIndex);

    //Bump read index, drop existing message to make room for our new incoming message
    VarsCmd.MessageQueues[QueueID].ReadIndex = (VarsCmd.MessageQueues[QueueID].ReadIndex + 1) % MESSAGES_PER_QUEUE;
  }
  else
  {
    //Allocate dope vector for message
    Status = cCmdAllocDopeVector(&GET_WRITE_MSG(QueueID), 1);
    if (IS_ERR(Status))
      return Status;
  }

  //Allocate storage for message
  Status = cCmdDVArrayAlloc(GET_WRITE_MSG(QueueID), Length);
  if (IS_ERR(Status))
  {
    //Clear the dope vector for the message, since we're unable to put a message there.
    cCmdFreeDopeVector(GET_WRITE_MSG(QueueID));
    SET_WRITE_MSG(QueueID, NOT_A_DS_ID);
    return Status;
  }

  //Copy message
  memmove(cCmdDVPtr(GET_WRITE_MSG(QueueID)), pData, Length);

  //Advance write index
  VarsCmd.MessageQueues[QueueID].WriteIndex = (VarsCmd.MessageQueues[QueueID].WriteIndex + 1) % MESSAGES_PER_QUEUE;

  return Status;
}


NXT_STATUS cCmdMessageGetSize(UWORD QueueID, UWORD * Size)
{
  DV_INDEX ReadDVIndex;

  if (Size == NULL)
    return (ERR_ARG);

  if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
  {
    *Size = 0;
    return (ERR_NO_PROG);
  }

  if (QueueID >= MESSAGE_QUEUE_COUNT)
  {
    *Size = 0;
    return (ERR_INVALID_QUEUE);
  }

  ReadDVIndex = GET_READ_MSG(QueueID);

  if (IS_DV_INDEX_SANE(ReadDVIndex))
  {
    *Size = (DV_ARRAY[ReadDVIndex].Count);
    return (NO_ERR);
  }
  else
  {
    *Size = 0;
    return (STAT_MSG_EMPTY_MAILBOX);
  }
}


NXT_STATUS cCmdMessageRead(UWORD QueueID, UBYTE * pBuffer, UWORD Length, UBYTE Remove)
{
  NXT_STATUS Status = NO_ERR;
  DV_INDEX ReadDVIndex;

  if (pBuffer == NULL)
    return (ERR_ARG);

  if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
    return (ERR_NO_PROG);

  if (QueueID >= MESSAGE_QUEUE_COUNT)
    return (ERR_INVALID_QUEUE);

  ReadDVIndex = GET_READ_MSG(QueueID);

  if (IS_DV_INDEX_SANE(ReadDVIndex))
  {
    //If Buffer doesn't have room for the entire message,
    //don't risk incomplete string floating around
    if (Length < DV_ARRAY[ReadDVIndex].Count)
      return (ERR_INVALID_SIZE);

    //Copy message
    memmove(pBuffer, cCmdDVPtr(ReadDVIndex), DV_ARRAY[ReadDVIndex].Count);

    if (Remove)
    {
      //Free memory used by message
      Status = cCmdFreeDopeVector(ReadDVIndex);
      if (IS_ERR(Status))
        return Status;

      SET_READ_MSG(QueueID, NOT_A_DS_ID);

      //Advance read index
      VarsCmd.MessageQueues[QueueID].ReadIndex = (VarsCmd.MessageQueues[QueueID].ReadIndex + 1) % MESSAGES_PER_QUEUE;
    }
  }
  else
  {
    //No message to read, message Queue is empty
    NXT_ASSERT(VarsCmd.MessageQueues[QueueID].ReadIndex == VarsCmd.MessageQueues[QueueID].WriteIndex);

    return (STAT_MSG_EMPTY_MAILBOX);
  }

  return Status;
}

//
// Datalog Queue function(s)
//

NXT_STATUS cCmdDatalogWrite(UBYTE * pData, UWORD Length)
{
  NXT_STATUS Status = NO_ERR;

  if (pData == NULL)
    return ERR_ARG;

  if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
    return (ERR_NO_PROG);

  //Can't accept oversize messages because we treat them as strings (truncation would remove null termination)
  if (Length > MAX_DATALOG_SIZE)
    return ERR_INVALID_SIZE;

  if (IS_DV_INDEX_SANE(GET_WRITE_DTLG()))
  {
    //A message is already there, the queue is full
    NXT_ASSERT(VarsCmd.DatalogBuffer.WriteIndex == VarsCmd.DatalogBuffer.ReadIndex);
    Status = STAT_MSG_BUFFERWRAP;
    //Bump read index, drop existing message to make room for our newly acquired datalog
    VarsCmd.DatalogBuffer.ReadIndex = (VarsCmd.DatalogBuffer.ReadIndex + 1) % DATALOG_QUEUE_DEPTH;
  }
  else
  {
    //Allocate dope vector for message
    Status = cCmdAllocDopeVector(&GET_WRITE_DTLG(), 1);
    if (IS_ERR(Status))
      return Status;
  }

  //Allocate storage for message
  Status |= cCmdDVArrayAlloc(GET_WRITE_DTLG(), Length);
  if (IS_ERR(Status))
  {
    //Clear the dope vector for the message, since we're unable to put a message there.
    cCmdFreeDopeVector(GET_WRITE_DTLG());
    SET_WRITE_DTLG(NOT_A_DS_ID);
    return Status;
  }

  //Copy message
  memmove(cCmdDVPtr(GET_WRITE_DTLG()), pData, Length);

  //Advance write index
  VarsCmd.DatalogBuffer.WriteIndex = (VarsCmd.DatalogBuffer.WriteIndex + 1) % DATALOG_QUEUE_DEPTH;

  return Status;
}

NXT_STATUS cCmdDatalogGetSize(UWORD * Size)
{
  DV_INDEX ReadDVIndex;

  if (Size == NULL)
    return (ERR_ARG);

  if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
  {
    *Size = 0;
    return (ERR_NO_PROG);
  }

  ReadDVIndex = GET_READ_DTLG();

  if (IS_DV_INDEX_SANE(ReadDVIndex))
  {
    *Size = (DV_ARRAY[ReadDVIndex].Count);
    return (NO_ERR);
  }
  else
  {
    *Size = 0;
    return (STAT_MSG_EMPTY_MAILBOX);
  }
}

NXT_STATUS cCmdDatalogRead(UBYTE * pBuffer, UWORD Length, UBYTE Remove)
{
  NXT_STATUS Status = NO_ERR;
  DV_INDEX ReadDVIndex;

  if (pBuffer == NULL)
    return (ERR_ARG);

  if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
    return (ERR_NO_PROG);

  ReadDVIndex = GET_READ_DTLG();

  if (IS_DV_INDEX_SANE(ReadDVIndex))
  {
    //If Buffer doesn't have room for the entire message,
    //don't risk incomplete string floating around
    if (Length < DV_ARRAY[ReadDVIndex].Count)
      return (ERR_INVALID_SIZE);

    //Copy message
    memmove(pBuffer, cCmdDVPtr(ReadDVIndex), DV_ARRAY[ReadDVIndex].Count);

    if (Remove)
    {
      //Free memory used by message
      Status = cCmdFreeDopeVector(ReadDVIndex);
      if (IS_ERR(Status))
        return Status;

      SET_READ_DTLG(NOT_A_DS_ID);

      //Advance read index
      VarsCmd.DatalogBuffer.ReadIndex = (VarsCmd.DatalogBuffer.ReadIndex + 1) % DATALOG_QUEUE_DEPTH;
    }
  }
  else
  {
    //No message to read, datalog Queue is empty
    NXT_ASSERT(VarsCmd.DatalogBuffer.ReadIndex == VarsCmd.DatalogBuffer.WriteIndex);

    return (STAT_MSG_EMPTY_MAILBOX);
  }

  return Status;
}


//
// Color Sensor Functions
//
NXT_STATUS cCmdColorSensorRead (UBYTE Port, SWORD * SensorValue, UWORD * RawArray, UWORD * NormalizedArray,
								SWORD * ScaledArray, UBYTE * InvalidData)
{
	ULONG i;
	//Make sure Port is valid for Color Sensor
	INPUTSTRUCT * pIn = &(pMapInput->Inputs[Port]);
	UBYTE sType = pIn->SensorType;
	if (!(sType == COLORFULL || sType == COLORRED || sType == COLORGREEN ||
		sType == COLORBLUE || sType == COLORNONE))
	{
		return (ERR_COMM_CHAN_NOT_READY); //TODO - is this the right error?
	}
	//Copy Detected Color
	*SensorValue = pIn->SensorValue;

	//Copy all raw, normalized and scaled data from I/O Map
	for (i=0; i<NO_OF_COLORS; i++){
		COLORSTRUCT * pColor = &(pMapInput->Colors[Port]);
		RawArray[i] = pColor->ADRaw[i];
		NormalizedArray[i] = pColor->SensorRaw[i];
		ScaledArray[i] = pColor->SensorValue[i];
	}
	//Copy the Invalid Data Flag
	*InvalidData = pIn->InvalidData;

	return NO_ERR;

}


//
// Dataspace Support functions
//

UBYTE cCmdIsDSElementIDSane(DS_ELEMENT_ID Index)
{
  if (Index < VarsCmd.DataspaceCount)
    return TRUE;
  else
    return FALSE;
}

void * cCmdResolveDataArg(DATA_ARG DataArg, UWORD Offset, TYPE_CODE * TypeCode)
{
  void * ret_val = NULL;

  //!!! DATA_ARG masking system only for internal c_cmd use!
  //    All normal bytecode arguments should go through top if() block.

    NXT_ASSERT(cCmdIsDSElementIDSane(DataArg));
    ret_val = cCmdDSPtr(DataArg, Offset);
    if (TypeCode)
      *TypeCode = VarsCmd.pDataspaceTOC[DataArg].TypeCode;

  //!!! Caller beware! If DataArg isn't sane, ret_val may be out of range or NULL!
  return ret_val;
}

// normal Resolve handles both, but this is specific to I/O args
void * cCmdResolveIODataArg(DATA_ARG DataArg, ULONG Offset, TYPE_CODE * TypeCode)
  {
  void * ret_val = NULL;

  ULONG ModuleID;
  ULONG FieldID;
    //DataArg refers to a field in the IO map
  // ModuleID = ((DataArg >> 9) & 0x1F);
  ModuleID = ((DataArg & 0x3FFF) >> 9);
  FieldID = (DataArg & 0x01FF);

    //!!! Preliminary bounds check -- still could allow invalid combos through
    if (ModuleID > MOD_OUTPUT || FieldID >= IO_OUT_FIELD_COUNT)
    {
      NXT_BREAK;
      return NULL;
    }

    ret_val = IO_PTRS[ModuleID][FieldID];
    if (TypeCode)
      *TypeCode = IO_TYPES[ModuleID][FieldID];
  return ret_val;
}

void cCmdSetValFlt(void * pVal, TYPE_CODE TypeCode, float NewVal)
{
  /*
   * According to C standard, converting a float to integer is implementation
   * defined when the number is out of integer bounds.
   *
   * To match original LEGO firmware, when casting float as integer, always
   * convert to signed value, then cast as destination type.
   */

  if (pVal)
  {
    if (TypeCode == TC_FLOAT)
      *(float*)pVal = NewVal;
    else
    {
      SLONG i = NewVal;

      switch (TypeCode)
      {
        case TC_ULONG:
        case TC_SLONG:
        {
          *(ULONG*)pVal = i;
        }
        break;

        case TC_UWORD:
        case TC_SWORD:
        {
          *(UWORD*)pVal = i;
        }
        break;

        case TC_UBYTE:
        case TC_SBYTE:
        {
          *(UBYTE*)pVal = i;
        }
        break;
      }
    }
  }

  return;
}

ULONG cCmdGetUByte(void * pVal);
ULONG cCmdGetSByte(void * pVal);
ULONG cCmdGetUWord(void * pVal);
ULONG cCmdGetSWord(void * pVal);
ULONG cCmdGetULong(void * pVal);
ULONG cCmdGetSLong(void * pVal);
ULONG cCmdGetError(void * pVal);
ULONG cCmdGetFloat(void * pVal);

void cCmdSetByte(void * pVal, ULONG NewVal);
void cCmdSetWord(void * pVal, ULONG NewVal);
void cCmdSetLong(void * pVal, ULONG NewVal);
void cCmdSetError(void * pVal, ULONG NewVal);


typedef ULONG (*pGetOperand)(void *);
static pGetOperand GetProcArray[11]= {cCmdGetUByte, cCmdGetUByte, cCmdGetSByte, cCmdGetUWord, cCmdGetSWord, cCmdGetULong, cCmdGetSLong, cCmdGetError, cCmdGetError, cCmdGetError, cCmdGetFloat}; // dup UByte to line up

typedef void (*pSetOperand)(void *, ULONG);
static pSetOperand SetProcArray[9]= {cCmdSetByte, cCmdSetByte, cCmdSetByte, cCmdSetWord, cCmdSetWord, cCmdSetLong, cCmdSetLong, cCmdSetError, cCmdSetError}; // dup UByte to line up

void cCmdSetError(void * pVal, ULONG NewVal) {
  NXT_BREAK;
}

void cCmdSetLong(void * pVal, ULONG NewVal) {
  *(ULONG*)pVal = NewVal;
}

void cCmdSetWord(void * pVal, ULONG NewVal) {
  *(UWORD*)pVal = (UWORD)NewVal;
}

void cCmdSetByte(void * pVal, ULONG NewVal) {
  *(UBYTE*)pVal = (UBYTE)NewVal;
}

// only works on simple types, equivalent to resolve and get, but faster
ULONG cCmdGetScalarValFromDataArg(DATA_ARG DataArg, UWORD Offset)
{
  DS_TOC_ENTRY *dsTOCPtr= &VarsCmd.pDataspaceTOC[DataArg];
  return GetProcArray[dsTOCPtr->TypeCode](VarsCmd.pDataspace + dsTOCPtr->DSOffset + Offset);
}


ULONG cCmdGetError(void * pVal) {
  NXT_BREAK;
  return 0;
}

ULONG cCmdGetULong(void * pVal) {
  return (ULONG)(*(ULONG*)pVal);
}

ULONG cCmdGetSLong(void * pVal) {
  return (SLONG)(*(SLONG*)pVal);
}

ULONG cCmdGetUWord(void * pVal) {
  return (UWORD)(*(UWORD*)pVal);
}

ULONG cCmdGetSWord(void * pVal) {
  return (SWORD)(*(SWORD*)pVal);
}

ULONG cCmdGetUByte(void * pVal) {
  return (UBYTE)(*(UBYTE*)pVal);
}

ULONG cCmdGetSByte(void * pVal) {
  return (SBYTE)(*(SBYTE*)pVal);
}

ULONG cCmdGetFloat(void * pVal) {
  float tempVal = *(float*)pVal;
  if (tempVal >= 0.0f) {
    tempVal += 0.5f;
  }
  else {
    tempVal -= 0.5f;
  }
  /*
   * According to C standard, converting a float to integer is implementation
   * defined when the number is out of integer bounds.
   *
   * To match original LEGO firmware, when casting float as integer, always
   * convert to signed value, then cast as destination type.
   */
  return (ULONG)(SLONG)tempVal;
}

ULONG cCmdGetVal(void * pVal, TYPE_CODE TypeCode)
{
  if (pVal)
  return GetProcArray[TypeCode](pVal);
  else
  //!!! Default return value places responsibility on caller to use this function wisely
  return 0;
}


float cCmdGetValFlt(void * pVal, TYPE_CODE TypeCode)
{
  if (pVal)
  {
    switch (TypeCode)
    {
      case TC_ULONG:
      {
        return (ULONG)(*(ULONG*)pVal);
      }

      case TC_SLONG:
      {
        return (SLONG)(*(SLONG*)pVal);
      }

      case TC_UWORD:
      {
        return (UWORD)(*(UWORD*)pVal);
      }

      case TC_SWORD:
      {
        return (SWORD)(*(SWORD*)pVal);
      }

      case TC_UBYTE:
      {
        return (UBYTE)(*(UBYTE*)pVal);
      }

      case TC_SBYTE:
      {
        return (SBYTE)(*(SBYTE*)pVal);
      }

      case TC_FLOAT:
      {
        return (float)(*(float*)pVal);
      }

      default:
      break;
    }
  }

  //!!! Default return value places responsibility on caller to use this function wisely
  return 0;
}



// Only for scalar types and no offset
void cCmdSetScalarValFromDataArg(DATA_ARG DataArg, ULONG NewVal)
{
  DS_TOC_ENTRY *dsTOCPtr= &VarsCmd.pDataspaceTOC[DataArg];
  SetProcArray[dsTOCPtr->TypeCode](VarsCmd.pDataspace + dsTOCPtr->DSOffset, NewVal);
}

void cCmdSetVal(void * pVal, TYPE_CODE TypeCode, ULONG NewVal)
{
  if (pVal)
    SetProcArray[TypeCode](pVal, NewVal);
}

void* cCmdDSPtr(DS_ELEMENT_ID DSElementID, UWORD Offset)
{
  void * pDSItem;
  DV_INDEX DVIndex;
  TYPE_CODE TypeCode;

  NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));

  TypeCode = cCmdDSType(DSElementID);
  if (TypeCode == TC_ARRAY)
  {
    //!!! Empty arrays return NULL.
    if (cCmdArrayCount(DSElementID, Offset) == 0)
      pDSItem = NULL;
    else
    {
      DVIndex = cCmdGetDVIndex(DSElementID, Offset);
      pDSItem = (VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset);
    }
  }
  else if (TypeCode == TC_CLUSTER)
  {
    NXT_ASSERT(cCmdClusterCount(DSElementID) != 0)

    //Returning pointer to the first element in the cluster
    pDSItem = cCmdDSPtr(INC_ID(DSElementID), Offset);
  }
  else
    pDSItem = (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset + Offset);

  NXT_ASSERT((UBYTE*)pDSItem < POOL_SENTINEL);

  return pDSItem;
}

void* cCmdDVPtr(DV_INDEX DVIndex)
{
  NXT_ASSERT(IS_DV_INDEX_SANE(DVIndex));
  return (VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset);
}


//!!! Recursive function
DS_ELEMENT_ID cCmdNextDSElement(DS_ELEMENT_ID CurrID)
{
  DS_ELEMENT_ID NextID;
  TYPE_CODE CurrType;
  UWORD ClusterCount, i;

  NXT_ASSERT(cCmdIsDSElementIDSane(CurrID));

  NextID = CurrID + 1;

  if (!cCmdIsDSElementIDSane(NextID))
    return NOT_A_DS_ID;

  CurrType = cCmdDSType(CurrID);

  if (CurrType == TC_ARRAY)
  {
    //Arrays contain two elements. Advance past the second one.
    NextID = cCmdNextDSElement(NextID);
  }
  else if (CurrType == TC_CLUSTER)
  {
    ClusterCount = cCmdClusterCount(CurrID);
    for (i = 0; i < ClusterCount; i++)
    {
      NextID = cCmdNextDSElement(NextID);
    }
  }

  return NextID;
}


//!!! Recursive function
UBYTE cCmdCompareDSType(DS_ELEMENT_ID DSElementID1, DS_ELEMENT_ID DSElementID2)
{
  TYPE_CODE Type1, Type2;
  UWORD i, Count1, Count2;

  Type1 = cCmdDSType(DSElementID1);
  Type2 = cCmdDSType(DSElementID2);

  if (Type1 != Type2)
    return FALSE;

  if (Type1 == TC_CLUSTER)
  {
    Count1 = cCmdClusterCount(DSElementID1);
    Count2 = cCmdClusterCount(DSElementID2);

    if(Count1 != Count2)
      return FALSE;

    DSElementID1 = INC_ID(DSElementID1);
    DSElementID2 = INC_ID(DSElementID2);

    for (i = 0; i < Count1; i++)
    {
      if (!cCmdCompareDSType(DSElementID1, DSElementID2))
        return FALSE;

      DSElementID1 = cCmdNextDSElement(DSElementID1);
      DSElementID2 = cCmdNextDSElement(DSElementID2);
    }
  }
  else if (Type1 == TC_ARRAY)
  {
    if (!cCmdCompareDSType(INC_ID(DSElementID1), INC_ID(DSElementID2)))
      return FALSE;
  }

  return TRUE;
}


//!!! Recursive function
UWORD cCmdCalcFlattenedSize(DS_ELEMENT_ID DSElementID, UWORD Offset)
{
  UWORD Size = 0;
  TYPE_CODE TypeCode;
  DV_INDEX DVIndex;
  UWORD i;
  UWORD Count;

  TypeCode = cCmdDSType(DSElementID);

  if (TypeCode == TC_ARRAY)
  {
    DVIndex = cCmdGetDVIndex(DSElementID, Offset);

    DSElementID = INC_ID(DSElementID);
    TypeCode = cCmdDSType(DSElementID);

    if (!IS_AGGREGATE_TYPE(TypeCode))
    {
      //Short circuit recursive calculation if our array sub-type is a scalar
      Size += DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count;
    }
    else
    {
      //If the sub type is an aggregate type, then it can contain arrays, so we have to recur
      for (i = 0; i < DV_ARRAY[DVIndex].Count; i++)
      {
        Size += cCmdCalcFlattenedSize(DSElementID, ARRAY_ELEM_OFFSET(DVIndex, i));
      }
    }
  }
  else if (TypeCode == TC_CLUSTER)
  {
    Count = cCmdClusterCount(DSElementID);

    DSElementID = INC_ID(DSElementID);
    for (i = 0; i < Count; i++)
    {
      Size += cCmdCalcFlattenedSize(DSElementID, Offset);
      DSElementID = cCmdNextDSElement(DSElementID);
    }
  }
  else //Scalar
  {
    Size += cCmdSizeOf(TypeCode);
  }
  return Size;
}


//!!! Recursive function
NXT_STATUS cCmdFlattenToByteArray(UBYTE * pByteArray, UWORD * pByteOffset, DS_ELEMENT_ID DSElementID, UWORD Offset)
{
  NXT_STATUS Status = NO_ERR;
  TYPE_CODE TypeCode;
  DV_INDEX DVIndex;
  UWORD i;
  UWORD Count;
  UBYTE *pVal;

  TypeCode = cCmdDSType(DSElementID);

  if (TypeCode == TC_ARRAY)
  {
    DVIndex = cCmdGetDVIndex(DSElementID, Offset);
    Count = DV_ARRAY[DVIndex].Count;

    DSElementID = INC_ID(DSElementID);
    TypeCode = cCmdDSType(DSElementID);
    if (!IS_AGGREGATE_TYPE(TypeCode))
    {
      //Short circuit recursive calculation if our array sub-type is a scalar
      Count = DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count;
      memmove((pByteArray + *pByteOffset), (VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset), Count);
      *pByteOffset += Count;
    }
    else
    {
      //If the sub type is an aggregate type, then it can contain arrays, so we have to recur
      for (i = 0; i < Count; i++)
      {
        cCmdFlattenToByteArray(pByteArray, pByteOffset, DSElementID, ARRAY_ELEM_OFFSET(DVIndex, i));
      }
    }
  }
  else if (TypeCode == TC_CLUSTER)
  {
    Count = cCmdClusterCount(DSElementID);

    DSElementID = INC_ID(DSElementID);
    for (i = 0; i < Count; i++)
    {
      cCmdFlattenToByteArray(pByteArray, pByteOffset, DSElementID, Offset);
      DSElementID = cCmdNextDSElement(DSElementID);
    }
  }
  else //Scalar
  {
    pVal = cCmdResolveDataArg(DSElementID, Offset, NULL);
    Count = cCmdSizeOf(TypeCode);

    memmove((pByteArray + *pByteOffset), pVal, Count);
    *pByteOffset += Count;
  }

  return Status;
}

NXT_STATUS cCmdUnflattenFromByteArray(UBYTE * pByteArray, UWORD * pByteOffset, DS_ELEMENT_ID DSElementID, UWORD Offset)
{
  NXT_STATUS Status = NO_ERR;
  TYPE_CODE TypeCode;
  DV_INDEX DVIndex;
  UWORD i;
  UWORD Count;
  UBYTE *pVal;

  TypeCode = cCmdDSType(DSElementID);

  if (TypeCode == TC_ARRAY)
  {
    DVIndex = cCmdGetDVIndex(DSElementID, Offset);
    Count = DV_ARRAY[DVIndex].Count;

    DSElementID = INC_ID(DSElementID);
    TypeCode = cCmdDSType(DSElementID);
    if (!IS_AGGREGATE_TYPE(TypeCode))
    {
      //Short circuit recursive calculation if our array sub-type is a scalar
      Count = DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count;
      memmove((VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset), (pByteArray + *pByteOffset), Count);
      *pByteOffset += Count;
    }
    else
    {
      //If the sub type is an aggregate type, then it can contain arrays, so we have to recur
      for (i = 0; i < Count; i++)
      {
        cCmdUnflattenFromByteArray(pByteArray, pByteOffset, DSElementID, ARRAY_ELEM_OFFSET(DVIndex, i));
      }
    }
  }
  else if (TypeCode == TC_CLUSTER)
  {
    Count = cCmdClusterCount(DSElementID);

    DSElementID = INC_ID(DSElementID);
    for (i = 0; i < Count; i++)
    {
      cCmdUnflattenFromByteArray(pByteArray, pByteOffset, DSElementID, Offset);
      DSElementID = cCmdNextDSElement(DSElementID);
    }
  }
  else //Scalar
  {
    pVal = cCmdResolveDataArg(DSElementID, Offset, NULL);
    Count = cCmdSizeOf(TypeCode);

    memmove(pVal, (pByteArray + *pByteOffset), Count);
    *pByteOffset += Count;
  }

  return Status;
}


UWORD cCmdClusterCount(DS_ELEMENT_ID DSElementID)
{
  UWORD ClusterCount;

  NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));
  NXT_ASSERT(cCmdDSType(DSElementID) == TC_CLUSTER);

  ClusterCount = VarsCmd.pDataspaceTOC[DSElementID].DSOffset;

  return ClusterCount;
}


UWORD cCmdGetDVIndex(DS_ELEMENT_ID DSElementID, UWORD Offset)
{
  UWORD DVIndex;

  NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY);

  DVIndex = *(UWORD *)(VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset + Offset);

  //Make sure we're returning a valid DVIndex
  NXT_ASSERT(DVIndex != 0 && DVIndex < DV_ARRAY[0].Count);

  return DVIndex;
}


UWORD cCmdArrayCount(DS_ELEMENT_ID DSElementID, UWORD Offset)
{
  DV_INDEX DVIndex;

  NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));
  NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY);

  DVIndex = cCmdGetDVIndex(DSElementID, Offset);
  return DV_ARRAY[DVIndex].Count;
}

TYPE_CODE cCmdArrayType(DS_ELEMENT_ID DSElementID)
{
  TYPE_CODE TypeCode;

  NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));
  NXT_ASSERT(cCmdIsDSElementIDSane(INC_ID(DSElementID)));
  NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY);

  TypeCode = VarsCmd.pDataspaceTOC[DSElementID + 1].TypeCode;

  return TypeCode;
}


DS_ELEMENT_ID cCmdGetDataspaceCount(void)
{
  return (VarsCmd.DataspaceCount);
}


UBYTE cCmdCompare(UBYTE CompCode, ULONG Val1, ULONG Val2, TYPE_CODE TypeCode1, TYPE_CODE TypeCode2)
{
  SLONG SVal1, SVal2;
  if (QUICK_UNSIGNED_TEST(TypeCode1) || QUICK_UNSIGNED_TEST(TypeCode2))
  {
    return ((CompCode == OPCC1_LT   && Val1 <  Val2)
         || (CompCode == OPCC1_GT   && Val1 >  Val2)
         || (CompCode == OPCC1_LTEQ && Val1 <= Val2)
         || (CompCode == OPCC1_GTEQ && Val1 >= Val2)
         || (CompCode == OPCC1_EQ   && Val1 == Val2)
         || (CompCode == OPCC1_NEQ  && Val1 != Val2));
  }
  else
  {
    SVal1 = (SLONG)Val1;
    SVal2 = (SLONG)Val2;
    return ((CompCode == OPCC1_LT   && SVal1 <  SVal2)
         || (CompCode == OPCC1_GT   && SVal1 >  SVal2)
         || (CompCode == OPCC1_LTEQ && SVal1 <= SVal2)
         || (CompCode == OPCC1_GTEQ && SVal1 >= SVal2)
         || (CompCode == OPCC1_EQ   && SVal1 == SVal2)
         || (CompCode == OPCC1_NEQ  && SVal1 != SVal2));
  }
}

UBYTE cCmdCompareFlt(UBYTE CompCode, float Val1, float Val2, TYPE_CODE TypeCode1, TYPE_CODE TypeCode2)
{
  //!!! add threshold to equality comparisons
  return ((CompCode == OPCC1_LT   && Val1 <  Val2)
       || (CompCode == OPCC1_GT   && Val1 >  Val2)
       || (CompCode == OPCC1_LTEQ && Val1 <= Val2)
       || (CompCode == OPCC1_GTEQ && Val1 >= Val2)
       || (CompCode == OPCC1_EQ   && Val1 == Val2)
       || (CompCode == OPCC1_NEQ  && Val1 != Val2));
}


NXT_STATUS cCmdCompareAggregates(UBYTE CompCode, UBYTE *ReturnBool, DATA_ARG Arg2, UWORD Offset2, DATA_ARG Arg3, UWORD Offset3)
{
  NXT_STATUS Status = NO_ERR;
  UBYTE Finished;

  Finished = FALSE;
  Status = cCmdRecursiveCompareAggregates(CompCode, ReturnBool, &Finished, Arg2, Offset2, Arg3, Offset3);
  if (Finished == FALSE)
  {
    //If Finished has not been set to TRUE, it means that it was unable to find an inequality, thereby ending the comparison.
    //Both elements are equal. Assign the proper value to ReturnBool
    *ReturnBool = (CompCode == OPCC1_EQ || CompCode == OPCC1_GTEQ || CompCode == OPCC1_LTEQ);
  }

  return Status;
}


//!!! Recursive function
NXT_STATUS cCmdRecursiveCompareAggregates(UBYTE CompCode, UBYTE *ReturnBool, UBYTE *Finished, DATA_ARG Arg2, UWORD Offset2, DATA_ARG Arg3, UWORD Offset3)
{
  //The value of Finished must be set to FALSE before calling this function.
  //We are able to determine the result of the comparison once we find an inequality.
  //Once an inequality is found, Finished is set to TRUE and ReturnBool is set based on the CompCode.
  //A call to this function will return with Finished still equal to FALSE if both elements are equal in value and count.
  //It is the caller of this function's job to set ReturnBool if this function returns with Finished == FALSE.

  NXT_STATUS Status = NO_ERR;
  TYPE_CODE TypeCode2, TypeCode3;
  DV_INDEX DVIndex2, DVIndex3;
  ULONG ArgVal2, ArgVal3;
  UWORD Count2, Count3, MinCount;
  UWORD i;

  TypeCode2 = cCmdDSType(Arg2);
  TypeCode3 = cCmdDSType(Arg3);

  //Make sure the two things we're comparing are the same type
  if (IS_AGGREGATE_TYPE(TypeCode2) && (TypeCode2 != TypeCode3))
  {
    NXT_BREAK;
    return ERR_ARG;
  }

  //Simple case, both args are scalars. Solve and return.
  if (!IS_AGGREGATE_TYPE(TypeCode2))
  {
    ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, Offset2);
    ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, Offset3);

    //Once we find an inequality, we can determine the result of the comparison
    *Finished = cCmdCompare(OPCC1_NEQ, ArgVal2, ArgVal3, TypeCode2, TypeCode3);

    if (*Finished)
      *ReturnBool = cCmdCompare(CompCode, ArgVal2, ArgVal3, TypeCode2, TypeCode3);

    return Status;
  }

  // Initialize local variables for each argument

  if (TypeCode2 == TC_ARRAY)
  {
    Count2 = cCmdArrayCount(Arg2, Offset2);
    DVIndex2 = cCmdGetDVIndex(Arg2, Offset2);
    Offset2 = DV_ARRAY[DVIndex2].Offset;

    Count3 = cCmdArrayCount(Arg3, Offset3);
    DVIndex3 = cCmdGetDVIndex(Arg3, Offset3);
    Offset3 = DV_ARRAY[DVIndex3].Offset;
  }
  else if (TypeCode2 == TC_CLUSTER)
  {
    Count2 = cCmdClusterCount(Arg2);
    Count3 = cCmdClusterCount(Arg3);
  }

  //Short circuit evaluation of EQ and NEQ if counts are different
  if (Count2 != Count3)
  {
    if ((CompCode == OPCC1_EQ) || (CompCode == OPCC1_NEQ))
    {
      *Finished = TRUE;
      *ReturnBool = (CompCode == OPCC1_NEQ);
      return Status;
    }
  }

  MinCount = (Count2 < Count3) ? Count2 : Count3;

  //Advance aggregate args to first sub-element for next call
  Arg2 = INC_ID(Arg2);
  Arg3 = INC_ID(Arg3);

  //
  // Loop through the sub-elements of aggregate arguments.
  // Call cCmdRecursiveCompareAggregates recursively with simpler type.
  //

  for (i = 0; i < MinCount; i++)
  {
    Status = cCmdRecursiveCompareAggregates(CompCode, ReturnBool, Finished, Arg2, Offset2, Arg3, Offset3);
    if (*Finished || IS_ERR(Status))
      return Status;

    //Advance aggregate args to next sub-element
    if (TypeCode2 == TC_ARRAY)
    {
      Offset2 += DV_ARRAY[DVIndex2].ElemSize;
      Offset3 += DV_ARRAY[DVIndex3].ElemSize;
    }
    else if (TypeCode2 == TC_CLUSTER)
    {
      Arg2 = cCmdNextDSElement(Arg2);
      Arg3 = cCmdNextDSElement(Arg3);
    }
  }

  //All elements in aggregates type up to MinCount are equal. Count discrepancy determines comparison outcome.
  if (Count2 != Count3)
  {
    *Finished = TRUE;
    *ReturnBool = cCmdCompare(CompCode, Count2, Count3, TC_UWORD, TC_UWORD);
  }
  //Else, no size discrepancy. Elements are equal. Comparison still not resolved,
  //so return !Finished and status back up the call chain for further comparison

  return Status;
}

ULONG gClearProfileInfo= 0, bigExecTime= 0;
#if VMProfilingCode
void UpdateProfileInfo(ULONG shortOp, CODE_WORD *pInstr, ULONG execTime, ULONG InstrSize)
{
  ULONG j;
  ULONG opCode;

  if(execTime > 500 && shortOp == 8)
    bigExecTime= shortOp;
  if(gClearProfileInfo) {
    ExecutedInstrs= 0;
    CmdCtrlTime= 0;
    OverheadTime= 0;
    CmdCtrlCalls= 0;
    LastAvgCount= 0;
    for(j= 0; j < 255; j++)
      CmdCtrlClumpTime[j]= 0;
    for(j= 0; j < OPCODE_COUNT; j++) {
      InstrProfile[j].Avg= 0;
      InstrProfile[j].Time= 0;
      InstrProfile[j].Count= 0;
      InstrProfile[j].Max= 0;
    }
    for(j= 0; j < SYSCALL_COUNT; j++) {
      SysCallProfile[j].Avg= 0;
      SysCallProfile[j].Time= 0;
      SysCallProfile[j].Count= 0;
      SysCallProfile[j].Max= 0;
    }
    for(j= 0; j < NUM_SHORT_OPCODE_COUNT; j++) {
      ShortInstrProfile[j].Avg= 0;
      ShortInstrProfile[j].Time= 0;
      ShortInstrProfile[j].Count= 0;
      ShortInstrProfile[j].Max= 0;
    }
    for(j= 0; j < NUM_INTERP_FUNCS; j++) {
      InterpFuncProfile[j].Avg= 0;
      InterpFuncProfile[j].Time= 0;
      InterpFuncProfile[j].Count= 0;
      InterpFuncProfile[j].Max= 0;
    }
    gClearProfileInfo= FALSE;
  }
  ExecutedInstrs ++;
  if(shortOp > 7) // shortop bit set
  {
    ShortInstrProfile[shortOp-8].Time += execTime;
    ShortInstrProfile[shortOp-8].Count++;
    if(execTime > ShortInstrProfile[shortOp-8].Max)
      ShortInstrProfile[shortOp-8].Max= execTime;
  }
  else
  {
    opCode = OP_CODE(pInstr);
    InstrProfile[opCode].Time += execTime;
    InstrProfile[opCode].Count++;
    if(execTime > InstrProfile[opCode].Max)
      InstrProfile[opCode].Max= execTime;
    if(opCode == OP_SYSCALL)
    {
      SysCallProfile[GetDataArg(pInstr[1])].Time += execTime;
      SysCallProfile[GetDataArg(pInstr[1])].Count++;
      if(execTime > SysCallProfile[GetDataArg(pInstr[1])].Max)
        SysCallProfile[GetDataArg(pInstr[1])].Max= execTime;
    }

    InterpFuncProfile[InstrSize].Time += execTime;
    InterpFuncProfile[InstrSize].Count++;
    if(execTime > InterpFuncProfile[InstrSize].Max)
      InterpFuncProfile[InstrSize].Max= execTime;
  }
  if(ExecutedInstrs - LastAvgCount > 999) // every N instrs, update avgs
  {
    for(j= 0; j < OPCODE_COUNT; j++)
      if(InstrProfile[j].Count)
        InstrProfile[j].Avg= InstrProfile[j].Time/InstrProfile[j].Count;
    for(j= 0; j < SYSCALL_COUNT; j++)
      if(SysCallProfile[j].Count)
        SysCallProfile[j].Avg= SysCallProfile[j].Time/SysCallProfile[j].Count;
    for(j= 0; j < NUM_SHORT_OPCODE_COUNT; j++)
      if(ShortInstrProfile[j].Count)
        ShortInstrProfile[j].Avg= ShortInstrProfile[j].Time/ShortInstrProfile[j].Count;
    for(j= 0; j < NUM_INTERP_FUNCS; j++)
      if(InterpFuncProfile[j].Count)
        InterpFuncProfile[j].Avg= InterpFuncProfile[j].Time/InterpFuncProfile[j].Count;
    LastAvgCount= ExecutedInstrs;
  }
}
#endif


//
// Interpreter Functions
//

NXT_STATUS cCmdInterpFromClump()
{
  CLUMP_ID Clump= VarsCmd.RunQ.Head;
  NXT_STATUS Status = NO_ERR;
  CLUMP_REC * pClumpRec;
  CODE_WORD * pInstr, *lastClumpInstr;
  UBYTE InstrSize;
  ULONG shortOp, nextMSTick;
  SLONG i;
#if VM_BENCHMARK
  ULONG InstrTime = dTimerRead();
#endif

  if (!cCmdIsClumpIDSane(Clump)) // this means all clumps are asleep
    return TIMES_UP;

  //Resolve clump record structure and current instruction pointer
  pClumpRec = &(VarsCmd.pAllClumps[Clump]);
  pInstr = pClumpRec->PC; // abs
  lastClumpInstr= pClumpRec->CodeEnd; // abs

  i= gInstrsToExecute;
  nextMSTick= dTimerGetNextMSTickCnt();
  do
  {
#if VMProfilingCode
    ULONG instrStartTime;
    instrStartTime= dTimerReadHiRes();
#endif

    ULONG instrWord= *(UWORD*)pInstr;
    shortOp= (instrWord>>8) & 0x0F;
    if(shortOp > 7) // shortop bit set
      Status= ShortInterpFuncs[shortOp - 8](pInstr);
    else
    { // we know this is a long instr, dispatch on num params, which correlates to size
      InstrSize = INSTR_SIZE(instrWord); // keep in a local for profiling
      Status = (*InterpFuncs[InstrSize])(pInstr);
    }

#if VMProfilingCode
    UpdateProfileInfo(shortOp, pInstr, dTimerReadHiRes() - instrStartTime, InstrSize);
#endif

afterCompaction:
    if (Status == NO_ERR)
      pInstr += gPCDelta;
    else if (Status == CLUMP_DONE) // already requeued
    {
      pClumpRec->PC = pClumpRec->CodeStart;
      pClumpRec->CurrFireCount = pClumpRec->InitFireCount;
      return Status;
    }
    else if (Status == CLUMP_SUSPEND || Status == BREAKOUT_REQ || Status == ROTATE_QUEUE)  // already requeued
    {
      pClumpRec->PC = pInstr + gPCDelta;
    //Throw error if we ever advance beyond the clump's codespace
    if (pInstr > lastClumpInstr)
      {
        NXT_BREAK;
        Status = ERR_INSTR;
      }
      return Status;
    }
    else if (Status == ERR_MEM)
    {
    //Memory is full. Compact dataspace and try the instruction again.
    //!!! Could compact DopeVectorArray here
    cCmdDSCompact();
      if(shortOp > 7) // shortop bit set
        Status= ShortInterpFuncs[shortOp - 8](pInstr);
      else
        Status = (*InterpFuncs[InstrSize])(pInstr);
	  if(Status == ERR_MEM)
        return Status;
    else
        goto afterCompaction;
    }
    else // other errors, breakout, stop
      return Status;

    //Throw error if we ever advance beyond the clump's codespace
    if (pInstr > lastClumpInstr)
    {
      NXT_BREAK;
      Status = ERR_INSTR;
    }

#if VM_BENCHMARK
  //Increment opcode count
  VarsCmd.OpcodeBenchmarks[OP_CODE(pInstr)][0]++;

  InstrTime = dTimerRead() - InstrTime;
  if (InstrTime > 1)
  {
    VarsCmd.OpcodeBenchmarks[OP_CODE(pInstr)][1]++;
    if (InstrTime > VarsCmd.OpcodeBenchmarks[OP_CODE(pInstr)][2])
      VarsCmd.OpcodeBenchmarks[OP_CODE(pInstr)][2] = InstrTime;
  }
    VarsCmd.InstrCount++;
#endif

    //Count one more instruction for this pass
    if ((SLONG)(nextMSTick - dTimerReadTicks()) <= 0) // HWTimer has passed ms tick limit
      Status= TIMES_UP;
    else if(--i <= 0)
      Status= ROTATE_QUEUE;
  } while (!Status);
  pClumpRec->PC= pInstr;
  return (Status);
}


NXT_STATUS cCmdInterpUnop1(CODE_WORD * const pCode)
{
  NXT_STATUS Status = NO_ERR;
  UBYTE opCode;
  DATA_ARG Arg1;

  NXT_ASSERT(pCode != NULL);

  gPCDelta= 2;
  opCode = OP_CODE(pCode);
  Arg1   = pCode[1];

  switch (opCode)
  {
    case OP_JMP:
    {
      gPCDelta= (SWORD)Arg1;
      Status = NO_ERR;
    }
    break;

    case OP_ACQUIRE:
    {
      NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
      NXT_ASSERT(VarsCmd.pDataspaceTOC[Arg1].TypeCode == TC_MUTEX);

      Status = cCmdAcquireMutex((MUTEX_Q *)cCmdDSScalarPtr(Arg1, 0));
    }
    break;

    case OP_RELEASE:
    {
      NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
      NXT_ASSERT(VarsCmd.pDataspaceTOC[Arg1].TypeCode == TC_MUTEX);

      Status = cCmdReleaseMutex((MUTEX_Q *)cCmdDSScalarPtr(Arg1, 0));
    }
    break;

    case OP_SUBRET:
    {
      NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));

      //Take Subroutine off RunQ
      //Add Subroutine's caller to RunQ
      cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.RunQ.Head);
      cCmdEnQClump(&(VarsCmd.RunQ), *((CLUMP_ID *)cCmdDSScalarPtr(Arg1, 0)));

      Status = CLUMP_DONE;
    }
    break;

    case OP_FINCLUMPIMMED:
    {
        CLUMP_ID Clump= VarsCmd.RunQ.Head; // DeQ changes Head, use local val
        cCmdDeQClump(&(VarsCmd.RunQ), Clump); //Dequeue finalized clump
        cCmdSchedDependent(Clump, (CLUMP_ID)Arg1);  // Use immediate form

        Status = CLUMP_DONE;
    }
    break;

    case OP_GETTICK:
    {
      cCmdSetScalarValFromDataArg(Arg1, dTimerReadNoPoll());
    }
    break;

    case OP_STOP:
    {
      //Unwired Arg1 means always stop
      if (Arg1 == NOT_A_DS_ID)
        Status = STOP_REQ;
      else if (cCmdGetScalarValFromDataArg(Arg1, 0) > 0)
        Status = STOP_REQ;
    }
    break;

    default:
    {
      //Fatal error: Unrecognized instruction
      NXT_BREAK;
      Status = ERR_INSTR;
    }
    break;
  }

  return (Status);
}

ULONG scalarNots= 0, scalarBrtst= 0, scalarUn2Other= 0, scalarUn2Dispatch= 0, polyUn2Dispatch= 0;
NXT_STATUS cCmdInterpScalarUnop2(CODE_WORD * const pCode)
{
  NXT_STATUS Status;
  UBYTE opCode;

  NXT_ASSERT(pCode != NULL);
  opCode = OP_CODE(pCode);
  DATA_ARG Arg1, Arg2;

  scalarUn2Dispatch ++;
  if(opCode == OP_NOT) // t2 && t3 guaranteed scalar
  {
    gPCDelta= 3;
    Arg1 = pCode[1];
    Arg2 = pCode[2];
    ULONG ArgVal1, ArgVal2;

    ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0);
    //!!! OP_NOT is logical, *not* bit-wise.
    //This differs from the other logical ops because we don't distinguish booleans from UBYTEs.
    ArgVal1=  (!ArgVal2);
    cCmdSetScalarValFromDataArg(Arg1, ArgVal1);
    Status = NO_ERR;
    scalarNots ++;
  }
  else if(opCode == OP_BRTST)
  {
    ULONG Branch, compare= COMP_CODE(pCode);
    ULONG TypeCode;

    Arg1 = pCode[1];
    Arg2 = pCode[2];
    TypeCode = cCmdDSType(Arg2);

    if(Arg2 == NOT_A_DS_ID)
    {
      Branch= ((compare == OPCC1_EQ)
              || (compare == OPCC1_LTEQ)
              || (compare == OPCC1_GTEQ));
    }
    else
    {
      if(compare == OPCC1_EQ && TypeCode == TC_UBYTE) // very common for loops
      {
        UBYTE *pBRVal = (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[Arg2].DSOffset);
        Branch= *pBRVal == 0;
      }
      else
      {
        SLONG SVal1 = (SLONG)cCmdGetScalarValFromDataArg(Arg2, 0);
        Branch= ((compare == OPCC1_EQ   && SVal1 == 0)
              || (compare == OPCC1_NEQ  && SVal1 != 0)
              || (compare == OPCC1_GT   && SVal1 >  0)
              || (compare == OPCC1_LT   && SVal1 <  0)
              || (compare == OPCC1_LTEQ && SVal1 <= 0)
              || (compare == OPCC1_GTEQ && SVal1 >= 0));
      }
    }
    if (Branch)
      gPCDelta =  (SWORD)Arg1;
    else
      gPCDelta= 3;
    Status = NO_ERR;
    scalarBrtst ++;
  }
  else {
    Status= cCmdInterpUnop2(pCode);
    scalarUn2Other ++;
  }
  return Status;
}

NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode)
{
  NXT_STATUS Status = NO_ERR;
  UBYTE opCode;
  DATA_ARG Arg1;
  DATA_ARG Arg2;
  void *pArg1 = NULL, *pArg2 = NULL;
  TYPE_CODE TypeCode1, TypeCode2;

  ULONG i;
  UWORD ArgC;
  static UBYTE * ArgV[MAX_CALL_ARGS + 1];

  polyUn2Dispatch ++;
  UWORD Count;
  UWORD Offset;
  SLONG TmpSLong;
  ULONG TmpULong;
  ULONG ArgVal2;
  float FltArgVal2;
  char Buffer[30];
  char FormatString[5];
  UBYTE CheckTrailingZeros = 0;

  NXT_ASSERT(pCode != NULL);

  gPCDelta= 3;
  opCode = OP_CODE(pCode);
  Arg1   = pCode[1];
  Arg2   = pCode[2];

  if (opCode == OP_NEG || opCode == OP_NOT || opCode == OP_TST || opCode == OP_SQRT || opCode == OP_ABS)
  {
    return cCmdInterpPolyUnop2(*pCode, Arg1, 0, Arg2, 0);
  }

  switch (opCode)
  {
    case OP_MOV:
    {
      Status= cCmdMove(Arg1, Arg2);
    }
    break;

    case OP_SET:
    {
      //!!! Should throw error if TypeCode1 is non-scalar
      //    Accepting non-scalar destinations could have unpredictable results!
      cCmdSetScalarValFromDataArg(Arg1, Arg2);
    }
    break;

    case OP_BRTST:
    {
        //!!!BDUGGAN BRTST w/ Float?
      ULONG Branch, compare= COMP_CODE(pCode);
      ULONG TypeCode = cCmdDSType(Arg2);
      if(compare == OPCC1_EQ && TypeCode == TC_UBYTE) // very common for loops
      {
        UBYTE *pBRVal = (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[Arg2].DSOffset);
        Branch= *pBRVal == 0;
      }
      else
      {
        SLONG SVal1 = (SLONG)cCmdGetScalarValFromDataArg(Arg2, 0);
        Branch= ((compare == OPCC1_EQ   && SVal1 == 0)
              || (compare == OPCC1_NEQ  && SVal1 != 0)
              || (compare == OPCC1_GT   && SVal1 >  0)
              || (compare == OPCC1_LT   && SVal1 <  0)
              || (compare == OPCC1_LTEQ && SVal1 <= 0)
              || (compare == OPCC1_GTEQ && SVal1 >= 0));
      }
      if (Branch)

      {
        gPCDelta =  (SWORD)Arg1;
        Status = NO_ERR;
      }
    }
    break;

    case OP_FINCLUMP:
    {
      CLUMP_ID Clump= VarsCmd.RunQ.Head; // DeQ changes Head, use local val
      cCmdDeQClump(&(VarsCmd.RunQ), Clump); //Dequeue finalized clump
      cCmdSchedDependents(Clump, (SWORD)Arg1, (SWORD)Arg2);
      Status = CLUMP_DONE;
    }
    break;

    case OP_SUBCALL:
    {
      NXT_ASSERT(cCmdIsClumpIDSane((CLUMP_ID)Arg1));
      NXT_ASSERT(!cCmdIsClumpOnQ(&(VarsCmd.RunQ), (CLUMP_ID)Arg1));

      NXT_ASSERT(cCmdIsDSElementIDSane(Arg2));

      *((CLUMP_ID *)(cCmdDSScalarPtr(Arg2, 0))) = VarsCmd.RunQ.Head;

      cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.RunQ.Head); //Take caller off RunQ
      cCmdEnQClump(&(VarsCmd.RunQ), (CLUMP_ID)Arg1);  //Add callee to RunQ

      Status = CLUMP_SUSPEND;
    }
    break;

    case OP_ARRSIZE:
    {
      cCmdSetScalarValFromDataArg(Arg1, cCmdArrayCount(Arg2, 0));
    }
    break;

    case OP_SYSCALL:
    {
      if (Arg1 >= SYSCALL_COUNT)
      {
        NXT_BREAK;
        Status = ERR_INSTR;
        break;
      }

      ArgC = cCmdClusterCount(Arg2);

      if (ArgC > MAX_CALL_ARGS)
      {
        NXT_BREAK;
        Status = ERR_INSTR;
        break;
      }

      if (ArgC > 0)
      {
        Arg2 = INC_ID(Arg2);

        for (i = 0; i < ArgC; i++)
        {
          if (cCmdDSType(Arg2) == TC_ARRAY)
          {
            //Storing pointer to array's DV_INDEX
            //!!! This resolve is different than cCmdDSPtr
            // since SysCalls may need the DVIndex to re-alloc arrays
            ArgV[i] = VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[Arg2].DSOffset;
          }
          else
          {
            ArgV[i] = cCmdDSPtr(Arg2, 0);
          }

          //If any argument fails to resolve, return a fatal error.
          if (ArgV[i] == NULL)
          {
            Status = ERR_BAD_PTR;
            break;
          }

          Arg2 = cCmdNextDSElement(Arg2);
        }
      }
      else
      {
        i = 0;
      }

      //ArgV list is null terminated
      ArgV[i] = NULL;

      Status = (*SysCallFuncs[Arg1])(ArgV);
    }
    break;

    case OP_FLATTEN:
    {
      //Flatten Arg2 to a NULL terminated string

      //Assert that the destination is a string (array of bytes)
      NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY);
      NXT_ASSERT(cCmdDSType(INC_ID(Arg1)) == TC_UBYTE);

      Count = cCmdCalcFlattenedSize(Arg2, 0);
      //Add room for NULL terminator
      Count++;
      Status = cCmdDSArrayAlloc(Arg1, 0, Count);
      if (IS_ERR(Status))
        return Status;

      pArg1 = cCmdResolveDataArg(Arg1, 0, NULL);
      Offset = 0;

      Status = cCmdFlattenToByteArray(pArg1, &Offset, Arg2, 0);
      //Append NULL terminator
      *((UBYTE *)pArg1 + Offset) = 0;
      Offset++;
      NXT_ASSERT(Offset == Count);
    }
    break;

    case OP_NUMTOSTRING:
    {
      //Assert that the destination is a string (array of bytes)
      NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY);
      NXT_ASSERT(cCmdDSType(INC_ID(Arg1)) == TC_UBYTE);

      //Make sure we're trying to convert a scalar to a string
      TypeCode2= cCmdDSType(Arg2);
      NXT_ASSERT(!IS_AGGREGATE_TYPE(TypeCode2));

      if (TypeCode2 == TC_FLOAT)
      {
        pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);
        FltArgVal2 = cCmdGetValFlt(pArg2, TypeCode2);
       // is number too big for display? then format differently and don't bother with trailing zeros
        if ((FltArgVal2 > 9999999999999.99f)||(FltArgVal2 < -999999999999.99f)){ // these are the widest %.2f numbers that will fit on display
          strcpy (FormatString, "%.6g");
        }
        else{
          strcpy (FormatString, "%.2f");
          CheckTrailingZeros = 1;
        }
        Count = sprintf(Buffer, FormatString, FltArgVal2);
        Count++; //add room for null terminator

        if (CheckTrailingZeros){
          // Determine if the trailing digits are zeros. If so, drop them
          if (Buffer[Count-2] == 0x30) { // NOTE: 0x30 is ASCII 0
            if (Buffer[Count-3] == 0x30){
              strcpy (FormatString, "%.0f"); // the last two digits = 0, copy as integer
              Count = Count - 3; // don't need memory for decimal and 2 ascii characters
            }
            else {
              strcpy (FormatString, "%.1f"); // only the 2nd digit = 0 so drop it, but keep the tenths place
              Count = Count - 1; // don't need memory for 2nd ascii character
            }
          }
        }
      }
      else
      {
      ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0);
      //Calculate size of array
      if (ArgVal2 == 0)
        Count = 1;
      else {
        Count = 0;
        SLONG digits= 0;
        ULONG Tmp= 1;
        if (TypeCode2 == TC_SLONG || TypeCode2 == TC_SWORD || TypeCode2 == TC_SBYTE)
        {
          TmpSLong = (SLONG)ArgVal2;
          //Add room for negative sign
          if (TmpSLong < 0) {
            Count++;
            TmpULong= -TmpSLong;
            }
          else
            TmpULong= ArgVal2;
        }
        else
          TmpULong= ArgVal2;

        while (Tmp <= TmpULong && digits < 10) { // maxint is ten digits, max
          Tmp *= 10;
          digits++;
        }
        Count += digits;
      }
      //add room for NULL terminator
      Count++;
      }

      //Allocate array
      Status = cCmdDSArrayAlloc(Arg1, 0, Count);
      if (IS_ERR(Status))
        return Status;

      pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1);

      //Populate array
      if (TypeCode2 == TC_FLOAT)
      {
        sprintf(pArg1, FormatString, FltArgVal2);
      }
      else if (TypeCode2 == TC_SLONG || TypeCode2 == TC_SWORD || TypeCode2 == TC_SBYTE)
      {
        sprintf(pArg1, "%d", (SLONG)ArgVal2);
      }
      else
      {
        sprintf(pArg1, "%u", ArgVal2);
      }
    }
    break;

    case OP_STRTOBYTEARR:
    {
      NXT_ASSERT((cCmdDSType(Arg1) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg1)) == TC_UBYTE));
      NXT_ASSERT((cCmdDSType(Arg2) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg2)) == TC_UBYTE));

      Count = cCmdArrayCount(Arg2, 0);

      if (Count > 0)
      {
        Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)(Count - 1));
        if (IS_ERR(Status))
          return Status;

        pArg1 = cCmdResolveDataArg(Arg1, 0, NULL);
        pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);

        memmove(pArg1, pArg2, Count - 1);
      }
    }
    break;

    case OP_BYTEARRTOSTR:
    {
      NXT_ASSERT((cCmdDSType(Arg1) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg1)) == TC_UBYTE));
      NXT_ASSERT((cCmdDSType(Arg2) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg2)) == TC_UBYTE));

      Count = cCmdArrayCount(Arg2, 0);

      Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)(Count + 1));
      if (IS_ERR(Status))
        return Status;

      pArg1 = cCmdResolveDataArg(Arg1, 0, NULL);
      pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);

      memmove(pArg1, pArg2, Count);
      *((UBYTE *)pArg1 + Count) = '\0';
    }
    break;

    case OP_WAIT:
    {
      ULONG wait= 0;
      //Unwired Arg2 defaults to wait 0, which rotates queue
      if (Arg2 != NOT_A_DS_ID)
        wait= cCmdGetScalarValFromDataArg(Arg2, 0);
      if(wait == 0)
        Status= ROTATE_QUEUE;
      else
        Status = cCmdSleepClump(wait + IOMapCmd.Tick); // put to sleep, to wake up wait ms in future
      if(Arg1 != NOT_A_DS_ID)
        cCmdSetScalarValFromDataArg(Arg1, dTimerReadNoPoll());
    }
    break;

  default:
    {
      //Fatal error: Unrecognized instruction
      NXT_BREAK;
      Status = ERR_INSTR;
    }
    break;
  }

  return (Status);
}


NXT_STATUS cCmdInterpPolyUnop2(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset1Param, DATA_ARG Arg2, UWORD Offset2Param)
{
  NXT_STATUS Status = NO_ERR;
  TYPE_CODE TypeCode1, TypeCode2;
  DV_INDEX DVIndex1, DVIndex2;
  ULONG ArgVal1, ArgVal2;
  float FltArgVal1, FltArgVal2;
  UWORD Count1, Count2, Offset1= Offset1Param, Offset2= Offset2Param;
  UWORD MinArrayCount;
  UWORD i;
  //!!! AdvCluster is intended to catch the case where sources are Cluster and an Array of Clusters.
  // In practice, the logic it uses is broken, leading to some source cluster elements being ignored.
  UBYTE AdvCluster;

  void * pArg1 = NULL,
        *pArg2 = NULL;

  TypeCode1 = cCmdDSType(Arg1);
  TypeCode2 = cCmdDSType(Arg2);

  //Simple case, scalar. Solve and return.
  if (!IS_AGGREGATE_TYPE(TypeCode2))
  {
    NXT_ASSERT(!IS_AGGREGATE_TYPE(TypeCode1));

    pArg1 = cCmdResolveDataArg(Arg1, Offset1, &TypeCode1);

    if (TypeCode1 == TC_FLOAT || TypeCode2 == TC_FLOAT)
    {
      pArg2 = cCmdResolveDataArg(Arg2, Offset2, &TypeCode2);
      FltArgVal2 = cCmdGetValFlt(pArg2, TypeCode2);
      FltArgVal1 = cCmdUnop2Flt(Code, FltArgVal2, TypeCode2);
      cCmdSetValFlt(pArg1, TypeCode1, FltArgVal1);
    }
    else
    {
      ArgVal2= cCmdGetScalarValFromDataArg(Arg2, Offset2);
      if(OP_CODE(&Code) == OP_MOV)
        ArgVal1= ArgVal2;
      else
        ArgVal1 = cCmdUnop2(Code, ArgVal2, TypeCode2);
      cCmdSetVal(pArg1, TypeCode1, ArgVal1);
    }
    return Status;
  }

  //At least one of the args is an aggregate type

  if(TypeCode1 == TC_ARRAY && TypeCode2 == TC_ARRAY) {
    TYPE_CODE tc1, tc2;
    tc1=  cCmdDSType(INC_ID(Arg1));
    tc2=  cCmdDSType(INC_ID(Arg2));
    if(tc1 <= TC_LAST_INT_SCALAR && tc1 == tc2) {
      void *pArg1, *pArg2;
      ULONG Count = cCmdArrayCount(Arg2, Offset2);
      Status = cCmdDSArrayAlloc(Arg1, Offset1, Count);
      if (IS_ERR(Status))
        return Status;
      pArg1 = cCmdResolveDataArg(Arg1, Offset1, NULL);
      pArg2 = cCmdResolveDataArg(Arg2, Offset2, NULL);
      memmove(pArg1, pArg2, Count * cCmdSizeOf(tc1));
      return Status;
    }
  }


  //
  // Initialize Count and ArrayType local variables for each argument
  //

  if (TypeCode2 == TC_ARRAY)
  {
    Count2 = cCmdArrayCount(Arg2, Offset2);
    DVIndex2 = cCmdGetDVIndex(Arg2, Offset2);
    Offset2 = DV_ARRAY[DVIndex2].Offset;
  }
  else if (TypeCode2 == TC_CLUSTER)
  {
    Count2 = cCmdClusterCount(Arg2);
  }

  if (TypeCode1 == TC_ARRAY)
  {
    if (TypeCode2 != TC_ARRAY)
    {
      //If output is an array, but source is not an array, that's a fatal error!
      NXT_BREAK;
      return (ERR_ARG);
    }
    if(Count2 == 0) { // both arrays, input is empty, is output already empty?
      Count1= cCmdArrayCount(Arg1, Offset1);
      if(Count1 == 0)
        return NO_ERR;
    }

    MinArrayCount = Count2;

    //Make sure the destination array is the proper size to hold the result
    Status = cCmdDSArrayAlloc(Arg1, Offset1, MinArrayCount);
    if (IS_ERR(Status))
      return Status;

    if(MinArrayCount == 0) // if we emptied array, nothing else to do.
      return NO_ERR;
    Count1 = MinArrayCount;
    DVIndex1 = cCmdGetDVIndex(Arg1, Offset1);
    Offset1 = DV_ARRAY[DVIndex1].Offset;
    AdvCluster = FALSE;
  }
  else if (TypeCode1 == TC_CLUSTER)
  {
    Count1 = cCmdClusterCount(Arg1);
    AdvCluster = TRUE;
  }

  //Advance aggregate args to first sub-element for next call
  if (IS_AGGREGATE_TYPE(TypeCode1))
    Arg1 = INC_ID(Arg1);
  if (IS_AGGREGATE_TYPE(TypeCode2))
    Arg2 = INC_ID(Arg2);

  //
  // Loop through the sub-elements of aggregate arguments.
  // Call cCmdInterpPolyUnop2 recursively with simpler type.
  //
  for (i = 0; i < Count1; i++)
  {
    Status = cCmdInterpPolyUnop2(Code, Arg1, Offset1, Arg2, Offset2);
    if (IS_ERR(Status))
      return Status;

    //Advance aggregate args to next sub-element
    if (TypeCode1 == TC_ARRAY)
      Offset1 += DV_ARRAY[DVIndex1].ElemSize;
    else if ((TypeCode1 == TC_CLUSTER) && AdvCluster)
      Arg1 = cCmdNextDSElement(Arg1);

    if (TypeCode2 == TC_ARRAY)
      Offset2 += DV_ARRAY[DVIndex2].ElemSize;
    else if ((TypeCode2 == TC_CLUSTER) && AdvCluster)
      Arg2 = cCmdNextDSElement(Arg2);
  }
  return Status;
}


ULONG cCmdUnop2(CODE_WORD const Code, ULONG Operand, TYPE_CODE TypeCode)
{
  UBYTE opCode;

  opCode = OP_CODE((&Code));
  if(opCode == OP_MOV)
    return Operand;
  else if(opCode == OP_NEG)
    return (-((SLONG)Operand));
  else if(opCode == OP_NOT)
    //!!! OP_NOT is logical, *not* bit-wise.
    //This differs from the other logical ops because we don't distinguish booleans from UBYTEs.
    return (!Operand);
  else if(opCode == OP_TST)
    return cCmdCompare(COMP_CODE((&Code)), Operand, 0, TypeCode, TypeCode);
  else if(opCode == OP_ABS)
    return abs((SLONG)Operand);
  else
  {
    //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller)
    NXT_BREAK;
    return 0;
  }
}

float cCmdUnop2Flt(CODE_WORD const Code, float Operand, TYPE_CODE TypeCode)
{
  UBYTE opCode;

  opCode = OP_CODE((&Code));
  if(opCode == OP_MOV)
    return Operand;
  else if(opCode ==  OP_NEG)
    return (-(Operand));
  else if(opCode == OP_NOT)
    //!!! OP_NOT is logical, *not* bit-wise.
    //This differs from the other logical ops because we don't distinguish booleans from UBYTEs.
    return (!Operand);
  else if(opCode == OP_TST)
    return cCmdCompareFlt(COMP_CODE((&Code)), Operand, 0, TypeCode, TypeCode);
  else if(opCode == OP_ABS)
    return fabsf(Operand);
  else if(opCode == OP_SQRT)
    return sqrtf(Operand);
#if 0
  else if(opCode == OP_SIN)
    return sinf(Operand);
  else if(opCode == OP_COS)
    return cosf(Operand);
  else if(opCode == OP_TAN)
    return tanf(Operand);
  else if(opCode == OP_ASIN)
    return asinf(Operand);
  else if(opCode == OP_ACOS)
    return acosf(Operand);
  else if(opCode == OP_ATAN)
    return atanf(Operand);
#endif
  else
  {
    //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller)
    NXT_BREAK;
    return 0;
  }
}

NXT_STATUS cCmdIOGetSet(ULONG opCode, DATA_ARG Arg1, DATA_ARG Arg2, DATA_ARG Arg3)
{
  ULONG ArgVal1, ArgVal2;
  TYPE_CODE TypeCode2;
  void *pArg2 = NULL;
  switch(opCode)
  {
    case OP_GETOUT:
    {
      ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0);
      Arg2 = (UWORD)(0x200 | (Arg3 + ArgVal2 * IO_OUT_FPP));
      pArg2 = cCmdResolveIODataArg(Arg2, 0, &TypeCode2);
      cCmdSetScalarValFromDataArg(Arg1, cCmdGetVal(pArg2, TypeCode2));
    }
    break;
    //!!! All IO map access commands should screen illegal port values!
    //    Right now, cCmdResolveIODataArg's implementation allows SETIN/GETIN to access arbitrary RAM!
    case OP_SETIN:
    {
      ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0);
      Arg2 = (UWORD)(Arg3 + ArgVal2 * IO_IN_FPP);
      pArg2 = cCmdResolveIODataArg(Arg2, 0, &TypeCode2);
      ArgVal1 = cCmdGetScalarValFromDataArg(Arg1, 0);
      cCmdSetVal(pArg2, TypeCode2, ArgVal1);
    }
    break;
    case OP_GETIN:
    {
      TYPE_CODE TypeCode1;
      void * pArg1;
      ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0);
      Arg2 = (UWORD)(Arg3 + ArgVal2 * IO_IN_FPP);
      pArg2 = cCmdResolveIODataArg(Arg2, 0, &TypeCode2);
      TypeCode1= cCmdDSType(Arg1);
      pArg1= cCmdDSScalarPtr(Arg1, 0);
      if(TypeCode1 <= TC_SBYTE && TypeCode1 <= TC_SBYTE) // seems really common
        *(UBYTE*)pArg1= *(UBYTE*)pArg2;
      else
        cCmdSetVal(pArg1, TypeCode1, cCmdGetVal(pArg2, TypeCode2));
    }
    break;
  }
  return NO_ERR;
}

ULONG scalarCmp= 0, scalarFloatCmp= 0, recursiveCmp= 0, PolyScalarCmp= 0, polyPolyCmp= 0, scalarOther= 0, scalarBinopDispatch= 0, polyBinopDispatch= 0;
NXT_STATUS cCmdInterpScalarBinop(CODE_WORD * const pCode)
{
  NXT_STATUS Status;
  UBYTE opCode;
  UBYTE CmpBool;

  NXT_ASSERT(pCode != NULL);
  opCode = OP_CODE(pCode);
    DATA_ARG Arg1, Arg2, Arg3;

  scalarBinopDispatch ++;
  if(opCode == OP_CMP) // t2 && t3 guaranteed scalar or string
  {
    gPCDelta= 4;
    Arg1 = pCode[1];
    Arg2 = pCode[2];
    Arg3 = pCode[3];
    ULONG ArgVal1, ArgVal2, ArgVal3;
    TYPE_CODE TypeCode2, TypeCode3;
    DS_TOC_ENTRY *dsTOC2Ptr= &VarsCmd.pDataspaceTOC[Arg2];
    DS_TOC_ENTRY *dsTOC3Ptr= &VarsCmd.pDataspaceTOC[Arg3];

    TypeCode2 = dsTOC2Ptr->TypeCode;
    TypeCode3 = dsTOC3Ptr->TypeCode;
    if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) {
      ArgVal2= GetProcArray[TypeCode2](VarsCmd.pDataspace + dsTOC2Ptr->DSOffset);
      ArgVal3= GetProcArray[TypeCode3](VarsCmd.pDataspace + dsTOC3Ptr->DSOffset);
    ArgVal1= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3);
      DS_TOC_ENTRY *dsTOC1Ptr= &VarsCmd.pDataspaceTOC[Arg1];
      SetProcArray[dsTOC1Ptr->TypeCode](VarsCmd.pDataspace + dsTOC1Ptr->DSOffset, ArgVal1);
    scalarCmp++;
  Status = NO_ERR;
  }
    else if (TypeCode2 == TC_ARRAY) // two strings
    {
      // memcmp(); here or in compareagg, could use memcmp to speed up string compares ???
      Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0);
      cCmdSetScalarValFromDataArg(Arg1, CmpBool);
      recursiveCmp++;
    }
    else { // floats
      Status = cCmdInterpPolyBinop(*pCode, Arg1, 0, Arg2, 0, Arg3, 0);
      scalarFloatCmp++;
    }
  }
  else if(opCode == OP_BRCMP) { // t2 and t3 guaranteed scalar
      TYPE_CODE TypeCode2, TypeCode3;
      ULONG ArgVal2, ArgVal3;

      Arg1 = pCode[1];
      Arg2 = pCode[2];
      Arg3 = pCode[3];
      TypeCode2= cCmdDSType(Arg2);
      TypeCode3= cCmdDSType(Arg3);
      ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0);
      ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0);
      CmpBool= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3);

      if (CmpBool)
        gPCDelta =  (SWORD)Arg1;
  else
        gPCDelta= 4;
      Status= NO_ERR;
    }
  else if(opCode >= OP_SETIN && opCode <= OP_GETOUT) {
      Arg1 = pCode[1];
      Arg2 = pCode[2];
      Arg3 = pCode[3];
    Status= cCmdIOGetSet(opCode, Arg1, Arg2, Arg3);
    gPCDelta= 4;
    }
  else {
    scalarOther ++;
    Status= cCmdInterpBinop(pCode);
  }
  return Status;
}


NXT_STATUS cCmdInterpBinop(CODE_WORD * const pCode)
{
  NXT_STATUS Status = NO_ERR;
  UBYTE opCode;
  DATA_ARG Arg1, Arg2, Arg3;
  ULONG ArgVal3;
  UBYTE CmpBool;
  DV_INDEX DVIndex1, DVIndex2;
  UWORD i;

  polyBinopDispatch ++;
  gPCDelta= 4;

  NXT_ASSERT(pCode != NULL);
    opCode = OP_CODE(pCode);
    Arg1 = pCode[1];
    Arg2 = pCode[2];
    Arg3 = pCode[3];

  if (opCode <= OP_XOR) // && ! OP_NEG, can't happen since it is unop
    Status= cCmdInterpPolyBinop(opCode, Arg1, 0, Arg2, 0, Arg3, 0);
  else if(opCode >= OP_SETIN && opCode <= OP_GETOUT)
    Status= cCmdIOGetSet(opCode, Arg1, Arg2, Arg3);
  else {
  switch (opCode)
  {
    case OP_CMP:
    {
        TYPE_CODE TypeCode2= cCmdDSType(Arg2), TypeCode3= cCmdDSType(Arg3);
      if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) {
          ULONG ArgVal1, ArgVal2, ArgVal3;
          ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0);
          ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0);
          ArgVal1= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3);
          cCmdSetScalarValFromDataArg(Arg1, ArgVal1);
          PolyScalarCmp++;
        }
      else if (IS_AGGREGATE_TYPE(TypeCode2) && IS_AGGREGATE_TYPE(TypeCode3) && !IS_AGGREGATE_TYPE(cCmdDSType(Arg1)))
      {
        //Compare Aggregates
        Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0);
        cCmdSetScalarValFromDataArg(Arg1, CmpBool);
        recursiveCmp++;
      }
      else
      {
        //Compare Elements
        Status = cCmdInterpPolyBinop(*pCode, Arg1, 0, Arg2, 0, Arg3, 0);
        polyPolyCmp++;
      }
    }
    break;

    case OP_BRCMP:
    {
      TYPE_CODE TypeCode2= cCmdDSType(Arg2), TypeCode3= cCmdDSType(Arg3);
      if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) {
        ULONG ArgVal2, ArgVal3;
        ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0);
        ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0);
        CmpBool= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3);
      }
      else //Compare Aggregates
      Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0);

      if (CmpBool)
          gPCDelta =  (SWORD)Arg1;
    }
    break;

    case OP_INDEX:
    {
        ArgVal3 = (Arg3 != NOT_A_DS_ID) ? cCmdGetScalarValFromDataArg(Arg3, 0) : 0;

      DVIndex2 = cCmdGetDVIndex(Arg2, 0);
      if (ArgVal3 >= DV_ARRAY[DVIndex2].Count)
        return (ERR_ARG);

      Status = cCmdInterpPolyUnop2(OP_MOV, Arg1, 0, INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, ArgVal3));
    }
    break;

    case OP_ARRINIT:
    {
      //Arg1 - Dst, Arg2 - element type/default val, Arg3 - length

        NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY);

        ArgVal3 = (Arg3 != NOT_A_DS_ID) ? cCmdGetScalarValFromDataArg(Arg3, 0) : 0;

      Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)ArgVal3);
        if (!IS_ERR(Status))
        {
      DVIndex1 = cCmdGetDVIndex(Arg1, 0);
          if(cCmdDSType(Arg2) <= TC_LAST_INT_SCALAR)
          {
            ULONG val= cCmdGetScalarValFromDataArg(Arg2, 0);
            TYPE_CODE TypeCode= cCmdDSType(INC_ID(Arg1));
            for (i = 0; i < ArgVal3; i++) // could init ptr and incr by offset GM???
      {
        //copy Arg2 into each element of Arg1
              cCmdSetVal(VarsCmd.pDataspace + ARRAY_ELEM_OFFSET(DVIndex1, i), TypeCode, val);
            }
          }
          else
            for (i = 0; i < ArgVal3; i++)  //copy Arg2 into each element of Arg1
        Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, i), Arg2, 0);
      }
    }
    break;

    default:
    {
      //Fatal error: Unrecognized instruction
      NXT_BREAK;
      Status = ERR_INSTR;
    }
    break;
  }
  }
  return (Status);
}


NXT_STATUS cCmdInterpPolyBinop(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset1, DATA_ARG Arg2, UWORD Offset2, DATA_ARG Arg3, UWORD Offset3)
{
  NXT_STATUS Status = NO_ERR;
  TYPE_CODE TypeCode1, TypeCode2, TypeCode3;
  DV_INDEX DVIndex1, DVIndex2, DVIndex3;
  ULONG ArgVal1, ArgVal2, ArgVal3;
  float FltArgVal1, FltArgVal2, FltArgVal3;
  UWORD Count1, Count2, Count3;
  UWORD MinArrayCount;
  UWORD i;
  //!!! AdvCluster is intended to catch the case where sources are Cluster and an Array of Clusters.
  // In practice, the logic it uses is broken, leading to some source cluster elements being ignored.
  UBYTE AdvCluster;

  void * pArg1 = NULL,
        *pArg2 = NULL,
        *pArg3 = NULL;

  TypeCode1 = cCmdDSType(Arg1);
  TypeCode2 = cCmdDSType(Arg2);
  TypeCode3 = cCmdDSType(Arg3);

  //Simple case, both args are scalars. Solve and return.
  if ((!IS_AGGREGATE_TYPE(TypeCode2)) && (!IS_AGGREGATE_TYPE(TypeCode3)))
  {
    NXT_ASSERT(!IS_AGGREGATE_TYPE(TypeCode1));

    pArg1 = cCmdResolveDataArg(Arg1, Offset1, NULL);

    if (TypeCode1 == TC_FLOAT || TypeCode2 == TC_FLOAT || TypeCode3 == TC_FLOAT){
      pArg2 = cCmdResolveDataArg(Arg2, Offset2, NULL);
      pArg3 = cCmdResolveDataArg(Arg3, Offset3, NULL);
      FltArgVal2 = cCmdGetValFlt(pArg2, TypeCode2);
      FltArgVal3 = cCmdGetValFlt(pArg3, TypeCode3);
      FltArgVal1 = cCmdBinopFlt(Code, FltArgVal2, FltArgVal3, TypeCode2, TypeCode3);
      cCmdSetValFlt(pArg1, TypeCode1, FltArgVal1);
    }
    else
    {
    ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, Offset2);
    ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, Offset3);
    ArgVal1 = cCmdBinop(Code, ArgVal2, ArgVal3, TypeCode2, TypeCode3);
    cCmdSetVal(pArg1, TypeCode1, ArgVal1);
    }
    return Status;
  }

  //At least one of the args is an aggregate type

  //
  // Initialize Count and ArrayType local variables for each argument
  //

  if (TypeCode2 == TC_ARRAY)
  {
    Count2 = cCmdArrayCount(Arg2, Offset2);
    DVIndex2 = cCmdGetDVIndex(Arg2, Offset2);
    Offset2 = DV_ARRAY[DVIndex2].Offset;
  }
  else if (TypeCode2 == TC_CLUSTER)
  {
    Count2 = cCmdClusterCount(Arg2);
  }

  if (TypeCode3 == TC_ARRAY)
  {
    Count3 = cCmdArrayCount(Arg3, Offset3);
    DVIndex3 = cCmdGetDVIndex(Arg3, Offset3);
    Offset3 = DV_ARRAY[DVIndex3].Offset;
  }
  else if (TypeCode3 == TC_CLUSTER)
  {
    Count3 = cCmdClusterCount(Arg3);
  }


  if (TypeCode1 == TC_ARRAY)
  {
    //If the destination is an array, make sure it has enough memory to hold the result
    if ((TypeCode2 == TC_ARRAY) && (TypeCode3 == TC_ARRAY))
    {
      if (Count2 < Count3)
        MinArrayCount = Count2;
      else
        MinArrayCount = Count3;
    }
    else if (TypeCode2 == TC_ARRAY)
      MinArrayCount = Count2;
    else if (TypeCode3 == TC_ARRAY)
      MinArrayCount = Count3;
    else
    {
      //If output is an array, but no sources are arrays, that's a fatal error!
      NXT_BREAK;
      return (ERR_ARG);
    }

    //Make sure the destination array is the proper size to hold the result
    Status = cCmdDSArrayAlloc(Arg1, Offset1, MinArrayCount);
    if (IS_ERR(Status))
      return Status;

    Count1 = MinArrayCount;
    DVIndex1 = cCmdGetDVIndex(Arg1, Offset1);
    Offset1 = DV_ARRAY[DVIndex1].Offset;
    AdvCluster = FALSE;
  }
  else if (TypeCode1 == TC_CLUSTER)
  {
    Count1 = cCmdClusterCount(Arg1);
    AdvCluster = TRUE;
  }

  //Advance aggregate args to first sub-element for next call
  if (IS_AGGREGATE_TYPE(TypeCode1))
    Arg1 = INC_ID(Arg1);
  if (IS_AGGREGATE_TYPE(TypeCode2))
    Arg2 = INC_ID(Arg2);
  if (IS_AGGREGATE_TYPE(TypeCode3))
    Arg3 = INC_ID(Arg3);

  //
  // Loop through the sub-elements of aggregate arguments.
  // Call cCmdInterpPolyBinop recursively with simpler type.
  //

  for (i = 0; i < Count1; i++)
  {
    Status = cCmdInterpPolyBinop(Code, Arg1, Offset1, Arg2, Offset2, Arg3, Offset3);
    if (IS_ERR(Status))
      return Status;

    //Advance aggregate args to next sub-element
    if (TypeCode1 == TC_ARRAY)
      Offset1 += DV_ARRAY[DVIndex1].ElemSize;
    else if ((TypeCode1 == TC_CLUSTER) && AdvCluster)
      Arg1 = cCmdNextDSElement(Arg1);

    if (TypeCode2 == TC_ARRAY)
      Offset2 += DV_ARRAY[DVIndex2].ElemSize;
    else if ((TypeCode2 == TC_CLUSTER) && AdvCluster)
      Arg2 = cCmdNextDSElement(Arg2);

    if (TypeCode3 == TC_ARRAY)
      Offset3 += DV_ARRAY[DVIndex3].ElemSize;
    else if ((TypeCode3 == TC_CLUSTER) && AdvCluster)
      Arg3 = cCmdNextDSElement(Arg3);
  }

  return Status;
}


ULONG cCmdBinop(CODE_WORD const Code, ULONG LeftOp, ULONG RightOp, TYPE_CODE LeftType, TYPE_CODE RightType)
{
  UBYTE opCode;

  opCode = OP_CODE((&Code));

  switch (opCode)
  {
    case OP_ADD:
    {
      return LeftOp + RightOp;
    }

    case OP_SUB:
    {
      return LeftOp - RightOp;
    }

    case OP_MUL:
    {
      return LeftOp * RightOp;
    }

    case OP_DIV:
    {
      //Catch divide-by-zero for a portable, well-defined result.
      //(x / 0) = 0. Thus Spake LOTHAR!! (It's technical.)
      if (RightOp == 0)
        return 0;

      if (IS_SIGNED_TYPE(LeftType) && IS_SIGNED_TYPE(RightType))
        return ((SLONG)LeftOp) / ((SLONG)RightOp);
      else if (IS_SIGNED_TYPE(LeftType))
        return ((SLONG)LeftOp) / RightOp;
      else if (IS_SIGNED_TYPE(RightType))
        return LeftOp / ((SLONG)RightOp);
      else
        return LeftOp / RightOp;
    }

    case OP_MOD:
    {
      //As with OP_DIV, make sure (x % 0) = x is well-defined
      if (RightOp == 0)
        return (LeftOp);

      if (IS_SIGNED_TYPE(LeftType) && IS_SIGNED_TYPE(RightType))
        return ((SLONG)LeftOp) % ((SLONG)RightOp);
      else if (IS_SIGNED_TYPE(LeftType))
        return ((SLONG)LeftOp) % RightOp;
      else if (IS_SIGNED_TYPE(RightType))
        return LeftOp % ((SLONG)RightOp);
      else
        return LeftOp % RightOp;
    }

    case OP_AND:
    {
      return (LeftOp & RightOp);
    }

    case OP_OR:
    {
      return (LeftOp | RightOp);
    }

    case OP_XOR:
    {
      return ((LeftOp | RightOp) & (~(LeftOp & RightOp)));
    }

    case OP_CMP:
    {
      return cCmdCompare(COMP_CODE((&Code)), LeftOp, RightOp, LeftType, RightType);
    }

    default:
    {
      //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller)
      NXT_BREAK;
      return 0;
    }
  }
}


float cCmdBinopFlt(CODE_WORD const Code, float LeftOp, float RightOp, TYPE_CODE LeftType, TYPE_CODE RightType)
{
  UBYTE opCode;

  opCode = OP_CODE((&Code));

  switch (opCode)
  {
    case OP_ADD:
    {
      return LeftOp + RightOp;
    }

    case OP_SUB:
    {
      return LeftOp - RightOp;
    }

    case OP_MUL:
    {
      return LeftOp * RightOp;
    }

    case OP_DIV:
    {
      //Catch divide-by-zero for a portable, well-defined result.
      //(x / 0) = 0. Thus Spake LOTHAR!! (It's technical.)
      if (RightOp == 0)
        return 0;

      return LeftOp / RightOp;
    }

    case OP_MOD:
    {
      //As with OP_DIV, make sure (x % 0) = x is well-defined
      if (RightOp == 0)
        return (LeftOp);

      return (SLONG)LeftOp % (SLONG)RightOp;
    }

    case OP_AND:
    {
      return ((SLONG)LeftOp & (SLONG)RightOp);
    }

    case OP_OR:
    {
      return ((SLONG)LeftOp | (SLONG)RightOp);
    }

    case OP_XOR:
    {
      return (((SLONG)LeftOp | (SLONG)RightOp) & (~((SLONG)LeftOp & (SLONG)RightOp)));
    }

    case OP_CMP:
    {
      return cCmdCompareFlt(COMP_CODE((&Code)), LeftOp, RightOp, LeftType, RightType);
    }

    default:
    {
      //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller)
      NXT_BREAK;
      return 0;
    }
  }
}

NXT_STATUS cCmdInterpNoArg(CODE_WORD * const pCode)
{
  //Fatal error: Unrecognized instruction (no current opcodes have zero instructions)
  NXT_BREAK;
  return (ERR_INSTR);
}

NXT_STATUS cCmdInterpShortError(CODE_WORD * const pCode)
{
  //Fatal error: Unrecognized instruction (no current opcodes have zero instructions)
  NXT_BREAK;
  return (ERR_INSTR);
}

NXT_STATUS cCmdInterpShortSubCall(CODE_WORD * const pCode)
{
  NXT_STATUS Status;
  DATA_ARG Arg1, Arg2;

  gPCDelta= 2;
  Arg1 = GetDataArg(SHORT_ARG(pCode) + pCode[1]);
  Arg2 = GetDataArg(pCode[1]);
  NXT_ASSERT(cCmdIsClumpIDSane((CLUMP_ID)Arg1));
  NXT_ASSERT(!cCmdIsClumpOnQ(&(VarsCmd.RunQ), (CLUMP_ID)Arg1));

  NXT_ASSERT(cCmdIsDSElementIDSane(Arg2));

  *((CLUMP_ID *)(cCmdDSScalarPtr(Arg2, 0))) = VarsCmd.RunQ.Head;

  cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.RunQ.Head); //Take caller off RunQ
  cCmdEnQClump(&(VarsCmd.RunQ), (CLUMP_ID)Arg1);  //Add callee to RunQ

  Status = CLUMP_SUSPEND;

  return Status;
}

ULONG moveSameInt= 0, moveDiffInt= 0, moveFloat= 0, moveIntFloat= 0, moveFloatInt= 0, moveArrInt= 0, moveOther= 0;
NXT_STATUS cCmdMove(DATA_ARG Arg1, DATA_ARG Arg2)
{
  NXT_STATUS Status;
  DS_TOC_ENTRY  *TOC1Ptr= &VarsCmd.pDataspaceTOC[Arg1],
                *TOC2Ptr= &VarsCmd.pDataspaceTOC[Arg2];
  TYPE_CODE tc1= TOC1Ptr->TypeCode, tc2= TOC2Ptr->TypeCode;
  void *pArg1, *pArg2;

  if(tc1 <= TC_LAST_INT_SCALAR && tc2 <= TC_LAST_INT_SCALAR)
  {
    // if tc1 == tc2, do long, byte, then word assignment
    if(tc1 == tc2)
    {
      moveSameInt++;
      pArg1= VarsCmd.pDataspace + TOC1Ptr->DSOffset;
      pArg2= VarsCmd.pDataspace + TOC2Ptr->DSOffset;
      if(tc1 >= TC_ULONG)
        *(ULONG*)pArg1= *(ULONG*)pArg2;
      else if(tc1 <= TC_SBYTE)
        *(UBYTE*)pArg1= *(UBYTE*)pArg2;
      else
        *(UWORD*)pArg1= *(UWORD*)pArg2;
      Status= NO_ERR;
    }
  else
    {
      moveDiffInt++;
      ULONG val= cCmdGetScalarValFromDataArg(Arg2, 0);
      cCmdSetScalarValFromDataArg(Arg1, val);
      Status= NO_ERR;
    }
  }
  else if(tc1 == TC_FLOAT && tc2 == TC_FLOAT) { // float to float
    moveFloat++;
    pArg1= VarsCmd.pDataspace + TOC1Ptr->DSOffset;
    pArg2= VarsCmd.pDataspace + TOC2Ptr->DSOffset;
    *(float*)pArg1= *(float*)pArg2;
    Status= NO_ERR;
  }
  else if(tc1 == TC_FLOAT && tc2 <= TC_LAST_INT_SCALAR) { // int to float
    moveIntFloat++;
    pArg1= VarsCmd.pDataspace + TOC1Ptr->DSOffset;
    pArg2= VarsCmd.pDataspace + TOC2Ptr->DSOffset;
    if (tc2 == TC_SLONG)
      *(float*)pArg1 = *(SLONG*)pArg2;
    else if (tc2 == TC_ULONG)
      *(float*)pArg1 = *(ULONG*)pArg2;
    else if (tc2 == TC_SBYTE)
      *(float*)pArg1 = *(SBYTE*)pArg2;
    else if (tc2 == TC_UBYTE)
      *(float*)pArg1 = *(UBYTE*)pArg2;
    else if (tc2 == TC_UWORD)
      *(float*)pArg1 = *(UWORD*)pArg2;
    else
      *(float*)pArg1= *(SWORD*)pArg2;
    Status= NO_ERR;
  }
  else if(tc2 == TC_FLOAT && tc1 <= TC_LAST_INT_SCALAR) { // float to int
    moveFloatInt++;
    /*
     * According to C standard, converting a float to integer is implementation
     * defined when the number is out of integer bounds.
     *
     * To match original LEGO firmware, when casting float as integer, always
     * convert to signed value, then cast as destination type.
     */
    pArg1= VarsCmd.pDataspace + TOC1Ptr->DSOffset;
    pArg2= VarsCmd.pDataspace + TOC2Ptr->DSOffset;
    if (tc1 == TC_SLONG || tc1 == TC_ULONG)
      *(SLONG*)pArg1 = (SLONG)*(float*)pArg2;
    else if (tc1 == TC_SBYTE || tc1 == TC_UBYTE)
      *(SBYTE*)pArg1 = (SLONG)*(float*)pArg2;
    else
      *(SWORD*)pArg1 = (SLONG)*(float*)pArg2;
    Status= NO_ERR;
  }
  //!!! Optimized move for arrays of ints.
  else if ((tc1 == TC_ARRAY) && (tc2 == TC_ARRAY)
      && ((TOC1Ptr+1)->TypeCode <= TC_LAST_INT_SCALAR)
      && ((TOC1Ptr+1)->TypeCode == (TOC2Ptr+1)->TypeCode))
  {
    ULONG Count;
    moveArrInt++;
    Count = cCmdArrayCount(Arg2, 0);
    Status = cCmdDSArrayAlloc(Arg1, 0, Count);
    if (IS_ERR(Status))
      return Status;

    pArg1 = cCmdResolveDataArg(Arg1, 0, NULL);
    pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);

    memmove(pArg1, pArg2, Count * cCmdSizeOf((TOC1Ptr+1)->TypeCode));
  }
  else { // if ((tc1 == TC_CLUSTER) && (tc2 == TC_CLUSTER))
    moveOther++;
    Status = cCmdInterpPolyUnop2(OP_MOV, Arg1, 0, Arg2, 0);
  }
  return Status;
}


NXT_STATUS cCmdInterpShortMove(CODE_WORD * const pCode)
{
  NXT_STATUS Status;
  DATA_ARG Arg1, Arg2;

  Arg1 = GetDataArg(SHORT_ARG(pCode) + pCode[1]);
  Arg2 = GetDataArg(pCode[1]);
  Status= cCmdMove(Arg1, Arg2);

  gPCDelta= 2;
  return Status;
}

NXT_STATUS cCmdInterpShortAcquire(CODE_WORD * const pCode)
{
  NXT_STATUS Status;
  DATA_ARG Arg1;

  gPCDelta= 1;
  Arg1 = GetDataArg(SHORT_ARG(pCode));
  NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
  NXT_ASSERT(cCmdDSType(Arg1) == TC_MUTEX);

  Status = cCmdAcquireMutex((MUTEX_Q *)cCmdDSScalarPtr(Arg1, 0));

  return Status;
}

NXT_STATUS cCmdInterpShortRelease(CODE_WORD * const pCode)
{
  NXT_STATUS Status;
  DATA_ARG Arg1;

  gPCDelta= 1;
  Arg1 = GetDataArg(SHORT_ARG(pCode));
  NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
  NXT_ASSERT(cCmdDSType(Arg1) == TC_MUTEX);

  Status = cCmdReleaseMutex((MUTEX_Q *)cCmdDSScalarPtr(Arg1, 0));

  return Status;
}


//OP_SETOUT gets it's own interpreter function because it is relatively complex
// (called from cCmdInterpOther())
//This also serves as a convenient breakpoint stop for investigating output module behavior
NXT_STATUS cCmdExecuteSetOut(CODE_WORD * const pCode)
{
  TYPE_CODE TypeCodeField, TypeCodeSrc, TypeCodePortArg;
  void *pField = NULL,
       *pSrc   = NULL,
       *pPort  = NULL;
  DS_ELEMENT_ID PortArg;
  UWORD PortCount, InstrSize;
  ULONG Port, FieldTableIndex, i, j;
  DV_INDEX DVIndex;

  //Arg1 = InstrSize
  //Arg2 = port number or list of ports
  //Arg3 and beyond = FieldID, src DSID tuples

  //Calculate number of tuples
  //!!! Might want to throw ERR_INSTR if instrSize and tuples don't check out
  InstrSize = (pCode[1] / 2);

  //Second argument may specify a single port or an array list.
  //Figure out which and resolve accordingly.
  PortArg = pCode[2];
  TypeCodePortArg = cCmdDSType(PortArg);

  if (TypeCodePortArg == TC_ARRAY)
  {
    DVIndex = cCmdGetDVIndex(PortArg, 0);
    PortCount = cCmdArrayCount(PortArg, 0);
  }
  else
    PortCount = 1;

  //For each port, process all the tuples
  for (i = 0; i < PortCount; i++)
  {
    if (TypeCodePortArg == TC_ARRAY)
    {
      pPort = (UBYTE*)cCmdResolveDataArg(INC_ID(PortArg), ARRAY_ELEM_OFFSET(DVIndex, i), NULL);
      Port = cCmdGetVal(pPort, cCmdDSType(INC_ID(PortArg)));
    }
    else
    {
      Port = cCmdGetScalarValFromDataArg(PortArg, 0);
    }

    //If user specified a valid port, process the tuples.  Else, this port is a no-op
    if (Port < NO_OF_OUTPUTS)
    {
      for (j = 3; j < InstrSize; j += 2)
      {
        FieldTableIndex = (Port * IO_OUT_FPP) + pCode[j];
        pSrc = cCmdResolveDataArg(pCode[j + 1], 0, &TypeCodeSrc);

        //If FieldTableIndex is valid, go ahead and set the value
        if (FieldTableIndex < IO_OUT_FIELD_COUNT)
        {
          pField = IO_PTRS[MOD_OUTPUT][FieldTableIndex];
          TypeCodeField = IO_TYPES[MOD_OUTPUT][FieldTableIndex];
          cCmdSetVal(pField, TypeCodeField, cCmdGetVal(pSrc, TypeCodeSrc));
        }
        //Else, compiler is nutso! Return fatal error.
        else
          return (ERR_INSTR);
      }
    }
  }

  return (NO_ERR);
}


NXT_STATUS cCmdInterpOther(CODE_WORD * const pCode)
{
  NXT_STATUS Status = NO_ERR;
  UBYTE opCode;
  DATA_ARG Arg1, Arg2, Arg3, Arg4, Arg5;
  TYPE_CODE TypeCode1, TypeCode2, TypeCode3, TypeCode5;
  ULONG ArgVal2, ArgVal3, ArgVal4, ArgVal5;
  UWORD ArrayCount1, ArrayCount2, ArrayCount3, ArrayCount4;
  UWORD MinCount;
  UWORD i,j;
  DV_INDEX DVIndex1, DVIndex2, DVIndex4,TmpDVIndex;
  UWORD SrcCount;
  DS_ELEMENT_ID TmpDSID;
  UWORD DstIndex;
  UWORD Size;
  UWORD Offset;

  void  *pArg1 = NULL;
  void  *pArg2 = NULL;
  void  *pArg3 = NULL;
  void  *pArg5 = NULL;

  NXT_ASSERT(pCode != NULL);

  ULONG sz= INSTR_SIZE(*(UWORD*)pCode);
  if (sz == VAR_INSTR_SIZE)
    sz = ((UWORD*)pCode)[1];
  gPCDelta= sz/2; // advance words, sz is in bytes

  opCode = OP_CODE(pCode);

  switch (opCode)
  {

    case OP_REPLACE:
    {
      //Arg1 - Dst
      //Arg2 - Src
      //Arg3 - Index
      //Arg4 - New val / array of vals

      Arg1 = pCode[1];
      Arg2 = pCode[2];
      Arg3 = pCode[3];
      Arg4 = pCode[4];

      NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY);
      NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY);

      //Copy Src to Dst
      //!!! Could avoid full data copy if we knew which portion to overwrite
      if (Arg1 != Arg2)
      {
        Status= cCmdMove(Arg1, Arg2);
        if (IS_ERR(Status))
          return Status;
      }

      DVIndex1 = cCmdGetDVIndex(Arg1, 0);
      //Copy new val to Dst
      if (Arg3 != NOT_A_DS_ID)
      {
        pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3);
        ArgVal3 = cCmdGetVal(pArg3, TypeCode3);
      }
      else
      {
        //Index input unwired
        ArgVal3 = 0;
      }

      ArrayCount1 = cCmdArrayCount(Arg1, 0);
      //Bounds check
      //If array index (ArgVal3) is out of range, just pass out the copy of Src (effectively no-op)
      if (ArgVal3 >= ArrayCount1)
        return (NO_ERR);

      if (cCmdDSType(Arg4) != TC_ARRAY)
      {
        Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, ArgVal3), Arg4, 0);
        if (IS_ERR(Status))
          return Status;
      }
      else
      {
        DVIndex4 = cCmdGetDVIndex(Arg4, 0);

        ArrayCount4 = cCmdArrayCount(Arg4, 0);
        if (ArrayCount1 - ArgVal3 < ArrayCount4)
          MinCount = (UWORD)(ArrayCount1 - ArgVal3);
        else
          MinCount = ArrayCount4;

        for (i = 0; i < MinCount; i++)
        {
          Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, ArgVal3 + i), INC_ID(Arg4), ARRAY_ELEM_OFFSET(DVIndex4, i));
          if (IS_ERR(Status))
            return Status;
        }
      }
    }
    break;

    case OP_ARRSUBSET:
    {
      //Arg1 - Dst
      //Arg2 - Src
      //Arg3 - Index
      //Arg4 - Length

      Arg1 = pCode[1];
      Arg2 = pCode[2];
      Arg3 = pCode[3];
      Arg4 = pCode[4];

      NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY);
      NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY);

      ArrayCount2 = cCmdArrayCount(Arg2, 0);

      if (Arg3 != NOT_A_DS_ID)
        ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, 0);
      else //Index input unwired
        ArgVal3 = 0;

      if (Arg4 != NOT_A_DS_ID)
        ArgVal4 = cCmdGetScalarValFromDataArg(Arg4, 0);
      else //Length input unwired, set to "rest"
        ArgVal4 = (UWORD)(ArrayCount2 - ArgVal3);

      //Bounds check
      if (ArgVal3 > ArrayCount2)
      {
        //Illegal range - return empty subset
        Status = cCmdDSArrayAlloc(Arg1, 0, 0);
        return Status;
      }

      //Set MinCount to "rest"
      MinCount = (UWORD)(ArrayCount2 - ArgVal3);

      // Copy "Length" if it is less than "rest"
      if (ArgVal4 < (ULONG)MinCount)
        MinCount = (UWORD)ArgVal4;

      //Allocate Dst array
      Status = cCmdDSArrayAlloc(Arg1, 0, MinCount);
      if (IS_ERR(Status))
        return Status;

      DVIndex1 = cCmdGetDVIndex(Arg1, 0);
      DVIndex2 = cCmdGetDVIndex(Arg2, 0);

      //Move src subset to dst
      for (i = 0; i < MinCount; i++)
      {
        Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, i), INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, ArgVal3 + i));
        if (IS_ERR(Status))
          return Status;
      }
    }
    break;

    case OP_STRSUBSET:
    {
      //Arg1 - Dst
      //Arg2 - Src
      //Arg3 - Index
      //Arg4 - Length

      Arg1 = pCode[1];
      Arg2 = pCode[2];
      Arg3 = pCode[3];
      Arg4 = pCode[4];

      NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY);
      NXT_ASSERT(cCmdDSType(INC_ID(Arg1)) == TC_UBYTE);
      NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY);
      NXT_ASSERT(cCmdDSType(INC_ID(Arg2)) == TC_UBYTE);

      ArrayCount2 = cCmdArrayCount(Arg2, 0);

      //Remove NULL from Count
      ArrayCount2--;

      if (Arg3 != NOT_A_DS_ID)
        ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, 0);
      else //Index input unwired
        ArgVal3 = 0;

      if (Arg4 != NOT_A_DS_ID)
        ArgVal4 = cCmdGetScalarValFromDataArg(Arg4, 0);
      else //Length input unwired, set to "rest"
        ArgVal4 = (UWORD)(ArrayCount2 - ArgVal3);

      //Bounds check
      if (ArgVal3 > ArrayCount2)
      {
        //Illegal range - return empty string
        Status = cCmdDSArrayAlloc(Arg1, 0, 1);
        if (!IS_ERR(Status))
        {
          pArg1 = cCmdResolveDataArg(Arg1, 0, NULL);
          *((UBYTE *)pArg1) = '\0';
        }
        return Status;
      }

      //Set MinCount to "rest"
      MinCount = (UWORD)(ArrayCount2 - ArgVal3);

      // Copy "Length" if it is less than "rest"
      if (ArgVal4 < (ArrayCount2 - ArgVal3))
        MinCount = (UWORD)ArgVal4;

      //Allocate Dst array
      Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)(MinCount + 1));
      if (IS_ERR(Status))
        return Status;

      pArg1 = cCmdResolveDataArg(Arg1, 0, NULL);
      pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);

      //Move src subset to dst
      memmove((UBYTE *)pArg1, (UBYTE *)pArg2 + ArgVal3, MinCount);

      //Append NULL terminator to Dst
      *((UBYTE *)pArg1 + MinCount) = '\0';

    }
    break;

    case OP_SETOUT:
    {
      Status = cCmdExecuteSetOut(pCode);
    }
    break;

    case OP_ARRBUILD:
    {
      // Arg1 - Instruction Size in bytes
      // Arg2 - Dst
      // Arg3-N - Srcs

      Arg2 = pCode[2];

      NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY);

      //Number of Srcs = total code words - 3 (account for opcode word, size, and Dst)
      //!!! Argument access like this is potentially unsafe.
      //A function/macro which checks proper encoding would be better
      SrcCount = (pCode[1] / 2) - 3;

      //Calculate Dst array count
      ArrayCount2 = 0;
      for (i = 0; i < SrcCount; i++)
      {
        TmpDSID = pCode[3 + i];
        NXT_ASSERT(cCmdIsDSElementIDSane(TmpDSID));

        //If the type descriptors are the same, then the input is an array, not a single element
        if (cCmdCompareDSType(Arg2, TmpDSID))
        {
          NXT_ASSERT(cCmdDSType(TmpDSID) == TC_ARRAY);
          ArrayCount2 += cCmdArrayCount(TmpDSID, 0);
        }
        else
        {
          //Assert that the output is an array of this input type
          NXT_ASSERT(cCmdCompareDSType(INC_ID(Arg2), TmpDSID));
          ArrayCount2++;
        }
      }

      //Allocate Dst array
      Status = cCmdDSArrayAlloc(Arg2, 0, ArrayCount2);
      if (IS_ERR(Status))
        return Status;

      DVIndex2 = cCmdGetDVIndex(Arg2, 0);

      //Move Src(s) to Dst
      DstIndex = 0;
      for (i = 0; i < SrcCount; i++)
      {
        TmpDSID = pCode[3 + i];

        //If the type descriptors are the same, then the input is an array, not a single element
        if (cCmdCompareDSType(Arg2, TmpDSID))
        {
          NXT_ASSERT(cCmdDSType(TmpDSID) == TC_ARRAY);
          TmpDVIndex = cCmdGetDVIndex(TmpDSID, 0);
          // if flat, use memmove, otherwise this stuff
          if(cCmdDSType(INC_ID(TmpDSID)) <= TC_LAST_INT_SCALAR)
          {
            memmove(VarsCmd.pDataspace + ARRAY_ELEM_OFFSET(DVIndex2, DstIndex), VarsCmd.pDataspace + DV_ARRAY[TmpDVIndex].Offset, (UWORD)(DV_ARRAY[TmpDVIndex].ElemSize * DV_ARRAY[TmpDVIndex].Count));
            DstIndex += DV_ARRAY[TmpDVIndex].Count;
          }
          else
          for (j = 0; j < DV_ARRAY[TmpDVIndex].Count; j++)
          {
            Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, DstIndex), INC_ID(TmpDSID), ARRAY_ELEM_OFFSET(TmpDVIndex, j));
            if (IS_ERR(Status))
              return Status;
            DstIndex++;
          }
        }
        else
        {
          //Assert that the output is an array of this input type
          NXT_ASSERT(cCmdCompareDSType(INC_ID(Arg2), TmpDSID));
          Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, DstIndex), TmpDSID, 0);
          if (IS_ERR(Status))
            return Status;
          DstIndex++;
        }
      }

      NXT_ASSERT(DstIndex == ArrayCount2);
    }
    break;

    case OP_STRCAT:
    {
      // Arg1 - Instruction Size in bytes
      // Arg2 - Dst
      // Arg3-N - Srcs

      Arg2 = pCode[2];

      //Make sure Dst arg is a string
      NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY);
      NXT_ASSERT(cCmdDSType(INC_ID(Arg2)) == TC_UBYTE);

      //Number of Srcs = total code words - 3 (account for opcode word, size, and Dst)
      //!!! Argument access like this is potentially unsafe.
      //A function/macro which checks proper encoding would be better
      SrcCount = (pCode[1] / 2) - 3;

      //Calculate Dst array count
      ArrayCount2 = 0;
      for (i = 0; i < SrcCount; i++)
      {
        TmpDSID = pCode[3 + i];
        NXT_ASSERT(cCmdIsDSElementIDSane(TmpDSID));

        //Make sure Src arg is a string
        //!!! Type checks here should be richer to allow array of strings as input (match LabVIEW behavior)
        NXT_ASSERT(cCmdDSType(TmpDSID) == TC_ARRAY);

        if (cCmdDSType(INC_ID(TmpDSID)) != TC_UBYTE)
        {
          NXT_BREAK;
          return ERR_ARG;
        }

        ArrayCount3 = cCmdArrayCount(TmpDSID, 0);
        NXT_ASSERT(ArrayCount3 > 0);
        //Subtract NULL terminator from Src array count
        ArrayCount3--;

        //Increase Dst array count by Src array count
        ArrayCount2 += ArrayCount3;
      }

      //Add room for NULL terminator
      ArrayCount2++;

      //Allocate Dst array
      Status = cCmdDSArrayAlloc(Arg2, 0, ArrayCount2);
      if (IS_ERR(Status))
        return Status;

      //Move Src(s) to Dst
      DstIndex = 0;
      pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);
      for (i = 0; i < SrcCount; i++)
      {
        TmpDSID = pCode[3 + i];

        pArg3 = cCmdResolveDataArg(TmpDSID, 0, NULL);

        ArrayCount3 = cCmdArrayCount(TmpDSID, 0);
        NXT_ASSERT(ArrayCount3 > 0);
        //Subtract NULL terminator from Src array count
        ArrayCount3--;

        memmove((UBYTE *)pArg2 + DstIndex, pArg3, ArrayCount3);
        DstIndex += ArrayCount3;
      }

      //Append NULL terminator to Dst
      *((UBYTE *)pArg2 + DstIndex) = '\0';
      DstIndex++;

      NXT_ASSERT(DstIndex == ArrayCount2);
    }
    break;

    case OP_UNFLATTEN:
    {
      //Arg1 - Dst
      //Arg2 - Err (output)
      //Arg3 - Src (byte stream)
      //Arg4 - Type

      //The Type arg is a preallocated structure of the exact size you
      //want to unflatten into. This allows us to support unflattening arbitrary types.

      //!!! Currently, both outputs must have valid destinations.
      //    It would be trivial to handle NOT_A_DS_ID to avoid dummy
      //     allocations when outputs are unused.

      Arg1 = pCode[1];
      Arg2 = pCode[2];
      Arg3 = pCode[3];
      Arg4 = pCode[4];

      //Move Type template to Dst
      //This provides a default value for Dst and makes sure Dst is properly sized
      Status= cCmdMove(Arg1, Arg4);
      if (IS_ERR(Status))
        return Status;

      //Resolve error data pointer
      pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2);

      //Make sure Arg3 is a String
      NXT_ASSERT(cCmdDSType(Arg3) == TC_ARRAY);
      NXT_ASSERT(cCmdDSType(INC_ID(Arg3)) == TC_UBYTE);

      ArrayCount3 = cCmdArrayCount(Arg3, 0);
      //Take NULL terminator out of count
      ArrayCount3--;

      Size = cCmdCalcFlattenedSize(Arg4, 0);

      //Check that we have a proper type template to unflatten into
      if (ArrayCount3 == Size)
      {
        pArg3 = cCmdResolveDataArg(Arg3, 0, NULL);
        Offset = 0;
        Status = cCmdUnflattenFromByteArray(pArg3, &Offset, Arg1, 0);

        //!!! Status ignored from cCmdUnflattenFromByteArray
        //    If future revisions of this function provide better error checking,
        //    Err arg should be conditionally set based on the result.
        //Unflatten succeeded; set Err arg to FALSE
        cCmdSetVal(pArg2, TypeCode2, FALSE);

        NXT_ASSERT(Offset == Size);
      }
      else
      {
        //Unflatten failed; set Err arg to TRUE
        cCmdSetVal(pArg2, TypeCode2, TRUE);
      }
    }
    break;

    case OP_STRINGTONUM:
    {
      float ArgValF;
      SLONG decimals= 0;
      UBYTE cont= TRUE;
      // Arg1 - Dst number (output)
      // Arg2 - Offset past match (output)
      // Arg3 - Src string
      // Arg4 - Offset
      // Arg5 - Default (type/value)

      //!!! Currently, both outputs must have valid destinations.
      //    It would be trivial to handle NOT_A_DS_ID to avoid dummy
      //     allocations when outputs are unused.

      Arg1 = pCode[1];
      Arg2 = pCode[2];
      Arg3 = pCode[3];
      Arg4 = pCode[4];
      Arg5 = pCode[5];

      pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1);
      pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3);

      if (Arg4 != NOT_A_DS_ID)
        ArgVal4 = cCmdGetScalarValFromDataArg(Arg4, 0);
      else //Offset input unwired
        ArgVal4 = 0;

      if (Arg5 != NOT_A_DS_ID)
      {
        pArg5 = cCmdResolveDataArg(Arg5, 0, &TypeCode5);
        ArgVal5 = cCmdGetVal(pArg5, TypeCode5);
      }
      else //Default input unwired
      {
        ArgVal5 = 0;
      }

      //Read number from string
      if (sscanf(((PSZ)pArg3 + ArgVal4), "%f", &ArgValF) == 1)
      {
        i = (UWORD)ArgVal4;
        //Scan until we see the number, consumes negative sign too
        while ((((UBYTE *)pArg3)[i] < '0') || (((UBYTE *)pArg3)[i] > '9'))
          i++;

        //Scan until we get past the number and no more than one decimal
        while (cont) {
          if ((((UBYTE *)pArg3)[i] >= '0') && (((UBYTE *)pArg3)[i] <= '9'))
            i++;
          else if(((UBYTE *)pArg3)[i] == '.' && !decimals) {
            i++;
            decimals++;
            }
          else
            cont= FALSE;
        }
        ArgVal2 = i;
      }
      else
      {
        //Number wasn't found in string, use defaults
        ArgValF = ArgVal5;
        ArgVal2 = 0;
      }

      //Set outputs
      cCmdSetValFlt(pArg1, TypeCode1, ArgValF);
      cCmdSetScalarValFromDataArg(Arg2, ArgVal2);
    }
    break;

    default:
    {
      //Fatal error: Unrecognized instruction
      NXT_BREAK;
      Status = ERR_INSTR;
    }
    break;
  }

  return (Status);
}


//
//Support functions for lowspeed (I2C devices, i.e. ultrasonic sensor) communications
//

//Simple lookup table for pMapLowSpeed->ChannelState[Port] values
//This is used to keep VM status code handling consistent
//...and ChannelState gives us too much information, anyway...
static const NXT_STATUS MapLStoVMStat[6] =
{
  NO_ERR,             //LOWSPEED_IDLE,
  STAT_COMM_PENDING,  //LOWSPEED_INIT,
  STAT_COMM_PENDING,  //LOWSPEED_LOAD_BUFFER,
  STAT_COMM_PENDING,  //LOWSPEED_COMMUNICATING,
  ERR_COMM_BUS_ERR,   //LOWSPEED_ERROR,
  STAT_COMM_PENDING,  //LOWSPEED_DONE (really means c_lowspeed state machine is resetting)
};


//cCmdLSCheckStatus
//Check lowspeed port status, optionally returning bytes available in the buffer for reading
NXT_STATUS cCmdLSCheckStatus(UBYTE Port)
{
  if (Port >= NO_OF_LOWSPEED_COM_CHANNEL)
  {
    return (ERR_COMM_CHAN_INVALID);
  }

  INPUTSTRUCT * pInput = &(pMapInput->Inputs[Port]);

  //If port is not configured properly ahead of time, report that error
  //!!! This seems like the right policy, but may restrict otherwise valid read operations...
  if (!(pInput->SensorType == LOWSPEED_9V || pInput->SensorType == LOWSPEED)
   || !(pInput->InvalidData == FALSE))
  {
    return (ERR_COMM_CHAN_NOT_READY);
  }

  return (MapLStoVMStat[pMapLowSpeed->ChannelState[Port]]);
}

//cCmdLSCalcBytesReady
//Calculate true number of bytes available in the inbound LS buffer
UBYTE cCmdLSCalcBytesReady(UBYTE Port)
{
  SLONG Tmp;

  //Expect callers to validate Port, but short circuit here to be safe.
  if (Port >= NO_OF_LOWSPEED_COM_CHANNEL)
    return 0;
  
  LSBUF * pInBuf = &(pMapLowSpeed->InBuf[Port]);

  //Normally, bytes available is a simple difference.
  Tmp = pInBuf->InPtr - pInBuf->OutPtr;

  //If InPtr is actually behind OutPtr, circular buffer has wrapped.  Account for wrappage...
  if (Tmp < 0)
    Tmp = (pInBuf->InPtr + (SIZE_OF_LSBUF - pInBuf->OutPtr));

  return (UBYTE)(Tmp);
}

//cCmdLSWrite
//Write BufLength bytes into specified port's lowspeed buffer and kick off comm process to device
NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseLength)
{
  if (Port >= NO_OF_LOWSPEED_COM_CHANNEL)
  {
    return (ERR_COMM_CHAN_INVALID);
  }

  if (BufLength > SIZE_OF_LSBUF || ResponseLength > SIZE_OF_LSBUF)
  {
    return (ERR_INVALID_SIZE);
  }

  INPUTSTRUCT * pInput = &(pMapInput->Inputs[Port]);
  UBYTE * pChState = &(pMapLowSpeed->ChannelState[Port]);
  LSBUF * pOutBuf = &(pMapLowSpeed->OutBuf[Port]);

  //Only start writing process if port is properly configured and c_lowspeed module is ready
  if ((pInput->SensorType == LOWSPEED_9V || pInput->SensorType == LOWSPEED)
   && (pInput->InvalidData == FALSE)
   && (*pChState == LOWSPEED_IDLE || *pChState == LOWSPEED_ERROR))
  {
    pOutBuf->InPtr = 0;
    pOutBuf->OutPtr = 0;

    memcpy(pOutBuf->Buf, pBuf, BufLength);
    pOutBuf->InPtr = (UBYTE)BufLength;

    pMapLowSpeed->InBuf[Port].BytesToRx = ResponseLength;

    *pChState = LOWSPEED_INIT;
    pMapLowSpeed->State |= (COM_CHANNEL_ONE_ACTIVE << Port);

    return (NO_ERR);
  }
  else
  {
    //!!! Would be more consistent to return STAT_COMM_PENDING if c_lowspeed is busy
    return (ERR_COMM_CHAN_NOT_READY);
  }
}


//cCmdLSRead
//Read BufLength bytes from specified port's lowspeed buffer
NXT_STATUS cCmdLSRead(UBYTE Port, UBYTE BufLength, UBYTE * pBuf)
{
  UBYTE BytesReady, BytesToRead;

  if (Port >= NO_OF_LOWSPEED_COM_CHANNEL)
  {
    return (ERR_COMM_CHAN_INVALID);
  }

  if (BufLength > SIZE_OF_LSBUF)
  {
    return (ERR_INVALID_SIZE);
  }

  BytesReady = cCmdLSCalcBytesReady(Port);

  if (BufLength > BytesReady)
  {
    return (ERR_COMM_CHAN_NOT_READY);
  }

  BytesToRead = BufLength;

  LSBUF * pInBuf = &(pMapLowSpeed->InBuf[Port]);

  //If the bytes we want to read wrap around the end, we must first read the end, then reset back to the beginning
  if (pInBuf->OutPtr + BytesToRead >= SIZE_OF_LSBUF)
  {
    BytesToRead = SIZE_OF_LSBUF - pInBuf->OutPtr;
    memcpy(pBuf, pInBuf->Buf + pInBuf->OutPtr, BytesToRead);
    pInBuf->OutPtr = 0;
    pBuf += BytesToRead;
    BytesToRead = BufLength - BytesToRead;
  }

  memcpy(pBuf, pInBuf->Buf + pInBuf->OutPtr, BytesToRead);
  pInBuf->OutPtr += BytesToRead;

  return (NO_ERR);
}


//
//Wrappers for OP_SYSCALL
//

//
//cCmdWrapFileOpenRead
//ArgV[0]: (Function return) Loader status, U16 return
//ArgV[1]: File Handle, U8 return
//ArgV[2]: Filename, CStr
//ArgV[3]: Length, U32 return
NXT_STATUS cCmdWrapFileOpenRead(UBYTE * ArgV[])
{
  LOADER_STATUS LStatus;
  DV_INDEX DVIndex;

  //Resolve array argument
  DVIndex = *(DV_INDEX *)(ArgV[2]);
  ArgV[2] = cCmdDVPtr(DVIndex);

  LStatus = pMapLoader->pFunc(OPENREAD, ArgV[2], NULL, (ULONG *)ArgV[3]);

  //Add entry into FileHandleTable
  if (LOADER_ERR(LStatus) == SUCCESS)
  {
    VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)][0] = 'r';
    strcpy((PSZ)(VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)] + 1), (PSZ)(ArgV[2]));
  }

  //Status code in high byte of LStatus
  *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
  //File handle in low byte of LStatus
  *(ArgV[1]) = LOADER_HANDLE(LStatus);

  return NO_ERR;
}

//cCmdWrapFileOpenWrite
//ArgV[0]: (Function return) Loader status, U16 return
//ArgV[1]: File Handle, U8 return
//ArgV[2]: Filename, CStr
//ArgV[3]: Length, U32 return
NXT_STATUS cCmdWrapFileOpenWrite(UBYTE * ArgV[])
{
  LOADER_STATUS LStatus;
  DV_INDEX DVIndex;

  //Resolve array argument
  DVIndex = *(DV_INDEX *)(ArgV[2]);
  ArgV[2] = cCmdDVPtr(DVIndex);

  LStatus = pMapLoader->pFunc(OPENWRITEDATA, ArgV[2], NULL, (ULONG *)ArgV[3]);

  //Add entry into FileHandleTable
  if (LOADER_ERR(LStatus) == SUCCESS)
  {
    VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)][0] = 'w';
    strcpy((PSZ)(VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)] + 1), (PSZ)(ArgV[2]));
  }

  //Status code in high byte of LStatus
  *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
  //File handle in low byte of LStatus
  *(ArgV[1]) = LOADER_HANDLE(LStatus);

  return NO_ERR;
}

//cCmdWrapFileOpenAppend
//ArgV[0]: (Function return) Loader status, U16 return
//ArgV[1]: File Handle, U8 return
//ArgV[2]: Filename, CStr
//ArgV[3]: Length Remaining, U32 return
NXT_STATUS cCmdWrapFileOpenAppend(UBYTE * ArgV[])
{
  LOADER_STATUS LStatus;
  DV_INDEX DVIndex;

  //Resolve array argument
  DVIndex = *(DV_INDEX *)(ArgV[2]);
  ArgV[2] = cCmdDVPtr(DVIndex);

  LStatus = pMapLoader->pFunc(OPENAPPENDDATA, ArgV[2], NULL, (ULONG *)ArgV[3]);

  //Add entry into FileHandleTable
  if (LOADER_ERR(LStatus) == SUCCESS)
  {
    VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)][0] = 'w';
    strcpy((PSZ)(VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)] + 1), (PSZ)(ArgV[2]));
  }

  //Status code in high byte of LStatus
  *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
  //File handle in low byte of LStatus
  *(ArgV[1]) = LOADER_HANDLE(LStatus);

  return NO_ERR;
}

//cCmdWrapFileRead
//ArgV[0]: (Function return) Loader status, U16 return
//ArgV[1]: File Handle, U8 in/out
//ArgV[2]: Buffer, CStr out
//ArgV[3]: Length, U32 in/out
NXT_STATUS cCmdWrapFileRead(UBYTE * ArgV[])
{
  NXT_STATUS Status = NO_ERR;
  LOADER_STATUS LStatus;
  DV_INDEX DVIndex;

  //Resolve array argument
  DVIndex = *(DV_INDEX *)(ArgV[2]);
  //Size Buffer to Length
  //Add room for null terminator to length
  Status = cCmdDVArrayAlloc(DVIndex, (UWORD)(*(ULONG *)ArgV[3] + 1));
  if (IS_ERR(Status))
    return Status;

  ArgV[2] = cCmdDVPtr(DVIndex);
  LStatus = pMapLoader->pFunc(READ, ArgV[1], ArgV[2], (ULONG *)ArgV[3]);

  //Tack on NULL terminator
  //Note that loader code may have adjusted length (*ArgV[3]) if all requested data was not available
  //!!! Better solution would be to resize buffer to new length + 1,
  //     but then you must also be wary of side effects if resize allocation fails!
  *(ArgV[2] + *(ULONG *)ArgV[3]) = '\0';

  //Status code in high byte of LStatus
  *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
  //File handle in low byte of LStatus
  *(ArgV[1]) = LOADER_HANDLE(LStatus);

  return Status;
}

//cCmdWrapFileWrite
//ArgV[0]: (Function return) Loader status, U16 return
//ArgV[1]: File Handle, U8 in/out
//ArgV[2]: Buffer, CStr
//ArgV[3]: Length, U32 return
NXT_STATUS cCmdWrapFileWrite(UBYTE * ArgV[])
{
  LOADER_STATUS LStatus;
  DV_INDEX DVIndex;

  //Resolve array argument
  DVIndex = *(DV_INDEX *)(ArgV[2]);
  ArgV[2] = cCmdDVPtr(DVIndex);

  LStatus = pMapLoader->pFunc(WRITE, ArgV[1], ArgV[2], (ULONG *)ArgV[3]);

  //Status code in high byte of LStatus
  *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);
  //File handle in low byte of LStatus
  *(ArgV[1]) = LOADER_HANDLE(LStatus);

  return NO_ERR;
}

//cCmdWrapFileClose
//ArgV[0]: (Function return) Loader status, U16 return
//ArgV[1]: File Handle, U8
NXT_STATUS cCmdWrapFileClose(UBYTE * ArgV[])
{
  LOADER_STATUS LStatus;

  //!!! This bounds check also exists in dLoaderCloseHandle(), but we provide an explicit error code
  if (*(ArgV[1]) >= MAX_HANDLES)
  {
    *((UWORD *)ArgV[0]) = ILLEGALHANDLE;
    return NO_ERR;
  }

  LStatus = pMapLoader->pFunc(CLOSE, ArgV[1], NULL, NULL);

  //Clear entry in FileHandleTable
  memset(VarsCmd.FileHandleTable[*(ArgV[1])], 0, FILENAME_LENGTH + 2);

  //Status code in high byte of LStatus
  *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);

  return NO_ERR;
}

//cCmdWrapFileResolveHandle
//ArgV[0]: (Function return) Loader status, U16 return
//ArgV[1]: File Handle, U8 return
//ArgV[2]: Write Handle?, Bool return
//ArgV[3]: Filename, CStr
NXT_STATUS cCmdWrapFileResolveHandle (UBYTE * ArgV[])
{
  UBYTE i;
  DV_INDEX DVIndex;

  //Resolve array argument
  DVIndex = *(DV_INDEX *)(ArgV[3]);
  ArgV[3] = cCmdDVPtr(DVIndex);

  for (i = 0; i < MAX_HANDLES; i++)
  {
    if (strcmp((PSZ)(ArgV[3]), (PSZ)(VarsCmd.FileHandleTable[i] + 1)) == 0)
    {
      *(ArgV[2]) = (VarsCmd.FileHandleTable[i][0] == 'w');
      break;
    }
  }

  if (i == MAX_HANDLES)
  {
    i = NOT_A_HANDLE;
    *((UWORD *)ArgV[0]) = HANDLEALREADYCLOSED;
  }
  else
  {
    *((UWORD *)ArgV[0]) = SUCCESS;
  }

  *(ArgV[1]) = i;

  return NO_ERR;
}


//cCmdWrapFileRename
//ArgV[0]: (Function return) Loader status, U16 return
//ArgV[1]: Old Filename, CStr
//ArgV[2]: New Filename, CStr
NXT_STATUS cCmdWrapFileRename (UBYTE * ArgV[])
{
  LOADER_STATUS LStatus;
  ULONG Tmp;
  DV_INDEX DVIndex;

  //Resolve array arguments
  DVIndex = *(DV_INDEX *)(ArgV[1]);
  ArgV[1] = cCmdDVPtr(DVIndex);
  DVIndex = *(DV_INDEX *)(ArgV[2]);
  ArgV[2] = cCmdDVPtr(DVIndex);

  //!!! Tmp placeholder passed into loader code to avoid illegal dereferencing.
  LStatus = pMapLoader->pFunc(RENAMEFILE, ArgV[1], ArgV[2], &Tmp);

  //Status code in high byte of LStatus
  *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);

  return NO_ERR;
}


//cCmdWrapFileDelete
//ArgV[0]: (Function return) Loader status, U16 return
//ArgV[1]: Filename, CStr
NXT_STATUS cCmdWrapFileDelete (UBYTE * ArgV[])
{
  LOADER_STATUS LStatus;
  DV_INDEX DVIndex;

  //Resolve array arguments
  DVIndex = *(DV_INDEX *)(ArgV[1]);
  ArgV[1] = cCmdDVPtr(DVIndex);

  LStatus = pMapLoader->pFunc(DELETE, ArgV[1], NULL, NULL);

  //Status code in high byte of LStatus
  *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus);

  return NO_ERR;
}

//
//cCmdWrapSoundPlayFile
//ArgV[0]: (Return value) Status code, SBYTE
//ArgV[1]: Filename, CStr
//ArgV[2]: Loop?, UBYTE (bool)
//ArgV[3]: Volume, UBYTE
//
NXT_STATUS cCmdWrapSoundPlayFile(UBYTE * ArgV[])
{
  DV_INDEX DVIndex;

  //Resolve array arguments
  DVIndex = *(DV_INDEX *)(ArgV[1]);
  UBYTE sndVol= *(ArgV[3]);
  ArgV[1] = cCmdDVPtr(DVIndex);

  //!!! Should check filename and/or existence and return error before proceeding
  strncpy((PSZ)(pMapSound->SoundFilename), (PSZ)(ArgV[1]), FILENAME_LENGTH);

  if (*(ArgV[2]) == TRUE)
    pMapSound->Mode = SOUND_LOOP;
  else
    pMapSound->Mode = SOUND_ONCE;

  if(sndVol > 4)
    sndVol= 4;
  pMapSound->Volume = sndVol;
  //SampleRate of '0' means "let file specify SampleRate"
  pMapSound->SampleRate = 0;
  pMapSound->Flags |= SOUND_UPDATE;

  *((SBYTE*)(ArgV[0])) = (NO_ERR);

  return (NO_ERR);
}

//
//cCmdWrapSoundPlayTone
//ArgV[0]: (Return value) Status code, SBYTE
//ArgV[1]: Frequency, UWORD
//ArgV[2]: Duration, UWORD
//ArgV[3]: Loop?, UBYTE (Boolean)
//ArgV[4]: Volume, UBYTE
//
NXT_STATUS cCmdWrapSoundPlayTone(UBYTE * ArgV[])
{
  UBYTE sndVol= *(ArgV[4]);
  pMapSound->Freq = *(UWORD*)(ArgV[1]);
  pMapSound->Duration = *(UWORD*)(ArgV[2]);
  if(sndVol > 4)
    sndVol= 4;
  pMapSound->Volume = sndVol;
  pMapSound->Flags |= SOUND_UPDATE;

  if (*(ArgV[3]) == TRUE)
    pMapSound->Mode = SOUND_TONE | SOUND_LOOP;
  else
    pMapSound->Mode = SOUND_TONE;

  *((SBYTE*)(ArgV[0])) = (NO_ERR);

  return (NO_ERR);
}

//
//cCmdWrapSoundGetState
//ArgV[0]: (Return value) sound module state, UBYTE
//ArgV[1]: Flags, UBYTE
//
NXT_STATUS cCmdWrapSoundGetState(UBYTE * ArgV[])
{
  *(ArgV[0]) = pMapSound->State;
  *(ArgV[1]) = pMapSound->Flags;
  return (NO_ERR);
}

//
//cCmdWrapSoundSetState
//ArgV[0]: (Return value) sound module state, UBYTE
//ArgV[1]: State, UBYTE
//ArgV[2]: Flags, UBYTE
//
NXT_STATUS cCmdWrapSoundSetState(UBYTE * ArgV[])
{
  pMapSound->State = *(ArgV[1]);
  //Return same state we just set, mostly for interface consistency
  *(ArgV[0]) = pMapSound->State;

  //OR in provided flags (usually 0)
  pMapSound->Flags |= *(ArgV[2]);

  return (NO_ERR);
}

//
//cCmdWrapReadButton
//ArgV[0]: (Function return) Status code, SBYTE
//ArgV[1]: Index (U8)
//ArgV[2]: Pressed (bool)
//ArgV[3]: Count (U8) (count of press-then-release cycles)
//ArgV[4]: ResetCount? (bool in)
//
NXT_STATUS cCmdWrapReadButton(UBYTE * ArgV[])
{
  UBYTE btnIndex;

  btnIndex = *((UBYTE*)(ArgV[1]));

  if (btnIndex < NO_OF_BTNS)
  {
    //Set pressed boolean output
    if (pMapButton->State[btnIndex] & PRESSED_STATE)
      *(ArgV[2]) = TRUE;
    else
      *(ArgV[2]) = FALSE;

    //Set count output
    *(ArgV[3]) = (UBYTE)(pMapButton->BtnCnt[btnIndex].RelCnt);

    //Optionally reset internal count
    if (*(ArgV[4]) != 0)
    {
      pMapButton->BtnCnt[btnIndex].RelCnt = 0;
      //Need to clear short and long counts too, because RelCnt depends on them.  No known side effects.
      pMapButton->BtnCnt[btnIndex].ShortRelCnt = 0;
      pMapButton->BtnCnt[btnIndex].LongRelCnt = 0;
    }

    // Set status code 'OK'
    *((SBYTE*)(ArgV[0])) = NO_ERR;
  }
  else
  {
    //Bad button index specified, return error and default outputs
    *((SBYTE*)(ArgV[0])) = ERR_INVALID_PORT;
    *(ArgV[2]) = FALSE;
    *(ArgV[3]) = 0;
  }

  return (NO_ERR);
}

//
//cCmdWrapCommLSWrite
//ArgV[0]: (return) Status code, SBYTE
//ArgV[1]: Port specifier, UBYTE
//ArgV[2]: Buffer to send, UBYTE array, only SIZE_OF_LSBUF bytes will be used
//ArgV[3]: ResponseLength, UBYTE, specifies expected bytes back from slave device
//
NXT_STATUS cCmdWrapCommLSWrite(UBYTE * ArgV[])
{
  SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
  UBYTE Port = *(ArgV[1]);
  UBYTE * pBuf;
  UWORD BufLength;
  UBYTE ResponseLength = *(ArgV[3]);
  DV_INDEX DVIndex;

  //Resolve array arguments
  DVIndex = *(DV_INDEX *)(ArgV[2]);
  pBuf = cCmdDVPtr(DVIndex);
  BufLength = DV_ARRAY[DVIndex].Count;

  *pReturnVal = cCmdLSWrite(Port, (UBYTE)BufLength, pBuf, ResponseLength);

  return (NO_ERR);
}

//
//cCmdWrapCommLSCheckStatus
//ArgV[0]: (return) Status code, SBYTE
//ArgV[1]: Port specifier, UBYTE
//ArgV[2]: BytesReady, UBYTE
//
NXT_STATUS cCmdWrapCommLSCheckStatus(UBYTE * ArgV[])
{
  UBYTE Port = *(ArgV[1]);

  *((SBYTE*)(ArgV[0])) = cCmdLSCheckStatus(Port);
  *((UBYTE*)(ArgV[2])) = cCmdLSCalcBytesReady(Port);

  return (NO_ERR);
}

//
//cCmdWrapCommLSRead
//ArgV[0]: (return) Status code, SBYTE
//ArgV[1]: Port specifier, UBYTE
//ArgV[2]: Buffer for data, UBYTE array, max SIZE_OF_LSBUF bytes will be written
//ArgV[3]: BufferLength, UBYTE, specifies size of buffer requested
//
NXT_STATUS cCmdWrapCommLSRead(UBYTE * ArgV[])
{
  SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
  UBYTE Port = *(ArgV[1]);
  UBYTE * pBuf;
  UBYTE BufLength = *(ArgV[3]);
  UBYTE BytesToRead;
  DV_INDEX DVIndex = *(DV_INDEX *)(ArgV[2]);
  NXT_STATUS AllocStatus;

  *pReturnVal = cCmdLSCheckStatus(Port);
  BytesToRead = cCmdLSCalcBytesReady(Port);

  //If channel is OK and has data ready for us, put the data into outgoing buffer
  if (!IS_ERR(*pReturnVal) && BytesToRead > 0)
  {
    //Limit buffer to available data
    if (BufLength > BytesToRead)
      BufLength = BytesToRead;

    AllocStatus = cCmdDVArrayAlloc(DVIndex, BufLength);
    if (IS_ERR(AllocStatus))
      return (AllocStatus);

    pBuf = cCmdDVPtr(DVIndex);
    *pReturnVal = cCmdLSRead(Port, BufLength, pBuf);
  }
  //Else, the channel has an error and/or there's no data to read; clear the output array
  else
  {
    AllocStatus = cCmdDVArrayAlloc(DVIndex, 0);
    if (IS_ERR(AllocStatus))
      return (AllocStatus);
  }

  return (NO_ERR);
}

//
//cCmdWrapRandomNumber
//ArgV[0]: (return) Random number, SWORD
//
NXT_STATUS cCmdWrapRandomNumber(UBYTE * ArgV[])
{
  static UBYTE count = 0;
  SWORD random;

  if (count == 0)
    srand(dTimerRead());

  if (count > 20)
    count = 0;
  else
    count++;

  //!!! IAR's implementation of the rand() library function returns signed values, and we want it that way.
  //Some stdlib implementations may return only positive numbers, so be wary if this code is ported.
  random = rand();

  *((SWORD *)ArgV[0]) = random;

  return NO_ERR;
}

//
//cCmdWrapGetStartTick
//ArgV[0]: (return) Start Tick, ULONG
//
NXT_STATUS cCmdWrapGetStartTick(UBYTE * ArgV[])
{
  *((ULONG *)ArgV[0]) = VarsCmd.StartTick;
  return NO_ERR;
}

//
//cCmdWrapMessageWrite
//ArgV[0]: (return) Error Code, SBYTE (NXT_STATUS)
//ArgV[1]: QueueID, UBYTE
//ArgV[2]: Message, CStr
//
NXT_STATUS cCmdWrapMessageWrite(UBYTE * ArgV[])
{
  NXT_STATUS Status = NO_ERR;
  DV_INDEX DVIndex;

  //Resolve array arguments
  DVIndex = *(DV_INDEX *)(ArgV[2]);
  ArgV[2] = cCmdDVPtr(DVIndex);

  Status = cCmdMessageWrite(*(UBYTE *)(ArgV[1]), ArgV[2], DV_ARRAY[DVIndex].Count);

  *(SBYTE *)(ArgV[0]) = Status;

  if (IS_FATAL(Status))
    return Status;
  else
    return (NO_ERR);
}



//
//cCmdWrapColorSensorRead
//ArgV[0]: (return) Error code, SBYTE
//ArgV[1]: Port, UBYTE
//ArgV[2]: SensorValue, SWORD
//ArgV[3]: RawArray, UWORD[NO_OF_COLORS]
//ArgV[4]: NormalizedArray, UWORD[NO_OF_COLORS]
//ArgV[5]: ScaledArray, SWORD[NO_OF_COLORS]
//ArgV[6]: InvalidData, UBYTE
//
NXT_STATUS cCmdWrapColorSensorRead (UBYTE * ArgV[])
{
	DV_INDEX DVIndex;
        NXT_STATUS Status = NO_ERR;
	//Resolve return val arguments
	SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
	//Resolve Port argument
	UBYTE Port = *(UBYTE*)(ArgV[1]);
	//Resolve SensorValue
	SWORD SensorValue = *(SWORD*)(ArgV[2]);
	//Resolve RawArray as array
	DVIndex = *(DV_INDEX*)(ArgV[3]);
        NXT_ASSERT(IS_DV_INDEX_SANE(DestDVIndex));
        Status= cCmdDVArrayAlloc(DVIndex, NO_OF_COLORS);
        if (IS_ERR(Status))
          return (Status);
	ArgV[3] = cCmdDVPtr (DVIndex);
	//Resolve NormalizedArray as array
	DVIndex = *(DV_INDEX*)(ArgV[4]);
        NXT_ASSERT(IS_DV_INDEX_SANE(DestDVIndex));
        Status= cCmdDVArrayAlloc(DVIndex, NO_OF_COLORS);
        if (IS_ERR(Status))
          return (Status);
	ArgV[4] = cCmdDVPtr (DVIndex);
	//Resolve ScaledArray as array
	DVIndex = *(DV_INDEX*)(ArgV[5]);
        NXT_ASSERT(IS_DV_INDEX_SANE(DestDVIndex));
        Status= cCmdDVArrayAlloc(DVIndex, NO_OF_COLORS);
	if (IS_ERR(Status))
          return (Status);
        ArgV[5] = cCmdDVPtr (DVIndex);
	//Resolve InvalidData
	UBYTE InvalidData = *(UBYTE*)(ArgV[6]);

	//call implementation with unwrapped parameters
	*pReturnVal = cCmdColorSensorRead (Port, &SensorValue, (UWORD*)ArgV[3], (UWORD*)ArgV[4], (SWORD*)ArgV[5], &InvalidData);

	*(ArgV[2]) = SensorValue;
        *(ArgV[6]) = InvalidData;

        if (IS_ERR(*pReturnVal)){
		return (*pReturnVal);
	}
	return NO_ERR;
}


#define UNPACK_STATUS(StatusWord) ((SBYTE)(StatusWord))

NXT_STATUS cCmdBTCheckStatus(UBYTE Connection)
{
  //If specified connection is invalid, return error code to the user.
  if (Connection >= SIZE_OF_BT_CONNECT_TABLE)
  {
    return (ERR_INVALID_PORT);
  }

  //INPROGRESS means a request is currently pending completion by the comm module
  if (VarsCmd.CommStat == INPROGRESS)
  {
    return (STAT_COMM_PENDING);
  }
  //Translate BTBUSY to ERR_COMM_CHAN_NOT_READY
  //And check if specified connection is indeed configured
  else if (VarsCmd.CommStat == (SWORD)BTBUSY
       || (pMapComm->BtConnectTable[Connection].Name[0]) == '\0')
  {
    return (ERR_COMM_CHAN_NOT_READY);
  }
  else
  {
    return (UNPACK_STATUS(VarsCmd.CommStat));
  }
}

//Default packet to send for a remote MESSAGE_READ command.
//3rd byte must be replaced with remote mailbox (QueueID)
//4th byte must be replaced with local mailbox
static UBYTE RemoteMsgReadPacket[5] = {0x00, 0x13, 0xFF, 0xFF, 0x01};

//
//cCmdWrapMessageRead
//ArgV[0]: (return) Error Code, SBYTE (NXT_STATUS)
//ArgV[1]: QueueID, UBYTE
//ArgV[2]: Remove, UBYTE
//ArgV[3]: (return) Message, CStr
//
NXT_STATUS cCmdWrapMessageRead(UBYTE * ArgV[])
{
  NXT_STATUS Status = NO_ERR;
  NXT_STATUS AllocStatus = NO_ERR;
  UBYTE QueueID = *(UBYTE *)(ArgV[1]);
  DV_INDEX DestDVIndex = *(DV_INDEX *)(ArgV[3]);
  UWORD MessageSize;
  UBYTE i;

  NXT_ASSERT(IS_DV_INDEX_SANE(DestDVIndex));

  //Check Next Message's size
  Status = cCmdMessageGetSize(QueueID, &MessageSize);

  //If there is a valid message in local mailbox, read it
  if (!IS_ERR(Status) && MessageSize > 0 )
  {
    //!!! Also check for EMPTY_MAILBOX status?
    //Size destination string
    AllocStatus = cCmdDVArrayAlloc(DestDVIndex, MessageSize);
    if (IS_ERR(AllocStatus))
      return AllocStatus;

    //Get Message
    //!!! Should more aggressively enforce null termination before blindly copying to dataspace
    Status = cCmdMessageRead(QueueID, cCmdDVPtr(DestDVIndex), MessageSize, *(ArgV[2]));
  }
  else
  {
    //Clear destination string
    AllocStatus = cCmdDVArrayAlloc(DestDVIndex, 1);
    if (IS_ERR(AllocStatus))
      return AllocStatus;

    //Successful allocation, make sure first byte is null terminator
    *(UBYTE*)(cCmdDVPtr(DestDVIndex)) = '\0';
  }

  //If there were no local messages, see if there are any waiting in our slaves' outboxes
  if (Status == STAT_MSG_EMPTY_MAILBOX && QueueID < INCOMING_QUEUE_COUNT)
  {
    //If there's an old error code hanging around, clear it before proceeding.
    //!!! Clearing error here means bytecode status checking loops could get false SUCCESS results?
    if (VarsCmd.CommStat < 0)
      VarsCmd.CommStat = SUCCESS;

    //Search through possible slaves, looking for valid connection
    for (i = 0; i < SIZE_OF_BT_CONNECT_TABLE - 1; i++)
    {
      //Advance CommCurrConnection and limit to 1, 2, or 3 (only slave connection slots are checked)
      VarsCmd.CommCurrConnection++;
      if (VarsCmd.CommCurrConnection == SIZE_OF_BT_CONNECT_TABLE)
        VarsCmd.CommCurrConnection = 1;

      if (cCmdBTCheckStatus(VarsCmd.CommCurrConnection) == NO_ERR)
        break;
    }

    //If there is at least one configured slave connection, make a remote read request
    if (i < SIZE_OF_BT_CONNECT_TABLE - 1)
    {
      //Outgoing QueueID on slave device is the local QueueID + INCOMING_QUEUE_COUNT
      RemoteMsgReadPacket[2] = QueueID + INCOMING_QUEUE_COUNT;
      RemoteMsgReadPacket[3] = QueueID;

      //Request comm module to send assembled packet and not go idle until response comes back (or error)
      pMapComm->pFunc(SENDDATA, sizeof(RemoteMsgReadPacket), VarsCmd.CommCurrConnection, TRUE, RemoteMsgReadPacket, (UWORD*)&(VarsCmd.CommStat));

      //Read status back after SENDDATA call so bytecode gets STAT_COMM_PENDING or error
      Status = cCmdBTCheckStatus(VarsCmd.CommCurrConnection);

      //If our request was accepted, set the DirtyComm flag so stream will get cleaned up later
      if (Status == STAT_COMM_PENDING)
        VarsCmd.DirtyComm = TRUE;
    }
  }

  *(SBYTE *)(ArgV[0]) = Status;
  if (IS_FATAL(Status))
    return Status;
  else
    return (NO_ERR);
}


//
//cCmdWrapCommBTCheckStatus
//ArgV[0]: (return) Status byte, SBYTE
//ArgV[1]: Connection index, 0-3
//
NXT_STATUS cCmdWrapCommBTCheckStatus(UBYTE * ArgV[])
{
  *((SBYTE*)(ArgV[0])) = cCmdBTCheckStatus(*(ArgV[1]));

  return (NO_ERR);
}

//
//cCmdWrapCommBTWrite
//ArgV[0]: (return) Status byte, SBYTE
//ArgV[1]: Connection index, 0-3
//ArgV[2]: Buffer
//
NXT_STATUS cCmdWrapCommBTWrite(UBYTE * ArgV[])
{
  SBYTE * pReturnVal =  (SBYTE*)(ArgV[0]);
  UBYTE Connection   = *(ArgV[1]);
  UBYTE * pBuf;
  UWORD BufLength;
  DV_INDEX DVIndex;

  //Resolve array arguments
  DVIndex = *(DV_INDEX *)(ArgV[2]);
  pBuf = cCmdDVPtr(DVIndex);

  BufLength = DV_ARRAY[DVIndex].Count;

  //If there's an old error code hanging around, clear it before proceeding.
  if (VarsCmd.CommStat < 0)
    VarsCmd.CommStat = SUCCESS;

  //!!! Only first 256 bytes could possibly make it through! Should return error on longer input?
  //!!! Not requesting a wait-for-response because only known use doesn't read responses.
  pMapComm->pFunc(SENDDATA, (UBYTE)BufLength, Connection, FALSE, pBuf, (UWORD*)&(VarsCmd.CommStat));

  //!!! Reasonable to wrap below code in cCmdCommBTCheckStatus?
  //INPROGRESS means our request was accepted by His Funkiness of pFunc
  if (VarsCmd.CommStat == (SWORD)INPROGRESS)
  {
    *pReturnVal = STAT_COMM_PENDING;

    //Set DirtyComm flag so stream is reset after program ends
    VarsCmd.DirtyComm = TRUE;
  }
  //Translate BTBUSY to ERR_COMM_CHAN_NOT_READY
  else if (VarsCmd.CommStat == (SWORD)BTBUSY)
  {
    *pReturnVal = ERR_COMM_CHAN_NOT_READY;
  }
  else
  {
    *pReturnVal = UNPACK_STATUS(VarsCmd.CommStat);
  }

  return (NO_ERR);
}

//
//cCmdWrapCommBTRead
//ArgV[0]: (return) Status byte, SBYTE
//ArgV[1]: Count to read
//ArgV[2]: Buffer
//
NXT_STATUS cCmdWrapCommBTRead(UBYTE * ArgV[])
{
  //SBYTE * pReturnVal =  (SBYTE*)(ArgV[0]);
  //UBYTE * pBuf       =  (ArgV[2]);
  //!!! should provide length and/or connection to read?

  //!!! This syscall is not implemented; return fatal error.
  return (ERR_INSTR);
}

//
//cCmdWrapKeepAlive
//ArgV[0]: (return) Current timer limit in ms, ULONG
//
NXT_STATUS cCmdWrapKeepAlive(UBYTE * ArgV[])
{
  pMapUi->Flags |= UI_RESET_SLEEP_TIMER;

  //Convert UI's minute-based timeout value to millisecs
  //Milliseconds are the "natural" time unit in user-land.
  *(ULONG*)(ArgV[0]) = (pMapUi->SleepTimeout * 60 * 1000);

  return (NO_ERR);
}



#define MAX_IOM_BUFFER_SIZE 64
//
//cCmdWrapIOMapRead
//ArgV[0]: (return) Status byte, SBYTE
//ArgV[1]: Module name, CStr
//ArgV[2]: Offset, UWORD
//ArgV[3]: Count, UWORD
//ArgV[4]: Buffer, UBYTE array
//
NXT_STATUS cCmdWrapIOMapRead(UBYTE * ArgV[])
{
  UWORD LStatus;
  NXT_STATUS Status;

  SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
  UWORD Offset = *(UWORD*)(ArgV[2]);
  //Our copy of 'Count' must be a ULONG to match the loader interface
  ULONG Count = *(UWORD*)(ArgV[3]);

  DV_INDEX DVIndex;

  //Buffer for return of FINDFIRSTMODULE call, structure defined in protocol doc
  //We need it to transfer the ModuleID to the IOMAPREAD call
  UBYTE FindBuffer[FILENAME_LENGTH + 10];
  //Buffer to store data and offset in for IOMAPREAD call
  //!!! Constant size means only limited reads and writes
  UBYTE DataBuffer[MAX_IOM_BUFFER_SIZE + 2];

  if (Count > MAX_IOM_BUFFER_SIZE)
  {
    //Request to read too much data at once; clear buffer, return error.
    DVIndex = *(DV_INDEX *)(ArgV[4]);
    *pReturnVal = cCmdDVArrayAlloc(DVIndex, 0);
    if (IS_ERR(*pReturnVal))
      return (*pReturnVal);

    *pReturnVal = ERR_INVALID_SIZE;
    return (NO_ERR);
  }

  //Resolve module name
  DVIndex = *(DV_INDEX *)(ArgV[1]);
  ArgV[1] = cCmdDVPtr(DVIndex);

  //Find module by name.  Note that wildcards are accepted, but only first match matters.
  LStatus = pMapLoader->pFunc(FINDFIRSTMODULE, ArgV[1], FindBuffer, NULL);

  if (LOADER_ERR(LStatus) == SUCCESS)
  {
    //Module was found, transfer Offset into first two bytes of DataBuffer and attempt to read
    *(UWORD*)(DataBuffer) = Offset;
    LStatus = pMapLoader->pFunc(IOMAPREAD, &(FindBuffer[FILENAME_LENGTH + 1]), DataBuffer, &Count);

    if (LOADER_ERR(LStatus) == SUCCESS)
    {
      //No error from IOMAPREAD, so copy the data into VM's dataspace
      //Size destination array
      DVIndex = *(DV_INDEX *)(ArgV[4]);
      Status = cCmdDVArrayAlloc(DVIndex, (UWORD)Count);
      if (IS_ERR(Status))
      {
        //Alloc failed, so close handle and return
        pMapLoader->pFunc(CLOSEMODHANDLE, NULL, NULL, NULL);
        return (Status);
      }

      //Alloc succeeded, so resolve and copy away
      ArgV[4] = cCmdDVPtr(DVIndex);
      memcpy(ArgV[4], &(DataBuffer[2]), Count);
    }
  }

  *pReturnVal = LOADER_ERR_BYTE(LStatus);

  pMapLoader->pFunc(CLOSEMODHANDLE, NULL, NULL, NULL);
  return (NO_ERR);
}

//
//cCmdWrapIOMapWrite
//ArgV[0]: (return) Status byte, SBYTE
//ArgV[1]: Module name, CStr
//ArgV[2]: Offset, UWORD
//ArgV[3]: Buffer, UBYTE array
//
NXT_STATUS cCmdWrapIOMapWrite(UBYTE * ArgV[])
{
  UWORD LStatus;

  SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
  UWORD Offset = *(UWORD*)(ArgV[2]);

  //Our copy of 'Count' must be a ULONG to match the loader interface
  ULONG Count;
  DV_INDEX DVIndex;

  //Buffer for return of FINDFIRSTMODULE call, structure defined in protocol doc
  //We need it to transfer the ModuleID to the IOMAPREAD call
  UBYTE FindBuffer[FILENAME_LENGTH + 10];
  //Buffer to store data and offset in for IOMAPREAD call
  //!!! Constant size means only limited reads and writes
  UBYTE DataBuffer[MAX_IOM_BUFFER_SIZE + 2];

  //Resolve module name and buffer
  DVIndex = *(DV_INDEX *)(ArgV[1]);
  ArgV[1] = cCmdDVPtr(DVIndex);

  DVIndex = *(DV_INDEX *)(ArgV[3]);
  ArgV[3] = cCmdDVPtr(DVIndex);
  Count = DV_ARRAY[DVIndex].Count;

  if (Count > MAX_IOM_BUFFER_SIZE)
  {
    //Request to read too much data at once; return error and give up
    *pReturnVal = ERR_INVALID_SIZE;
    return (NO_ERR);
  }

  LStatus = pMapLoader->pFunc(FINDFIRSTMODULE, ArgV[1], FindBuffer, NULL);

  if (LOADER_ERR(LStatus) == SUCCESS)
  {
    //Module was found, transfer Offset into first two bytes of DataBuffer, copy data into rest of buffer, then write
    *(UWORD*)(DataBuffer) = Offset;
    memcpy(&(DataBuffer[2]), ArgV[3], Count);
    LStatus = pMapLoader->pFunc(IOMAPWRITE, &(FindBuffer[FILENAME_LENGTH + 1]), DataBuffer, &Count);
  }

  *pReturnVal = LOADER_ERR_BYTE(LStatus);

  pMapLoader->pFunc(CLOSEMODHANDLE, NULL, NULL, NULL);
  return (NO_ERR);
}

#if VM_BENCHMARK
void cCmdWriteBenchmarkFile()
{
  LOADER_STATUS LStatus;
  UBYTE Handle;
  ULONG BenchFileSize;
  ULONG i, Length;
  UBYTE Buffer[256];

  //Remove old benchmark file, create a new one
  strcpy((char *)Buffer, "benchmark.txt");
  pMapLoader->pFunc(DELETE, Buffer, NULL, NULL);
  BenchFileSize = 2048;
  LStatus = pMapLoader->pFunc(OPENWRITEDATA, Buffer, NULL, &BenchFileSize);

  if (!LOADER_ERR(LStatus))
  {
    //Write Benchmark file
    Handle = LOADER_HANDLE(LStatus);

    //Header
    sprintf((char *)Buffer, "Program Name: %s\r\n", VarsCmd.ActiveProgName);
    Length = strlen((char *)Buffer);
    LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);

    sprintf((char *)Buffer, "InstrCount: %d\r\n", VarsCmd.InstrCount);
    Length = strlen((char *)Buffer);
    LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);

    sprintf((char *)Buffer, "Time: %d\r\n", IOMapCmd.Tick - VarsCmd.StartTick);
    Length = strlen((char *)Buffer);
    LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);

    sprintf((char *)Buffer, "Instr/Tick: %d\r\n", VarsCmd.Average);
    Length = strlen((char *)Buffer);
    LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);

    sprintf((char *)Buffer, "CmdCtrl Calls: %d\r\n", VarsCmd.CmdCtrlCount);
    Length = strlen((char *)Buffer);
    LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);

    sprintf((char *)Buffer, "OverTime Rounds: %d\r\n", VarsCmd.OverTimeCount);
    Length = strlen((char *)Buffer);
    LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);

    sprintf((char *)Buffer, "Max OverTime Length: %d\r\n", VarsCmd.MaxOverTimeLength);
    Length = strlen((char *)Buffer);
    LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);

    sprintf((char *)Buffer, "CompactionCount: %d\r\n", VarsCmd.CompactionCount);
    Length = strlen((char *)Buffer);
    LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);

    sprintf((char *)Buffer, "LastCompactionTick: %d\r\n", VarsCmd.LastCompactionTick);
    Length = strlen((char *)Buffer);
    LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);

    sprintf((char *)Buffer, "MaxCompactionTime: %d\r\n", VarsCmd.MaxCompactionTime);
    Length = strlen((char *)Buffer);
    LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);

    //opcode benchmarks
    sprintf((char *)Buffer, "Op\tCnt\tOver\tMax\r\n");
    Length = strlen((char *)Buffer);
    LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
    for (i = 0; i < OPCODE_COUNT; i++)
    {
      sprintf((char *)Buffer, "%x\t%d\t%d\t%d\t%d\r\n", i, VarsCmd.OpcodeBenchmarks[i][0], VarsCmd.OpcodeBenchmarks[i][1], VarsCmd.OpcodeBenchmarks[i][2], VarsCmd.OpcodeBenchmarks[i][3]);
      Length = strlen((char *)Buffer);
      LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length);
    }
    //close file
    LStatus = pMapLoader->pFunc(CLOSE, &Handle, NULL, NULL);
  }
}
#endif


/////////////////////////////////////////////////////////////
// Dymanic syscall implementations
////////////////////////////////////////////////////////////

//
//cCmdWrapDatalogWrite
//ArgV[0]: (return) Error Code, SBYTE (NXT_STATUS)
//ArgV[1]: Message, CStr
//
NXT_STATUS cCmdWrapDatalogWrite(UBYTE * ArgV[])
{
  NXT_STATUS Status = NO_ERR;
  DV_INDEX DVIndex;

  //Resolve array arguments
  DVIndex = *(DV_INDEX *)(ArgV[1]);
  ArgV[1] = cCmdDVPtr(DVIndex);

  Status = cCmdDatalogWrite(ArgV[1], DV_ARRAY[DVIndex].Count);

  *(SBYTE *)(ArgV[0]) = Status;

  if (IS_FATAL(Status))
    return Status;
  else
    return (NO_ERR);
}

//
//cCmdWrapDatalogGetTimes
//ArgV[0]: SyncTime, U32
//ArgV[1]: SyncTick, U32
//
NXT_STATUS cCmdWrapDatalogGetTimes(UBYTE * ArgV[])
{
  *((ULONG *)ArgV[1]) = IOMapCmd.SyncTime;
  *((ULONG *)ArgV[2]) = IOMapCmd.SyncTick;
  return (NO_ERR);
}

//
//cCmdWrapSetSleepTimeout
//ArgV[0]: (return) Status byte, SBYTE
//ArgV[1]: desired timer limit in ms, ULONG
//
NXT_STATUS cCmdWrapSetSleepTimeout(UBYTE * ArgV[])
{
  ULONG value = *(ULONG*)(ArgV[1]);
  if(value==0)
  {
    pMapUi->SleepTimeout=0;
  }
  else if(value < 60000)
  {
    pMapUi->SleepTimeout=1;  //integer math would've made this zero
  }
  else
  {
    pMapUi->SleepTimeout= value / 60000;
  }
  return (NO_ERR);
}

// currently copied from LS, not finished.
//
//cCmdWrapCommHSWrite
//ArgV[0]: (return) Status code, SBYTE
//ArgV[1]: Port specifier, UBYTE
//ArgV[2]: Buffer to send, UBYTE array, only SIZE_OF_LSBUF bytes will be used
//ArgV[3]: ResponseLength, UBYTE, specifies expected bytes back from slave device
//
NXT_STATUS cCmdWrapCommHSWrite(UBYTE * ArgV[])
{
  SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
  UBYTE Port = *(ArgV[1]);
  UBYTE * pBuf;
  UWORD BufLength;
  UBYTE ResponseLength = *(ArgV[3]);
  DV_INDEX DVIndex;

  //Resolve array arguments
  DVIndex = *(DV_INDEX *)(ArgV[2]);
  pBuf = cCmdDVPtr(DVIndex);
  BufLength = DV_ARRAY[DVIndex].Count;

  *pReturnVal = cCmdLSWrite(Port, (UBYTE)BufLength, pBuf, ResponseLength);

  return (NO_ERR);
}

//
//cCmdWrapCommHSCheckStatus
//ArgV[0]: (return) Status code, SBYTE
//ArgV[1]: Port specifier, UBYTE
//ArgV[2]: BytesReady, UBYTE
//
NXT_STATUS cCmdWrapCommHSCheckStatus(UBYTE * ArgV[])
{
  UBYTE Port = *(ArgV[1]);

  *((SBYTE*)(ArgV[0])) = cCmdLSCheckStatus(Port);
  *((UBYTE*)(ArgV[2])) = cCmdLSCalcBytesReady(Port);

  return (NO_ERR);
}

//
//cCmdWrapCommHSRead
//ArgV[0]: (return) Status code, SBYTE
//ArgV[1]: Port specifier, UBYTE
//ArgV[2]: Buffer for data, UBYTE array, max SIZE_OF_LSBUF bytes will be written
//ArgV[3]: BufferLength, UBYTE, specifies size of buffer requested
//
NXT_STATUS cCmdWrapCommHSRead(UBYTE * ArgV[])
{
  SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
  UBYTE Port = *(ArgV[1]);
  UBYTE * pBuf;
  UBYTE BufLength = *(ArgV[3]);
  UBYTE BytesToRead;
  DV_INDEX DVIndex = *(DV_INDEX *)(ArgV[2]);
  NXT_STATUS AllocStatus;

  *pReturnVal = cCmdLSCheckStatus(Port);
  BytesToRead = cCmdLSCalcBytesReady(Port);

  //If channel is OK and has data ready for us, put the data into outgoing buffer
  if (!IS_ERR(*pReturnVal) && BytesToRead > 0)
  {
    //Limit buffer to available data
    if (BufLength > BytesToRead)
      BufLength = BytesToRead;

    AllocStatus = cCmdDVArrayAlloc(DVIndex, BufLength);
    if (IS_ERR(AllocStatus))
      return (AllocStatus);

    pBuf = cCmdDVPtr(DVIndex);
    *pReturnVal = cCmdLSRead(Port, BufLength, pBuf);
  }
  //Else, the channel has an error and/or there's no data to read; clear the output array
  else
  {
    AllocStatus = cCmdDVArrayAlloc(DVIndex, 0);
    if (IS_ERR(AllocStatus))
      return (AllocStatus);
  }

  return (NO_ERR);
}

//
//cCmdWrapCommBTOnOff
//ArgV[0]: (return) Status byte, SBYTE
//ArgV[1]: Power State, 0-1
//
NXT_STATUS cCmdWrapCommBTOnOff(UBYTE * ArgV[])
{
  UWORD retVal;
  NXT_STATUS status;
  SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);

  UBYTE powerState = *(ArgV[1]);
  if(powerState)
    status= pMapComm->pFunc(BTON, 0, 0, 0, NULL, &retVal);
  else
    status= pMapComm->pFunc(BTOFF, 0, 0, 0, NULL, &retVal);

  *pReturnVal= (status == SUCCESS) ? retVal : status;
  return (NO_ERR);
}

//
//cCmdWrapCommBTConnection
//ArgV[0]: (return) Status byte, SBYTE
//ArgV[1]: Action, UBYTE
//ArgV[2]: name, UBYTE array CStr
//ArgV[3]: connection slot, UBYTE
//
NXT_STATUS cCmdWrapCommBTConnection(UBYTE * ArgV[])
{
  UWORD retVal;
  NXT_STATUS status;
  SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
  UBYTE *nmPtr;

  UBYTE action = *(ArgV[1]);
  UBYTE connection = *(ArgV[3]);
  nmPtr = cCmdDVPtr(*(DV_INDEX *)(ArgV[2]));

  if(action) // Init
    status= pMapComm->pFunc(CONNECTBYNAME, 0, connection, 0, nmPtr, &retVal);
  else // Close
    status= pMapComm->pFunc(DISCONNECT, connection, 0, 0, NULL, &retVal);

  *pReturnVal= (status == SUCCESS) ? retVal : status;
  return (NO_ERR);
}


//
//cCmdWrapReadSemData
//ArgV[0]: return data, U8
//ArgV[1]: which (0=used, 1=request), U8
//
NXT_STATUS cCmdWrapReadSemData(UBYTE * ArgV[])
{
  if(!(*((UBYTE *)ArgV[1])))
    *((UBYTE *)ArgV[0])= gUsageSemData;
  else
    *((UBYTE *)ArgV[0])= gRequestSemData;
  return (NO_ERR);
}

//
//cCmdWrapWriteSemData
//ArgV[0]: return data, U8
//ArgV[1]: which (0=used, 1=request), U8
//ArgV[2]: newValue, U8
//ArgV[3]: action (0= OR, 1= AND), U8
//
NXT_STATUS cCmdWrapWriteSemData(UBYTE * ArgV[])
{
  UBYTE curVal, newVal, which= (*((UBYTE *)ArgV[1]));
  if(!which)
    curVal= gUsageSemData;
  else
    curVal= gRequestSemData;

  newVal= *((UBYTE *)ArgV[2]);

  if(*((UBYTE *)ArgV[3]))
    curVal &= ~newVal;
  else
    curVal |= newVal;

  if(!which)
    gUsageSemData= curVal;
  else
    gRequestSemData= curVal;
  *((UBYTE *)ArgV[0])= curVal;
  return (NO_ERR);
}


//
//cCmdWrapUpdateCalibCacheInfo
//ArgV[0]: return data, U8
//ArgV[1]: nm, UBYTE array CStr
//ArgV[2]: min, U16
//ArgV[3]: max , U16
//
NXT_STATUS cCmdWrapUpdateCalibCacheInfo(UBYTE * ArgV[])
{
  UBYTE *nm= cCmdDVPtr(*(DV_INDEX *)(ArgV[1]));
  SWORD min= (*((SWORD *)ArgV[2]));
  SWORD max= (*((SWORD *)ArgV[3]));

  cCmdUpdateCalibrationCache(nm, min, max);
  *((UBYTE *)ArgV[0])= SUCCESS;
  return (NO_ERR);
}

//
//cCmdWrapComputeCalibValue
//ArgV[0]: return data, U8
//ArgV[1]: nm, UBYTE array CStr
//ArgV[2]: raw, U16 ref in out
NXT_STATUS cCmdWrapComputeCalibValue (UBYTE * ArgV[])
{
  UBYTE *nm= cCmdDVPtr(*(DV_INDEX *)(ArgV[1]));
  SWORD raw= (*((SWORD *)ArgV[2]));

  *((UBYTE *)ArgV[0])=  cCmdComputeCalibratedValue(nm, &raw);
  (*((SWORD *)ArgV[2]))= raw;
  return (NO_ERR);
}

typedef struct {
  SWORD min, max;
  UBYTE nm[FILENAME_LENGTH + 1];
} CalibCacheType;

SBYTE gCalibCacheCnt= 0;
DV_INDEX gCalibCacheArrayDVIdx= NOT_A_DS_ID;
CalibCacheType *gCalibCacheArray= NULL;

SWORD cCmdGetCalibrationIndex(UBYTE *nm) {
  SBYTE i;
  for(i= 0; i < gCalibCacheCnt; i++)
    if(!strcmp((PSZ)nm, (PSZ)gCalibCacheArray[i].nm))
      break;
  return i;
}

NXT_STATUS cCmdComputeCalibratedValue(UBYTE *nm, SWORD *pRaw) {
  SBYTE i= cCmdGetCalibrationIndex(nm);
  NXT_STATUS status= ERR_RC_ILLEGAL_VAL;
  SLONG raw= *pRaw, range;
  if(i < gCalibCacheCnt) {
    status= SUCCESS;
    raw -= gCalibCacheArray[i].min;
    range= (gCalibCacheArray[i].max - gCalibCacheArray[i].min);
  }
  else
    range= 1023;
  raw *= 100;
  raw /= range;
  if(raw < 0) raw= 0;
  else if(raw > 100) raw= 100;
  *pRaw= raw;
  return status;
}


NXT_STATUS ResizeCalibCache(ULONG elements) { // alloc dv if needed, grow if needed. dv never freed. on boot, set to NOT_A_DS_ID. use cnt for valid elements.
  NXT_STATUS Status = NO_ERR;

  if(gCalibCacheArrayDVIdx == NOT_A_DS_ID)
    Status = cCmdAllocDopeVector(&gCalibCacheArrayDVIdx, sizeof(CalibCacheType));
  if(!IS_ERR(Status) && DV_ARRAY[gCalibCacheArrayDVIdx].Count < elements) //Allocate storage for cache element
    Status = cCmdDVArrayAlloc(gCalibCacheArrayDVIdx, elements);
  if(!IS_ERR(Status))
    gCalibCacheArray= cCmdDVPtr(gCalibCacheArrayDVIdx);
  // on error, does old DVIdx still point to array, or should we null out array???
  return Status;
}

// called to update min/max on existing cache element, and to add new named element
void cCmdUpdateCalibrationCache(UBYTE *nm, SWORD min, SWORD max) {
  SWORD i= cCmdGetCalibrationIndex(nm);
  NXT_STATUS Status = NO_ERR;

  if(i == gCalibCacheCnt) { // sensor wasn't found, insert into cache
    Status= ResizeCalibCache(gCalibCacheCnt+1);
    if(!IS_ERR(Status)) {
      gCalibCacheCnt++;
      strcpy((PSZ)gCalibCacheArray[i].nm, (PSZ)nm);
    }
  }
  if(!IS_ERR(Status)) {
    gCalibCacheArray[i].min= min;
    gCalibCacheArray[i].max= max;
  }
}

void cCmdLoadCalibrationFiles(void) {
  ULONG cnt, DataSize;
  UBYTE nm[FILENAME_LENGTH + 1], nmLen;
  SWORD Handle, HandleSearch;
  gCalibCacheCnt= 0;
  gCalibCacheArrayDVIdx= NOT_A_DS_ID;
  // file I/O to load all .cal files into cached globals used by scaling syscall
  HandleSearch = pMapLoader->pFunc(FINDFIRST, (UBYTE *)"*.cal", nm, &cnt); // returns total files and nm of first one
  while (LOADER_ERR(HandleSearch) == SUCCESS) { // if we have a file, process it by closing and opening
    SWORD min= 0, max= 0, tmp;
    ULONG length;
    pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(HandleSearch), NULL, NULL);
    Handle = pMapLoader->pFunc(OPENREAD, nm, NULL, &DataSize);
    if (LOADER_ERR(Handle) == SUCCESS && DataSize == 4) {
      // access data, two bytes for min and two for max
      length= 2;
      pMapLoader->pFunc(READ,LOADER_HANDLE_P(Handle),(UBYTE*)&tmp,&length);
      if (length == 2)
        min= tmp;
      length= 2;
      pMapLoader->pFunc(READ,LOADER_HANDLE_P(Handle),(UBYTE*)&tmp,&length);
      if (length == 2)
        max= tmp;
    }
    pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(Handle), NULL, NULL);
    // update calibration cache with nm, min, and max
    nmLen= strlen((PSZ)nm) - 4; // chop off .cal extension
    nm[nmLen]= 0;
    cCmdUpdateCalibrationCache(nm, min, max);

    HandleSearch = pMapLoader->pFunc(FINDNEXT, LOADER_HANDLE_P(HandleSearch), nm, &cnt);
  }
  pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(HandleSearch), NULL, NULL);
}

//
//cCmdWrapListFiles
//ArgV[0]: return data, SBYTE
//ArgV[1]: pattern, UBYTE array CStr
//ArgV[2]: list, UBYTE array CStr array ref in out
NXT_STATUS cCmdWrapListFiles (UBYTE * ArgV[])
{
  ULONG fileSize, matchCount=0, i=0, oldCount;
  SWORD HandleSearch;
  NXT_STATUS Status = NO_ERR;
  DV_INDEX listIdx, *list;
  UBYTE *strTemp, *pattern;
  UBYTE name[FILENAME_LENGTH + 1];

  //Resolve array arguments
  pattern = cCmdDVPtr(*(DV_INDEX *)(ArgV[1]));
  listIdx = *(DV_INDEX *)(ArgV[2]);

  HandleSearch = pMapLoader->pFunc(FINDFIRST, pattern, name, &fileSize); // returns first file matching pattern

  //Count how many files we're going to have
  while (LOADER_ERR(HandleSearch) == SUCCESS)
  {
    matchCount++;
    pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(HandleSearch), NULL, NULL);
    HandleSearch = pMapLoader->pFunc(FINDNEXT, LOADER_HANDLE_P(HandleSearch), name, &fileSize);
  }

  HandleSearch = pMapLoader->pFunc(FINDFIRST, pattern, name, &fileSize); // returns first file matching pattern

  oldCount = DV_ARRAY[listIdx].Count;  // Check to see how many dope vectors are already in the array (if they passed us a non-blank array of strings)

  Status = cCmdDVArrayAlloc(listIdx, matchCount);  // Size the top-level array
  if(IS_ERR(Status))
     return Status;

  list = (DV_INDEX*)(VarsCmd.pDataspace + DV_ARRAY[listIdx].Offset); // Get a pointer into the dataspace for the array of DV_INDEXes

  while (LOADER_ERR(HandleSearch) == SUCCESS && !IS_ERR(Status))
  {
      pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(HandleSearch), NULL, NULL);   // Close the handle that we automatically opened above
      // Allocate a new dope vector if one doesn't already exist
      if(i >= oldCount)
        Status = cCmdAllocDopeVector(&(list[i]), sizeof(char));

      // Allocate the string buffer for output array[i]
      if(!IS_ERR(Status))
        Status = cCmdDVArrayAlloc(list[i], strlen((PSZ)name) + 1);

      if(!IS_ERR(Status))
      {
        strTemp = VarsCmd.pDataspace + DV_ARRAY[list[i]].Offset; // Get a pointer into the dataspace for this string
        strcpy((PSZ)strTemp, (PSZ)name);
      }
      i++;

      HandleSearch = pMapLoader->pFunc(FINDNEXT, LOADER_HANDLE_P(HandleSearch), name, &fileSize);
  }

  *(SBYTE *)(ArgV[0]) = Status;

  return Status;
}

#ifdef SIM_NXT
// Accessors for simulator library code
SWORD cCmdGetCodeWord(CLUMP_ID Clump, CODE_INDEX Index)
{
  if (Clump == NOT_A_CLUMP)
  {
    NXT_ASSERT(Index < VarsCmd.CodespaceCount);
    return (VarsCmd.pCodespace[Index]);
  }
  else
  {
    NXT_ASSERT(cCmdIsClumpIDSane(Clump));
#error   // CodeStart is now absolute, but not sure how to fix
    return (((SWORD)VarsCmd.pCodespace[VarsCmd.pAllClumps[Clump].CodeStart + Index]));
  }
}


UBYTE * cCmdGetDataspace(UWORD *DataspaceSize)
{
  if (DataspaceSize)
    *DataspaceSize = VarsCmd.DataspaceSize;
  return (VarsCmd.pDataspace);
}


DOPE_VECTOR * cCmdGetDopeVectorPtr()
{
  return VarsCmd.MemMgr.pDopeVectorArray;
}


MEM_MGR cCmdGetMemMgr(void)
{
  return VarsCmd.MemMgr;
}


ULONG cCmdGetPoolSize()
{
  return VarsCmd.PoolSize;
}
#endif

#else //!ENABLE_VM
//
//Implementations of standard interface if VM is disabled.
//Place low-level test code here if VM is causing issues.
//Test code must implement cCmdInit(), cCmdCtrl(), and cCmdExit() at a minimum.
//Recommend using a pattern like #include "c_cmd_alternate.c"
//

//!!! !ENABLE_VM implementations really should provide a placeholder function for this pointer
//IOMapCmd.pRCHandler = &cCmdHandleRemoteCommands;
#include "c_cmd_alternate.c"

#endif //ENABLE_VM
