Projet_RT/software/monitor/monitor/DestijlCommandManager.cs

//
//  DestijlCommandManager.cs
//
//  Author:
//       Di MERCURIO Sébastien <dimercur@insa-toulouse.fr>
//
//  Copyright (c) 2018 INSA - DGEI
//
//  This program is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program.  If not, see <http://www.gnu.org/licenses/>.

using System;

namespace monitor
{
    /// <summary>
    /// Commands and options parameters used in Destijl project when communicating with server
    /// </summary>
    public static class DestijlCommandList
    {
        public const string HeaderMtsComDmb = "COM";
        public const string HeaderMtsDmbOrder = "DMB";
        public const string HeaderMtsCamera = "CAM";
        public const string HeaderMtsMessage = "MSG";

        public const string DataComOpen = "o";
        public const string DataComClose = "C";

        public const string DataCamOpen = "A";
        public const string DataCamClose = "I";
        public const string DataCamAskArena = "y";
        public const string DataCamArenaConfirm = "x";
        public const string DataCamInfirm = "z";
        public const string DataCamComputePosition = "p";
        public const string DataCamStopComputePosition = "s";

        public const string HeaderStmAck = "ACK";
        public const string HeaderStmNoAck = "NAK";
        public const string HeaderStmLostDmb = "LCD";
        public const string HeaderStmImage = "IMG";
        public const string HeaderStmPos = "POS";
        public const string HeaderStmMes = "MSG";
        public const string HeaderStmBat = "BAT";
    }

    /// <summary>
    /// Commands used for robot messages
    /// </summary>
    public static class RobotCommandList
    {
        public const string RobotPing = "p";
        public const string RobotReset = "r";
        public const string RobotStartWithoutWatchdog = "u";
        public const string RobotStartWithWatchdog = "W";
        public const string RobotGetBattery = "v";
        public const string RobotGetBusyState = "b";
        public const string RobotMove = "M";
        public const string RobotTurn = "T";
        public const string RobotGetVersion = "V";
        public const string RobotPowerOff = "z";
    }

    /// <summary>
    /// Specialization class for command manager, which implemnent destijl protocol between monitor and server
    /// </summary>
    public class DestijlCommandManager
    {
        /// <summary>
        /// Command Manager object
        /// </summary>
        private CommandManager commandManager = null;

        /// <summary>
        /// Part of received message corresponding to command header
        /// </summary>
        private string receivedHeader = null;

        /// <summary>
        /// Part of received message corresponding to command data
        /// </summary>
        private string receivedData = null;

        /// <summary>
        /// Callback for sending received data to application level
        /// </summary>
        public delegate void CommandReceivedEvent(string header, string data, byte[] buffer);
        public CommandReceivedEvent commandReceivedEvent = null;

        /// <summary>
        /// Timeout used for command with acknowledge
        /// </summary>
        public double timeout = 100; 

        /// <summary>
        /// List of available return status
        /// </summary>
        public enum CommandStatus
        {
            Success,
            Rejected,
            InvalidAnswer,
            Busy,
            CommunicationLostWithRobot,
            CommunicationLostWithServer
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="monitor.DestijlCommandManager"/> class.
        /// </summary>
        /// <param name="callback">Callback reference for reception of data</param>
        public DestijlCommandManager(CommandReceivedEvent callback)
        {
            commandManager = new CommandManager(OnCommandReceived);
            this.commandReceivedEvent += callback;
        }

        /// <summary>
        /// Releases unmanaged resources and performs other cleanup operations before the
        /// <see cref="monitor.DestijlCommandManager"/> is reclaimed by garbage collection.
        /// </summary>
        ~DestijlCommandManager()
        {
            if (commandManager != null) commandManager.Close();
        }

        /// <summary>
        /// Callback used for receiving data from lower layer (CommandManager class)
        /// </summary>
        /// <param name="msg">String containing received message</param>
        /// <param name="buffer">Raw buffer to be used when data are not in ascii format (image for example)</param>
        private void OnCommandReceived(string msg, byte[] buffer)
        {
            // Firstly, split message in (at least) two part : header, and data
            string[] msgs = msg.Split(':');

            // If it exist at least on element in string array, it should be command header
            if (msgs.Length >= 1) receivedHeader = msgs[0];
            else receivedHeader = null;

            // if msgs array contains at least two elements, second element is normally data
            if (msgs.Length >= 2) receivedData = msgs[1];
            else receivedData = null;

            // when split is done, provide data to application
            this.commandReceivedEvent?.Invoke(receivedHeader, receivedData, buffer);
        }

        /// <summary>
        /// Open the specified hostname server, using default port number.
        /// </summary>
        /// <returns>true if connection succeded, false otherwise</returns>
        /// <param name="hostname">Hostname to connect to</param>
        public bool Open(string hostname)
        {
            return this.Open(hostname, Client.defaultPort);
        }

        /// <summary>
        /// Open connection to server "host", with port number "port"
        /// </summary>
        /// <returns>true if connection succeded, false otherwise</returns>
        /// <param name="hostname">Hostname to connect to</param>
        /// <param name="port">Port number for connection</param>
        public bool Open(string hostname, int port)
        {
            if (commandManager != null) return commandManager.Open(hostname, port);
            else return false;
        }

        /// <summary>
        /// Close connection to server
        /// </summary>
        public void Close()
        {
            if (commandManager != null) commandManager.Close();
        }

        /// <summary>
        /// Creates the command to send to server, based on header and data provided
        /// </summary>
        /// <returns>The command string</returns>
        /// <param name="header">Header part of the command</param>
        /// <param name="data">Data part of the command</param>
        private string CreateCommand(string header, string data)
        {
            return header + ":" + data;
        }

        /// <summary>
        /// Provide DestijlCommandManager.CommandStatus based on status received by CommandManager.SendCommand and answer string 
        /// </summary>
        /// <returns>Status compatible with DestijlCommandManager.CommandStatus type</returns>
        /// <param name="localStatus">Status provided by CommandManager.SendCommand</param>
        /// <param name="answer">Answer provided by CommandManager.SendCommand</param>
        private CommandStatus DecodeStatus(CommandManager.CommandManagerStatus localStatus, string answer)
        {
            CommandStatus status = CommandStatus.Success;

            // if timeout occures, return CommandStatus.CommunicationLostWithServer
            if (localStatus == CommandManager.CommandManagerStatus.Timeout) status = CommandStatus.CommunicationLostWithServer;
            // if a command is currently processed, return Busy
            else if (localStatus == CommandManager.CommandManagerStatus.Busy) status = CommandStatus.Busy;
            else
            {
                if (answer != null)
                {
                    // if command is not acknowledged, return Rejected
                    if (answer.ToUpper().Contains(DestijlCommandList.HeaderStmNoAck)) status = CommandStatus.Rejected;
                    // if communication is lost with robot, return CommunicationLostWithRobot
                    else if (answer.ToUpper().Contains(DestijlCommandList.HeaderStmLostDmb)) status = CommandStatus.CommunicationLostWithRobot;
                    // if answer is empty, communication with robot is lost
                    else if (answer.Length == 0) status = CommandStatus.CommunicationLostWithServer;
                    //else status = CommandStatus.InvalidAnswer;
                }
            }

            return status;
        }

        /// <summary>
        /// Open communication with robot and wait acknowledge
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus RobotOpenCom()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsComDmb, DestijlCommandList.DataComOpen),
                out answer,
                this.timeout);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Close communication with robot and wait acknowledge
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus RobotCloseCom()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsComDmb, DestijlCommandList.DataComClose),
                out answer,
                this.timeout);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Ping the robot.
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus RobotPing()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotPing),
                out answer,
                this.timeout);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Reset robot and let it in idle mode
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus RobotReset()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotReset),
                out answer,
                0);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Start robot, enabling watchdog
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus RobotStartWithWatchdog()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotStartWithWatchdog),
                out answer,
                this.timeout);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Start robot, without enabling watchdog 
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus RobotStartWithoutWatchdog()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotStartWithoutWatchdog),
                out answer,
                this.timeout);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Move robot forward or backward, for a distance expressed in millimeter
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        /// <param name="distance">Distance of mouvment, in millimeter</param>
        public CommandStatus RobotMove(int distance)
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotMove + "=" + distance),
                out answer,
                0);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Make robot turn left or right, for a given angle
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        /// <param name="angle">Angle of turn, in degree (negative for left, positive for right)</param>
        public CommandStatus RobotTurn(int angle)
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotTurn + "=" + angle),
                out answer,
                0);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Request robot battery level
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus RobotGetBattery()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotGetBattery),
                out answer,
                0);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Request robot firmware version
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        /// <param name="version">todo</param>
        public CommandStatus RobotGetVersion(out string version)
        {
            CommandManager.CommandManagerStatus localStatus;
            CommandStatus status = CommandStatus.Success;
            version = "";

            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotGetVersion),
                out answer,
                this.timeout);

            if (localStatus == CommandManager.CommandManagerStatus.AnswerReceived)
            {
                string[] msg = answer.Split(':');

                if (msg.Length > 1)
                {
                    version = msg[1];
                }
            }
            else if (localStatus == CommandManager.CommandManagerStatus.Timeout)
            {
                status = CommandStatus.CommunicationLostWithServer;
            }

            return status;
        }

        /// <summary>
        /// Power off robot
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus RobotPowerOff()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsDmbOrder, RobotCommandList.RobotPowerOff),
                out answer,
                0);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Open camera on remote device
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus CameraOpen()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamOpen),
                out answer,
                this.timeout);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Close camera on remote device
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus CameraClose()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamClose),
                out answer,
                0);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Request still image of detected arena
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus CameraAskArena()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamAskArena),
                out answer,
                0);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Confirm arena detection (after requesting image of detected arena, using CameraAskArena
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus CameraArenaConfirm()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamArenaConfirm),
                out answer,
                0);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Reject arena detected (after requesting image of detected arena, using CameraAskArena
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus CameraArenaInfirm()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamInfirm),
                out answer,
                0);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Request robot position computing
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus CameraComputePosition()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamComputePosition),
                out answer,
                0);

            return DecodeStatus(localStatus, answer);
        }

        /// <summary>
        /// Stop robot position computing
        /// </summary>
        /// <returns>Command status (see DecodeStatus)</returns>
        public CommandStatus CameraStopComputePosition()
        {
            CommandManager.CommandManagerStatus localStatus;
            string answer;

            localStatus = commandManager.SendCommand(
                CreateCommand(DestijlCommandList.HeaderMtsCamera, DestijlCommandList.DataCamStopComputePosition),
                out answer,
                0);

            return DecodeStatus(localStatus, answer);
        }
    }
}