using System;
using System.IO;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Linq;
using System.Text;
using System.Net;
using System.Net.Sockets;

namespace EdgeTech.SonarComms.Demo
{
    public class CPortIO
    {
        const string _versionInfo = "$Header: portIO.cs  Revision:1.29  Fri Aug 03 10:04:46 2012  mps $";
        private string versionInfo;
        const int SONAR_MSG_HDR_SIZE = 16;

        // Class data members
        static byte[] HdrAndData;
        static byte[] Data;
        static byte[] SonarData;
        public int LargestMessageSize = 0;
        Socket socket = null;
        IPAddress ipAddress;
        int PortNumber;
        public AsyncCallback pfnCallBack = null;
        IAsyncResult asynResult;
        private static Object thisLock = new Object();
        public string name;
        public int connectCount = 0;
        public int disConnectCount = 0;
        public int SDM_HDR_SIZE = Marshal.SizeOf(typeof(SonarMessage.SonarDataMessageHeaderType));
        public string[] cmdTypeStr = { "SET  ", "GET  ", "REPLY", "ERROR", "PLAYBACK", "QUALITY", "UNKNOWN" };
        public string[] systemTypeStr = {
            "No Acquisition",
            "Single frequency multi-pulse sidescan",
            "Single frequency sidescan - 560A - 272 towfish",
            "Simultaneous dual frequency sidescan - 560D - DF1000 towfish",
            "Simultaneous dual frequency sidescan",
            "Sub-bottom sonar",
            "Combined sub-bottom and dual frequency sidescan",
            "Single frequency synthetic aperature sidescan",
            "Single frequency sidescan",
            "Simultaneous dual frequency multi-pulse sidescan",
            "4125 ultra portable sidescan - two disparate configurations",
            "One frequency focused sidescan, one frequency standard",
            "Single frequency multipulse Bathymetric system",
            "Single frequency monopulse Bathymetric system"
        };

        // Internal structure with buffer to receive incoming data
        public class CSocketPacket
        {
            public Socket thisSocket;
            public int numBytes;
            public byte[] dataBuffer;
            public int bufSize;
            // Constructor
            public CSocketPacket( int bufSize )
            {
                this.bufSize = bufSize;
                dataBuffer = new byte[bufSize];
            }
        }

        public CSocketPacket theSocPkt;

        public struct Packet
        {
            public SonarMessage.Header msgHdr;
            public object msg;
        }

        public Packet packet;

        // Constructor
        public CPortIO(string name, int bufSize) 
        {
            //if (Utils.doThisPrint(2)) Console.WriteLine("Opening {0} socket...", name);
            this.name = name;
            theSocPkt = new CSocketPacket( bufSize );
        }

        public bool isConnected = false;

        // This struct is used to extract the ping number from the sonar data header
        public struct pingNumStruct
        {
            public int dummy1;
            public int dummy2;
            public uint pingNum;
        }

        private pingNumStruct getPingNum;

        // Class methods begin here ---------------------------------------------------------------------------------

        public void setIPAddress(IPAddress ipaddr)
        {
            ipAddress = ipaddr;
        }

        public void setPortNumber(int portNum)
        {
            PortNumber = portNum;
        }

        // This method attempts to connect with the channel.
        // Returns true if successful, false otherwise
        // If successful then start listening for data, otherwise simply return
        public bool Connect(int maxBufSize)
        {
            closeSocket();  // Close socket just in case

            if (socket == null)  // Should always be true due to closeSocket(), but with multi-threads running...
            {
                try  // Send connection
                {
                    // Create the TCP/IP socket instance here:
                    if (Utils.doThisPrint(2)) Console.WriteLine("Connecting {0} to sonar at IP address: {1}, port {2}", name, ipAddress.ToString(), PortNumber);
                    socket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
                    // Tell TCP/IP our desired max buffer sizes
                    socket.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReceiveBuffer, maxBufSize);
                    socket.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.SendBuffer, maxBufSize);
                    // Disable Nagle algorithm so Timestamp messages will be delivered promptly
                    //         Note: For argument 3: A "1" disables Nagle, a "0" enables Nagle
                    socket.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.NoDelay, 1);
                    IPEndPoint ipEnd = new IPEndPoint(ipAddress, PortNumber);
                    socket.Connect(ipEnd);
                    //isConnected = true;  // Assume true if exception is not thrown
                    isConnected = isSocketConnected();
                    if (isConnected)
                    {
                        WaitForData();
                    }
                }
                catch (SocketException se)
                {
                    if(Utils.doThisPrint(1)) Console.WriteLine("{0} socket exception error: {1}", name, se.Message);
                    socket = null;
                    isConnected = false;
                    return false;
                }
            }
            else  // If socket is not null then we should already be connected
            {
                isConnected = isSocketConnected();
                if (isConnected)
                {
                    WaitForData();
                }
            }
            return isConnected;
        }

        public void closeSocket()
        {
            if (socket == null) return;  // Socket is already closed.
            if (Utils.doThisPrint(2)) Console.WriteLine("Closing {0} socket...", name);
            lock (thisLock)
            {
                try
                {
                    socket.Shutdown(SocketShutdown.Both);
                }
                catch (SocketException se)
                {
                    if(Utils.doThisPrint(3)) Console.WriteLine("{0} closeSocket exception error: {1}", name, se.Message);
                }
                socket.Close();
                socket = null;
                isConnected = false;
            }
        }

        // This is where we send a command to sonar
        public void sendCommand(SonarMessage.SonarMessageType message, SonarMessage.SonarCommandType command, 
                                byte subSystemNum, int ChannelNumber, object parameter, uint sizeofParameter)
        {
            if (socket == null) return;
            try
            {
                // First check and see if the send channel is open for writing
                // Note: This will not detect a disconnected cable nor a situation wherein
                //       the other side has shutdown unexpectedly
                // Allow 10 milliseconds (10000 microseconds) for other end to respond
                bool ChannelIsGood = socket.Poll(10000, SelectMode.SelectWrite);
                if (ChannelIsGood)
                {
                    packet = new Packet();
                    packet.msgHdr = new SonarMessage.Header();
                    // Fill-in the header
                    packet.msgHdr.StartOfHeader = 0x1601;
                    packet.msgHdr.VersionNumber = 0x0A;
                    packet.msgHdr.SessionID = 0;
                    packet.msgHdr.MessageType = (UInt16) message;
                    packet.msgHdr.CommandType = (byte) command;  // Get, Set, or Reply
                    packet.msgHdr.SubSystemNumber = subSystemNum;
                    packet.msgHdr.ChannelNumber = (byte) ChannelNumber;
                    packet.msgHdr.SequenceNumber = 0;
                    packet.msgHdr.reserved = 0;
                    //packet.msgHdr.ByteCount = (uint)Marshal.SizeOf(parameter);
                    packet.msgHdr.ByteCount = sizeofParameter;
                    if (message == SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_SELECT)
                    {
                        packet.msg = (char[]) parameter;
                    }
                    else
                    {
                        packet.msg = parameter;
                    }
                    //int packetSize = Marshal.SizeOf(packet.msg);
                    int packetSize = Marshal.SizeOf(packet.msgHdr) + (int) sizeofParameter;
                    byte[] buf = new byte[packetSize];
                    if (Utils.StructToBuf(packet, packetSize, buf))  // Convert struct packet to byte array
                    {
                        if (Utils.doThisPrint(5))
                        {
                            if (packet.msgHdr.ByteCount == 4)
                            {
                                if (parameter.GetType() == typeof(int))
                                {
                                    int int32Val = (int)parameter;
                                    Console.WriteLine("To   sonar cmd: ({0}) Msg: {1} Chn {2}, subSystem {3}, int32Val {4}",
                                                       cmdTypeStr[(int)command], getMessageTypeString(packet.msgHdr.MessageType),
                                                       packet.msgHdr.ChannelNumber, packet.msgHdr.SubSystemNumber, int32Val);
                                }
                                if (parameter.GetType() == typeof(uint))
                                {
                                    uint int32Val = (uint)parameter;
                                    Console.WriteLine("To   sonar cmd: ({0}) Msg: {1} Chn {2}, subSystem {3}, int32Val {4}",
                                                       cmdTypeStr[(int)command], getMessageTypeString(packet.msgHdr.MessageType),
                                                       packet.msgHdr.ChannelNumber, packet.msgHdr.SubSystemNumber, int32Val);
                                }
                            }
                            else
                            {
                                Console.WriteLine("To   sonar cmd: ({0}) Msg: {1} Chn {2}, subSystem {3}, ByteCount {4}",
                                                  cmdTypeStr[(int)command], getMessageTypeString(packet.msgHdr.MessageType),
                                                  packet.msgHdr.ChannelNumber, packet.msgHdr.SubSystemNumber, packet.msgHdr.ByteCount);
                            }
                        }

                        if (message == SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_SELECT)
                        {
                            packageParameter((char[])parameter, ref buf);
                        }
                        else if(sizeofParameter == 4)
                        {
                            if (parameter.GetType() == typeof(int))
                            {
                                int kParam = Convert.ToInt32((int)parameter);
                                //int kParam = (int)Convert.ToInt32((UInt32)parameter);
                                //uint kParam = (uint)Convert.ToInt32((uint)parameter);
                                packageIntegerParameter(kParam, ref buf);
                            }
                            if (parameter.GetType() == typeof(uint))
                            {
                                int kParam = Convert.ToInt32((uint)parameter);
                                //int kParam = (int)Convert.ToInt32((UInt32)parameter);
                                //uint kParam = (uint)Convert.ToInt32((uint)parameter);
                                packageIntegerParameter(kParam, ref buf);
                            }
                        }
                        else if (sizeofParameter == 12)
                        {
                            MainProgram.PingCouplingParametersType pct = (MainProgram.PingCouplingParametersType)parameter;
                            packagePingCouplingParametersType(pct, ref buf);
                        }
                        // Here is where we actually send the command to Sonar
                        int bytesSent = socket.Send(buf, packetSize, SocketFlags.None);
                        if (bytesSent != packetSize)
                        {
                            if (Utils.doThisPrint(1)) Console.WriteLine("Error 1 in {0}:sendCommand: Bytes sent: {1} != Bytes requested: {2}", 
                                                                        name, bytesSent, packetSize);
                        }
                    }
                    else
                    {
                        if (Utils.doThisPrint(1)) Console.WriteLine("Error 2 in {0}:sendCommand: Cannot convert structure to byte array.", name);
                    }
                }
                else
                {
                    if(Utils.doThisPrint(1)) Console.WriteLine("Error 3 in {0}:sendCommand: Socket connection is bad.", name);
                }
            }
            catch (SocketException se)
            {
                if(Utils.doThisPrint(1)) Console.WriteLine("Socket exception error in {0}:sendCommand: {1}", name, se.Message);
                closeSocket();
            } // try...catch
        }

        int waitOffset = 0;
        int waitTotalBytes = 0;

        public void WaitForData()
        {
            int maxRead = 0;

            //if(Utils.doThisPrint(3)) Console.WriteLine("In WaitForData()");
            try
            {
                if (pfnCallBack == null)
                {
                    pfnCallBack = new AsyncCallback(OnDataReceived);
                }
                // CSocketPacket theSocPkt = new CSocketPacket();
                theSocPkt.thisSocket = socket;
                //for(int k=0; k<theSocPkt.dataBuffer.Length; k++) 
                //{
                //    theSocPkt.dataBuffer[k]=0;
                //}
                // now start to listen for any data...
                if (waitOffset < SONAR_MSG_HDR_SIZE)
                    maxRead = SONAR_MSG_HDR_SIZE - waitOffset;
                else
                    maxRead = waitTotalBytes - waitOffset;

                if (maxRead + waitOffset > theSocPkt.dataBuffer.Length)
                    maxRead = 0;

                asynResult = socket.BeginReceive(theSocPkt.dataBuffer, waitOffset, maxRead, SocketFlags.None, pfnCallBack, theSocPkt);
                //if(Utils.doThisPrint(3)) Console.WriteLine("Receiver now listening for data from sonar.");
            }
            catch (SocketException se)
            {
                if(Utils.doThisPrint(1)) Console.WriteLine("{0} WaitForData() socket exception error: {1}", name, se.Message);
            }
        }

        // This is the interrupt handler that gets invoked whenever data comes from the server on the receive channel
        public void OnDataReceived(IAsyncResult asyn)
        {
            bool RxSocketisClosed;

            try
            {
                CSocketPacket theSockId = (CSocketPacket)asyn.AsyncState;
                //end receive...
                int iRx = 0;
                iRx = theSockId.thisSocket.EndReceive(asyn);

                if (iRx <= 0)
                {
                    // close the socket here
                    try
                    {
                        RxSocketisClosed = theSockId.thisSocket.Poll(10000, System.Net.Sockets.SelectMode.SelectRead);
                        if (RxSocketisClosed)  // We've received notification that the server has closed the socket
                        {
                            closeSocket();  // Close it on our end
                            if (Utils.doThisPrint(1)) Console.WriteLine("The {0} channel has been closed by sonar.", name);
                        }
                    }
                    catch (SocketException se)
                    {
                        closeSocket();  // Close it on our end
                        if (Utils.doThisPrint(1)) Console.WriteLine("{0}:OnDataReceived() socket exception error: {1}", name, se.Message);
                    }
                    return; // If iRx=0 there is nothing to do
                }

                // Partial header
                if (iRx + waitOffset < SONAR_MSG_HDR_SIZE)
                {
                    waitOffset += iRx;
                    WaitForData();
                    return;
                }

                if (iRx + waitOffset == SONAR_MSG_HDR_SIZE)
                {
                    int bytes;
                    bytes = theSocPkt.dataBuffer[12] + theSocPkt.dataBuffer[13] * 256 + theSocPkt.dataBuffer[14] * 65536 +
                        theSocPkt.dataBuffer[15] * 16777216;
                    if (bytes > 100000000)
                    {
                        // very bad
                        closeSocket();  // Close it on our end
                        if (Utils.doThisPrint(1)) Console.WriteLine("Socket error on the {0} channel.  Socket will be closed.", name);
                        return;
                    }
                    if (bytes > 0)
                    {
                        waitTotalBytes = bytes + 16;
                        waitOffset += iRx;
                        WaitForData();
                        return;
                    }
                }
                if (iRx + waitOffset < waitTotalBytes)
                {
                    waitOffset += iRx;
                    WaitForData();
                    return;
                }
                // have all the data
                waitOffset += iRx;
                if (waitOffset != waitTotalBytes)
                {
                    waitOffset = 0;
                    return;
                }

                // have complete message
                waitOffset = 0;
                theSockId.numBytes = waitTotalBytes;
                waitTotalBytes = 0;

                // Safety check:  Flag buffer overflow.  This should never happen.
                if (iRx > theSockId.bufSize)
                {
                    int numBytesLost = iRx - theSockId.bufSize;
                    iRx = theSockId.bufSize;
                    theSockId.numBytes = iRx;
                    if(Utils.doThisPrint(1)) Console.WriteLine("{0} buffer overflow error.  Number of bytes lost = {1}", name, iRx);
                }

                if (iRx > LargestMessageSize) LargestMessageSize = iRx;  // Track largest size required for debugging
                
                if (name.Equals("CommandChannel"))
                {
                    processCommandData();
                }

                if (name.Equals("DataChannel"))
                {
                    processData();
                }

                WaitForData();
            }
            catch (ObjectDisposedException ode)
            {
                System.Diagnostics.Debugger.Log(0, "1", "\nOnDataReceived() ObjectDisposedException error: "
                                                    + ode.Message);
            }
            catch (SocketException se)
            {
                closeSocket();  // Close it on our end
                disConnectCount++;
                if(Utils.doThisPrint(1)) Console.WriteLine("{0}:OnDataReceived() socket exception error #2: {1}", name, se.Message);
            }
        }

        // Process command-reply messages received from sonar.  These messages are in response to commands sent to sonar.
        private void processCommandData()
        {
            HdrAndData = getData();
            int numBytes = getNumBytes();
            SonarMessage.Header hdr = (SonarMessage.Header) Utils.BufToStruct(HdrAndData, typeof(SonarMessage.Header));
            int DataLength = (int)hdr.ByteCount;
            Data = RemoveHeader(HdrAndData, 16, DataLength);  // Data is a managed byte array
            if (Data == null)
            {
                Console.WriteLine("Null Data in processCommandData()");
                return;
            }

            // What kind of command is this - Set, Get, Reply, Error, other?  0=>Set, 1=>Get, 2=>Reply, Error=3
            int index = hdr.CommandType;
            if (index < 0 || index > 5) index = 6;
            if (Utils.doThisPrint(5) || MainProgram.getSystemTypeOnly)
            {
                //if (hdr.MessageType == (UInt16)SonarMessage.SonarMessageType.SONAR_MESSAGE_ALIVE)
                //{
                //    int sequenceNumber = 0;
                //   // Console.WriteLine("From sonar cmd: ({0}) Msg: {1} Sequence Number {2}",
                //     //                 cmdTypeStr[index], getMessageTypeString(hdr.MessageType), sequenceNumber);
                //}
                //else
                if (hdr.ByteCount == 4)
                {
                    int int32Val = 0;
                    byteArrayToInt(Data, ref int32Val);
                    if (MainProgram.getSystemTypeOnly && hdr.MessageType == (UInt16)SonarMessage.SonarMessageType.SONAR_MESSAGE_SYSTEM_TYPE)
                    {
                        Console.WriteLine("System Type = {0}", getSystemTypeString(int32Val));
                        // Close-out sockets before exiting
                        MainProgram.cmdChannel.closeSocket();
                        MainProgram.dataChannel.closeSocket();
                        Environment.Exit(0);
                    }
                    else
                    {
                        Console.WriteLine("From sonar cmd: ({0}) Msg: {1} Chn {2}, subSystem {3}, int32Val {4}",
                                          cmdTypeStr[index], getMessageTypeString(hdr.MessageType),
                                          hdr.ChannelNumber, hdr.SubSystemNumber, int32Val);
                    }
                }
                else
                {
                    Console.WriteLine("From sonar cmd: ({0}) Msg: {1} Chn {2}, subSystem {3}, ByteCount {4}",
                                      cmdTypeStr[index], getMessageTypeString(hdr.MessageType),
                                      hdr.ChannelNumber, hdr.SubSystemNumber, hdr.ByteCount);
                }
            }
            if(cmdTypeStr[index].Equals("ERROR", StringComparison.Ordinal)) return;

            if (hdr.MessageType == (UInt16)SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_LIST)
            {
                SonarMessage.SonarMessagePingType smpt = new SonarMessage.SonarMessagePingType();
                SonarMessage.SonarMessagePingEnhancedType smpet = new SonarMessage.SonarMessagePingEnhancedType();
                int smptSize = Marshal.SizeOf(typeof(SonarMessage.SonarMessagePingType));
                if (Utils.doThisPrint(5)) Console.WriteLine(" ");
                if(Utils.doThisPrint(5)) Console.WriteLine("{0} Pulse list returned by sonar:",
                                          getSubSystemString(hdr.SubSystemNumber));
                if (hdr.ByteCount > Marshal.SizeOf(typeof(SonarMessage.SonarMessagePingType)))  // We have a ping list
                {
                    IntPtr dptr = Marshal.AllocHGlobal(DataLength);
                    unsafe
                    {
                        //byte* dst = (byte*)dptr.ToPointer();
                        // Copy the array to unmanaged memory.
                        Marshal.Copy(Data, 0, dptr, DataLength);
                        byte[] buf = new byte[smptSize];
                        int k = 0;
                        while (k < SonarMessage.MAX_NUM_ENTRIES_IN_PULSE_LIST)
                        {
                            if (dptr == null) break;
                            Marshal.Copy(dptr, buf, 0, smptSize);
                            smpt = (SonarMessage.SonarMessagePingType)Utils.BufToStruct(buf, typeof(SonarMessage.SonarMessagePingType));
                           // smpet = (SonarMessage.SonarMessagePingEnhancedType)Utils.BufToStruct(buf, 
                             //                     typeof(SonarMessage.SonarMessagePingEnhancedType));
                            if (smpt.fileName == "")  // Hmm...  Apparently, in C# a null string is not the same as ""
                            {
                                break;
                            }
                            else
                            {
                                if(Utils.doThisPrint(5)) Console.WriteLine("{0}, PRR={1} Hz, {2}-{3} Hz, {4} msec, ID={5}", smpt.fileName, smpt.maxPingRate,
                                                  smpt.fMin, smpt.fMax, smpt.time, smpt.pulseID);
                            }
                            if (hdr.SubSystemNumber == MainProgram.SUB_BOTTOM)
                            {
                                MainProgram.subBottomPulseList.pulseList[k] = smpt;
                            }
                            if (hdr.SubSystemNumber == MainProgram.SIDE_SCAN_LOW)
                            {
                                MainProgram.sideScanLowPulseList.pulseList[k] = smpt;
                            }
                            if (hdr.SubSystemNumber == MainProgram.SIDE_SCAN_HIGH)
                            {
                                MainProgram.sideScanHighPulseList.pulseList[k] = smpt;
                            }
                            dptr += smptSize;
                            k++;
                        }
                        if (hdr.SubSystemNumber == MainProgram.SUB_BOTTOM)
                        {
                            MainProgram.subBottomPulseList.pulseListDefined = true;
                            MainProgram.subBottomPulseList.numPulses = k;
                        }
                        if (hdr.SubSystemNumber == MainProgram.SIDE_SCAN_LOW)
                        {
                            MainProgram.sideScanLowPulseList.pulseListDefined = true;
                            MainProgram.sideScanLowPulseList.numPulses = k;
                        }
                        if (hdr.SubSystemNumber == MainProgram.SIDE_SCAN_HIGH)
                        {
                            MainProgram.sideScanHighPulseList.pulseListDefined = true;
                            MainProgram.sideScanHighPulseList.numPulses = k;
                        }
                        MainProgram.pulseListDefined = true;
                    }   // unsafe
                }
                else // Just one ping item
                {
                    smpt = (SonarMessage.SonarMessagePingType) Utils.BufToStruct(Data, typeof(SonarMessage.SonarMessagePingType));
                    if (smpt.fileName != "")  // Hmm...  Apparently, in C# a null string is not the same as ""
                    {
                        if(Utils.doThisPrint(4)) Console.WriteLine("{0}, PRR={1} Hz, {2}-{3} Hz, {4} msec, ID={5}", smpt.fileName, smpt.maxPingRate,
                                          smpt.fMin, smpt.fMax, smpt.time, smpt.pulseID);
                    }
                    if (hdr.SubSystemNumber == MainProgram.SUB_BOTTOM)
                    {
                        MainProgram.subBottomPulseList.pulseList[0] = smpt;
                        MainProgram.subBottomPulseList.pulseListDefined = true;
                        MainProgram.subBottomPulseList.numPulses = 1;
                    }
                    if (hdr.SubSystemNumber == MainProgram.SIDE_SCAN_LOW)
                    {
                        MainProgram.sideScanLowPulseList.pulseList[0] = smpt;
                        MainProgram.sideScanLowPulseList.pulseListDefined = true;
                        MainProgram.sideScanLowPulseList.numPulses = 1;
                    }
                    if (hdr.SubSystemNumber == MainProgram.SIDE_SCAN_HIGH)
                    {
                        MainProgram.sideScanHighPulseList.pulseList[0] = smpt;
                        MainProgram.sideScanHighPulseList.pulseListDefined = true;
                        MainProgram.sideScanHighPulseList.numPulses = 1;
                    }
                }
            }   // SONAR_MESSAGE_PING_LIST
        }

        // Process sonar data received from sonar
        private void processData()
        {
            Object thisLock2 = new Object();
            //StringBuilder dataLog;

            //lock (thisLock2)  // This may no longer be necessary.  Remove if possible.
            {
                HdrAndData = getData();
                int numBytes = getNumBytes();
                SonarMessage.Header hdr = (SonarMessage.Header)Utils.BufToStruct(HdrAndData, typeof(SonarMessage.Header));
                if (hdr.StartOfHeader != 0x1601)
                {
                    string msg;
                    msg = String.Format("Invalid header.  Dropping {0} bytes on the floor.", numBytes);
                    Console.WriteLine(msg);
                    //dataLog = MainProgram.getDataLog();
                    //dataLog.Append(msg + MainProgram.NL);
                    return;
                }
                int DataLength = (int)hdr.ByteCount;
                Data = RemoveHeader(HdrAndData, 16, DataLength);  // Data is a managed byte array
                if (Data == null)
                {
                    //Console.WriteLine("In processData(), Glitch1: DataLength = {0}.", DataLength);
                    return;
                }
                getPingNum = (pingNumStruct)Utils.BufToStruct(Data, typeof(pingNumStruct));
                uint pingNum = getPingNum.pingNum;
                if (hdr.MessageType==80 && pingNum>500000)
                {
                    Console.WriteLine("In processData(), Glitch2: pingNum = {0}", pingNum);
                }

                string currentData;

                if (hdr.MessageType == 80)
                {
                    currentData = String.Format("From sonar data: subsys {0}, Ch {1}, msg {2}, Bytes {3}, Ping {4} ",
                                                                hdr.SubSystemNumber, hdr.ChannelNumber, hdr.MessageType, hdr.ByteCount, pingNum);
                    // * * * * * * *   H E R E   I T   IS   -   R A W   D A T A   F R O M   S O N A R   * * * * * * * * //
                    int SonarDataLength = DataLength - SDM_HDR_SIZE;
                    SonarData = RemoveHeader(Data, SDM_HDR_SIZE, SonarDataLength);  
                    // C# note: SonarData is a managed byte array
                    // ToDo: Do your sonar data processing here on byte array SonarData having length SonarDataLength
                    // Your custom code goes here...

                    // End of sonar data processing
                }
                else
                {
                    if (hdr.ByteCount == 4)
                    {
                        int int32Val = 0;
                        byteArrayToInt(Data, ref int32Val);
                        currentData = String.Format("From sonar data: subsys {0}, Ch {1}, msg {2}, int32Val {3}",
                                                          hdr.SubSystemNumber, hdr.ChannelNumber, hdr.MessageType, int32Val);
                    }
                    else
                    {
                        currentData = String.Format("From sonar data: subsys {0}, Ch {1}, msg {2}, Bytes {3}",
                                                          hdr.SubSystemNumber, hdr.ChannelNumber, hdr.MessageType, hdr.ByteCount);
                    }
                    // ToDo: Handle other message types, e.g. Nav data here
                }
                if (Utils.doThisPrint(4)) Console.WriteLine(currentData);
                //dataLog = MainProgram.getDataLog();
                //dataLog.Append(currentData + MainProgram.NL);
            }
        }

        private string getMessageTypeString(UInt16 msgType)
        {
            switch (msgType)
            {
                case (UInt16) SonarMessage.SonarMessageType.SONAR_MESSAGE_ALIVE:
                    return "Keep Alive..............";
                case (UInt16) SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_SELECT:
                    return "Ping Select.............";
                case (UInt16) SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_RATE:
                    return "Ping Rate...............";
                case (UInt16) SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_RANGE:
                    return "Ping Range..............";
                case (UInt16) SonarMessage.SonarMessageType.SONAR_MESSAGE_PING:
                    return "Ping Enable/Disable.....";
                case (UInt16) SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_FISH_LIST:
                    return "Ping Fish List..........";
                case (UInt16) SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_TRIGGER:
                    return "Ping Trigger............";
                case (UInt16) SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_COUPLING_PARAMETERS:
                    return "Ping Coupling Parameters";
                case (UInt16) SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_MODE:
                    return "Ping Mode...............";
                case (UInt16) SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_LIST:
                    return "Ping List...............";
                case (UInt16)SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_LIST_ENHANCED:
                    return "Ping List Enhanced......";
                case (UInt16)SonarMessage.SonarMessageType.SONAR_MESSAGE_PING_MAX_SAMPLES:
                    return "Ping Max Samples........";
                case (UInt16)SonarMessage.SonarMessageType.SONAR_MESSAGE_DATA_ACTIVE:
                    return "Sonar Msg Data Active...";
                case (UInt16)SonarMessage.SonarMessageType.SONAR_MESSAGE_SYSTEM_TYPE:
                    return "Sonar Msg System Type...";
                default:
                    return "Unknown.................";
            }
        }

        public bool isSocketConnected()
        {
            if (socket == null) return false;

            // This is how you can determine whether a socket is still connected.
            bool blockingState = socket.Blocking;
            try
            {
                byte[] tmp = new byte[1];

                socket.Blocking = false;
                socket.Send(tmp, 0, 0);
                return true;
            }
            catch (SocketException e)
            {
                // 10035 == WSAEWOULDBLOCK
                if (e.NativeErrorCode.Equals(10035))
                    if(Utils.doThisPrint(3)) Console.WriteLine("{0} channel socket connection test: Still Connected, but the Send would block", name);
                else
                {
                    if(Utils.doThisPrint(3)) Console.WriteLine("Disconnected: error code {0}!", e.NativeErrorCode);
                }
            }
            finally
            {
                socket.Blocking = blockingState;
            }

            return socket.Connected;
        }

        public byte[] getData()
        {
            return theSocPkt.dataBuffer;
        }

        public int getNumBytes()
        {
            return theSocPkt.numBytes;
        }

        private byte[] RemoveHeader(byte[] HdrAndData, int HdrSize, int numBytes)
        {
            int dataLen = HdrAndData.Length - HdrSize;
            if (dataLen > 0 && dataLen >= numBytes)
            {
                byte[] data = new byte[numBytes];
                for (int k = 0; k < numBytes; k++)
                {
                    data[k] = HdrAndData[HdrSize + k];
                }
                return data;
            }
            else
            {
                return null;
            }
        }

        public static string getSubSystemString(int subSystemNumber)
        {
            string sb = "Sub Bottom";
            string ssl = "Side Scan Low";
            string ssh = "Side Scan High";
            string unk = "Unknown subsystem";

            if (subSystemNumber == MainProgram.SUB_BOTTOM) return sb;
            if (subSystemNumber == MainProgram.SIDE_SCAN_LOW) return ssl;
            if (subSystemNumber == MainProgram.SIDE_SCAN_HIGH) return ssh;
            return unk;
        }

        public string getVersionInfo()
        {
            versionInfo = _versionInfo.Remove(0, 9);
            string endStr = " mps $";
            char[] end = endStr.ToCharArray();
            versionInfo = versionInfo.TrimEnd(end);
            return versionInfo;
        }

        // @mps
        void dumpBuf(byte[] buf)
        {
            for (int k = 0; k < buf.Length; k++)
            {
                Console.WriteLine("{0}: {1}", k, buf[k]);
            }
        }

        // @mps
        void packageParameter(char[] parameter, ref byte[] buf)
        {
            string fname = new string(parameter);
            int j, k;
            for(k=16; k<buf.Length; k++) buf[k] = 0;
            for (k = 0; k < fname.Length; k++)
            {
                j = k + 16;
                buf[j] = (byte)fname[k];
            }
        }

        // Note: kParam must be positive.
        void packageIntegerParameter(int kParam, ref byte[] buf)
        {
            int rem = kParam;
            byte[] a = new byte[4];
            int j = 0;
            int k = 0;

            while (rem > 0)
            {
                a[k] = (byte) (rem % 0x100);
                rem /= 0x100;
                k = k + 1;
                if (k == 4) break;
            }

            for (k = 0; k < 4; k++)
            {
                j = k + 16;
                buf[j] = a[k];
            }
        }

        void packagePingCouplingParametersType(MainProgram.PingCouplingParametersType pct, ref byte[] buf)
        {
            byte[] b0 = new byte[4];
            byte[] b1 = new byte[4];
            byte[] b2 = new byte[4];
            intToByteArray((int)pct.subSystem, ref b0);
            intToByteArray((int)pct.triggerDivisor, ref b1);
            intToByteArray((int)pct.triggerDelay, ref b2);

            int j, k;
            for (k = 0; k < 4; k++)
            {
                j = k + 16;
                buf[j] = b0[k];
                j = k + 20;
                buf[j] = b1[k];
                j = k + 24;
                buf[j] = b2[k];
            }
        }

        void intToByteArray(int value, ref byte[] buf)
        {
            int rem = value;
            int k = 0;

            while (rem > 0)
            {
                buf[k] = (byte)(rem % 0x100);
                rem /= 0x100;
                k = k + 1;
                if (k == 4) break;
            }
        }

        void byteArrayToInt(byte[] buf, ref int value)
        {
            value = 0;
            int scale = 1;
            if (buf.Length != 4) return;
            for (int k = 0; k < 4; k++)
            {
                value += scale * Convert.ToInt32(buf[k]);
                scale *= 256;
            }
        }

        void writeLostData(string fname, byte[] buf, int numBytes)
        {
            //using (FileStream fs = new FileStream(fname, FileMode.Open, FileAccess.Write))
            using (FileStream fs = new FileStream(fname, FileMode.CreateNew))
            {
                // Create the writer for data.
                using (BinaryWriter bw = new BinaryWriter(fs))
                {
                    bw.Write(buf);
                }
            }
            Console.WriteLine("{0} bytes written to {1}", numBytes, fname);
        }

        public string getSystemTypeString(int index)
        {
            int numStrings = systemTypeStr.Length;
            if (index < 0 || index >= numStrings)
            {
                return "Unknown";
            }
            else
            {
                return systemTypeStr[index];
            }
        }

    }  // End class CPortIO
}