From 95a2a82111ec836ddee053536f3974b49f7df34f Mon Sep 17 00:00:00 2001
From: Vynwg <me@vynwg.com>
Date: Thu, 23 Nov 2023 17:38:55 -0300
Subject: [PATCH] look4sat communication, finally!

---
 platformio.ini |  2 +-
 src/comm.cpp   | 44 ++++++++++++++++++++++++++++++--------------
 src/comm.h     |  8 +++++---
 src/main.cpp   | 14 +++++++++++++-
 src/servo.cpp  |  3 ++-
 src/servo.h    |  2 +-
 6 files changed, 52 insertions(+), 21 deletions(-)

diff --git a/platformio.ini b/platformio.ini
index f8b7c01..6ef56c1 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -17,4 +17,4 @@ lib_deps =
 	dfrobot/DFRobot_QMC5883@^1.0.0
 	juerd/ESP-WiFiSettings@^3.8.0
 	https://github.com/sivar2311/WebMonitor
-; upload_flags = --auth=senha1234
+upload_flags = --auth=senha1234
diff --git a/src/comm.cpp b/src/comm.cpp
index fe0d3fd..1597c8b 100644
--- a/src/comm.cpp
+++ b/src/comm.cpp
@@ -21,33 +21,49 @@ void CommunicationsController::initWifi() {
 void CommunicationsController::init() {
     initOta();
     initWifi();
+
+    while (WiFi.status() != WL_CONNECTED);
+
     WebMonitor.begin();
+    l4sServer.begin();
 }
 
-void CommunicationsController::process() {
+byte CommunicationsController::process(float *azimuth, float *altitude) {
     ArduinoOTA.handle();
-
-    if (!l4s) {
-        return;
+    create_connection();
+
+    if (l4s) {
+        while (l4s.connected()) {
+            handle_client();
+            byte resp = parseLook4Sat(azimuth, altitude);
+            ArduinoOTA.handle();
+
+            if (resp == 1) {
+                return 1;
+            }
+        }
     }
 
-    if (l4s.connected()) {
-
-        handle_client();
-    } else {
-        create_connection();
-    }
+    return 0;
 }
 
 void CommunicationsController::handle_client() {
     l4s.flush();
     received = recv();
-    WebMonitor.println(received);
 }
 
-void CommunicationsController::parseLook4Sat(float* azimuth, float* altitude) {
-    *azimuth  = received.substring(0, 3).toFloat();
-    *altitude = received.substring(3, 6).toFloat(); //TODO
+byte CommunicationsController::parseLook4Sat(float* azimuth, float* altitude) {
+    String tmp = received.substring(9);
+    int spaceIndex = tmp.indexOf(' ');
+
+    if (spaceIndex != -1) {
+        *azimuth = tmp.substring(0, spaceIndex).toFloat();
+        *altitude = tmp.substring(spaceIndex+1).toFloat();
+
+        return 1;
+    }
+
+    return 0;
 }
 
 void CommunicationsController::create_connection() {
diff --git a/src/comm.h b/src/comm.h
index 731e751..a3ea71e 100644
--- a/src/comm.h
+++ b/src/comm.h
@@ -12,9 +12,11 @@
 #ifndef COMM_H
 #define COMM_H
 
+
+
 class CommunicationsController {
     private:
-    String received;
+    String received = "Ç";
     WiFiClient l4s;
     WiFiServer l4sServer{L4S_PORT};
 
@@ -26,8 +28,8 @@ class CommunicationsController {
 
     public:
     void init();
-    void process();
-    void parseLook4Sat(float* azimuth, float* altitude);
+    byte process(float* azimuth, float* altitude);
+    byte parseLook4Sat(float* azimuth, float* altitude);
 };
 
 #endif
diff --git a/src/main.cpp b/src/main.cpp
index 1b5c370..ca91fe9 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -21,7 +21,19 @@ void setup() {
     delay(1000);
 }
 
+float azimuth = 0.0;
+float altitude = 0.0;
+
 void loop() {
-    comms.process();
+    byte resp = comms.process(&azimuth, &altitude);
+
+    if (resp != 1) {
+        return;
+    }
+
     controller.updateHeading();
+
+    WebMonitor.println(azimuth);
+    WebMonitor.println(altitude);
+    controller.moveToAzAlt(azimuth, altitude);
 }
diff --git a/src/servo.cpp b/src/servo.cpp
index 9b3b365..0582d04 100644
--- a/src/servo.cpp
+++ b/src/servo.cpp
@@ -5,6 +5,7 @@ void ServoController::initServos() {
     HrzSrv.attach(12,  500, 2500);
     delay(50); // Espera 50ms para o motor se mover
     VrtSrv.attach(13,  500, 2500);
+    delay(50);
 }
 
 void ServoController::initMag() {
@@ -60,7 +61,7 @@ void ServoController::updateHeading() {
     headingDegrees /= HEADING_BUFFER_LENGTH;
 }
 
-void ServoController::moveToAzAlt() {
+void ServoController::moveToAzAlt(float azimuth, float altitude) {
 
 }
 
diff --git a/src/servo.h b/src/servo.h
index 406eded..07474bd 100644
--- a/src/servo.h
+++ b/src/servo.h
@@ -21,7 +21,7 @@ class ServoController {
     public:
         void initServos();
         void initMag();
-        void moveToAzAlt();
+        void moveToAzAlt(float azimuth, float altitude);
         void moveToRADec();
         void updateHeading();
 };