From d11ac07a0cf68528403cc9b5ad4d92a46b80424e Mon Sep 17 00:00:00 2001
From: Jasem Mutlaq <mutlaqja@ikarustech.com>
Date: Fri, 1 Dec 2023 21:11:37 +0300
Subject: [PATCH] Add reverse support to starlight EFS focuser. Not tested on
 hardware

---
 drivers.xml                |   2 +-
 drivers/focuser/si_efs.cpp | 103 ++++++++++++++++++++++++++++++++++++-
 drivers/focuser/si_efs.h   |   9 +++-
 3 files changed, 110 insertions(+), 4 deletions(-)

diff --git a/drivers.xml b/drivers.xml
index a8a9dd7bf7..d680a682d9 100644
--- a/drivers.xml
+++ b/drivers.xml
@@ -487,7 +487,7 @@
         </device>
         <device label="Starlight EFS" manufacturer="Starlight Instruments">
             <driver name="SI EFS">indi_siefs_focus</driver>
-            <version>0.1</version>
+            <version>0.2</version>
         </device>
         <device label="Lacerta MFOC" manufacturer="Lacerta Optics">
             <driver name="Lacerta MFOC">indi_lacerta_mfoc_focus</driver>
diff --git a/drivers/focuser/si_efs.cpp b/drivers/focuser/si_efs.cpp
index 4d4ef33adc..87d0d80dcb 100644
--- a/drivers/focuser/si_efs.cpp
+++ b/drivers/focuser/si_efs.cpp
@@ -39,6 +39,7 @@ const std::map<SIEFS::SI_COMMANDS, std::string> SIEFS::CommandsMap =
     {SIEFS::SI_FAST_IN, "Fast In"},
     {SIEFS::SI_FAST_OUT, "Fast Out"},
     {SIEFS::SI_HALT, "Halt"},
+    {SIEFS::SI_MOTOR_POLARITY, "Motor Polarity"},
 };
 
 const std::map<SIEFS::SI_MOTOR, std::string> SIEFS::MotorMap =
@@ -51,9 +52,13 @@ const std::map<SIEFS::SI_MOTOR, std::string> SIEFS::MotorMap =
 
 SIEFS::SIEFS()
 {
-    setVersion(0, 1);
+    setVersion(0, 2);
 
-    FI::SetCapability(FOCUSER_CAN_ABS_MOVE | FOCUSER_CAN_REL_MOVE | FOCUSER_CAN_ABORT | FOCUSER_CAN_SYNC);
+    FI::SetCapability(FOCUSER_CAN_ABS_MOVE |
+                      FOCUSER_CAN_REL_MOVE |
+                      FOCUSER_CAN_ABORT |
+                      FOCUSER_CAN_SYNC |
+                      FOCUSER_CAN_REVERSE);
     setSupportedConnections(CONNECTION_NONE);
 }
 
@@ -93,6 +98,10 @@ bool SIEFS::Connect()
             FocusRelPosN[0].min  = 0;
         }
 
+        bool reversed = isReversed();
+        FocusReverseS[INDI_ENABLED].s = reversed ? ISS_ON : ISS_OFF;
+        FocusReverseS[INDI_DISABLED].s = reversed ? ISS_OFF : ISS_ON;
+        FocusReverseSP.s = IPS_OK;
         SetTimer(getCurrentPollingPeriod());
     }
 
@@ -539,3 +548,93 @@ bool SIEFS::SetFocuserMaxPosition(uint32_t ticks)
 
     return rc;
 }
+
+bool SIEFS::ReverseFocuser(bool enabled)
+{
+    return setReversed(enabled);
+}
+
+bool SIEFS::setReversed(bool enabled)
+{
+    int rc = 0;
+    uint8_t command[2] = {0};
+    uint8_t response[2] = {0};
+
+    command[0] = SI_MOTOR_POLARITY;
+    command[1] = enabled ? 1 : 0;
+
+    LOGF_DEBUG("CMD <%02X> <%02X>", command[0], command[1]);
+
+    if (isSimulation())
+        rc = 1;
+    else
+        rc = hid_write(handle, command, 2);
+
+    if (rc < 0)
+    {
+        LOGF_ERROR("setReversed: Error writing to device (%s)", hid_error(handle));
+        return false;
+    }
+
+    if (isSimulation())
+    {
+        rc = 2;
+        response[0] = command[0];
+        // Normal
+        response[1] = 0;
+    }
+    else
+        rc = hid_read_timeout(handle, response, 2, SI_TIMEOUT);
+
+    if (rc < 0)
+    {
+        LOGF_ERROR("setReversed: Error reading from device (%s)", hid_error(handle));
+        return false;
+    }
+
+    LOGF_DEBUG("RES <%02X %02X>", response[0], response[1]);
+
+    return true;
+}
+
+bool SIEFS::isReversed()
+{
+    int rc = 0;
+    uint8_t command[1] = {0};
+    uint8_t response[2] = {0};
+
+    command[0] = SI_MOTOR_POLARITY;
+
+    LOGF_DEBUG("CMD <%02X>", command[0]);
+
+    if (isSimulation())
+        rc = 1;
+    else
+        rc = hid_write(handle, command, 1);
+
+    if (rc < 0)
+    {
+        LOGF_ERROR("setReversed: Error writing to device (%s)", hid_error(handle));
+        return false;
+    }
+
+    if (isSimulation())
+    {
+        rc = 2;
+        response[0] = command[0];
+        // Normal
+        response[1] = 0;
+    }
+    else
+        rc = hid_read_timeout(handle, response, 2, SI_TIMEOUT);
+
+    if (rc < 0)
+    {
+        LOGF_ERROR("setReversed: Error reading from device (%s)", hid_error(handle));
+        return false;
+    }
+
+    LOGF_DEBUG("RES <%02X %02X>", response[0], response[1]);
+
+    return response[1] != 0;
+}
diff --git a/drivers/focuser/si_efs.h b/drivers/focuser/si_efs.h
index e6a7bbad12..52990d81d7 100644
--- a/drivers/focuser/si_efs.h
+++ b/drivers/focuser/si_efs.h
@@ -38,7 +38,8 @@ class SIEFS : public INDI::Focuser
                        SI_MAX_POS,
                        SI_FAST_IN  = 0x11,
                        SI_FAST_OUT = 0x12,
-                       SI_HALT     = 0xFF
+                       SI_HALT     = 0xFF,
+                       SI_MOTOR_POLARITY = 0x61
                      } SI_COMMANDS;
 
 
@@ -66,6 +67,8 @@ class SIEFS : public INDI::Focuser
         virtual bool SyncFocuser(uint32_t ticks) override;
         virtual bool SetFocuserMaxPosition(uint32_t ticks) override;
 
+        virtual bool ReverseFocuser(bool enabled) override;
+
     private:
         /**
          * @brief setPosition Set Position (Either Absolute or Maximum)
@@ -91,6 +94,10 @@ class SIEFS : public INDI::Focuser
         bool setMaxPosition(uint32_t ticks);
         bool getMaxPosition(uint32_t *ticks);
 
+        // Polarity
+        bool isReversed();
+        bool setReversed(bool enabled);
+
         bool sendCommand(SI_COMMANDS targetCommand);
         bool getStatus();