From 65b2881b064fe66c9dba99b8a646d767989977aa Mon Sep 17 00:00:00 2001
From: Alex Voinea <voinea.dragos.alexandru@gmail.com>
Date: Mon, 28 Feb 2022 17:47:09 +0100
Subject: [PATCH] PAT9125 first prototype

---
 Firmware/Filament_sensor.h | 59 ++++++++++++++++++++++++++++++++++----
 Firmware/ultralcd.cpp      | 14 ++++-----
 2 files changed, 59 insertions(+), 14 deletions(-)

diff --git a/Firmware/Filament_sensor.h b/Firmware/Filament_sensor.h
index 7eba962e3..7342fa7b4 100644
--- a/Firmware/Filament_sensor.h
+++ b/Firmware/Filament_sensor.h
@@ -168,7 +168,6 @@ protected:
     }
     
     void triggerError() {
-        // deinit(); //not sure if I should call this here.
         state = State::error;
         
         /// some message, idk
@@ -223,6 +222,7 @@ public:
             default:
                 return false;
         }
+        return false;
     }
     
     bool getFilamentPresent() {
@@ -447,23 +447,39 @@ public:
             deinit(); //deinit first if there was an error.
         }
         puts_P(PSTR("fsensor::init()"));
-        ;//
+        
         settings_init(); //also sets the state to State::initializing
+        
+        if (!pat9125_init()) {
+            deinit();
+            triggerError();
+            ;//
+        }
+#ifdef IR_SENSOR_PIN
+        else if (!READ(IR_SENSOR_PIN)) {
+            ;// MK3 fw on MK3S printer
+        }
+#endif //IR_SENSOR_PIN
     }
     
     void deinit() {
         puts_P(PSTR("fsensor::deinit()"));
         ;//
         state = State::disabled;
+        filter = 0;
     }
     
     bool update() {
         switch (state) {
             case State::initializing:
-                // state = State::ready; //the IR sensor gets ready instantly as it's just a gpio read operation.
+                if (!updatePAT9125()) {
+                    break; // still not stable. Stay in the initialization state.
+                }
                 oldFilamentPresent = getFilamentPresent(); //initialize the current filament state so that we don't create a switching event right after the sensor is ready.
-                // fallthru
+                state = State::ready;
+                break;
             case State::ready: {
+                updatePAT9125();
                 postponedLoadEvent = false;
                 bool event = checkFilamentEvents();
                 
@@ -476,16 +492,47 @@ public:
             default:
                 return false;
         }
+        return false;
     }
     
     bool getFilamentPresent() {
-        return false;///
+        return filterFilPresent;
     }
     
     void settings_init() {
         Filament_sensor::settings_init();
+        jamDetection = eeprom_read_byte((uint8_t*)EEPROM_FSENSOR_JAM_DETECTION);
+    }
+private:
+    static constexpr uint16_t pollingPeriod = 10; //[ms]
+    static constexpr uint8_t filterCnt = 5; //how many checks need to be done in order to determine the filament presence precisely.
+    ShortTimer pollingTimer;
+    uint8_t filter;
+    uint8_t filterFilPresent;
+    bool jamDetection;
+    
+    bool updatePAT9125() {
+        
+        if (!pollingTimer.running() || pollingTimer.expired(pollingPeriod)) {
+            pollingTimer.start();
+            if (!pat9125_update()) {
+                init(); //try to reinit.
+            }
+            
+            bool present = (pat9125_s < 17) || (pat9125_s >= 17 && pat9125_b >= 50);
+            if (present != filterFilPresent) {
+                filter++;
+            }
+            else if (filter) {
+                filter--;
+            }
+            if (filter >= filterCnt) {
+                filter = 0;
+                filterFilPresent = present;
+            }
+        }
+        return (filter == 0); //return stability
     }
-protected:
 };
 #endif //(FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
 
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 63c960f30..8aa8056cf 100755
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -3445,15 +3445,13 @@ static void lcd_show_sensors_state()
 		lcd_print_state(finda_state);
 	}
 #ifdef FILAMENT_SENSOR
-	if (ir_sensor_detected) {
-		idler_state = fsensor.getFilamentPresent();
-		lcd_puts_at_P(0, 1, _T(MSG_FSENSOR));
-		lcd_set_cursor(LCD_WIDTH - 3, 1);
-		lcd_print_state(idler_state);
-	}
+	idler_state = fsensor.getFilamentPresent();
+	lcd_puts_at_P(0, 1, _T(MSG_FSENSOR));
+	lcd_set_cursor(LCD_WIDTH - 3, 1);
+	lcd_print_state(idler_state);
 #endif //FILAMENT_SENSOR
 
-#ifdef PAT9125
+#if defined(FILAMENT_SENSOR) && (FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
     // Display X and Y difference from Filament sensor
     // Display Light intensity from Filament sensor
     //  Frame_Avg register represents the average brightness of all pixels within a frame (324 pixels). This
@@ -3470,7 +3468,7 @@ static void lcd_show_sensors_state()
                     "S: %3d     Yd:%6d"),
                  pat9125_b, pat9125_x,
                  pat9125_s, pat9125_y);
-#endif //PAT9125
+#endif //defined(FILAMENT_SENSOR) && (FILAMENT_SENSOR_TYPE == FSENSOR_PAT9125)
 }
 
 void lcd_menu_show_sensors_state()                // NOT static due to using inside "Marlin_main" module ("manage_inactivity()")