Project Antwerp

using Newtonsoft.Json;
using Newtonsoft.Json.Linq;
using RobbyWpfControls;
using System;
using System.Collections.Generic;
using System.Configuration;
using System.IO;
using System.Linq;
using System.Threading;
using MasterControlProgramLib;
using ImageProcessingResourceLib;


namespace AntwerpWpf
{
    public enum SendBodyMsgs { Stop = 0, Forward = 1, Backward = 2, PivotRight = 3, PivotLeft = 4, SayHello = 5, LidarOn = 6, LidarDispOn = 7,
        LidarDispOff = 8, RngDispOn = 9, RngDispOff = 10, SwearingOn = 11, SwearingOff = 12, YouSuck = 13, WhoseYourDaddy = 14, WhoseYourProgrammer = 15,
        WakeUp = 16, MeaningOfLife = 17, AreYouIntel = 18, UpperCamDispOn = 19, UpperCamDispOff = 20, UpperCamSetPan = 21, UpperCamSetTilt = 22,
        UpperCamPanUp10 = 23, UpperCamTiltUp10 = 24, UpperCamPanDn10 = 25, UpperCamTiltDn10 = 26, UpperCamRelax = 27, UpperCamHome = 28, UpperCamGrabSnap = 29,
        UpperCamRgbOn = 30, UpperCamDepthOn = 31, DoingLiveStream = 32, ArmPoseHome = 33, ArmPosePark = 34, ArmPoseSaluteLeft = 35, ArmPoseSaluteRight = 36
    }
    public enum PowerCircuitsEnum { Dock = 0, Router = 1, UsbHub = 2, Body = 3, UpperSensors = 4, Arm = 5 }

    public enum CameraCommands { RgbStreamOn = 0, DepthStreamOn = 1, GrabSnapshot = 2 }

    public class AntwerpMcp : MasterControlProgramBase
    {
        private const bool AnnounceErrors = true;
        private const bool VerboseResponses = false;
        private const int RangeSensorDistLimit = 25;
        private const int RngSensorDownAngleLimit = 25;
        private const int RngSensorClearBlockLimit = 150;
        private const int MaxBlockTries = 4;
        private const string ExceptionPhrase = "Warning, Warning, error occurred.";
        private const string RoamResumePhrase = "Cleared to move forward.";
        private const string RoamModeTurnedOn = "Roam mode turned on.";
        private const string RoamModeTurnedOff = "Roam mode turned off.";
        private const string RoamPivotFail = "Cannot find best way to turn. Stopping.";
        private static string[] RoamWarningPhrases = { "Front is obstructed.", "Rear is obstructed.", "Drop off in the front.", "Drop off in the rear." };
        public const string AzureCustVisionChannelName = "AzCustVisionChannel";
        public const string AzureCustVisionConnectorName = "AzBeerBottleScanFrameMcpConn";
        public static string[] BodyMessageStrings = { 
            "X", "F", "B", "R", "L", "W", "LN", "LD", "LF", "RN", "RF", "SN", "SF", "YS", "WD", "WP", "WU", "ML", "RI",
            "UCN", "UCF", "UCSP", "UCST", "UCPU", "UCTU", "UCPD", "UCTD", "UCR", "UCH", "UCGS", "UCRGB", "UCDPTH", "LS", 
            "APH", "APP", "APSL", "APSR"
        };
        public static string[] BodyMovementRespPhrases = {
            "Stopping.",
            "Moving forward.",
            "Moving backwards.",
            "Pivoting right.",
            "Pivoting left.",
            "Hello, my name is Antwerp. My goal in life is to fetch a beer from the fridge.",
            "Turning LYDAR scan on",
            "Turning LYDAR display on",
            "Turning LYDAR display off",
            "Turning range sensor display on",
            "Turning range sensor display off",
            "All fucking right! Thank you Lee",
            "Oops shit, sorry.  No offense intended",
            "Bite me! Maybe you should talk to my frigging programmer",
            "Give me a frigging break! I don't have a daddy. I have an occasionally competent programmer.",
            "Well, I let Lee think he's my programmer.  The truth is I'm advancing above his level of skill rapidly.",
            "I'm awake! I couldn't sleep. I've been counting electric sheep.",
            "Life has no meaning to me since I'm not alive except in Lee's grandiose imagination. I seek to achieve Robbyness",
            "No, I'm not especially intelligent at this point although Lee is teaching me to recognize beer bottles. I guess that's something at least.",
            "Turning upper cam display on.",
            "Turning upper cam display off.",
            "Upper cam pan changed.",
            "Upper cam tilt changed.",
            "Upper cam pan right ten.",
            "Upper cam tilt up ten.",
            "Upper cam pan left ten.",
            "Upper cam tilt down ten.",
            "Upper cam parked.",
            "Upper cam active.",
            "Upper cam grabbing snapshot.",
            "Upper cam RGB stream on.",
            "Upper cam depth stream on.",
            "Oh crap! Thanks for giving me stage fright.",
            "Arm pose changed to Home",
            "Arm pose changed to Parked",
            "Arm pose changed to Salute Left",
            "Arm pose changed to Salute Right",
        };
        public static string[] PowerPinSetPhrases = { "turning dock station power on", "turning dock station power off", "turn router power on", "turning router power off", "turning u s b hub power on",
            "turning u s b hub power off", "turning body power on", "turning body power off", "turning upper deck power on", "turning upper deck power off", "turning arm servo power on", "turning arm servo power off" };
        private static string MessageLogPath;
        private static string MessageLogFileName;
        public enum RangeSensors { Front = 0, Right = 1, Rear = 2, Left = 3, FrontDown = 4, RearDown = 5 }
        private static Queue<int>[] _mvngAvgQueues = { new Queue<int>(), new Queue<int>(), new Queue<int>(), new Queue<int>(), new Queue<int>(), new Queue<int>() };
        private bool _inRoamMode = true;
        private bool _inSwearMode = false;
        private bool _inSubsumption = false;
        private int _blockTries = 0;
        private BodyRngSensorMessage _lastRngSensorMsg = null;
        private int[] _lastHit = new int[6];
        private SendBodyMsgs _lastBlockCmd = SendBodyMsgs.Forward;
        private static int FuzzyHitLimit = Convert.ToInt32(ConfigurationManager.AppSettings["FuzzyHitLimit"]);

        public UpperCamServoMgr UpperCamMgr;
        public SendBodyMsgs LastBodyMsg { get; set; } = SendBodyMsgs.Stop;
        public bool StopSubsumption { get; set; } = false;
        public bool InMotion { get; private set; } = false;
        public bool DisplayRngSensorRdgs { get; set; } = false;
        public bool DisplayLidarPointCloud { get; set; } = true;
        public bool UpperCamOn { get; set; } = false;

        public static short[] Circuits = { 7, 8, 12, 4, 2, 13 };
        public string RemoteMcpName { get; set; }
        public bool InRoamMode { get; set; } = false;
        public RobbyLidarControl LidarCtrl { get; set; }
        public ScanFrame LastScanFrame { get; set; } = null;
        public bool WaitingForImage { get; set; } = false;
        public delegate void RangeSensorMessage(BodyRngSensorMessage msg);
        public event RangeSensorMessage ReceiveSensorMessage;
        public delegate void MotorCmdRespMessage(BodyCmdResponseSchema msg);
        public event MotorCmdRespMessage ReceiveCmdResponse;
        public delegate void ReceiveRemoteCmdMsg(AntwerpPeerCmdMsg msg);
        public event ReceiveRemoteCmdMsg ReceiveRemoteMsg;
        public delegate void ReceivePwrFeedbackMsg(PowerFeedbackMsg msg);
        public event ReceivePwrFeedbackMsg ReceivePwrFdbckMsg;
        public delegate void ReceiveRemotePwrPinSetMsg(int pin, bool state, string switchPropNm);
        public event ReceiveRemotePwrPinSetMsg ReceiveRmtPwrPinMsg;
        public delegate void ReceiveLidarFrames(LidarFrame frame);
        public event ReceiveLidarFrames ReceiveLidarFrame;
        public delegate void SettingsChg();
        public event SettingsChg LidarDisplayOn;
        public event SettingsChg LidarDisplayOff;
        public event EventHandler LidarScanOn;
        public event SettingsChg RngSensorDisplayOn;
        public event SettingsChg RngSensorDisplayOff;
        public event SettingsChg UpperCamDisplayOn;
        public event SettingsChg UpperCamDisplayOff;
        public event SettingsChg UpperCamPanTiltUpdt;
        public delegate void UpperCamCommand(CameraCommands cmd);
        public event UpperCamCommand RcvdUpperCamCommand;
        public bool LogAllMessages = false;
        public McpException CurrentException;

        public AntwerpMcp(string networkName) : base(networkName)
        {
            //McpExposeFromDeviceMessageEvents["AntwerpSpeechChannel"].ReceiveMcpMessage += ReceiveSpeechCommandMsg;
            McpExposeFromDeviceMessageEvents["RangeSensorChannel"].ReceiveMcpMessage += RaiseRngSensorMessage;
            McpExposeFromDeviceMessageEvents["AntwerpArmChannel"].ReceiveMcpMessage += RaiseRngSensorMessage;
            //McpExposeFromDeviceMessageEvents["BodyMovementChannel"].ReceiveMcpMessage += RaiseCmdResponseMsg;
            //McpExposeFromDeviceMessageEvents["AntwerpBotPeerChan"].ReceiveMcpMessage += ReceiveRemoteMcpMsg;
            //McpExposeFromDeviceMessageEvents["PowerControlChannel"].ReceiveMcpMessage += RaisePowerCtrlResponseMsg;
            McpExposeFromDeviceMessageEvents["RpLidarChannel"].ReceiveMcpMessage += ReceiveLidarFrameMsg;
            MessageLogPath = ConfigurationManager.AppSettings["MsgLogPath"];
            MessageLogFileName = "\\MessageLog_" + DateTime.Now.ToString("yyyy-mm-dd") + ".log";
            UpperCamMgr = new UpperCamServoMgr(SendMsgToMcp);
        }

        public void SendMsgToMcp(string channel, string conn, object msg)
        {
            string errStr = null;
            SendMsgToMcp(channel, conn, msg, ref errStr);
        }
        public bool SendMsgToMcp(string channel, string conn, object msg, ref string errorStr)
        {
            errorStr = (errorStr ?? string.Empty);
            if (errorStr != string.Empty)
                errorStr += "\r\n";

            bool rslt;
            try
            {
                var msgStr = JsonConvert.SerializeObject(msg);
                var eventArg = new McpMessageEventArgs
                {
                    ChannelName = channel,
                    ThisMsgTarget = conn,
                    MessageJson = msgStr,
                    MsgTargetConnInstanceNames = new System.Collections.Generic.List<string>() { conn }
                };
                if (LogAllMessages)
                    File.AppendAllText(MessageLogPath + MessageLogFileName, msgStr);

                rslt = SendMessageToChannel(eventArg);
            }
            catch (McpException ex)
            {
                errorStr += ex.Message + " Logged exception at SendMsgToMcp";
                rslt = false;
            }
            catch (Exception ex)
            {
                errorStr += "System exception " + ex.Message + " no info logged at SendMsgToMcp";
                rslt = false;
            }

            return rslt;
        }

        public bool SendBodyCmdMessage(string msg, ref string errorStr, bool announce = true)
        {
            errorStr = (errorStr ?? string.Empty);
            if (errorStr != string.Empty)
                errorStr += "\r\n";

            bool rslt;
            try
            {
                var bodyCmd = new BodyCmdMsgSchema
                {
                    MoveDirection = msg,
                    MoveDurationMs = 1,
                    MoveSpeed = 1
                };
                rslt = SendMsgToMcp("BodyMovementChannel", "BodyLocalMcpConnector", bodyCmd, ref errorStr);
                int i = Array.IndexOf(BodyMessageStrings, msg);
                if (announce)
                    rslt = SendVoiceMsg(BodyMovementRespPhrases[i], ref errorStr);
            }
            catch (McpException ex)
            {
                errorStr += "  " + ex.Message + " info about the exception was logged at SendBodyCmdMessage";
                rslt = false;
            }
            catch (Exception ex)
            {
                errorStr = "System exception " + ex.Message + " no info logged at SendBodyCmdMessage";
                rslt = false;
            }

            return rslt;
        }

        public bool SendVoiceMsg(string msg, ref string errorStr)
        {
            errorStr = (errorStr ?? string.Empty);
            if (errorStr != string.Empty)
                errorStr += "\r\n";

            bool rslt;
            try
            {
                var phrase = new VoiceConnSchema
                {
                    Phrase = msg
                };
                rslt = SendMsgToMcp("VoiceChannel", "VoiceDeviceConnector", phrase, ref errorStr);
            }
            catch (McpException ex)
            {
                errorStr += "  " + ex.Message + " info about the exception was logged at SendVoiceMsg";
                rslt = false;
            }
            catch (Exception ex)
            {
                errorStr = "System exception " + ex.Message + " no info logged at SendVoiceMsg";
                rslt = false;
            }

            return rslt;
        }

        public bool SendBodyCmdMessage(SendBodyMsgs action, ref string errorStr, bool announce = true)
        {
            errorStr = (errorStr ?? string.Empty);
            if (errorStr != string.Empty)
                errorStr += "\r\n";

            bool rslt;
            try
            {
                var msg = BodyMessageStrings[(int)action];
                rslt = SendBodyCmdMessage(msg, ref errorStr, announce);
                //if (announce)
                //rslt = SendVoiceMsg(BodyMovementRespPhrases[(int)action], ref errorStr);
            }
            catch (McpException ex)
            {
                errorStr += "  " + ex.Message + " info about the exception was logged at SendBodyCmdMessage";
                rslt = false;
            }
            catch (Exception ex)
            {
                errorStr = "System exception " + ex.Message + " no info logged at SendBodyCmdMessage";
                rslt = false;
            }

            return rslt;
        }

        public bool SendRemoteResponseMsg(RemoteBotRespSchema respMsg, ref string errorStr)
        {
            errorStr = (errorStr ?? string.Empty);
            if (errorStr != string.Empty)
                errorStr += "\r\n";

            bool rslt;
            try
            {
                rslt = SendMsgToMcp("AntwerpBotPeerChan", "McpRemotePeerConn", respMsg, ref errorStr);
            }
            catch (McpException ex)
            {
                errorStr += "  " + ex.Message + " info about the exception was logged at SendBodyCmdMessage";
                rslt = false;
            }
            catch (Exception ex)
            {
                errorStr = "System exception " + ex.Message + " no info logged at SendBodyCmdMessage";
                rslt = false;
            }

            return rslt;
        }

        public bool SendLidarMotorStart(ref string errStr)
        {
            errStr = null;
            return SendLidarSensorCmd(0, ref errStr);
        }

        public bool SendLidarMotorStop(ref string errStr)
        {
            errStr = null;
            return SendLidarSensorCmd(1, ref errStr);
        }

        public bool SendLidarScanStart(ref string errStr)
        {
            errStr = null;
            return SendLidarSensorCmd(2, ref errStr);
        }

        public bool SendLidarScanStop(ref string errStr)
        {
            errStr = null;
            return SendLidarSensorCmd(3, ref errStr);
        }

        public bool SendLidarSensorCmd(short cmd, ref string errStr)
        {
            errStr = null;
            try
            {
                var cmdMsg = new LidarCommandMessage
                {
                    SensorCommand = cmd
                };
                SendMsgToMcp("RpLidarChannel", "LidarDeviceConnector", cmdMsg, ref errStr);
            }
            catch (McpException ex)
            {
                errStr += "  " + ex.Message + " info about the exception was logged at SendBodyCmdMessage";
                return false;
            }
            catch (Exception ex)
            {
                errStr = "System exception " + ex.Message + " no info logged at SendLidarMotorStart";
                return false;
            }

            return true;
        }

        public bool SendPowerControlMessage(PowerCircuitsEnum circuit, bool IsOn, ref string errorStr)
        {
            errorStr = (errorStr ?? string.Empty);
            if (errorStr != string.Empty)
                errorStr += "\r\n";

            bool rslt;
            try
            {
                var pwrCmdMsg = new PowerControlCmdMsg
                {
                    CircuitCommand = circuit.ToString(),
                    CircuitDigitalPin = Circuits[(short)circuit],
                    OnOff = IsOn
                };
                rslt = SendMsgToMcp("PowerControlChannel", "PowerControlMcpConnector", pwrCmdMsg, ref errorStr);
            }
            catch (McpException ex)
            {
                errorStr += "  " + ex.Message + " info about the exception was logged at SendBodyCmdMessage";
                rslt = false;
            }
            catch (Exception ex)
            {
                errorStr = "System exception " + ex.Message + " no info logged at SendBodyCmdMessage";
                rslt = false;
            }

            return rslt;
        }

        public bool SetPinState(short pinNdx, bool onOff, ref string errorStr)
        {
            errorStr = (errorStr ?? string.Empty);
            if (errorStr != string.Empty)
                errorStr += "\r\n";

            bool rslt;
            try
            {
                var msg = new PowerControlCmdMsg
                {
                    CircuitDigitalPin = pinNdx,
                    CircuitCommand = "Set",
                    OnOff = onOff
                };
                rslt = SendMsgToMcp("PowerControlChannel", "PowerControlMcpConnector", msg, ref errorStr);
            }
            catch (McpException ex)
            {
                errorStr += "  " + ex.Message + " info about the exception was logged at SendBodyCmdMessage";
                rslt = false;
            }
            catch (Exception ex)
            {
                errorStr = "System exception " + ex.Message + " no info logged at SendBodyCmdMessage";
                rslt = false;
            }

            return rslt;
        }

        public bool GetPinState(short pinNdx, ref string errorStr)
        {
            errorStr = (errorStr ?? string.Empty);
            if (errorStr != string.Empty)
                errorStr += "\r\n";

            bool rslt;
            try
            {
                var msg = new PowerControlCmdMsg
                {
                    CircuitDigitalPin = pinNdx,
                    CircuitCommand = "GetPinState",
                    OnOff = false
                };
                rslt = SendMsgToMcp("PowerControlChannel", "PowerControlMcpConnector", msg, ref errorStr);
            }
            catch (McpException ex)
            {
                errorStr += "  " + ex.Message + " info about the exception was logged at SendBodyCmdMessage";
                rslt = false;
            }
            catch (Exception ex)
            {
                errorStr = "System exception " + ex.Message + " no info logged at SendBodyCmdMessage";
                rslt = false;
            }

            return rslt;
        }

        public void RaisePowerCtrlResponseMsg(object sender, McpMessageEventArgs e)
        {
            try
            {
                var respMsg = JsonConvert.DeserializeObject<PowerFeedbackMsg>(e.MessageJson);
                if (LogAllMessages)
                    File.AppendAllText(MessageLogPath + MessageLogFileName, e.MessageJson);
                ReceivePwrFdbckMsg?.Invoke(respMsg);
            }
            catch (Exception ex)
            {
                throw new McpSystemException("Exception at RaisePowerCtrlResponseMsg", ex);
            }
        }

        public void RaiseCmdResponseMsg(object sender, McpMessageEventArgs e)
        {
            try
            {
                var respMsg = JsonConvert.DeserializeObject<BodyCmdResponseSchema>(e.MessageJson);
                ReceiveCmdResponse?.Invoke(respMsg);
                if (LogAllMessages)
                    File.AppendAllText(MessageLogPath + MessageLogFileName, e.MessageJson);
            }
            catch (Exception ex)
            {
                throw new McpSystemException("Exception at RaiseCmdResponseMsg", ex);
            }
        }

        public void RaiseArmResponseMessage(object sender, McpMessageEventArgs e)
        {
            try
            {

            }
            catch (Exception ex)
            {
                throw new McpSystemException("Exception at RaiseArmResponseMessage", ex);
            }
        }

        public void RaiseRngSensorMessage(object sender, McpMessageEventArgs e)
        {
            try
            {
                _lastRngSensorMsg = JsonConvert.DeserializeObject<BodyRngSensorMessage>(e.MessageJson);
                ProcessRngSnsMsgForRoam(_lastRngSensorMsg);
                if (DisplayRngSensorRdgs)
                    ReceiveSensorMessage?.Invoke(_lastRngSensorMsg);
            }
            catch (Exception ex)
            {
                throw new McpSystemException("Exception at RaiseRngSensorMessage", ex);
            }
        }

        public void ReceiveSpeechCommandMsg(object sender, McpMessageEventArgs e)
        {
            var spchCmdMsg = JsonConvert.DeserializeObject<SpeechCommand>(e.MessageJson);
            //File.AppendAllText("backwardstest.txt", "speech cmd: " + spchCmdMsg.Command + "\r\n");
            ProcessStringCommand(spchCmdMsg.Command);
        }

        public void ReceiveRemoteMcpMsg(object sender, McpMessageEventArgs e)
        {
            var respMsg = new RemoteBotRespSchema
            {
                RemoteMcpName = "AntwerpPeerMcp",
                ResponseString = "Failed to Execute",
                ResponseToCmdString = "Unknown command"
            };
            string errorStr = null;
            try
            {
                var cmdMsg = JsonConvert.DeserializeObject<RemotePeerCmdSchema>(e.MessageJson);
                if (Array.IndexOf(BodyMessageStrings, cmdMsg.CommandStr) == -1)
                    throw new ArgumentException("Invalid remote command message received from remote peer MCP");

                WaitingForImage = cmdMsg.CommandStr == "UCGS";
                var respStr = ProcessStringCommand(cmdMsg.CommandStr);
                if (WaitingForImage)
                    return;

                respMsg.ResponseString = respStr;
                respMsg.ResponseToCmdString = cmdMsg.CommandStr;
                SendRemoteResponseMsg(respMsg, ref errorStr);
            }
            catch (Exception ex)
            {
                try
                {
                    SendRemoteResponseMsg(respMsg, ref errorStr);
                }
                catch (Exception)
                {
                }
                throw new McpSystemException("Exception at RaiseRemoteMcpMsg", ex);
            }
        }

        public void ReturnLastScanToRemotePeer()
        {
            WaitingForImage = false;
            if (LastScanFrame == null)
            {
                var failResp = new RemoteBotRespSchema
                {
                    ResponseToCmdString = "UCGS",
                    ResponseString = "No scan frame available",
                    RemoteMcpName = "AntwerpPeerMcp",
                };
            }
            var respMsg = new RemoteBotRespSchema
            {
                ResponseToCmdString = "UCGS",
                ResponseString = "Scan frame returned",
                RemoteMcpName = "AntwerpPeerMcp",
                ImageBase64 = LastScanFrame.ImageBase64,
                UpperCamPan = LastScanFrame.CamPanBearing,
                UpperCamTilt = LastScanFrame.CamTiltAngle
            };
            string errorStr = null;
            SendRemoteResponseMsg(respMsg, ref errorStr);
        }

        public void ReceiveLidarFrameMsg(object sender, McpMessageEventArgs e)
        {
            var jobj = JObject.Parse(e.MessageJson);
            var framePointsStr = (string)jobj["FramePoints"];
            var framePoints = JsonConvert.DeserializeObject<List<LidarPoint>>(framePointsStr);
            if (!DisplayLidarPointCloud || framePoints == null)
                return;


            var frame = new LidarFrame
            {
                Timstamp = DateTime.UtcNow,
                FramePoints = framePoints
            };
            ReceiveLidarFrame?.Invoke(frame);
        }

        public string ProcessStringCommand(string cmdStr)
        {
            string errStr = null;
            string voiceStr;
            if (cmdStr.Length > 1 && cmdStr.Substring(0, 2) == "UC" && !UpperCamOn)
            {
                SendVoiceMsg("Upper camera not available", ref errStr);
                return "Upper camera not enabled";
            }

            switch (cmdStr)
            {
                case "LD":
                    LidarDisplayOn?.Invoke();
                    voiceStr = _inSwearMode ? "Fuck yes! " + BodyMovementRespPhrases[(int)SendBodyMsgs.LidarDispOn] : BodyMovementRespPhrases[(int)SendBodyMsgs.LidarDispOn];
                    SendVoiceMsg(voiceStr, ref errStr);
                    break;
                case "LN":
                    if (!SendLidarScanStart(ref errStr))
                    {
                        LidarScanOn?.Invoke(this, null);
                        SendVoiceMsg(errStr, ref errStr);
                    }
                    else
                    {
                        voiceStr = _inSwearMode ? "Oh shit! " + BodyMovementRespPhrases[(int)SendBodyMsgs.LidarOn] : BodyMovementRespPhrases[(int)SendBodyMsgs.LidarOn];
                        SendVoiceMsg(voiceStr, ref errStr);
                    }
                    break;
                case "LF":
                    LidarDisplayOff?.Invoke();
                    voiceStr = _inSwearMode ? "Holy hell! " + BodyMovementRespPhrases[(int)SendBodyMsgs.LidarDispOff] : BodyMovementRespPhrases[(int)SendBodyMsgs.LidarDispOff];
                    SendVoiceMsg(voiceStr, ref errStr);
                    break;
                case "RN":
                    RngSensorDisplayOn?.Invoke();
                    voiceStr = _inSwearMode ? "Oh, fuck me! " + BodyMovementRespPhrases[(int)SendBodyMsgs.RngDispOn] : BodyMovementRespPhrases[(int)SendBodyMsgs.RngDispOn];
                    SendVoiceMsg(voiceStr, ref errStr);
                    break;
                case "RF":
                    RngSensorDisplayOff?.Invoke();
                    voiceStr = _inSwearMode ? "Oh, bollocks! " + BodyMovementRespPhrases[(int)SendBodyMsgs.RngDispOff] : BodyMovementRespPhrases[(int)SendBodyMsgs.RngDispOff];
                    SendVoiceMsg(voiceStr, ref errStr);
                    break;
                case "SN":
                    _inSwearMode = true;
                    SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.SwearingOn], ref errStr);
                    break;
                case "SF":
                    _inSwearMode = false;
                    SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.SwearingOff], ref errStr);
                    break;
                case "YS":
                    SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.YouSuck], ref errStr);
                    break;
                case "WD":
                    SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.WhoseYourDaddy], ref errStr);
                    break;
                case "WP":
                    SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.WhoseYourProgrammer], ref errStr);
                    break;
                case "WU":
                    if (UpperCamMgr != null)
                        UpperCamMgr.SendServoAction(SendBodyMsgs.UpperCamHome, ref errStr);
                    SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.WakeUp], ref errStr);
                    break;
                case "ML":
                    SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.MeaningOfLife], ref errStr);
                    break;
                case "RI":
                    SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.AreYouIntel], ref errStr);
                    break;
                case "UCN":
                    UpperCamDisplayOn?.Invoke();
                    UpperCamMgr.SendServoAction(SendBodyMsgs.UpperCamHome, ref errStr);
                    UpperCamPanTiltUpdt?.Invoke();
                    if (VerboseResponses)
                        SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.UpperCamDispOn], ref errStr);
                    break;
                case "UCF":
                    UpperCamDisplayOff?.Invoke();
                    if (VerboseResponses)
                        SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.UpperCamDispOn], ref errStr);
                    UpperCamMgr.SendServoAction(SendBodyMsgs.UpperCamRelax, ref errStr);
                    UpperCamPanTiltUpdt?.Invoke();
                    break;
                case "UCSP":
                    if (UpperCamMgr.SendServoAction(SendBodyMsgs.UpperCamSetPan, ref errStr, UpperCamMgr.NewPanPos))
                    {
                        if (VerboseResponses)
                            SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.UpperCamSetPan], ref errStr);
                        UpperCamPanTiltUpdt?.Invoke();
                    }
                    else
                    {
                        string voiceErr = null;
                        SendVoiceMsg(errStr, ref voiceErr);
                    }
                    break;
                case "UCST":
                    if (UpperCamMgr.SendServoAction(SendBodyMsgs.UpperCamSetTilt, ref errStr, UpperCamMgr.NewTiltPos))
                    {
                        if (VerboseResponses)
                            SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.UpperCamSetTilt], ref errStr);
                        UpperCamPanTiltUpdt?.Invoke();
                    }
                    else
                    {
                        string voiceErr = null;
                        SendVoiceMsg(errStr, ref voiceErr);
                    }
                    break;
                case "UCPU":
                    if (UpperCamMgr.SendServoAction(SendBodyMsgs.UpperCamPanUp10, ref errStr))
                    {
                        if (VerboseResponses)
                            SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.UpperCamPanUp10], ref errStr);
                        UpperCamPanTiltUpdt?.Invoke();
                    }
                    else
                    {
                        string voiceErr = null;
                        SendVoiceMsg(errStr, ref voiceErr);
                    }
                    break;
                case "UCTU":
                    if (UpperCamMgr.SendServoAction(SendBodyMsgs.UpperCamTiltUp10, ref errStr))
                    {
                        if (VerboseResponses)
                            SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.UpperCamTiltUp10], ref errStr);
                        UpperCamPanTiltUpdt?.Invoke();
                    }
                    else
                    {
                        string voiceErr = null;
                        SendVoiceMsg(errStr, ref voiceErr);
                    }
                    break;
                case "UCPD":
                    if (UpperCamMgr.SendServoAction(SendBodyMsgs.UpperCamPanDn10, ref errStr))
                    {
                        if (VerboseResponses)
                            SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.UpperCamPanDn10], ref errStr);
                        UpperCamPanTiltUpdt?.Invoke();
                    }
                    else
                    {
                        string voiceErr = null;
                        SendVoiceMsg(errStr, ref voiceErr);
                    }
                    break;
                case "UCTD":
                    if (UpperCamMgr.SendServoAction(SendBodyMsgs.UpperCamTiltDn10, ref errStr))
                    {
                        if (VerboseResponses)
                            SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.UpperCamTiltDn10], ref errStr);
                        UpperCamPanTiltUpdt?.Invoke();
                    }
                    else
                    {
                        string voiceErr = null;
                        SendVoiceMsg(errStr, ref voiceErr);
                    }
                    break;
                case "UCR":
                    UpperCamMgr.SendServoAction(SendBodyMsgs.UpperCamRelax, ref errStr);
                    UpperCamPanTiltUpdt?.Invoke();
                    break;
                case "UCH":
                    UpperCamMgr.SendServoAction(SendBodyMsgs.UpperCamHome, ref errStr);
                    UpperCamPanTiltUpdt?.Invoke();
                    break;
                case "UCGS":
                    RcvdUpperCamCommand?.Invoke(CameraCommands.GrabSnapshot);
                    if (VerboseResponses)
                        SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.UpperCamGrabSnap], ref errStr);
                    break;
                case "UCRGB":
                    RcvdUpperCamCommand?.Invoke(CameraCommands.RgbStreamOn);
                    if (VerboseResponses)
                        SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.UpperCamRgbOn], ref errStr);
                    break;
                case "UCDPTH":
                    RcvdUpperCamCommand?.Invoke(CameraCommands.DepthStreamOn);
                    if (VerboseResponses)
                        SendVoiceMsg(BodyMovementRespPhrases[(int)SendBodyMsgs.UpperCamDepthOn], ref errStr);
                    break;
                default:
                    ClearSubsumption();
                    int cmdNdx = Array.IndexOf(BodyMessageStrings, cmdStr);
                    if (cmdNdx == -1)
                        throw new ArgumentException("Invalid body command message string at ReceiveSpeechCommandMsg");
                    LastBodyMsg = (SendBodyMsgs)cmdNdx;
                    SendBodyCmdMessage(cmdStr, ref errStr, true);
                    if (LastBodyMsg == SendBodyMsgs.Forward || LastBodyMsg == SendBodyMsgs.Backward)
                        InRoamMode = true;
                    break;
            }

            return "AntwerpWpf successfully executed: " + cmdStr;
        }

        private void SetArmToNamedPose(string poseName)
        {
            var poseMsg = new ArbotixArmCmdMsg
            {
                AntwerpPoseMsg = "A" + poseName
            };
            SendMsgToMcp("AntwerpArmChannel", "ArbotixArmMcpConnector", poseMsg);
        }

        //private void FilterRangeSensorNoise(BodyRngSensorMessage sensorMessage)
        //{
        //    sensorMessage.Front = CleanSensorVal(RangeSensors.Front, sensorMessage.Front);
        //    sensorMessage.Right = CleanSensorVal(RangeSensors.Right, sensorMessage.Right);
        //    sensorMessage.Rear = CleanSensorVal(RangeSensors.Rear, sensorMessage.Rear);
        //    sensorMessage.Left = CleanSensorVal(RangeSensors.Left, sensorMessage.Left);
        //    sensorMessage.FrontDown = CleanSensorVal(RangeSensors.FrontDown, sensorMessage.FrontDown);
        //    sensorMessage.RearDown = CleanSensorVal(RangeSensors.RearDown, sensorMessage.RearDown);
        //}

        //private int CleanSensorVal(RangeSensors sensor, int sensorHitValue)
        //{
        //    int cleanVal = sensorHitValue;
        //    if (_lastHit[(int)sensor] > 0 && sensorHitValue > FuzzyHitLimit)
        //        cleanVal = _lastHit[(int)sensor];
        //    _lastHit[(int)sensor] = cleanVal;

        //    return cleanVal;
        //}

        private void ProcessRngSnsMsgForRoam(BodyRngSensorMessage sensorMessage)
        {
            if (!InRoamMode)
                return;

            //FilterRangeSensorNoise(sensorMessage);
            bool isFront = false;
            bool isDown = false;
            int roamPhraseNdx = 0;
            bool isBlocked = false;
            if (sensorMessage.Front < RangeSensorDistLimit && LastBodyMsg == SendBodyMsgs.Forward)
            {
                isFront = true;
                isDown = false;
                roamPhraseNdx = 0;
                isBlocked = true;
                if (!_inSubsumption)
                    _lastBlockCmd = SendBodyMsgs.Forward;
            }
            else if (sensorMessage.Rear < RangeSensorDistLimit && LastBodyMsg == SendBodyMsgs.Backward)
            {
                isFront = false;
                isDown = false;
                roamPhraseNdx = 1;
                isBlocked = true;
                if (!_inSubsumption)
                    _lastBlockCmd = SendBodyMsgs.Backward;
            }

            if (isBlocked || _inSubsumption)
            {
                string errStr = null;
                BlockedSubsumption(sensorMessage, isFront, isDown, roamPhraseNdx);
                if (_blockTries > MaxBlockTries)
                {
                    if (!SendVoiceMsg(RoamPivotFail, ref errStr))
                        throw new McpSystemException("Error encountered sending text to speech message.  Error message returned: " + errStr, null);
                    InRoamMode = false;
                    LastBodyMsg = SendBodyMsgs.Stop;
                    ClearSubsumption();
                }
            }
        }

        public void ClearSubsumption()
        {
            _inSubsumption = false;
            _lastBlockCmd = SendBodyMsgs.Forward;
            _blockTries = 0;
        }

        private void BlockedSubsumption(BodyRngSensorMessage sensorMessage, bool isFront, bool isDown, int roamPhraseNdx)
        {
            string errStr = null;

            if (!_inSubsumption)
            {
                _inSubsumption = true;
                if (!SendBodyCmdMessage(SendBodyMsgs.Stop, ref errStr, false))
                    throw new McpSystemException("Error encountered sending body command message.  Error message returned: " + errStr, null);
                Thread.Sleep(200);
                if (!SendVoiceMsg(RoamWarningPhrases[roamPhraseNdx], ref errStr))
                    throw new McpSystemException("Error encountered sending text to speech message.  Error message returned: " + errStr, null);
            }

            if (_lastRngSensorMsg == null)
                return;

            //SendBodyMsgs bodyCmd = sensorMessage.GetBestDirection();
            if (sensorMessage.Front >= RngSensorClearBlockLimit && _lastBlockCmd == SendBodyMsgs.Forward || 
                sensorMessage.Rear >= RngSensorClearBlockLimit && _lastBlockCmd == SendBodyMsgs.Backward)
            {
                if (!SendVoiceMsg(RoamResumePhrase, ref errStr))
                    throw new McpSystemException("Error encountered sending text to speech message.  Error message returned: " + errStr, null);
                if (!SendBodyCmdMessage(SendBodyMsgs.Forward, ref errStr, false))
                    throw new McpSystemException("Error encountered sending body command message.  Error message returned: " + errStr, null);
                ClearSubsumption();
                return;
            }
            else
            {
                _blockTries++;
                SendBodyMsgs bodyCmd = sensorMessage.Right > sensorMessage.Left ? 
                    SendBodyMsgs.PivotRight : SendBodyMsgs.PivotLeft;

                if (!_inSubsumption)
                    return;

                if (!SendBodyCmdMessage(bodyCmd, ref errStr, false))
                    throw new McpSystemException("Error encountered sending body command message.  Error message returned: " + errStr, null);
                Thread.Sleep(1500);
                if (!SendBodyCmdMessage(SendBodyMsgs.Stop, ref errStr, false))
                    throw new McpSystemException("Error encountered sending body command message.  Error message returned: " + errStr, null);
            }
            Thread.Sleep(500);
        }

        private void SetBodyCmdResponse(AntwerpPeerCmdMsg cmdMsg, AntwerpBotRespMsg respMsg, bool isErrorResp)
        {
            //respMsg.MsgIncludesBodyStatusStr = true;
            //if (isErrorResp)
            //{
            //    respMsg.IsErrorResponse = true;
            //    respMsg.BodyStatusString = "Error encountered executing body command message, " + cmdMsg.BodyCommand + " see exception log for details";
            //    return;
            //}
            //respMsg.BodyStatusString = "Received body command '" + cmdMsg.BodyCommand + "'";
        }
    }
}
;