From 4ae7390d45bb3a61c9eb153571b1e764dd99c52e Mon Sep 17 00:00:00 2001
From: CommanderRedYT <ccommanderred@gmail.com>
Date: Thu, 21 Oct 2021 20:40:28 +0200
Subject: [PATCH] Added hybrid mode

---
 main/displays/menus/defaultmodesettingsmenu.h |  5 +++
 main/modes/defaultmode.h                      | 32 ++++++++++++++++++-
 main/texts.h                                  |  1 +
 3 files changed, 37 insertions(+), 1 deletion(-)

diff --git a/main/displays/menus/defaultmodesettingsmenu.h b/main/displays/menus/defaultmodesettingsmenu.h
index 36e6456..13a6c4a 100644
--- a/main/displays/menus/defaultmodesettingsmenu.h
+++ b/main/displays/menus/defaultmodesettingsmenu.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <TFT_eSPI.h>
+
 // local includes
 #include "menudisplay.h"
 #include "utils.h"
@@ -8,6 +10,7 @@
 #include "menuitem.h"
 #include "actions/toggleboolaction.h"
 #include "actions/switchscreenaction.h"
+#include "actions/dummyaction.h"
 #include "checkboxicon.h"
 #include "icons/back.h"
 #include "texts.h"
@@ -122,6 +125,8 @@ class DefaultModeSettingsMenu :
 public:
     DefaultModeSettingsMenu()
     {
+        auto diff = std::abs(settings.hybrid.activationLimit - settings.hybrid.deactivationLimit);
+        if (diff < 20) { constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_LIMITS_TO_NEAR>, StaticFont<2>, StaticColor<TFT_RED>, DummyAction>>(); }
         constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_MODELMODE>,          SwitchScreenAction<DefaultModeModelModeChangeDisplay>>>();
         constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_HYBRIDMODE>,         SwitchScreenAction<DefaultModeHybridModelModeChangeDisplay>>>();
         constructMenuItem<makeComponent<MenuItem, StaticText<TEXT_SQUAREGAS>,          ToggleBoolAction, CheckboxIcon,  DefaultModeSquareGasAccessor>>();
diff --git a/main/modes/defaultmode.h b/main/modes/defaultmode.h
index 5654126..e97a8be 100644
--- a/main/modes/defaultmode.h
+++ b/main/modes/defaultmode.h
@@ -23,6 +23,7 @@ public:
 
     bool waitForGasLoslass{false};
     bool waitForBremsLoslass{false};
+    bool hybridModeActivated{false};
 
 private:
     espchrono::millis_clock::time_point lastTime{espchrono::millis_clock::now()};
@@ -128,7 +129,36 @@ void DefaultMode::update()
         lastPwm = pwm;
         lastTime = now;
 
-        const auto pair = split(settings.defaultMode.modelMode);
+        auto pair = split(settings.defaultMode.modelMode);
+
+        if (settings.hybrid.enable)
+        {
+            auto activationLimit = settings.hybrid.activationLimit;
+            auto deactivationLimit = settings.hybrid.deactivationLimit;
+            auto diff = std::abs(activationLimit - deactivationLimit);
+
+            if (diff < 20)
+            {
+                int half = (diff / 2) + 0.5;
+                deactivationLimit -= half;
+                activationLimit += half;
+            }
+
+            if (!hybridModeActivated && (pwm > activationLimit))
+            {
+                hybridModeActivated = true;
+            }
+            else if (hybridModeActivated && (pwm < deactivationLimit))
+            {
+                hybridModeActivated = false;
+            }
+
+            if (hybridModeActivated)
+            {
+                pair = split(settings.hybrid.hybridMode);
+            }
+        }
+
         for (bobbycar::protocol::serial::MotorState &motor : motorsInController(controllers.front))
         {
             motor.ctrlTyp = pair.first;
diff --git a/main/texts.h b/main/texts.h
index 30d4608..ba7bef7 100644
--- a/main/texts.h
+++ b/main/texts.h
@@ -214,6 +214,7 @@ constexpr char TEXT_ADDBRAKEVAL[] = "Sub Brake";
 constexpr char TEXT_HYBRIDENABLE[] = "Enable Hybrid mode";
 constexpr char TEXT_HYBRIDACTIVATIONLIMIT[] = "Hybrid activation-limit";
 constexpr char TEXT_HYBRIDDEACTIVATIONLIMIT[] = "Hybrid deactivation-limit";
+constexpr char TEXT_LIMITS_TO_NEAR[] = "Hybrid limits too near (>20)";
 //constexpr char TEXT_BACK[] = "Back";
 
 //DynamicDebugMenu