private void cancelCurrentOperation()
        {
            switch (_internalPendingDriveOperation)
            {
                case drive.DriveRequestOperation.DriveDistance:
                    {
            #if TRACELOG
                        Tracer.Trace("cancelCurrentOperation() - DriveDistance - completionPort.ItemCount=" + completionPort.ItemCount);
            #endif

                        if (pendingDriveDistance != null)
                        {
                            EncoderTicksEnabled = false;
                            // send notification to subscription manager
                            pendingDriveDistance.Body.DriveDistanceStage = drive.DriveStage.Canceled;
                            double distanceTraveled = pendingDriveDistance.Body.Distance / 2.0d;        // TODO: need to compute real distance traveled
                            pendingDriveDistance.Body.Distance = distanceTraveled;
                            SendNotification<drive.DriveDistance>(_subMgrPort, pendingDriveDistance);
                            pendingDriveDistance = null;
                        }
                    }
                    break;

                case drive.DriveRequestOperation.RotateDegrees:
                    {
            #if TRACELOG
                        Tracer.Trace("cancelCurrentOperation() - RotateDegrees - completionPort.ItemCount=" + completionPort.ItemCount);
            #endif

                        if (pendingRotateDegrees != null)
                        {
                            EncoderTicksEnabled = false;
                            // send notification to subscription manager
                            pendingRotateDegrees.Body.RotateDegreesStage = drive.DriveStage.Canceled;
                            double angleRotated = pendingRotateDegrees.Body.Degrees / 2.0d;             // TODO: need to compute real angle rotated
                            pendingRotateDegrees.Body.Degrees = angleRotated;
                            SendNotification<drive.RotateDegrees>(_subMgrPort, pendingRotateDegrees);
                            pendingRotateDegrees = null;
                        }
                    }
                    break;

                case drive.DriveRequestOperation.NotSpecified:
                    break;

                default:
                    Tracer.Trace("Warning: cancelCurrentOperation() - no pending wait-type operation to cancel - current operation = " + _internalPendingDriveOperation);
                    break;
            }
            _internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
        }
        /// <summary>
        /// Internal rotate degrees handler
        /// </summary>
        /// <param name="rotateDegrees"></param>
        /// <returns></returns>
        public virtual IEnumerator<ITask> InternalRotateDegreesHandler(drive.RotateDegrees rotateDegrees)
        {
            #if TRACELOG
            Tracer.Trace("InternalRotateDegreesHandler() - DriveStage." + rotateDegrees.Body.RotateDegreesStage);
            #endif

            switch (rotateDegrees.Body.RotateDegreesStage)
            {
                case drive.DriveStage.InitialRequest:
                    cancelCurrentOperation();
                    pendingRotateDegrees = rotateDegrees;
                    // _state.InternalPendingDriveOperation = drive.DriveRequestOperation.RotateDegrees; - not available in Proxy
                    _internalPendingDriveOperation = drive.DriveRequestOperation.RotateDegrees;
                    SpawnIterator<double, double>(rotateDegrees.Body.Degrees, rotateDegrees.Body.Power, RotateUntilDegrees);
                    break;

                case drive.DriveStage.Started:
                    SendNotification<drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
                    break;

                case drive.DriveStage.Completed:
                    pendingRotateDegrees = null;
                    // _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; - not available in Proxy
                    _internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
                    SendNotification<drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
                    break;

                case drive.DriveStage.Canceled:
                    pendingRotateDegrees = null;
                    // _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; - not available in Proxy
                    _internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
                    SendNotification<drive.RotateDegrees>(_subMgrPort, rotateDegrees.Body);
                    break;
            }

            yield break;
        }
        /// <summary>
        /// Service Startup Handler
        /// </summary>
        protected override void Start()
        {
            LogInfo("TrackRoamerDriveService:: Start() ");

            InitState();

            _internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;

            // send configuration commands to partner services
            SpawnIterator(ConfigureDrive);

            _state.TimeStamp = DateTime.Now;

            //needed for HttpPost
            _httpUtilities = DsspHttpUtilitiesService.Create(Environment);

            base.Start();

            // Interleave to manage internal drive operations (driveDistance and RotateDegrees)
            Activate(
                new Interleave(
                    new ExclusiveReceiverGroup(
                                    Arbiter.ReceiveWithIterator(true, this.encodersPollingPort, this.PollEncoders),
                                    Arbiter.ReceiveWithIteratorFromPortSet<drive.DriveDistance>(true, _internalDriveOperationsPort, InternalDriveDistanceHandler),
                                    Arbiter.ReceiveWithIteratorFromPortSet<drive.RotateDegrees>(true, _internalDriveOperationsPort, InternalRotateDegreesHandler)
                    ),
                    new ConcurrentReceiverGroup())
                   );
        }
        /// <summary>
        /// Internal drive distance operation handler
        /// </summary>
        /// <param name="driveDistance"></param>
        /// <returns></returns>
        public virtual IEnumerator<ITask> InternalDriveDistanceHandler(drive.DriveDistance driveDistance)
        {
            #if TRACELOG
            Tracer.Trace("InternalDriveDistanceHandler() - DriveStage." + driveDistance.Body.DriveDistanceStage);
            #endif

            switch (driveDistance.Body.DriveDistanceStage)
            {
                case drive.DriveStage.InitialRequest:
                    cancelCurrentOperation();
                    pendingDriveDistance = driveDistance;   // we will need it for possible cancelation
                    // _state.InternalPendingDriveOperation = drive.DriveRequestOperation.DriveDistance; - not available in Proxy
                    _internalPendingDriveOperation = drive.DriveRequestOperation.DriveDistance;
                    SpawnIterator<double, double>(driveDistance.Body.Distance, driveDistance.Body.Power, DriveUntilDistance);
                    break;

                case drive.DriveStage.Started:
                    SendNotification<drive.DriveDistance>(_subMgrPort, driveDistance.Body);
                    break;

                case drive.DriveStage.Completed:
                    pendingDriveDistance = null;
                    // _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; - not available in Proxy
                    _internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
                    SendNotification<drive.DriveDistance>(_subMgrPort, driveDistance.Body);
                    break;

                case drive.DriveStage.Canceled:
                    pendingDriveDistance = null;
                    // _state.InternalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified; - not available in Proxy
                    _internalPendingDriveOperation = drive.DriveRequestOperation.NotSpecified;
                    SendNotification<drive.DriveDistance>(_subMgrPort, driveDistance.Body);
                    break;
            }

            yield break;
        }
        /// <summary>
        /// Notified subscribers a success or fault to completed Drive request.
        /// </summary>
        /// <param name="driveDistance"></param>
        /// <param name="success"></param>
        /// <param name="fault"></param>
        public void HandleDriveResponseForGenericOperationsNotifications(DriveDistance driveDistance, bool success, Fault fault)
        {
            if (fault == null)
            {
                if (driveDistance.Body.isGenericOperation == true)
                {
                    //notify subscribers of generic drive distance -- complete

                    switch (driveDistance.Body.DriveRequestOperation)
                    {
                        case pxdrive.DriveRequestOperation.DriveDistance:
                            pxdrive.DriveDistanceRequest driveDistanceRequest = new pxdrive.DriveDistanceRequest();
                            driveDistanceRequest.DriveDistanceStage = pxdrive.DriveStage.Completed;

                            pxdrive.DriveDistance driveDistanceUpdate = new pxdrive.DriveDistance(driveDistanceRequest);
                            SendNotification<pxdrive.DriveDistance>(_genericSubMgrPort, driveDistanceUpdate);
                            break;

                        case pxdrive.DriveRequestOperation.RotateDegrees:
                            pxdrive.RotateDegreesRequest rotateDegreesRequest = new pxdrive.RotateDegreesRequest();
                            rotateDegreesRequest.RotateDegreesStage = pxdrive.DriveStage.Completed;

                            pxdrive.RotateDegrees rotateDegreesUpdate = new pxdrive.RotateDegrees(rotateDegreesRequest);
                            SendNotification<pxdrive.RotateDegrees>(_genericSubMgrPort, rotateDegreesUpdate);
                            break;
                    }
                }
                else
                {
                    // Operation canceled.
                    driveDistance.Body.DriveDistanceStage = pxdrive.DriveStage.Canceled;
                    SendNotification<pxdrive.SetDrivePower>(_genericSubMgrPort, driveDistance.Body);
                }
                _internalPendingDriveOperation = pxdrive.DriveRequestOperation.NotSpecified;
            }
        }
        public virtual IEnumerator<ITask> GenericRotateDegreesHandler(pxdrive.RotateDegrees rotateDegrees)
        {
            if (_state.DistanceBetweenWheels <= 0)
            {
                rotateDegrees.ResponsePort.Post(Fault.FromException(new ArgumentOutOfRangeException("DistanceBetweenWheels must be specified in the Drive Configuration.")));
                yield break;
            }

            // set back response immediately or fault if drive is not enabled.
            ValidateDriveEnabledAndRespondHelper(rotateDegrees.ResponsePort);

            // distance = circumference / 360 * degreesToTurn
            double distance = Math.PI * _state.DistanceBetweenWheels * rotateDegrees.Body.Degrees / 360.0;

            // axleRotationDegrees = distance (meters) / wheelCircumference (pi * diameter) * 360
            long axleRotationDegrees = (long)Math.Round(Math.Abs(distance) / (Math.PI * _state.LeftWheel.WheelDiameter) * 360.0);

            LogVerbose(LogGroups.Console, "RotateDegrees: Wheel Distance: " + distance.ToString() + "  Axle Rotation Degrees: " + axleRotationDegrees.ToString());

            double leftDirection = 1.0, rightDirection = 1.0;
            if (rotateDegrees.Body.Degrees > 0)
                leftDirection = -1.0;
            else
                rightDirection = -1.0;

            DriveDistance drive = new DriveDistance(new SetDriveRequest());
            drive.Body.LeftStopAtRotationDegrees = axleRotationDegrees;
            drive.Body.RightStopAtRotationDegrees = axleRotationDegrees;
            drive.Body.StopState = MotorStopState.Brake;
            drive.Body.isGenericOperation = true;
            drive.Body.DriveRequestOperation = pxdrive.DriveRequestOperation.RotateDegrees;
            _internalPendingDriveOperation = pxdrive.DriveRequestOperation.RotateDegrees;
            bool synchronized = false;

            if (synchronized)
            {
                drive.Body.LeftPower = Math.Abs(rotateDegrees.Body.Power);
                drive.Body.RightPower = Math.Abs(rotateDegrees.Body.Power);
            }
            else
            {
                drive.Body.LeftPower = rotateDegrees.Body.Power * leftDirection;
                drive.Body.RightPower = rotateDegrees.Body.Power * rightDirection;
            }
            drive.ResponsePort = rotateDegrees.ResponsePort;

            // notify subscribers of rotate degrees start
            rotateDegrees.Body.RotateDegreesStage = pxdrive.DriveStage.Started;
            pxdrive.RotateDegrees rotateDegreesUpdate = new pxdrive.RotateDegrees(rotateDegrees.Body);
            SendNotification<pxdrive.RotateDegrees>(_genericSubMgrPort, rotateDegreesUpdate);

            _internalDrivePowerPort.Post(drive);
            yield break;
        }
        public virtual IEnumerator<ITask> GenericDriveDistanceHandler(pxdrive.DriveDistance driveDistance)
        {
            double distance = driveDistance.Body.Distance;

            // set back response immediately or fault if drive is not enabled.
            ValidateDriveEnabledAndRespondHelper(driveDistance.ResponsePort);

            // rotations = distance (meters) / circumference (pi * diameter)
            // degrees = rotations * 360
            double stopLeftWheelAtDegrees = Math.Round(Math.Abs(distance) / (Math.PI * _state.LeftWheel.WheelDiameter) * 360.0);
            double stopRightWheelAtDegrees = Math.Round(Math.Abs(distance) / (Math.PI * _state.RightWheel.WheelDiameter) * 360.0);

            DriveDistance drive = new DriveDistance(new SetDriveRequest());
            drive.Body.LeftPower = driveDistance.Body.Power;
            drive.Body.RightPower = driveDistance.Body.Power;
            drive.Body.LeftStopAtRotationDegrees = (long)stopLeftWheelAtDegrees;
            drive.Body.RightStopAtRotationDegrees = (long)stopRightWheelAtDegrees;
            drive.Body.StopState = MotorStopState.Brake;
            drive.Body.isGenericOperation = true;
            drive.Body.DriveRequestOperation = pxdrive.DriveRequestOperation.DriveDistance;
            _internalPendingDriveOperation = pxdrive.DriveRequestOperation.DriveDistance;
            drive.ResponsePort = driveDistance.ResponsePort;

            // notify subscribers of drive distance start
            driveDistance.Body.DriveDistanceStage = pxdrive.DriveStage.Started;
            pxdrive.DriveDistance driveDistanceUpdate = new pxdrive.DriveDistance(driveDistance.Body);
            SendNotification<pxdrive.DriveDistance>(_genericSubMgrPort, driveDistanceUpdate);

            _internalDrivePowerPort.Post(drive);
            yield break;
        }
        /// <summary>
        /// Initialize and validate startup DriveState
        /// </summary>
        private void InitializeState()
        {
            if (_state == null)
                _state = new DriveState();

            if (_state.LeftWheel == null)
                _state.LeftWheel = new WheelConfiguration();

            if (_state.RightWheel == null)
                _state.RightWheel = new WheelConfiguration();

            if (_state.PollingFrequencyMs == 0)
                _state.PollingFrequencyMs = Contract.DefaultPollingFrequencyMs;

            if (_state.LeftWheel.WheelDiameter == 0.0)
                _state.LeftWheel.WheelDiameter = 0.055;

            if (_state.RightWheel.WheelDiameter == 0.0)
                _state.RightWheel.WheelDiameter = 0.055;

            // Always initialize the runtime statistics when we start.
            _state.RuntimeStatistics = new RuntimeStatistics();

            _targetEncoderReachedPort[LEFT] = new Port<bool>();
            _targetEncoderReachedPort[RIGHT] = new Port<bool>();

            _state.Connected = false;

            _genericState.IsEnabled = true;
            _internalPendingDriveOperation = pxdrive.DriveRequestOperation.NotSpecified;
        }