// **********************************************************************************************************
//    LSPSMotion.cs
//    1/8/2024
//    NGI - Next Generation Interceptor
//    
//    Contract No. HQ0856-21-C-0003/1022000209
//    
//    THIS DOCUMENT DOES NOT CONTAIN TECHNOLOGY OR TECHNICAL DATA CONTROLLED UNDER EITHER THE U.S. 
//    INTERNATIONAL TRAFFIC IN ARMS REGULATIONS OR THE U.S. EXPORT ADMINISTRATION REGULATIONS.
//    
//    RAYTHEON PROPRIETARY: THIS DOCUMENT CONTAINS DATA OR INFORMATION PROPRIETARY TO RAYTHEON 
//    COMPANY AND IS RESTRICTED TO USE ONLY BY PERSONS AUTHORIZED BY RAYTHEON COMPANY IN WRITING TO USE IT. 
//    DISCLOSURE TO UNAUTHORIZED PERSONS WOULD LIKELY CAUSE SUBSTANTIAL COMPETITIVE HARM TO RAYTHEON 
//    COMPANY'S BUSINESS POSITION. NEITHER SAID DOCUMENT NOR ITS CONTENTS SHALL BE FURNISHED OR DISCLOSED 
//    TO OR COPIED OR USED BY PERSONS OUTSIDE RAYTHEON COMPANY WITHOUT THE EXPRESS WRITTEN APPROVAL OF 
//    RAYTHEON COMPANY.
//    
//    UNPUBLISHED WORK - COPYRIGHT RAYTHEON COMPANY.
//    
//    DESTRUCTION NOTICE: FOR CLASSIFIED DOCUMENTS FOLLOW THE PROCEDURES IN DOD 5220.22-M, 
//    NATIONAL INDUSTRIAL SECURITY PROGRAM OPERATING MANUAL, FEBRUARY 2006, 
//    INCORPORATING CHANGE 1, MARCH 28, 2013, CHAPTER 5, SECTION 7, OR DODM 5200.01-VOLUME 3, 
//    DOD INFORMATION SECURITY PROGRAM: PROTECTION OF CLASSIFIED INFORMATION, ENCLOSURE 3, 
//    SECTION 17. FOR CONTROLLED UNCLASSIFIED INFORMATION FOLLOW THE PROCEDURES IN DODM 5200.01-VOLUME 4, 
//    INFORMATION SECURITY PROGRAM: CONTROLLED UNCLASSIFIED INFORMATION.
//    
//    CONTROLLED BY: MISSILE DEFENSE AGENCY
//    CONTROLLED BY: GROUND-BASED MIDCOURSE DEFENSE PROGRAM OFFICE
//    CUI CATEGORY: CTI 
//    DISTRIBUTION/DISSEMINATION CONTROL: F 
//    POC: Alex Kravchenko (1118268)
// **********************************************************************************************************
using NLog;
using Raytheon.Common;
using Raytheon.Instruments.Lib;
using Raytheon.Instruments.LSPS;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Text.RegularExpressions;

namespace Raytheon.Instruments
{
   /// <summary>
   /// Low-Background Scanning Point Source instrument
   /// </summary>
   public class LspsChamber : ILspsChamber
   {
      public enum UutType
      {
         SENSOR_SELF_TEST_BOX = 0,
         SENSOR = 1,
         SENSOR_W_IMU = 2,
         IMU = 3,
         SEEKER_SELF_TEST_BOX = 4,
         SEEKER = 5,
         NOTCONNECTED
      };

      public enum FilterWheelPos
      {
         NEEDS_HOME = 0,
         HOMING = 1,
         MOVING = 2,
         HOME_SWITCH_ERROR = 3,
         POSITION_SWITCH_ERROR = 4,
         FIND_POSITION_ERROR = 5,
         IN_POSITION = 6
      };

      public enum Events
      {
         QUIT,
         SCRIPT_ABORT,
         NUM_EVENTS
      };

      public enum StartEvents
      {
         START,
         QUIT_THREAD,
         NUM_EVENTS
      };

      public enum TargetSize
      {
         BIG_TARGET = 7,
         SMALL_TARGET = 6
      };

      /// <summary>
      /// NLog logger
      /// </summary>
      private static ILogger _logger;
      /// <summary>
      /// Raytheon configuration
      /// </summary>
      private readonly IConfigurationManager _configurationManager;
      private readonly IConfiguration _configuration;

      private List<LSPS.LSPSPoint> _profiles = new List<LSPS.LSPSPoint>();
      private int _profileSteps = 0;
      private int _currentProfileStep = 0;

      public string DetailedStatus => throw new NotImplementedException();

      public bool DisplayEnabled { get => false; set => throw new NotImplementedException(); }
      public bool FrontPanelEnabled { get => false; set => throw new NotImplementedException(); }

      public InstrumentMetadata Info => throw new NotImplementedException();

      public string Name { get; set; }

      public SelfTestResult SelfTestResult => throw new NotImplementedException();

      public State Status => throw new NotImplementedException();

      private LSPS.TcpClientSock _tcpClientSock;

      private readonly int SendCommandResponseTimeoutMs; // ms
      private readonly int SleepDelayBeforeMonitoringMs; // ms

      private string lspsAddress_;
      private int lspsPort_;

      private static Dictionary<string, Stopwatch> commandToStopwatchDict = new Dictionary<string, Stopwatch>();

      public bool ClearErrors() => false;
      public SelfTestResult PerformSelfTest() => throw new NotImplementedException();

      /// <summary>
      /// Initialize the device
      /// </summary>
      public void Initialize()
      {
         _logger?.Trace($"Entering {this.GetType().Name}::{System.Reflection.MethodBase.GetCurrentMethod().Name}() method...");

         _tcpClientSock = new LSPS.TcpClientSock(lspsAddress_, lspsPort_);
         _tcpClientSock.Connect();
         _tcpClientSock.ClearReceiveBuffer();
      }

      /// <summary>
      /// Shuts down the device
      /// </summary>
      public void Shutdown()
      {
         _logger?.Trace($"Entering {this.GetType().Name}::{System.Reflection.MethodBase.GetCurrentMethod().Name}() method...");
         _tcpClientSock.Disconnect();
      }

      public void Reset()
      {
         _logger?.Trace($"Entering {this.GetType().Name}::{System.Reflection.MethodBase.GetCurrentMethod().Name}() method...");
         Shutdown();
         Initialize();
      }

      /// <summary>
      /// LSPS factory constructor
      /// </summary>
      /// <param name="deviceName"></param>
      /// <param name="configurationManager"></param>
      public LspsChamber(string deviceName, IConfigurationManager configurationManager, ILogger logger)
      {
         Name = deviceName;

         _logger = logger;

         _configurationManager = configurationManager;
         _configuration = _configurationManager.GetConfiguration(Name);

         SendCommandResponseTimeoutMs = _configuration.GetConfigurationValue("LSPSMotion", "SendCommandResponseTimeoutMs", 5000);
         SleepDelayBeforeMonitoringMs = _configuration.GetConfigurationValue("LSPSMotion", "SleepDelayBeforeMonitoringMs", 2000);

         lspsAddress_ = _configuration.GetConfigurationValue("LSPS_Controller", "address", "127.0.0.1");
         lspsPort_ = _configuration.GetConfigurationValue<int>("LSPS_Controller", "port", 2300);
      }

      #region Autofill Functions
      /// <summary>
      /// Autofill Cool Down command
      /// </summary>
      public void AutoFillCoolDown()
      {
         string command = LSPS.Autofill.GetFullCommand(LSPS.Autofill.Command.COOLDOWN);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
		/// Autofill Warm Up command
		/// </summary>
		public void AutoFillWarmUp()
      {
         string command = LSPS.Autofill.GetFullCommand(LSPS.Autofill.Command.WARMUP);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// Autofill Fill Force command
      /// </summary>
      public void AutoFillForce()
      {
         string command = LSPS.Autofill.GetFullCommand(LSPS.Autofill.Command.FORCE);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// Autofill Turn off command
      /// </summary>
      public void AutoFillTurnOff()
      {
         string command = LSPS.Autofill.GetFullCommand(LSPS.Autofill.Command.TURN_OFF);

         SendCommandAndGetResponse(command);
      }

      public LSPS.AutoFillState GetAutoFillState()
      {
         int state = 0;

         string command = LSPS.Autofill.GetFullCommand(LSPS.Autofill.Command.GET_STATE);

         string response = SendCommandAndGetResponse(command);
         string[] stringArray = response.Split(' ');
         Int32.TryParse(stringArray[0], out state);
         return (LSPS.AutoFillState)state;
      }

      public double GetTimeToFill()
      {
         double ttf = 0.0;

         string command = LSPS.Autofill.GetFullCommand(LSPS.Autofill.Command.GET_TIME_TO_FILL);

         Double.TryParse(SendCommandAndGetResponse(command), out ttf);
         return ttf;
      }
      #endregion Autofill Functions

      #region NIF Functions

      /// <summary>
      /// closes gate valve
      /// </summary>
      public void CloseLspsGateValve()
      {
         string command = LSPS.NIF.GetFullCommand(LSPS.NIF.Command.CLOSE_UUT_GV);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// opens gate valve
      /// </summary>
      public void OpenLspsGateValve()
      {
         string command = LSPS.NIF.GetFullCommand(LSPS.NIF.Command.OPEN_UUT_GV);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// Get UUT Gate Valve Position
      /// </summary>
      public GateValvePosition GetLspsGateValvePosition()
      {
         GateValvePosition pos = GateValvePosition.OPEN;
         int val = 0;
         string command = $"{LSPS.NIF.GetFullCommand(LSPS.NIF.Command.GET_INPUT_INDEX)} INDEX={(int)LSPS.NIF.InputIndex.UUT_GV_CLOSE}";
         string response = SendCommandAndGetResponse(command);
         string[] stringArray = response.Split(',');
         Int32.TryParse(stringArray[0], out val);

         if (val == 1)
            pos = GateValvePosition.CLOSE;

         return pos;

      }

      /// <summary>
      /// Get Input Index
      /// </summary>
      public int GetInputIndex(int inputIndex)
      {
         int val = 0;
         string command = $"{LSPS.NIF.GetFullCommand(LSPS.NIF.Command.GET_INPUT_INDEX)} INDEX={inputIndex}";
         string response = SendCommandAndGetResponse(command);
         string[] stringArray = response.Split(',');
         Int32.TryParse(stringArray[0], out val);
         return val;
      }

      public double GetTemperature(LSPS.TemperatureIndex tempIndex)
      {
         double val = 0;
         string command = $"{LSPS.NIF.GetFullCommand(LSPS.NIF.Command.GET_TEMP_INDEX)} INDEX={(int)tempIndex}";
         string response = SendCommandAndGetResponse(command);
         string[] stringArray = response.Split(',');
         Double.TryParse(stringArray[0], out val);

         return val;
      }
      #endregion

      #region RIO Functions

      /// <summary>
      /// closes gate valve
      /// </summary>
      public void CloseVhfGateValve()
      {
         string command = LSPS.RIO.GetFullCommand(LSPS.RIO.Command.GV_CLOSE);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// opens gate valve
      /// </summary>
      public void OpenVhfGateValve()
      {
         string command = LSPS.RIO.GetFullCommand(LSPS.RIO.Command.GV_OPEN);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// Get Gate Valve Chamber Pressure
      /// </summary>
      public double GetVhfGateValveChamberPressure()
      {
         string command = LSPS.RIO.GetFullCommand(LSPS.RIO.Command.GET_GV_CHAMBER_PRESSURE);

         string response = SendCommandAndGetResponse(command);
         double val = 0.0;
         Double.TryParse(response, out val);

         return val;
      }

      /// <summary>
      /// Get Gate Valve UUT Pressure
      /// </summary>
      public double GetVhfGateValveUutPressure()
      {
         string command = LSPS.RIO.GetFullCommand(LSPS.RIO.Command.GET_GV_CANISTER_PRESSURE);

         string response = SendCommandAndGetResponse(command);
         double val = 0.0;
         Double.TryParse(response, out val);

         return val;
      }

      /// <summary>
      /// Get VHF Gate Valve Closed Input
      /// </summary>
      public int GetVhfGateValveClosedInput()
      {
         string command = LSPS.RIO.GetFullCommand(LSPS.RIO.Command.GET_GV_CLOSED_INPUT);

         string response = SendCommandAndGetResponse(command);
         int val = 0;
         Int32.TryParse(response, out val);

         return val;
      }

      /// <summary>
      /// Get VHF Gate Valve Open Input
      /// </summary>
      public int GetVhfGateValveOpenInput()
      {
         string command = LSPS.RIO.GetFullCommand(LSPS.RIO.Command.GET_GV_OPEN_INPUT);

         string response = SendCommandAndGetResponse(command);
         int val = 0;
         Int32.TryParse(response, out val);

         return val;
      }
      #endregion

      #region Chamber Vacuum Functions
      /// <summary>
      /// Get Chamber Pressure (CVAC)
      /// </summary>
      public double GetLspsChamberPressure()
      {
         string command = LSPS.ChamberVacuum.GetFullCommand(LSPS.ChamberVacuum.Command.GET_CVAC);

         string response = SendCommandAndGetResponse(command);
         double val = 0.0;
         Double.TryParse(response, out val);

         return val;
      }

      /// <summary>
      /// Get Sensor Pressure (SVAC)
      /// </summary>
      public double GetLspsUutPressure()
      {
         string command = LSPS.ChamberVacuum.GetFullCommand(LSPS.ChamberVacuum.Command.GET_SVAC);

         string response = SendCommandAndGetResponse(command);
         double val = 0.0;
         Double.TryParse(response, out val);

         return val;
      }
      #endregion

      #region GALIL Functions
      /// <summary>
      /// home filter wheel
      /// </summary>
      public void FilterWheelHome()
      {
         string command = LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.FW_HOME);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// sets filter wheel position
      /// </summary>
      /// <param name="position"></param>
      public void SetFilterWheelPosition(int position)
      {
         string command = $"{LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.FW_SET_POS)} POS={position}";

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// Get filter wheel position
      /// </summary>
      public int GetFilterWheelPosition()
      {
         string command = $"{LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.FW_GET_POS)}";

         string response = SendCommandAndGetResponse(command);
         int val = 0;
         Int32.TryParse(response, out val);
         return val;
      }

      /// <summary>
      /// get filter wheel status
      /// </summary>
      public LSPS.TargetAndFilterWheelStatus GetFilterWheelStatus()
      {
         string command = LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.FW_GET_STATUS);

         string response = SendCommandAndGetResponse(command);

         int val = 0;
         Int32.TryParse(response, out val);

         return (LSPS.TargetAndFilterWheelStatus)val;
      }

      /// <summary>
      /// home filter wheel
      /// </summary>
      public void TargetWheelHome()
      {
         string command = LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.TW_HOME);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// Sets target wheel position
      /// </summary>
      /// <param name="position"></param>
      public void SetTargetWheelPosition(int position)
      {
         string command = $"{LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.TW_SET_POS)} POS={(int)position}";

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// Get target wheel position
      /// </summary>
      public int GetTargetWheelPosition()
      {
         string command = $"{LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.TW_GET_POS)}";

         string response = SendCommandAndGetResponse(command);
         int val = 0;
         Int32.TryParse(response, out val);
         return val;
      }

      /// <summary>
      /// get target wheel status
      /// </summary>
      public LSPS.TargetAndFilterWheelStatus GetTargetWheelStatus()
      {
         string command = LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.TW_GET_STATUS);

         string response = SendCommandAndGetResponse(command);

         int val = 0;
         Int32.TryParse(response, out val);

         return (LSPS.TargetAndFilterWheelStatus)val;
      }

      /// <summary>
      /// steering mirror home
      /// </summary>
      public void SteeringMirrorHome()
      {
         string command = LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_HOME);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// steering mirror stop
      /// </summary>
      public void SteeringMirrorStop()
      {
         string command = LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_STOP);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// steering mirror move
      /// </summary>
      public void SteeringMirrorMove(double az, double el, double speed)
      {
         string command = $"{LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_SET_BEAM_ANGLES)} AZ={az} EL={el} SPEED={speed}";

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// steering mirror get beam angles
      /// </summary>
      public Tuple<double, double> SteeringMirrorGetBeamAngles()
      {
         string command = $"{LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_GET_BEAM_ANGLES)}";

         string response = SendCommandAndGetResponse(command);
         string[] stringArray = response.Split(',');
         double az = 0.0;
         double el = 0.0;
         Double.TryParse(stringArray[0], out az);
         Double.TryParse(stringArray[1], out el);

         return new Tuple<double, double>(az, el);
      }

      /// <summary>
      /// steering mirror set beam speed
      /// </summary>
      public void SteeringMirrorSetBeamSpeed(double speed)
      {
         string command = $"{LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_SET_BEAM_SPEED)} SPEED={speed}";

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// steering mirror set right left arrow angle
      /// </summary>
      public void SteeringMirrorSetRightLeftArrowAngle(double angle)
      {
         string command = $"{LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_SET_RIGHT_LEFT_ARROW_ANGLE)} ANGLE={angle}";

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// steering mirror set up down arrow angle
      /// </summary>
      public void SteeringMirrorSetUpDownArrowAngle(double angle)
      {
         string command = $"{LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_SET_UP_DOWN_ARROW_ANGLE)} ANGLE={angle}";

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// steering mirror status
      /// </summary>
      public Tuple<LSPS.SteeringMirrorStatus, LSPS.SteeringMirrorStatus> GetSteeringMirrorStatus()
      {
         string command = LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_GET_AZ_STATUS);

         string response = SendCommandAndGetResponse(command);

         int az = 0;
         int el = 0;
         Int32.TryParse(response, out az);

         command = LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_GET_EL_STATUS);

         response = SendCommandAndGetResponse(command);
         Int32.TryParse(response, out el);

         return new Tuple<LSPS.SteeringMirrorStatus, LSPS.SteeringMirrorStatus>((LSPS.SteeringMirrorStatus)az, (LSPS.SteeringMirrorStatus)el);
      }

      /// <summary>
      /// steering mirror load profile from file
      /// </summary>
      public void SteeringMirrorLoadProfileFromFile(string profileFilePath)
      {
         try
         {
            using (var fileStream = new FileStream(profileFilePath, FileMode.Open, FileAccess.Read))
            using (var streamReader = new StreamReader(fileStream))
            {
               string line;
               while ((line = streamReader.ReadLine()) != null)
               {
                  Match regexMatch;
                  regexMatch = Regex.Match(line, @"^[\s]*([\d-\.]+)[\s]*,[\s]*([\d-\.]+)[\s]*,[\s]*([\d-\.]+).*", RegexOptions.IgnoreCase);

                  if (regexMatch.Success)
                  {
                     double az = 0.0;
                     double el = 0.0;
                     double speed = 0.0;

                     Double.TryParse(regexMatch.Groups[1].Value, out speed);
                     Double.TryParse(regexMatch.Groups[2].Value, out az);
                     Double.TryParse(regexMatch.Groups[3].Value, out el);

                     SteeringMirrorLoadProfileStep(az, el, speed);
                     _profileSteps++;
                  }
               }
            }
         }
         catch (IOException)
         {
            throw;
         }

         if (_profileSteps == 0)
         {
            throw new ChamberException("Invalid profile provided. Please load a valid Steering Mirror's profile!");
         }

         _currentProfileStep = 0;
      }

      /// <summary>
      /// steering mirror load profile step
      /// </summary>
      public void SteeringMirrorLoadProfileStep(double az, double el, double speed)
      {
         string command = $"{LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_LOAD_PROFILE)} AZ={az} EL={el} SPEED={speed}";

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// steering mirror reset profile
      /// </summary>
      public void SteeringMirrorResetProfile()
      {
         string command = LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_RESET_PROFILE);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// steering mirror run profile
      /// </summary>
      public void SteeringMirrorRunProfile(LSPS.SteeringMirrorProfileMode profileMode, LSPS.SteeringMirrorMovementMode movementMode)
      {
         string command = $"{LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_MOVE_PROFILE)} MODE={(int)profileMode} TYPE={(int)movementMode}";

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// steering mirror move to beginning of profile
      /// </summary>
      public void SteeringMirrorMoveToBeginningOfProfile()
      {
         _currentProfileStep = 0;
         SteeringMirrorRunNextStepInProfile();
      }

      /// <summary>
      /// steering mirror run next step in profile
      /// </summary>
      public void SteeringMirrorRunNextStepInProfile()
      {
         if (_profileSteps == 0)
         {
            string msg = "Please load a valid Steering Mirror's profile!";
            throw new ChamberException(msg);
         }

         if (_currentProfileStep == _profileSteps)
         {
            _currentProfileStep = 0;
         }

         string command = $"{LSPS.GALIL.GetFullCommand(LSPS.GALIL.Command.SM_GET_PROFILE)} INDEX={_currentProfileStep}";
         _currentProfileStep++;

         string response = SendCommandAndGetResponse(command);
         double az = 0.0;
         double el = 0.0;
         double speed = 0.0;
         string[] stringArray = response.Split(',');
         Double.TryParse(stringArray[0], out az);
         Double.TryParse(stringArray[1], out el);
         Double.TryParse(stringArray[2], out speed);

         SteeringMirrorMove(az, el, speed);
      }
      #endregion

      #region BlackBody Functions
      /// <summary>
      /// sets BlackBody setpoint temperature
      /// </summary>
      /// <param name="temperature"></param>
      /// <param name="waitOption"></param>
      /// <param name="pollingRateInMs"></param>
      /// <param name="timeOutInMs"></param>
      public void SetBlackBodySetpointTemperature(double temperature)
      {
         string command = $"{LSPS.BlackBody.GetFullCommand(LSPS.BlackBody.Command.CONTROL_SETPOINT)} SETPOINT={temperature}";

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// Get BlackBody Temperature
      /// </summary>
      public Tuple<double, double> GetBlackbodyTemperature()
      {
         string command = LSPS.BlackBody.GetFullCommand(LSPS.BlackBody.Command.GET_TEMP);

         string response = SendCommandAndGetResponse(command);

         string[] stringArray = response.Split(',');

         double tempA = 0.0;
         double tempB = 0.0;
         Double.TryParse(stringArray[0], out tempA);
         Double.TryParse(stringArray[1], out tempB);
         return new Tuple<double, double>(tempA, tempB);
      }

      /// <summary>
      /// Get BlackBody Rate of Change
      /// </summary>
      public double GetBlackbodyRateOfChange()
      {
         double val = 0.0;

         string command = LSPS.BlackBody.GetFullCommand(LSPS.BlackBody.Command.GET_RATE_OF_CHANGE);

         Double.TryParse(SendCommandAndGetResponse(command), out val);
         return val;
      }

      /// <summary>
      /// Get BlackBody Control State
      /// </summary>
      public LSPS.BlackBodyControlState GetBlackbodyControlState()
      {
         int val = 0;

         string command = LSPS.BlackBody.GetFullCommand(LSPS.BlackBody.Command.GET_CONTROL_STATE);

         Int32.TryParse(SendCommandAndGetResponse(command), out val);
         return (LSPS.BlackBodyControlState)val;
      }

      /// <summary>
      /// Get BlackBody Stability
      /// </summary>
      public Tuple<LSPS.Stability, LSPS.Stability> GetBlackbodyStability(int? logRateInMs = null)
      {
         string command = LSPS.BlackBody.GetFullCommand(LSPS.BlackBody.Command.GET_STABILITY);

         string response = SendCommandAndGetResponse(command, logRateInMs);

         string[] stringArray = response.Split(',');
         int stabilityA = 0;
         int stabilityB = 0;
         Int32.TryParse(stringArray[0], out stabilityA);
         Int32.TryParse(stringArray[1], out stabilityB);
         return new Tuple<LSPS.Stability, LSPS.Stability>((LSPS.Stability)stabilityA, (LSPS.Stability)stabilityB);
      }

      /// <summary>
      /// Get BlackBody Setpoint
      /// </summary>
      public double GetBlackbodySetpoint()
      {
         double val = 0.0;

         string command = LSPS.BlackBody.GetFullCommand(LSPS.BlackBody.Command.GET_CONTROL_SETPOINT);

         Double.TryParse(SendCommandAndGetResponse(command), out val);
         return val;
      }

      /// <summary>
      /// Get BlackBody Heater Percent
      /// </summary>
      public double GetBlackbodyHeaterPercent()
      {
         double val = 0.0;

         string command = LSPS.BlackBody.GetFullCommand(LSPS.BlackBody.Command.GET_HTR);

         Double.TryParse(SendCommandAndGetResponse(command), out val);
         return val;
      }

      /// <summary>
      /// Get BlackBody BIT
      /// </summary>
      public int GetBlackbodyBIT()
      {
         int val = 0;

         string command = LSPS.BlackBody.GetFullCommand(LSPS.BlackBody.Command.GET_BIT);

         Int32.TryParse(SendCommandAndGetResponse(command), out val);
         return val;
      }
      #endregion

      #region Chopper Functions

      /// <summary>
      /// Sets chopper frequency
      /// </summary>
      /// <param name="frequency"></param>
      public void SetChopperFrequency(double frequency)
      {
         string command = $"{LSPS.Chopper.GetFullCommand(LSPS.Chopper.Command.CW_SET_SETPOINT)} SETPOINT={frequency}";

         SendCommandAndGetResponse(command);
      }

      public double GetChopperFrequency()
      {
         double frequency = 0.0;

         string command = LSPS.Chopper.GetFullCommand(LSPS.Chopper.Command.CW_GET_CHOP_FREQ);

         Double.TryParse(SendCommandAndGetResponse(command), out frequency);
         return frequency;
      }

      public LSPS.Stability GetChopperStability()
      {
         string command = LSPS.Chopper.GetFullCommand(LSPS.Chopper.Command.CW_GET_STABILITY);

         int stability = 0;
         Int32.TryParse(SendCommandAndGetResponse(command), out stability);

         return (LSPS.Stability)stability;
      }

      /// <summary>
      /// Turn off chopper wheel
      /// </summary>
      public void TurnOffChopperWheel()
      {
         string command = LSPS.Chopper.GetFullCommand(LSPS.Chopper.Command.CW_TURN_OFF);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// Stopping chopper wheel at open position
      /// </summary>
      /// <exception cref="NotImplementedException"></exception>
      public void SetStopOpen()
      {
         string command = LSPS.Chopper.GetFullCommand(LSPS.Chopper.Command.CW_STOP_OPEN);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// Stopping chopper wheel at closed position
      /// </summary>
      /// <exception cref="NotImplementedException"></exception>
      public void SetStopClosed()
      {
         string command = LSPS.Chopper.GetFullCommand(LSPS.Chopper.Command.CW_STOP_CLOSED);

         SendCommandAndGetResponse(command);
      }

      /// <summary>
      /// Sets chopper state
      /// </summary>
      /// <param name="state"></param>
      public LSPS.ChopperState GetChopperState()
      {
         string command = LSPS.Chopper.GetFullCommand(LSPS.Chopper.Command.CW_GET_STATE);

         int state = 0;
         string response = SendCommandAndGetResponse(command);
         string[] stringArray = response.Split(' ');
         Int32.TryParse(stringArray[0], out state);

         return (LSPS.ChopperState)state;
      }

      #endregion Chopper Functions

      #region Misc Functions
      /// <summary>
      /// Send A Command. Only for SET commands.
      /// DO NOT use for GET Commands as they require processing of returned data
      /// </summary>
      /// <param name="command"></param>
      public void SendACommand(string command)
      {
         SendCommandAndGetResponse(command);
      }
      #endregion

      #region Undefined and Not Implemented

      /// <summary>
      /// not implemented
      /// </summary>
      /// <exception cref="NotImplementedException"></exception>
      public void StartJogging()
      {
         throw new NotImplementedException();
      }
      /// <summary>
      /// not implemented
      /// </summary>
      /// <exception cref="NotImplementedException"></exception>
      public void StopJogging()
      {
         throw new NotImplementedException();
      }

      /// <summary>
      /// 
      /// </summary>
      /// <param name="scene"></param>
      /// <exception cref="NotImplementedException"></exception>
      public void SetBackgroundScene(int scene)
      {
         throw new NotImplementedException();
      }

      /// <summary>
      /// 
      /// </summary>
      /// <param name="source"></param>
      /// <exception cref="NotImplementedException"></exception>
      public void SetTargetSource(int source)
      {
         throw new NotImplementedException();
      }

      /// <summary>
      /// ???
      /// </summary>
      /// <param name="message"></param>
      /// <exception cref="NotImplementedException"></exception>
      public void SendMessageGetResponse(uint message)
      {
         throw new NotImplementedException();
      }

      #endregion

      #region Private Functions
      /// <summary>
      /// standard message sending function
      /// </summary>
      /// <param name="command"></param>
      private string SendCommandAndGetResponse(string command, int? logRateInMs = null)
      {
         if (logRateInMs == null || ToLog(command, logRateInMs))
            _logger?.Trace("Executing LSPS Command: " + command);

         LSPS.LspsResponse response = new LSPS.LspsResponse(_tcpClientSock.SendCommandAndGetResponse(command + "\r\n"));

         if (response.commandType_ == LSPS.LspsResponse.CommandType.SET && !response.successful_)
         {
            throw new ChamberException("LSPS Command Failed with error: " + response.text_);
         }

         return response.text_;
      }

      /// <summary>
      /// If we have a thread sending command at a high rate, we want to slow down the logging of the command
      /// otherwise the log will be inundated with this high-rate message
      /// </summary>
      /// <param name="command"></param>
      private bool ToLog(string command, int? logRateInMs)
      {
         bool toLog = true;

         if (logRateInMs != null)
         {
            if (commandToStopwatchDict.ContainsKey(command))
            {
               if (commandToStopwatchDict[command].ElapsedMilliseconds >= logRateInMs)
               {
                  toLog = true;
                  commandToStopwatchDict[command].Reset();
                  commandToStopwatchDict[command].Start();
               }
               else
               {
                  toLog = false;
               }
            }
            else
            {
               toLog = true;
               commandToStopwatchDict[command] = new Stopwatch();
               commandToStopwatchDict[command].Start();
            }
         }

         return toLog;
      }
      #endregion
   }
}