SafeDispatch/SipComponent/RTPSender2.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Net;
using System.Net.Sockets;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace SipComponent
{
    class RTPSender2
    {
        #region Private Fields

        private UdpClient _client;           // UDP client which connects to RTP stream
        private IPEndPoint _endPoint;        // End point to send to

        readonly int _bitRate;
        Random _random = new Random();

        ushort _sequenceNumber;
        uint _timestamp;
        byte[] _ssrc = new byte[4];

        TypeOfCall _typeOfCall = TypeOfCall.HALF_DUPLEX;

        System.Timers.Timer _timerHeartbeat, _timerHearbeatQuery, _timerPTTRequests;
        
        /// <summary>
        /// <para>RTP voice packet duration, in ms</para>
        /// <para>60ms for AMBE, 20ms or 60ms for G.711</para>
        /// </summary>
        private int _packetDuration = 20;

        private int _nbOfSentPTTrequests = 0;
        private object _lockerPTTRequest = new object();

        // Timestamp
        System.Diagnostics.Stopwatch _timestampStopwatch = new System.Diagnostics.Stopwatch();
        object _lockerTimestamp = new object();
        uint _initialTimestamp;
        long _factor;

        // DMR fields
        //

        DMR dmrClass = null;
        int _dmrSource, _dmrDestination;
        TSNinfo _tsnInfo = null;

        SenderStatus _voiceStatus = SenderStatus.Ended;

        // Voice buffer
        VoiceBuffer2 _PCMAvoiceBuffer;
        AutoResetEvent _autoResetEvent = new AutoResetEvent(false);
        const int _minTimeToSleep = 5;
        const int _maxTimeToSleep = 19;
        int _timeToSleep;
        Task _takeBytesFromBufferTask = null;

        object _lockerOnSend = new object();

        private bool _stopped = false;

        private RtpCallType _halfDuplexCallType = RtpCallType.Private;

        #endregion


        #region Public Properties

        public int DMR_Destination
        {
            get { return _dmrDestination; }
        }

        public TSNstatus SentTSNstatus
        {
            get {
                if (_tsnInfo != null)
                    return _tsnInfo.Status;
                else
                    return TSNstatus.Ended;
            }
        }

        internal TypeOfCall TypeOfCall
        {
            get { return _typeOfCall; }
        }

        #endregion

        internal RTPSender2(UdpClient udpClient, int audioBitRate, IPEndPoint endPointToSend, int dmrSourceID, int dmrDestinationID, bool sendHeartbeatQuery, TypeOfCall typeOfCall, 
            RtpCallType halfDuplexCallType = RtpCallType.Private)
        {
            _client = udpClient;
            
            _bitRate = audioBitRate;
            _endPoint = endPointToSend;
            _sequenceNumber = (ushort)_random.Next();
            _timestamp = _initialTimestamp = (uint)_random.Next(1000);
            _random.NextBytes(_ssrc);

            // Dmr
            dmrClass = new DMR();
            _dmrSource = dmrSourceID;
            _dmrDestination = dmrDestinationID;
            _halfDuplexCallType = halfDuplexCallType;

            // Timestamp
            // get the stopwatch's nb of ticks per milisecond 
            long nbOfTicksPerMs = System.Diagnostics.Stopwatch.Frequency / 1000;
            // we need a timer with 8000 ticks per second (8 ticks/ms)
            // _factor represents how many times the stopwatch's frequency is bigger than the required frequency
            // we will use this when calculating the timestamp
            _factor = nbOfTicksPerMs / 8;
            _timestampStopwatch.Start();

            _timeToSleep = _minTimeToSleep + _maxTimeToSleep / 2;

            _typeOfCall = typeOfCall;
            
            if (_typeOfCall == TypeOfCall.HALF_DUPLEX)
            {
                // Timer Ptt request
                _timerPTTRequests = new System.Timers.Timer(DMR.T_request);
                _timerPTTRequests.Elapsed += _timerPTTRequests_Elapsed;

                if (sendHeartbeatQuery)
                {
                    // Timer hearbeatQuery
                    _timerHearbeatQuery = new System.Timers.Timer(DMR.T_heartbeat * 1000);
                    _timerHearbeatQuery.Elapsed += _timerHearbeatQuery_Elapsed;
                    _timerHearbeatQuery_Elapsed(null, null);
                    _timerHearbeatQuery.Start();
                }

                // Timer heartbeat
                _timerHeartbeat = new System.Timers.Timer(DMR.T_heartbeat * 1000);
                _timerHeartbeat.Elapsed += _timerHeartbeat_Elapsed;
            }
            else if (_typeOfCall == TypeOfCall.FULL_DUPLEX)
            {
                CreateVoiceBuffer();
            }
            
            
        }

        void _timerPTTRequests_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            if (!SendPTTRequest())
            {
                _timerPTTRequests.Stop();
                lock (_lockerPTTRequest)
                {
                    if (_tsnInfo.Status == TSNstatus.Requesting)
                    {
                        // Send PTT request returned false because max nb of requests was sent 
                        _tsnInfo.Status = TSNstatus.Denied;
                    }
                    else
                    {
                        // Send PTT request returned false because the request was granted or denied
                        return;
                    }
                }
                OnSentMaxNumberOfPTTrequests();
            }
        }

        void _timerHearbeatQuery_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            SendPTTHeartbeatQuery();
        }

        void _timerHeartbeat_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            SendPTTHeartbeat();
        }

        #region Public Methods

        public void SendAudio(byte[] audioBuffer, int bufferLength, AudioFormat format)
        {
            if (_typeOfCall == TypeOfCall.HALF_DUPLEX)
            {
                if (_tsnInfo.Status == TSNstatus.Granted)
                {
                    if (_voiceStatus == SenderStatus.Ended)
                    {
                        // this is the first packet
                        // Create voice buffer
                        CreateVoiceBuffer();
                        _voiceStatus = SenderStatus.SendingVoice;
                        OnSendingPTTStart();
                    }

                    byte[] PCMAbuffer = audioBuffer;
                    if (format == AudioFormat.PCM)
                    {
                        // Encode in PCMA format
                        PCMAbuffer = PCMA_Encode(audioBuffer, 0, bufferLength);
                    }

                    // Add to voice buffer
                    _PCMAvoiceBuffer.AddBytes(PCMAbuffer);
                }
            }
            else if (_typeOfCall == TypeOfCall.FULL_DUPLEX)
            {

                byte[] PCMAbuffer = audioBuffer;
                if (format == AudioFormat.PCM)
                {
                    // Encode in PCMA format
                    PCMAbuffer = PCMA_Encode(audioBuffer, 0, bufferLength);
                }

                // Add to voice buffer
                _PCMAvoiceBuffer.AddBytes(PCMAbuffer);
            }
        }

        public void StartSendingPTTrequests(byte tsn)
        {
            if (!_timerPTTRequests.Enabled)
            {
                lock (_lockerPTTRequest)
                {
                    _tsnInfo = new TSNinfo(tsn);
                    _nbOfSentPTTrequests = 0;
                    _tsnInfo.Status = TSNstatus.Requesting;
                }

                _timerPTTRequests_Elapsed(null, null);
                _timerPTTRequests.Start();
            }
        }

        public void StopSendingPTTrequests(bool granted)
        {
            if (_timerPTTRequests != null)
            {
                if (_timerPTTRequests.Enabled)
                {
                    lock (_lockerPTTRequest)
                    {
                        if (granted)
                            _tsnInfo.Status = TSNstatus.Granted;
                        else
                            _tsnInfo.Status = TSNstatus.Denied;
                    }
                }
            }
        }



        public bool IsRequestingPTT(byte tsn)
        {
            lock (_lockerPTTRequest)
            {
                return (_tsnInfo.TSN == tsn && _tsnInfo.Status == TSNstatus.Requesting);
            }
        }
        public void StartSendingHeartbeat()
        {
            if (_timerHeartbeat != null)
            {
                if (!_timerHeartbeat.Enabled)
                {
                    SendPTTHeartbeat();
                    _timerHeartbeat.Start();
                }
            }
        }

        public void StopSendingHeartbeat()
        {
            if (_timerHeartbeat != null)
            {
                if (_timerHeartbeat.Enabled)
                {
                    _timerHeartbeat.Stop();
                }
            }

        }
        public void StopSendingHeartbeatQuery()
        {
            if (_timerHearbeatQuery != null)
            {
                if (_timerHearbeatQuery.Enabled)
                {
                    _timerHearbeatQuery.Stop();
                }
            }
        }

        public void SendPTTEnd()
        {
            if (_voiceStatus == SenderStatus.SendingVoice)
            {
                _voiceStatus = SenderStatus.Ended;
                StopUsingVoiceBuffer();
            }

            _tsnInfo.Status = TSNstatus.Ended;

            GetNewTimestamp(ref _timestamp);
            byte[] pttEnd = dmrClass.GeneratePTTEnd(
                _dmrDestination,
                _dmrSource,
                _tsnInfo.TSN,
                ref _sequenceNumber,
                _timestamp,
                _ssrc);
            try
            {
                lock (_lockerOnSend)
                {
                    _client.Send(pttEnd, pttEnd.Length, _endPoint);
                }
            }
            catch (Exception ex)
            {
                //SafeMobileLib.Utils.WriteLine("Eroare la SendPttEnd\n" + ex.Message, ConsoleColor.Red);
                ; // TO DO
                throw;
            }
        }

        //public void SendPTTStart()
        //{
        //    lock (_lockerSendingVoice)
        //    {
        //        _voiceStatus = SenderStatus.SendingVoice;
        //    }
        //    byte[] pttStart = dmrClass.GeneratePTTStart(
        //        _dmrDestination,
        //        _dmrSource,
        //        _TSN,
        //        ref _sequenceNumber,
        //        _timestamp,
        //        _ssrc);
        //    try
        //    {
        //        _client.Send(pttStart, pttStart.Length, _endPoint);
        //    }
        //    catch (Exception ex)
        //    {
        //        //SafeMobileLib.Utils.WriteLine("Eroare la SendPttStart\n" + ex.Message, ConsoleColor.Red);
        //        ; // TO DO
        //    }
        //}

        public void SendPTTGrant(byte tsn)
        {
            byte[] pttGrant = dmrClass.GeneratePTTGrant(
                _dmrDestination,
                _dmrSource,
                tsn,
                ref _sequenceNumber,
                _timestamp,
                _ssrc);
            lock (_lockerOnSend)
            {
                _client.Send(pttGrant, pttGrant.Length, _endPoint);
            }
        }

        public void SendPTTDeny(byte tsn)
        {
            byte[] pttDeny = dmrClass.GeneratePTTDeny(
                _dmrDestination,
                _dmrSource,
                tsn,
                ref _sequenceNumber,
                _timestamp,
                _ssrc);
            lock (_lockerOnSend)
            {
                _client.Send(pttDeny, pttDeny.Length, _endPoint);
            }
        }

        public void Stop()
        {
            _stopped = true;
            StopSendingHeartbeatQuery();
            StopSendingHeartbeat();
            StopSendingPTTrequests(false);
            StopUsingVoiceBuffer();
        }

        

        #endregion

        #region PTT Control Private Functions

        private void SendPTTHeartbeatQuery()
        {
            GetNewTimestamp(ref _timestamp);
            byte[] heartbeatQuery = dmrClass.GeneratePTTHeartbeatQuery(
                _dmrDestination,
                _dmrSource,
                ref _sequenceNumber,
                _timestamp,
                _ssrc);
            try
            {
                lock (_lockerOnSend)
                {
                    _client.Send(heartbeatQuery, heartbeatQuery.Length, _endPoint);
                }
            }
            catch (Exception ex)
            {
                //SafeMobileLib.Utils.WriteLine("Eroare la SendPttHeartbeatQuery\n" + ex.Message, ConsoleColor.Red);
                ; // TO DO
                throw;
            }
        }

        private void SendPTTHeartbeat()
        {
            GetNewTimestamp(ref _timestamp);
            byte[] heartbeat = dmrClass.GeneratePTTHeartbeat(
                _dmrDestination,
                _dmrSource,
                ref _sequenceNumber,
                _timestamp,
                _ssrc);
            lock (_lockerOnSend)
            {
                _client.Send(heartbeat, heartbeat.Length, _endPoint);
            }
        }



        private bool SendPTTRequest()
        {
            GetNewTimestamp(ref _timestamp);
            byte[] pttRequest = dmrClass.GeneratePTTRequest(
                _dmrDestination,
                _dmrSource,
                _tsnInfo.TSN,
                ref _sequenceNumber,
                _timestamp,
                _ssrc);
            try
            {
                lock (_lockerPTTRequest)
                {
                    if (_nbOfSentPTTrequests >= DMR.T_numreq || _tsnInfo.Status != TSNstatus.Requesting)
                        return false;
                    lock (_lockerOnSend)
                    {
                        _client.Send(pttRequest, pttRequest.Length, _endPoint);
                    }
                    _nbOfSentPTTrequests++;
                }
                return true;
            }
            catch (Exception ex)
            {
                //SafeMobileLib.Utils.WriteLine("Eroare la SendPttReq\n" + ex.Message, ConsoleColor.Red);
                ; // TO DO
                throw;
            }
        }

        #endregion


        #region Voice buffer

        private void CreateVoiceBuffer()
        {
            _PCMAvoiceBuffer = new VoiceBuffer2(3000);
            _PCMAvoiceBuffer.HalfFull += _voiceBuffer_HalfFull;
            _PCMAvoiceBuffer.Full += _voiceBuffer_Full;
            _PCMAvoiceBuffer.Empty += _voiceBuffer_Empty;
        }

        void _voiceBuffer_Empty(object sender, EventArgs e)
        {
            if (_timeToSleep < _maxTimeToSleep)
                _timeToSleep++;
   
        }

        void _voiceBuffer_Full(object sender, EventArgs e)
        {
            if (_timeToSleep > _minTimeToSleep)
                _timeToSleep--;
        }

        void _voiceBuffer_HalfFull(object sender, EventArgs e)
        {
            // Create task that sends 160 bytes of PCMA voice each 20 ms
            _takeBytesFromBufferTask = Task.Factory.StartNew(() => 
            {
                if (_typeOfCall == TypeOfCall.HALF_DUPLEX)
                {
                    // Send Start packet
                    byte[] b = new byte[160];
                    if (_PCMAvoiceBuffer.TakeBytes(b) == 160)
                    {
                        GetNewTimestamp(ref _timestamp);
                        byte[] packetStart = GenerateRTPAudio(b, _tsnInfo.TSN, _halfDuplexCallType, true);
                        // Send
                        lock (_lockerOnSend)
                        {
                            _client.Send(packetStart, packetStart.Length, _endPoint);
                        }
                        // Sleep?
                        Thread.Sleep(_timeToSleep);
                    }
                    else throw new SipClassException("Eroare la trimiterea pachetului Start PTT");
                }
                int nbOfBytesTaken = 0;
                do
                {
                    byte[] buff = new byte[160];
                    nbOfBytesTaken = _PCMAvoiceBuffer.TakeBytes(buff);
                    if (nbOfBytesTaken == 160)
                    {
                        // Get the timestamp
                        GetNewTimestamp(ref _timestamp);
                        // Create RTP packet
                        byte[] packet = (_typeOfCall == TypeOfCall.HALF_DUPLEX) ? GenerateRTPAudio(buff, _tsnInfo.TSN, _halfDuplexCallType, false) : 
                            GenerateRTPAudioForZoiper(buff);
                        // Send
                        lock (_lockerOnSend)
                        {
                            _client.Send(packet, packet.Length, _endPoint);
                        }
                        // Sleep?
                        Thread.Sleep(_timeToSleep);
                    }
                }
                while (!_stopped && nbOfBytesTaken != 0);
            });
        }

        private void StopUsingVoiceBuffer()
        {
            //_autoResetEvent.Set();
            if (_PCMAvoiceBuffer != null)
            {
                _PCMAvoiceBuffer.StopAdding();
                if (_takeBytesFromBufferTask != null)
                    _takeBytesFromBufferTask.Wait();
                _PCMAvoiceBuffer.HalfFull -= _voiceBuffer_HalfFull;
                _PCMAvoiceBuffer.Full -= _voiceBuffer_Full;
                _PCMAvoiceBuffer.Empty -= _voiceBuffer_Empty;
            }
        }

        #endregion



        #region Private Helper Functions

        private void GetNewTimestamp(ref uint timestamp)
        {
            lock (_lockerTimestamp)
            {
                uint exactNewTimestamp = _initialTimestamp + (uint)(_timestampStopwatch.ElapsedTicks / _factor);
                if (exactNewTimestamp - timestamp > 320)
                    timestamp = exactNewTimestamp;
            }

        }

        private void IncrementTimestamp(ref uint timestamp, int nrOfAudioBytes, int bitRate)
        {
            timestamp += (uint)(2 * nrOfAudioBytes * 8 / bitRate);
        }

        private byte[] PCMA_Encode(byte[] data, int offset, int length)
        {
            byte[] encoded = new byte[length / 2];
            int outIndex = 0;
            for (int n = 0; n < length; n += 2)
            {
                encoded[outIndex++] = NAudio.Codecs.ALawEncoder.LinearToALawSample(BitConverter.ToInt16(data, offset + n));
            }
            return encoded;
        }

        private byte[] GenerateRTPAudio(byte[] audioData, byte TSN, RtpCallType rtpCallType, bool isStartPacket)
        {
            // Headerul RTP fix 12 octeti
            // Headerul Extins DMR = 4 octeti (Code Type + extension length) +
            //                                 32 octeti (8 * 4)
            //                               = 36 octeti
            // 12 + 36 = 48 octeti
            // Se pare ca pachetele RTP care contin voce contin 160 octeti de voce (Pentru SIMOCO)
            // Conform document DMR, pt G.711 se pot trimite 20 ms sau 60 ms de voce in pachet
            // => 160 bytes sau 480 bytes de audio G.711
            int len = audioData.Length;
            if (len == 160 || len == 480)
            {
                byte[] rtpPacket = new byte[len + 48];

                lock (_lockerTimestamp)
                {
                    dmrClass.AddStandardVoiceRTPHeaders(rtpPacket, ref _sequenceNumber, _timestamp, _ssrc);
                    IncrementTimestamp(ref _timestamp, len, _bitRate);
                }
                dmrClass.AddStandardDMRExtension(rtpPacket, _dmrDestination, _dmrSource, rtpCallType, StartReason.OverContinues, EndReason.OverContinues, Priority.Zero);
                if (isStartPacket)
                    dmrClass.AddManufacturerSpecificExtension(rtpPacket, PTT_Type.Start, TSN, 0);
                else
                    dmrClass.AddManufacturerSpecificExtension(rtpPacket, PTT_Type.Progress, TSN, 0);

                // Copiez data audio
                Array.Copy(audioData, 0, rtpPacket, 48, len);
                return rtpPacket;
            }
            else
                throw new ApplicationException("Dimensiune neacceptata pentru bufferul de voce");
        }

        private byte[] GenerateRTPAudioForZoiper(byte[] audioData)
        {
            // Headerul RTP fix 12 octeti
            // Headerul Extins DMR = 4 octeti (Code Type + extension length) +
            //                                 32 octeti (8 * 4)
            //                               = 36 octeti
            // 12 + 36 = 48 octeti
            // Se pare ca pachetele RTP care contin voce contin 160 octeti de voce (Pentru SIMOCO)
            // Conform document DMR, pt G.711 se pot trimite 20 ms sau 60 ms de voce in pachet
            // => 160 bytes sau 480 bytes de audio G.711
            int len = audioData.Length;

            byte[] rtpPacket = new byte[len + 12];
            lock (_lockerTimestamp)
            {
                dmrClass.AddStandardVoiceRTPHeadersZoipper(rtpPacket, ref _sequenceNumber, _timestamp, _ssrc);
                IncrementTimestamp(ref _timestamp, len, _bitRate);
            }
            // Copiez data audio
            Array.Copy(audioData, 0, rtpPacket, 12, len);
            return rtpPacket;
        }


        #endregion

        #region Events

        public event EventHandler SentMaxNumberOfPTTrequests;
        public event EventHandler SendingPTTStart;

        private void OnSentMaxNumberOfPTTrequests()
        {
            EventHandler handler = SentMaxNumberOfPTTrequests;
            if (handler != null)
                handler(this, new EventArgs());

        }
        private void OnSendingPTTStart()
        {
            EventHandler handler = SendingPTTStart;
            if (handler != null)
                handler(this, new EventArgs());
        }

        #endregion


        private enum SenderStatus
        {
            SendingVoice,
            Ended
        }

    }

    internal enum TSNstatus
    {
        Requesting,
        Granted,
        Denied, 
        Ended
    }

    internal class TSNinfo
    {
        private TSNstatus _status = TSNstatus.Ended;

        public byte TSN { get; private set; }
        public TSNstatus Status {
            get { return _status; }
            set { _status = value; }
        }

        public TSNinfo(byte tsn)
        {
            TSN = tsn; 
        }
    }
}