/// Comment this line if you want to use USB connection mode
#define ETHERNET 

using System;
using System.Collections.Generic;
using System.Data;
using System.Data.Common;
using System.IO;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Xml.Linq;
using WBase;
using WCommunication;
using WEnumerations;
using WRecorder;
using WSensorData;
using WToolboxException;
using WUtils;

namespace VisioscanSetSampleDotNet
{
    internal class SensorDemo
    {
        // Basic communication settings
        WBaseSettings commSettings;

        // Basic communication mode
        WBaseCommunication communication;

        // Rawdata sensor's instance
        WRawDataSensor mySensor;
        
        // Miscelaneous variables        
        bool StopRawDataRead = false;       // Stops the thread for raw data collection
        string ip = "";                     // Current IP adress of the sensor
        int port = -1;                      // Current port adress of the sensor

        /// <summary>
        /// Change the ip of the sensor
        /// </summary>
        internal void ChangeNetworkSettings()
        {
            string newIp = "";
            Console.WriteLine("Please, enter the new IP address");
            newIp = Console.ReadLine();
            ip = newIp;
            WNetworkInformation netInfo = mySensor.GetNetworkInformation();
            netInfo.IpAddress = newIp;
            try
            {
                mySensor.SetNetworkInformation(netInfo);
            }
            catch (WCommunicationException e)
            {
                Console.WriteLine("Disconnection failed, which is normal ");
            }

#if  ETHERNET
            Console.WriteLine("Disconnecting the sensor");
            mySensor.Communication.Disconnect();
            ((WEthernetSettings)mySensor.Communication.Settings).Ip = newIp;
            ((WEthernetSettings)mySensor.Communication.Settings).TcpPort = netInfo.IpPort;
            ((WEthernetSettings)mySensor.Communication.Settings).UdpPort = netInfo.IpPort;
            Console.WriteLine("Connecting the sensor");
            mySensor.Communication.Connect();
            Console.WriteLine("New IP is " + ((WEthernetSettings)(mySensor.Communication.Settings)).Ip);
#endif

            Console.WriteLine("Connected, sensor IP is " + netInfo.IpAddress);
        }

        /// <summary>
        /// Connects to the sensor
        /// </summary>
        internal void Connect()
        {
#if ETHERNET
            if (port == -1)
            {
                Console.WriteLine("Please enter sensor ip  (Default sensor's ip is 192.168.1.2)");
                ip = Console.ReadLine();
                Console.WriteLine("Please enter sensor port (default senor's port is 3050)");
                port = int.Parse(Console.ReadLine());
            }
            commSettings = new WEthernetSettings(ip, port, port);
            communication = new WEthernetCommunication((WEthernetSettings)commSettings);
#else
            commSettings = new WUsbSettings(0x2161, 0x0405, 0x81, 0x1);
            communication = new WUsbCommunication((WUsbSettings)commSettings);
#endif
            try
            {
                mySensor = new WRawDataSensor(communication);
                // Initialize the sensor and establishing the connection
                mySensor.Initialize();
            }
            catch (WCommunicationException e)
            {
                Console.WriteLine(e.Message);
            }
            Console.WriteLine("Sensor connected");
        }

        /// <summary>
        /// Disconnects from the sensor
        /// </summary>
        internal void Disconnect()
        {
            // disconnect the channel
            mySensor.Communication.Disconnect();
            Console.WriteLine("Sensor disconnected");
        }

        /// <summary>
        /// Displays the senor's version
        /// </summary>
        internal void DisplaySensorVersion()
        {
           // Get network information
           WNetworkInformation netInfo = mySensor.GetNetworkInformation();

            // Get sensor version
           WSoftwareVersion version = mySensor.GetVersion();

           // Display data
           Console.WriteLine("Version: " + version.Version);
           Console.WriteLine("Revision: " + version.Revision);
           Console.WriteLine("Can number: " + version.CanNumber);
           Console.WriteLine("Hardware version: " + version.HardwareVersion);
           Console.WriteLine("Part number: " + version.PartNumber);
           Console.WriteLine("Prototype: " + version.Prototype);
           Console.WriteLine("ProductIdentifier: " + version.ProductTypeIdentification);
           Console.WriteLine("");
           Console.WriteLine("Sensor's IP: " + netInfo.IpAddress);
           Console.WriteLine("Sensor's subnet mask: " + netInfo.SubnetMask);
           Console.WriteLine("Sensor's gateway: " + netInfo.Gateway);
           Console.WriteLine("Sensor's port: " + netInfo.IpPort);
        }

        /// <summary>
        /// Operates the miscelaneous recorder function
        /// </summary>
        internal void OperateRecorder()
        {
            Thread rawDataThreadHandle = new Thread(new ParameterizedThreadStart(ReadRawDataSync));
            rawDataThreadHandle.Start(10000000); // RawData must be retrieven from the sensor to be able to record them

            WFileDataRecorder recorder = new WFileDataRecorder("RawdataRecord.dat");
            mySensor.DataRecorder = recorder;
            string command;
            Console.Clear();
            do
            {
                Console.WriteLine("");
                Console.WriteLine("Please enter a command (/!\\ commands are case sensitive)");
                Console.WriteLine("'record' to record RawData in a DAT file");
                Console.WriteLine("'play' to replay the DAT file");
                Console.WriteLine("'pause' to pause the replay");
                Console.WriteLine("'stop' to either stop record or play");
                Console.WriteLine("'framerate' to set replay framerate during replay");
                Console.WriteLine("'status' to get the recorder status");
                Console.WriteLine("'export' to export DAT file in a CSV");
                Console.WriteLine("'q' to leave this menu");

                command = Console.ReadLine();
                if (command == "record")
                {
                    // This will record the rawdata in the RawdataRecord.dat
                    mySensor.DataRecorder.Record();
                    Console.WriteLine("Recorder is recording");
                }
                if (command == "play")
                {
                    // This will replay the rawdata stored in the RawdataRecord.dat
                    // Data will be fetched by the mySensor.RetrieveRawData()
                    // Replay has the priority on the sensor raw data sent through communication layer
                    mySensor.DataRecorder.Play();
                    Console.WriteLine("Recorder is playing");
                }
                else if (command == "pause")
                {
                    // This will pause the replay (no effect on the recording)
                    mySensor.DataRecorder.Pause();
                    Console.WriteLine("Recorder is paused");
                }
                else if (command == "export")
                {
                    // Once stopped, will export to a CSV file
                    mySensor.DataRecorder.ExportToCsv("export.csv");
                    Console.WriteLine("Recorder is paused");
                }
                else if (command == "stop")
                {
                    // This will stop the player if playing or the recorder if recording
                    mySensor.DataRecorder.Stop();
                    Console.WriteLine("Recorder has  been stopped");

                }
                else if (command == "status")
                {
                    // Will show a status on the screen
                    RecorderStatus status = mySensor.DataRecorder.Status;
                    switch (status)
                    {
                        case RecorderStatus.Stopped:
                            Console.WriteLine("Recorder is stopped");
                            break;
                        case RecorderStatus.Playing:
                            Console.WriteLine("Recorder is playing");
                            break;
                        case RecorderStatus.Recording:
                            Console.WriteLine("Recorder is recording");
                            break;
                        case RecorderStatus.Paused:
                            Console.WriteLine("Recorder is paused");
                            break;
                    }
                }
                else if (command == "framerate")
                {
                    /// This will change the framerate
                    Console.WriteLine("Actual framerate is " + mySensor.DataRecorder.FrameRate + ", enter new framerate");
                    command  = Console.ReadLine();
                    mySensor.DataRecorder.FrameRate = int.Parse(command);
                    Console.WriteLine("Frame rate has been set to " + command);
                }
            } while (command != "q");

            StopRawDataRead = true;
            rawDataThreadHandle.Join();

            Console.Clear();
        }

        /// <summary>
        /// Reads the raw data from the sensor in a asynchronous mode
        /// </summary>
        /// <param name="count"></param>
        internal void ReadRawDataAsync(int count)
        {
            Thread rawDataThreadHandle = new Thread(new ParameterizedThreadStart(ReadRawDataSync));
            rawDataThreadHandle.Start(count);
            rawDataThreadHandle.Join();
        }

        /// <summary>
        /// Reads the raw data from the sensor in a synchronous mode
        /// </summary>
        /// <param name="count"></param>
        internal void ReadRawDataSync(object count)
        {
            int i = 0;
            do
            {
                WRawData data = mySensor.RetrieveRawData();

                // Do whatever you want with your data (plot, treat, display etc.)
                // For this example we will write that in a file.
                WriteDataToFile(data);

                i++;
            } while (i < int.Parse(count.ToString()) && !StopRawDataRead);

            Console.WriteLine("Exit ReadRawDataSync");
        }

        /// <summary>
        /// Manages the sensor's settings
        /// </summary>
        internal void SettingsManagement()
        {
            string command;
            Console.Clear();
            do
            {
                Console.WriteLine("");
                Console.WriteLine("Please enter a command (/!\\ commands are case sensitive)");
                Console.WriteLine("'version' to record RawData in a DAT file");
                Console.WriteLine("'name' to manage the sensor's name");
                Console.WriteLine("'anglerange' to manage the scan angle range");
                Console.WriteLine("'scanfreq' to manage the scan frequency and angle resolution");
                Console.WriteLine("'filter' to manage the data filter");
                Console.WriteLine("'contat' to manage the window contamination threshold");
                Console.WriteLine("'contaw' to get the window contamination status");
                Console.WriteLine("'packet' to manage the data packet type");
                Console.WriteLine("'direction' to manage the data output direction");
                Console.WriteLine("'protocol' to manage the output protocol");
                Console.WriteLine("'leds' to manage the sensor's leds status");
                Console.WriteLine("'lamps' to get the sensor's leds color");
                Console.WriteLine("'reset' to reset the sensor to factory default");
                Console.WriteLine("'q' to leave this menu");
                command = Console.ReadLine();
                Console.Clear();
                if (command == "version")
                {
                    DisplaySensorVersion();
                }
                else if (command == "name")
                {
                    try
                    {
                        string sensorname = mySensor.GetDeviceName();
                        Console.WriteLine("Current sensor's name is: " + sensorname);
                        Console.WriteLine("Please enter the new name");
                        sensorname = Console.ReadLine();

                        Console.WriteLine("Fetching '" + sensorname + "' in the sensor...");
                        mySensor.SetDeviceName(sensorname);
                        Console.WriteLine("Fetch succesful, new sensor name is " + mySensor.GetDeviceName());
                    }
                    catch (Exception e)
                    {
                        Console.WriteLine(e.Message);
                    }
                }
                else if (command == "anglerange")
                {
                    WAngleRange range = mySensor.GetAngleRange();
                    Console.WriteLine("Angle range boundary 1 " + range.Start + ", boundary 2 " + range.Stop);
                    int startAngle, stopAngle;
                    Console.WriteLine("Enter boundary 1 (between -47,6 and 226,6 bellow boundary 2)");
                    startAngle = int.Parse(Console.ReadLine());
                    Console.WriteLine("Enter boundary 2 (between -47,6 and 226,6 above boundary 1)");
                    stopAngle = int.Parse(Console.ReadLine());


                    range.Start = startAngle;
                    range.Stop = stopAngle;

                    if (mySensor.SetAngleRange(range).Result)
                    {
                        range = mySensor.GetAngleRange();
                        Console.WriteLine("Angle range boundary 1 " + range.Start + ", boundary 2 " + range.Stop);
                    }
                    else
                    {
                        Console.WriteLine("Failed");
                    }
                }

                else if (command == "scanfreq")
                {
                    AngularResolution res = mySensor.GetAngularResolution();
                    switch (res)
                    {
                        case  AngularResolution.ZeroToZeroPointTwoAtEightyHz:
                            Console.WriteLine("Current resolution is 0,2 @ 80hz");
                            break;
                        case  AngularResolution.ZeroToZeroPointOneAtFourtyHz:
                            Console.WriteLine("Current resolution is 0,1 @ 40hz");
                            break;
                    }
                    Console.WriteLine("Enter the new resolution (0 for 0,2@80hz, 1 for 0,1@40hz)");
                    int newRes = int.Parse(Console.ReadLine());
                    switch (newRes)
                    {
                        case 0:
                            res = AngularResolution.ZeroToZeroPointTwoAtEightyHz;
                            break;
                        case 1:
                            res = AngularResolution.ZeroToZeroPointOneAtFourtyHz;
                            break;
                    }
                    mySensor.SetAngularResolution(res);

                    res = mySensor.GetAngularResolution();
                    switch (res)
                    {
                        case AngularResolution.ZeroToZeroPointTwoAtEightyHz:
                            Console.WriteLine("New resolution is 0,2 @ 80hz");
                            break;
                        case AngularResolution.ZeroToZeroPointOneAtFourtyHz:
                            Console.WriteLine("New resolution is 0,1 @ 40hz");
                            break;
                    }
                }
                else if (command == "filter")
                {
                    FilterStatus filterStatus = mySensor.GetFilterStatus();
                    Console.WriteLine("Current filter status is: " + filterStatus);
                    Console.WriteLine("Please enter the filter status (0: OFF, 1: ON)");
                    int newfilterStatus = int.Parse(Console.ReadLine());
                    switch (newfilterStatus)
                    {
                        case 0:
                            filterStatus = FilterStatus.Off;
                            break;
                        case 1:
                            filterStatus = FilterStatus.On;
                            break;
                    }
                    Console.WriteLine("Fetching '" + filterStatus + "' in the sensor...");
                    mySensor.SetFilterStatus(filterStatus);
                    Console.WriteLine("Fetch succesful, new filter status is " + mySensor.GetFilterStatus());
                }
                else if (command == "contat")
                {
                    int warning, error;
                    Console.WriteLine("enter warning level (between 0 and 100 and bellow ERROR contamination threshold)");
                    warning = int.Parse(Console.ReadLine());
                    Console.WriteLine("enter error level (between 0 and 100 and above WARNING contamination threshold)");
                    error = int.Parse(Console.ReadLine());

                    WContaminationThreshold cont = new WContaminationThreshold();
                    cont.Warning = warning;
                    cont.Error = error;

                    if (mySensor.SetContaminationThreshold(cont).Result)
                    {
                        cont = mySensor.GetContaminationThreshold();
                        Console.WriteLine("Contamination threshold warning is " + cont.Warning + ", error is " + cont.Error);
                    }
                    else
                    {
                        Console.WriteLine("Failed");
                    }
                }
                else if (command == "contaw")
                {
                    try
                    {
                        WContaminationStatus contStatus = mySensor.GetContaminationStatus();
                        Console.WriteLine("Contamination status left: " + contStatus.DataLeft + ", middle: " + contStatus.DataMiddle + ", and right: " + contStatus.DataRight);
                    }
                    catch (Exception e)
                    {
                        Console.WriteLine(e.Message);
                    }
                }
                else if (command == "packet")
                {
                    ScanPacketType ptype = mySensor.GetDataPacketType();
                    switch (ptype)
                    {
                        case  ScanPacketType.Distance:
                            Console.WriteLine("Current packet type is set to distance only");
                            break;
                        case ScanPacketType.DistanceAndIntensity:
                            Console.WriteLine("Current packet type is set to distance & amplitude");
                            break;
                    }
                    Console.WriteLine("Enter the packet type (0 for distance, 1 for distance & amplitude)");
                    int newPType = int.Parse(Console.ReadLine());
                    switch (newPType)
                    {
                        case 0:
                            ptype = ScanPacketType.Distance;
                            break;
                        case 1:
                            ptype = ScanPacketType.DistanceAndIntensity;
                            break;
                    }
                    mySensor.SetDataPacketType(ptype);

                    ptype = mySensor.GetDataPacketType();
                    switch (ptype)
                    {
                        case ScanPacketType.Distance:
                            Console.WriteLine("New packet type is set to distance only");
                            break;
                        case ScanPacketType.DistanceAndIntensity:
                            Console.WriteLine("New packet type is set to distance & amplitude");
                            break;
                    }
                }
                else if (command == "direction")
                {
                    ScanningDirection scandir = mySensor.GetScanningDirection();
                    switch (scandir)
                    {
                        case ScanningDirection.Clockwise:
                            Console.WriteLine("Current scanning direction is clockwise");
                            break;
                        case ScanningDirection.CounterClockwise:
                            Console.WriteLine("Current scanning direction is counter clockwise");
                            break;
                    }
                    Console.WriteLine("Enter the new scanning directon (0 for CLOCKWISE, 1 for COUNTER CLOCKWISE)");
                    int newScanDir = int.Parse(Console.ReadLine());
                    switch (newScanDir)
                    {
                        case 0:
                            scandir = ScanningDirection.Clockwise;
                            break;
                        case 1:
                            scandir = ScanningDirection.CounterClockwise;
                            break;
                    }
                    mySensor.SetScanningDirection(scandir);

                    scandir = mySensor.GetScanningDirection();
                    switch (scandir)
                    {
                        case ScanningDirection.Clockwise:
                            Console.WriteLine("New scanning direction is clockwise");
                            break;
                        case ScanningDirection.CounterClockwise:
                            Console.WriteLine("New scanning direction is counter clockwise");
                            break;
                    }
                }
                else if (command == "protocol")
                {
                    Protocol protp = mySensor.GetRawDataExchangeMode();
                    switch (protp)
                    {
                        case Protocol.Udp:
                            Console.WriteLine("Current  mode is UDP");
                            break;
                        case Protocol.Tcp:
                            Console.WriteLine("Current mode is TCP");
                            break;
                    }
                    Console.WriteLine("Enter the new exchange mode (0 for UDP, 1 for TCP)");
                    int newProtocol = int.Parse(Console.ReadLine());
                    switch (newProtocol)
                    {
                        case 0:
                            protp = Protocol.Udp;
                            break;
                        case 1:
                            protp = Protocol.Tcp;
                            break;
                    }
                    mySensor.SetRawDataExchangeMode(protp);

                    protp = mySensor.GetRawDataExchangeMode();
                    switch (protp)
                    {
                        case Protocol.Udp:
                            Console.WriteLine("New  mode is UDP");
                            break;
                        case Protocol.Tcp:
                            Console.WriteLine("New mode is TCP");
                            break;
                    }
                }
                else if (command == "leds")
                {
                    WSensorLeds leds;
                    leds = mySensor.GetSensorLeds();
                    Console.WriteLine("Status led is " + leds.StatusLed + ", logo led is " + leds.LogoLed);

                    int status, logo;
                    Console.WriteLine("enter status led value");
                    status = int.Parse(Console.ReadLine());
                    Console.WriteLine("enter logo led value");
                    logo = int.Parse(Console.ReadLine());


                    leds.StatusLed = status == 1;
                    leds.LogoLed = logo == 1;

                    if (mySensor.SetSensorLeds(leds).Result)
                    {
                        leds = mySensor.GetSensorLeds();
                        Console.WriteLine("Status led is " + leds.StatusLed + ", logo led is " + leds.LogoLed);
                    }
                    else
                    {
                        Console.WriteLine("Failed");
                    }
                }
                else if (command == "lamps")
                {
                    WSensorLamps lamps;
                    lamps = mySensor.GetSensorLamps();
                    DisplayLamp(lamps);
                }
                else if (command == "reset")
                {
                    Console.WriteLine("Reseting sensor to its default");
                    mySensor.ResetDevice();
                    Console.WriteLine("Sensor has been reset to its default");
                }
            } while (command != "q");
            Console.WriteLine("");
        }

        /// <summary>
        /// Starts the MDI data collection from sensor
        /// </summary>
        internal void StartMdi()
        {
            Console.WriteLine("Starting MDI at sensor");
            mySensor.SendMdi();
            Console.WriteLine("Started");
        }

        /// <summary>
        /// Stops the MDI data collection from sensor
        /// </summary>
        internal void StopMdi()
        {
            Console.WriteLine("Stoping MDI at sensor");
            mySensor.StopMdi();
            Console.WriteLine("Stoped");
        }

        /// <summary>
        /// Utility to write data to a file
        /// </summary>
        /// <param name="toWrite"></param>
        private void WriteDataToFile(WRawData toWrite)
        {
            using (StreamWriter sw = File.AppendText("myrawdatadotnet.txt"))
            {
                for (int i = 0; i < toWrite.Angles.Count(); i++)
                {
                    string toDisplayOrWrite = toWrite.Indexes[i] + ";" + toWrite.Angles[i] + ";" + toWrite.Distances[i] + ";" + (long)toWrite.Timestamps[i];
                    sw.WriteLine(toDisplayOrWrite);
                }
            }
        }

        /// <summary>
        /// Utility to display the lamps states on the screen
        /// </summary>
        /// <param name="lamps"></param>
        private void DisplayLamp(WSensorLamps lamps)
        {
            string ledToDisplay = "";
            switch (lamps.Led1)
            {
                case LedColor.Black:
                    ledToDisplay = ledToDisplay + " led 1 is black ";
                    break;
                case LedColor.Red:
                    ledToDisplay = ledToDisplay + " led 1 is red ";
                    break;
                case LedColor.Green:
                    ledToDisplay = ledToDisplay + " led 1 is green ";
                    break;
                case LedColor.Orange:
                    ledToDisplay = ledToDisplay + " led 1 is orange ";
                    break;
            }
            switch (lamps.Led2)
            {
                case LedColor.Black:
                    ledToDisplay = ledToDisplay + " led 2 is black ";
                    break;
                case LedColor.Red:
                    ledToDisplay = ledToDisplay + " led 2 is red ";
                    break;
                case LedColor.Green:
                    ledToDisplay = ledToDisplay + " led 2 is green ";
                    break;
                case LedColor.Orange:
                    ledToDisplay = ledToDisplay + " led 2 is orange ";
                    break;
            }
            switch (lamps.Led3)
            {
                case LedColor.Black:
                    ledToDisplay = ledToDisplay + " led 3 is black ";
                    break;
                case LedColor.Red:
                    ledToDisplay = ledToDisplay + " led 3 is red ";
                    break;
                case LedColor.Green:
                    ledToDisplay = ledToDisplay + " led 3 is green ";
                    break;
                case LedColor.Orange:
                    ledToDisplay = ledToDisplay + " led 3 is orange ";
                    break;
            }
            switch (lamps.Led4)
            {
                case LedColor.Black:
                    ledToDisplay = ledToDisplay + " led 4 is black ";
                    break;
                case LedColor.Red:
                    ledToDisplay = ledToDisplay + " led 4 is red ";
                    break;
                case LedColor.Green:
                    ledToDisplay = ledToDisplay + " led 4 is green ";
                    break;
                case LedColor.Orange:
                    ledToDisplay = ledToDisplay + " led 4 is orange ";
                    break;
            }

            Console.WriteLine(ledToDisplay);
        }
    }
}
