/// <summary>
        /// Send I2C Command and post a response when it is completed.
        /// </summary>
        /// <param name="cmd"></param>
        /// <returns></returns>
        public virtual IEnumerator<ITask> I2cLowSpeedCommandHandler(SendLowSpeedCommand cmd)
        {
            bool done = false;
            bool retry = false;
            cmd.Body.RequireResponse = false;
            cmd.Body.TryCount = 1;
            yield return Arbiter.Choice(_brickPort.SendCommand(cmd.Body),
                delegate(LegoResponse rsp)
                {
                    if (cmd.Body.ExpectedI2CResponseSize == 0)
                    {
                        cmd.ResponsePort.Post(new LegoResponseLSRead());
                        done = true;
                    }
                },
                delegate(Fault fault)
                {
                    cmd.ResponsePort.Post(fault);
                    done = true;
                });

            if (done)
                yield break;

            // Call LSStatus until the data is ready
            _cmdLSGetStatus.Port = cmd.Body.Port;
            _cmdLSGetStatus.TryCount = 1;
            int lsBytesReady = 0;
            int statusReadCount = 0;
            do
            {
                //Debugger.Break();
                yield return Arbiter.Choice(_brickPort.SendCommand(_cmdLSGetStatus),
                    delegate(LegoResponse rsp)
                    {
                        LegoResponseLSGetStatus lsStatus = rsp as LegoResponseLSGetStatus;
                        if (lsStatus == null)
                        {
                            ////Debugger.Break();
                            lsStatus = new LegoResponseLSGetStatus(rsp.CommandData);
                        }

                        switch (lsStatus.ErrorCode)
                        {
                            case LegoErrorCode.Success:
                                lsBytesReady = lsStatus.BytesReady;
                                break;
                            case LegoErrorCode.PendingCommunicationTransactionInProgress:
                                // Just try status again
                                break;
                            case LegoErrorCode.CommunicationBusError:
                                retry = true;
                                break;
                            default:
                                cmd.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Lego Error Code: " + lsStatus.ErrorCode)));
                                done = true;
                                break;
                        }

                    },
                    delegate(Fault fault)
                    {
                        cmd.ResponsePort.Post(fault);
                        done = true;
                    });

                if (retry)
                {
                    retry = false;

                    // Reset the I2C port by reading 1 byte.
                    LegoLSWrite resetI2C = new I2CReadSonarSensor(cmd.Body.Port, UltraSonicPacket.FactoryZero);
                    //LegoLSWrite resetI2C = new I2CReadSonarSensor(cmd.Body.Port, UltraSonicPacket.ByteEight);
                    SendLowSpeedCommand sendReset = new SendLowSpeedCommand(resetI2C);
                    if (cmd.Body.ExpectedI2CResponseSize == 1 && NxtCommon.ByteArrayIsEqual(cmd.Body.CommandData, resetI2C.CommandData))
                    {
                        // we received a BusError on the reset command.
                        cmd.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Lego Error Code: " + LegoErrorCode.CommunicationBusError)));
                        done = true;
                    }
                    else
                    {
                        LogInfo(LogGroups.Console, "Resetting the I2C Bus on " + cmd.Body.Port.ToString());
                        SpawnIterator<SendLowSpeedCommand>(sendReset, I2cLowSpeedCommandHandler);
                        yield return Arbiter.Choice(sendReset.ResponsePort,
                            delegate(LegoResponse ok)
                            {
                                // try again
                                lsBytesReady = 0;
                            },
                            delegate(Fault fault)
                            {
                                cmd.ResponsePort.Post(fault);
                                done = true;
                            });
                    }
                }

                if (done)
                    yield break;

            } while (lsBytesReady < cmd.Body.ExpectedI2CResponseSize &&  (statusReadCount++ < 10));
            // Call LSRead to get the return packet
            LegoLSRead lsRead = new LegoLSRead(cmd.Body.Port);
            LegoResponse response = null;
            Fault lsReadFault = null;
            //Debugger.Break();
            while (lsBytesReady > 0)
            {
                yield return Arbiter.Choice(
                    _brickPort.SendCommand(lsRead),
                    delegate(LegoResponse rsp) { response = rsp; },
                    delegate(Fault fault) { lsReadFault = fault; });

                lsBytesReady -= cmd.Body.ExpectedI2CResponseSize;
            }
            //Debugger.Break();
            //if (response != null && lsBytesReady == 0 && response.CommandData != null && response.CommandData.Length >= 3 && response.CommandData[2] == 0)
            if (response != null && response.CommandData != null && response.CommandData.Length >= 3 && response.CommandData[2] == 0)
                cmd.ResponsePort.Post(response);
            else if (lsReadFault != null)
                cmd.ResponsePort.Post(lsReadFault);
            else
                cmd.ResponsePort.Post(Fault.FromException(new InvalidOperationException("I2C Communication Error")));

            yield break;
        }
 public virtual IEnumerator<ITask> SendLowSpeedCommandHandler(SendLowSpeedCommand cmd)
 {
     _internalI2CPort.Post(cmd);
     yield break;
 }
        /// <summary>
        /// Test a Sensor Port for an I2C Sensor
        /// </summary>
        /// <param name="response"></param>
        /// <param name="sensorPort"></param>
        /// <param name="sensorType"></param>
        /// <returns></returns>
        private IEnumerator<ITask> TestPortForI2CSensorHandler(Port<bool> response, LegoNxtPort sensorPort, string sensorType)
        {
            LogInfo("- TestPortForI2CSensorHandler");
            //Debugger.Break();
            // Read from I2C to find the device.
            bool found = false;
            bool abort = false;
            LegoSetInputMode setInputMode = null;

            if (_brickPort != null)
            {
                // Configure the port as an I2C sensor.
                setInputMode = new LegoSetInputMode((NxtSensorPort)sensorPort, LegoSensorType.I2C_9V, LegoSensorMode.RawMode);
                setInputMode.TryCount = 2;
                yield return Arbiter.Choice(_brickPort.SendCommand(setInputMode),
                    delegate(LegoResponse ok)
                    {
                        //Debugger.Break();
                        if (!ok.Success)
                            abort = true;
                        else
                            LogInfo(LogGroups.Console, string.Format("{0} set to {1} mode.", sensorPort, setInputMode.SensorType));

                    },
                    delegate(Fault fault)
                    {
                        abort = true;
                    });
            }
            else
                abort = true;

            if (abort)
            {
                LogInfo(LogGroups.Console, string.Format("Failure setting I2C mode on {0}.", sensorPort));
                response.Post(false);
                yield break;
            }

            SendLowSpeedCommand lsCmd = new SendLowSpeedCommand();
            I2CReadSensorType readSensors = new I2CReadSensorType((NxtSensorPort)sensorPort);

            LogInfo("The sensor port is: " + sensorPort.ToString());
            lsCmd.Body = readSensors;
            SpawnIterator<SendLowSpeedCommand>(lsCmd, SendLowSpeedCommandHandler);
            yield return Arbiter.Choice(lsCmd.ResponsePort,
                delegate(LegoResponse ok)
                {
                    //Debugger.Break();
                    I2CResponseSensorType sensorResponse = LegoResponse.Upcast<I2CResponseSensorType>(ok);
                    LogInfo(LogGroups.Console, string.Format("{0} I2C response {1} is {2}.", sensorPort, sensorResponse.ErrorCode, sensorResponse.SensorType));

                    if (sensorResponse.SensorType.Equals(sensorType, StringComparison.InvariantCultureIgnoreCase))
                    {
                        found = true;
                    }
                    else if (sensorType.IndexOf(',') >= 0)
                    {
                        foreach (string subtype in sensorType.Split(','))
                        {
                            if (sensorResponse.SensorType.Equals(subtype, StringComparison.InvariantCultureIgnoreCase))
                                found = true;
                        }
                    }
                    else
                    {
                        LogInfo(LogGroups.Console, string.Format("Found an unattached I2C Sensor from {0}: {1}", sensorResponse.Manufacturer, sensorResponse.SensorType));
                    }
                },
                delegate(Fault fault)
                {
                    string msg = (fault.Reason != null && fault.Reason.Length > 0 && !string.IsNullOrEmpty(fault.Reason[0].Value)) ? fault.Reason[0].Value : string.Empty;
                    LogError(LogGroups.Console, string.Format("{0} fault reading I2C Sensor Type: {1}.", sensorPort, msg));
                    abort = true;
                });

            if (!found)
            {
                LogInfo(LogGroups.Console, string.Format("Set {0} back to No Sensor.", sensorPort));

                setInputMode = new LegoSetInputMode((NxtSensorPort)sensorPort, LegoSensorType.NoSensor, LegoSensorMode.RawMode);
                yield return Arbiter.Choice(_brickPort.SendCommand(setInputMode),
                    delegate(LegoResponse ok)
                    {
                        if (!ok.Success)
                            abort = true;
                    },
                    delegate(Fault fault)
                    {
                        abort = true;
                    });

            }

            LogInfo(LogGroups.Console, string.Format("I2C ReadSensorType on {0} finished: {1}", sensorPort, found));

            response.Post(found);
            yield break;
        }
        /// <summary>
        /// Send Any Command, including a two-phase LowSpeed command (LSWrite)
        /// </summary>
        /// <param name="cmd"></param>
        /// <param name="priority"></param>
        /// <returns></returns>
        public PortSet<LegoResponse, Fault> SendAnyCommand(LegoCommand cmd, bool priority)
        {
            if (cmd.LegoCommandCode == LegoCommandCode.LSWrite)
            {
                LegoLSWrite cmdLsWrite = cmd as LegoLSWrite;
                if (cmdLsWrite == null)
                    cmdLsWrite = new LegoLSWrite(cmd);

                // Send Low Speed Command
                SendLowSpeedCommand sendCmd = new SendLowSpeedCommand(cmdLsWrite);
                _internalI2CPort.Post(sendCmd);
                return sendCmd.ResponsePort;
            }
            return _brickPort.SendCommand(cmd, priority);
        }