HawkeyeVision/framework/Inspectron.HawkEye/Protocol/ImageServerTCP.cs

using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Net;
using System.Net.Sockets;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using Inspectron.HawkEye.Protocol.Interfaces;
using Inspectron.HawkEye.UDPB;
using Newtonsoft.Json;

namespace Inspectron.HawkEye.Protocol
{
    public class ImageServerTCP
    {
        private readonly IImageSource _imageSource;
        private readonly ICameraControl _cameraControl;
        private readonly ILightControl _lightControl;
        private TcpListener _commandSocket;
        private TcpClient _imageSocket;

        private bool _autoTrigger;
        private bool _applySettings;
        
        private CameraSettings _imageSettingsToApply=new CameraSettings(){ImageSettings = new ImageSettings()};
        private TcpClient _lastClient;

        public ImageServerTCP(IImageSource imageSource, ICameraControl cameraControl, ILightControl lightControl)
        {
            _imageSource = imageSource;
            _cameraControl = cameraControl;
            _lightControl = lightControl;
            
        }

        public void Start()
        {
            _commandSocket = new TcpListener(IPAddress.Any, 27001);
            _commandSocket.Start();
            Console.WriteLine("listen tcp");
            var th = new Thread(ReceiveLoop);
            th.Start();
            if (File.Exists("settings.json"))
            {
                var settings = JsonConvert.DeserializeObject<CameraSettings>(File.ReadAllText("settings.json"));
                _imageSettingsToApply = settings;
                ApplyParameters(settings);
            }
        }

        public void ApplyParameters(CameraSettings cameraSettings, bool setLight = false)
        {
            Console.WriteLine("Setting parameters:"+JsonConvert.SerializeObject(cameraSettings,Formatting.Indented));
            _cameraControl.SetParameters(cameraSettings);
            if(setLight&&!cameraSettings.TriggerLights) _lightControl.SetLight(cameraSettings.LightPwm1, cameraSettings.LightPwm2);
            else _lightControl.SetLight(0, 0);

        }

        private void SaveSettingsLocally()
        {
            File.WriteAllText("settings.json", JsonConvert.SerializeObject(_imageSettingsToApply,Formatting.Indented));
        }
        byte[] _commandBuffer = new byte[1500];
        private Task _lastTriggerTask=Task.CompletedTask;

        private void ReceiveLoop()
        {
            while (true)
            {

                _lastClient=_commandSocket.AcceptTcpClient();
                
                do
                {
                    try
                    {
                        _lastClient.GetStream().Read(_commandBuffer, 0, 1500);
                    }
                    catch
                    {
                        break;
                    }

                    ProcessCommand(_commandBuffer);
                } while (_lastClient.Connected);
                CancelTrigger();
                _lightControl.SetLight(0, 0);
            }
        }

        private void TriggerLoop()
        {
            while (_autoTrigger)
            {

                if (_applySettings)
                {
                    ApplyParameters(_imageSettingsToApply,true);
                    _applySettings = false;
                }

                try
                {
                    SendImage();
                    
                }
                catch(Exception e)
                {
                    Console.WriteLine(e.ToString());
                    return;
                }
            }
        }

        private void ProcessCommand(byte[] data)
        {
            Console.WriteLine(((ECommand) data[0]).ToString());
            switch ((ECommand) data[0])
            {
                case ECommand.Connect:
                {
                    var port = new byte[4];
                    Array.Copy(data, 1, port, 0, 4);
                    _imageSocket=new TcpClient();
                    _imageSocket.Connect(new IPEndPoint((_lastClient.Client.RemoteEndPoint as IPEndPoint).Address, BitConverter.ToInt32(port,0)));
                    var databytes = new byte[1000];
                    var settings = Encoding.UTF8.GetBytes(JsonConvert.SerializeObject(_imageSettingsToApply));
                    Array.Copy(settings, 0, databytes, 1, settings.Length);
                    databytes[0] = (byte) ECommand.Settings;
                    _lastClient.Client.SendData(databytes);
                    Console.WriteLine("data sent");


                    _lightControl.SetLight(_imageSettingsToApply.LightPwm1, _imageSettingsToApply.LightPwm2);
                    _autoTrigger = false;
                }
                    break;
                case ECommand.GetCalibration:
                {
                    var databytes = new byte[1500];
                    if (File.Exists("calibration.calib"))
                    {
                        databytes[0] = (byte)ECommand.OK;
                        Array.Copy(File.ReadAllBytes("calibration.calib"),0,databytes,1,1500-1);
                        _lastClient.Client.SendData(databytes);
                    }
                    else
                    {
                        databytes[0] = (byte)ECommand.NotOK;
                        _lastClient.Client.SendData(databytes);
                    }
                }
                    break;
                case ECommand.Trigger:
                {
                    CancelTrigger();
                    _lastTriggerTask=Task.Run(() =>
                    {
                        try
                        {
                            SendImage();
                        }
                        catch (Exception e)
                        {
                            Console.WriteLine(e.ToString());
                        }
                        
                    });

                }
                    break;
                case ECommand.StartContinuous:
                {
                    _autoTrigger = true;
                    var th = new Thread(TriggerLoop);
                    th.Start();
                }
                    break;
                case ECommand.StopContinuous:
                {
                    _autoTrigger = false;
                }
                    break;
                case ECommand.Settings:
                {
                    CancelTrigger();
                    var databytes = new byte[999];
                    Array.Copy(data, 1, databytes, 0, 999);
                    var settings = JsonConvert.DeserializeObject<CameraSettings>(Encoding.UTF8.GetString(databytes));
                    _imageSettingsToApply = settings;
                    if (_autoTrigger)
                        _applySettings = true;
                    else
                        ApplyParameters(_imageSettingsToApply,true);

                    _lastClient.Client.Send(new []{(byte)EData.OK});
                    
                }
                    break;
                case ECommand.SaveCalibration:
                {
                    
                    var databytes = new byte[1500 - 1];
                    Array.Copy(data, 1, databytes, 0, 1500 - 1);
                    File.WriteAllBytes("calibration.calib",databytes);
                    
                    _lastClient.Client.Send(new[] { (byte)EData.OK });

                }
                    break;
                case ECommand.SaveSettings:
                {
                    SaveSettingsLocally();
                }
                    break;
                default:
                    throw new ArgumentOutOfRangeException();
            }
        }

        private void CancelTrigger()
        {
            _imageSource.CancelTrigger();
            _lastTriggerTask.Wait();

        }
        private void SendImage()
        {
            if (_imageSettingsToApply.TriggerLights)
            {
                _lightControl.SetLight(_imageSettingsToApply.LightPwm1, _imageSettingsToApply.LightPwm2);
            }
            
            var image = _imageSource.GetImage();
            if (image.Length == 0) return;
            if (_imageSettingsToApply.TriggerLights)
            {
                _lightControl.SetLight(0, 0);
            }
            
            var b = EncodeImage(image);

            _imageSocket.Client.SendData(b);
        }

        private static byte[] EncodeImage(byte[] image)
        {
            var b = new byte[image.Length + 5];
            b[0] = (byte) EData.Image;
            var byteSize = BitConverter.GetBytes(image.Length);
            Array.Copy(byteSize, 0, b, 1, 4);
            Array.Copy(image, 0, b, 5, image.Length);
            return b;
        }
        private static byte[] EncodeChanneledImage(byte[] image,byte channels)
        {
            var b = new byte[image.Length + 6];
            b[0] = (byte)EData.Image;
            b[1] = channels;
            var byteSize = BitConverter.GetBytes(image.Length);
            Array.Copy(byteSize, 0, b, 2, 4);
            Array.Copy(image, 0, b, 6, image.Length);
            return b;
        }
    }
}