using Aurora.Api;
using Aurora.Api.DataType;
using Aurora.Api.Devices;
using Hermes;
using MicroEpsilon;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Input;
using static AuroraHeightSensor.HeightSensor;

namespace AuroraHeightSensor.MicroEpsilon
{
    class HeightSensorME : IHeightSensor
    {
        MEDAQLib _Sensor;
        ERR_CODE _LastError = ERR_CODE.ERR_NOERROR;
        //public int Connect(IConnConfig config)
        public int Activate(IConnConfig config)
        {
            var devConfig = config as MEConfig;

            _Sensor = new MEDAQLib(devConfig.SensorName);
            _LastError = Open(devConfig.IPAddress);

            return _LastError == ERR_CODE.ERR_NOERROR ? E1.OK : E2.INVALID_ARGUMENT;
        }

        //public int Disconnect()
        public int Deactivate()
        {
            _Sensor.CloseSensor();
            _Sensor = null;

            return E1.OK;
        }

        public bool CheckConnected()
        {
            return _LastError == ERR_CODE.ERR_NOERROR;
        }

        public int MeasureAll(out List<double> SensorValue)
        {
            SensorValue = null;
            return E1.OK;
        }

        public int Measure(eChannel channel, out double value)
        {
            if (PollData(out value) == ERR_CODE.ERR_NOERROR) {
                return E1.OK;
            }

            return E2.NOT_CONNECTED;
        }

        public int SetZero(eChannel channel, bool isSet = true)
        {
            throw new NotImplementedException();
        }

        public int SetZeroShift(eChannel channel)
        {
            return E1.OK;
        }

        public int SetZeroShiftReset(eChannel channel)
        {
            return E1.OK;
        }

        public int Reset(eChannel channel)
        {
            return E1.OK;
        }

        ERR_CODE Error(string location)
        {
            string errText = "";
            _LastError = _Sensor.GetError(ref errText);
            return _LastError;
        }

        ERR_CODE Open(string ipAddress)
        {
            Console.WriteLine("Open sensor ...");

            _Sensor.SetParameterInt("IP_EnableLogging", 1);
            if (_Sensor.OpenSensorTCPIP(ipAddress) != ERR_CODE.ERR_NOERROR)
                return Error("OpenSensorTCPIP");

            return ERR_CODE.ERR_NOERROR;
        }

        ERR_CODE PollData(out double data)
        {
            data = -1;
            int[] rawData = { 0 };
            double[] scaledData = { 0 };

            if (_Sensor.Poll(rawData, scaledData, 1) != ERR_CODE.ERR_NOERROR)
                return Error("Poll");

            Console.Write("\rRaw: {0} / Scaled: {1,4:F}", rawData[0], scaledData[0]);

            data = scaledData[0];

            return ERR_CODE.ERR_NOERROR;
        }

        ERR_CODE GetParameters()
        {
            Console.WriteLine("Get all parameters ...");

            if (_Sensor.SetIntExecSCmd("Get_AllParameters", "SP_Additional", 1) != ERR_CODE.ERR_NOERROR)
                return Error("SetIntExecSCmd (Get_AllParameters)");

            string serial = "";
            if (_Sensor.GetParameterString("SA_SerialNumber", ref serial) != ERR_CODE.ERR_NOERROR)
                return Error("GetParameterString (SA_SerialNumber)");
            Console.WriteLine("SA_SerialNumber: {0}", serial);

            double range = 0;
            if (_Sensor.GetParameterDouble("SA_Range", ref range) != ERR_CODE.ERR_NOERROR)
                return Error("GetParameterDouble (SA_Range)");
            Console.WriteLine("Sensor range: {0} mm", range);

            return ERR_CODE.ERR_NOERROR;
        }

        ERR_CODE GetVideo()
        {
            Console.WriteLine("Get video signal ...");

            int oldMeasMode = 0;
            if (_Sensor.ExecSCmdGetInt("Get_MeasureMode", "SA_MeasureMode", ref oldMeasMode) != ERR_CODE.ERR_NOERROR)
                return Error("ExecSCmdGetInt (Get_MeasureMode)");

            if (_Sensor.SetIntExecSCmd("Set_MeasureMode", "SP_MeasureMode", 3/*Video*/) != ERR_CODE.ERR_NOERROR)
                return Error("SetIntExecSCmd (Set_MeasureMode)");

            _Sensor.SetParameterInt("SP_OutputVideoRaw_ETH", 0);
            _Sensor.SetParameterInt("SP_OutputVideoCorr_ETH", 1);
            if (_Sensor.ExecSCmd("Set_OutputVideo_ETH") != ERR_CODE.ERR_NOERROR)
                return Error("ExecSCmd (Set_OutputVideo_ETH)");

            if (_Sensor.ExecSCmd("Get_Video") != ERR_CODE.ERR_NOERROR)
                return Error("ExecSCmd (Get_Video)");

            byte[] bArray = null;
            if (_Sensor.GetParameterBinary("SA_VideoCorr", ref bArray) != ERR_CODE.ERR_NOERROR)
                return Error("GetParameterBinary (SA_VideoCorr)");

            short[] sArray = new short[bArray.Length / 2];
            Buffer.BlockCopy(bArray, 0, sArray, 0, bArray.Length);

            for (int i = 0; i < sArray.Length; i++) {
                Console.Write(sArray[i]);
                Console.Write(" ");
            }
            Console.WriteLine("");

            if (_Sensor.SetIntExecSCmd("Set_MeasureMode", "SP_MeasureMode", oldMeasMode) != ERR_CODE.ERR_NOERROR)
                return Error("SetIntExecSCmd (Set_MeasureMode)");

            return ERR_CODE.ERR_NOERROR;
        }

        ERR_CODE TransferData()
        {
            Console.WriteLine("Transfer data ...");

            System.Threading.Thread.Sleep(100); // Wait for data

            int avail = 0;
            if (_Sensor.DataAvail(ref avail) != ERR_CODE.ERR_NOERROR)
                return Error("DataAvail");

            int show = Math.Min(avail, 5);
            Console.WriteLine("{0} Values available, printing first {1} ones", avail, show);

            int[] rawData = new int[avail];
            double[] scaledData = new double[avail];
            int read = 0;
            if (_Sensor.TransferData(rawData, scaledData, avail, ref read) != ERR_CODE.ERR_NOERROR)
                return Error("TransferData");
            System.Diagnostics.Debug.Assert(avail == read);

            for (int i = 0; i < show; i++)
                Console.WriteLine("Value {0,2} Raw: {1} / Scaled: {2,4:F}", i + 1, rawData[i], scaledData[i]);
            Console.WriteLine("");

            return ERR_CODE.ERR_NOERROR;
        }

        public int SetLed(bool isOn)
        {
            throw new NotImplementedException();
        }

        public int StartStopAutoTransmit(bool isOn, ICommand callback)
        {
            //throw new NotImplementedException();
            return E1.OK;
        }
    }
}