#include <Arduino.h>
// #include <Stepper.h>

#include <SPI.h>
#include <Wire.h> //  Include I2C library
#include <Adafruit_GFX.h>     //  include Graphical Library
#include <Adafruit_SSD1306.h> // Oled library
#define OLED_RESET -1         // -1 is reset with Arduino

// Make a oled class
// Adafruit_SSD1306 oled(128, 64, &Wire, -1); //  182= Oled width, 64 = oled height, 4 is reset pin
Adafruit_SSD1306 oled(OLED_RESET); //use this with actual install
// Adafruit_SSD1306 oled(128, 64, &Wire, -1);

const int stepsPerRevolution = 123;
// Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11);

const float BETA = 3950; // should match the Beta Coefficient of the thermistor

//====================================================
#define DEBUG 0
#if DEBUG == 1
#define debug(x) Serial.print(x)
#define debugln(x) Serial.println(x)
#else
#define debug(x)
#define debugln(x)
#endif

//======================= Input Pins ==============================
const byte pin_Idle_Sw = 2;                                  // for the idle switch signal, 0 if TPS at idle position.
const byte pin_AC_Sw = 3;                                    // 12V when AC is on.
const byte pin_ModeSelect_ProgrammingModeRunningMode_Sw = 4; // default is pull up 5V = ProgrammingMode mode.
const byte pin_storeIdlePosAsDefault_Sw = 5;                 // default is pull up 5V, pull down = store value.
const byte pin_pulseSignal = 6;                              // for pulsing signal
const int pin_knop_ProgrammingModePreset_ISC = A1;           // variable resister to adjust preset ISC open position ProgrammingModely.
const int pin_Cooling_Water_Temp = A2;                       // cooling water temperature

// ====================== out put pins==============================
const byte pin_LED_storedNewIdlePos_Sw = 7; // to blink LED after store the new default idlePOS.
// const byte pin_AcRelay_Command = 8;         // to drive A/C relay after delaying 1 second.
const byte pin_stepper_pin1 = 9;
const byte pin_stepper_pin2 = 10;
const byte pin_stepper_pin3 = 11;
const byte pin_stepper_pin4 = 12;

//====================== Variables Parameters =========================
volatile byte val_IscStepCounter;
volatile byte val_Idle_Sw;                 // to check status of idle switch signal (0 (LOW) or 1 (HIGH))
volatile byte val_AC_Sw;                   // to check status of AC clutch switch signal (0 (LOW) or 1 (HIGH))
volatile byte val_Selected_Mode;           // 0 = ProgrammingMode (digital pin, LOW), 1 = RunningMode (digial pin, HIGH)
volatile int val_Mapped_PresetIscPos;      // read pod position then mapped to range betwen 0 to maxIscStep.
volatile int val_presetIscPosAtIdle_acOff; // to store the ISC position that ISC can jump to it immediately to get the proper idle speed when the AC is on.
volatile int val_presetIscPosAtIdle_acOn;  // to store ISC position when AC is on.
byte val_storeIdlePosAsDefault_Sw;         // 0 or 1, switch status
byte stepInterval;
byte maxIscStep; // ISC motor steps between fully close to fully open = 123 from actual tested motor.
String directionText;
String selectedModeText;
byte directionEnum;
int val_ProgrammingModeSet_sPos;
volatile int val_Analog_Cooling_Water_Temp; // to measure temperature of cooling water (0-1023, 10bit, 0-4092 for 12 bit boards)
unsigned int numberOfPulse;                 // if pulse > 100 means engine has started and running.
unsigned int rpmNow;
unsigned int currentRPM;
int stepNum;
int stepToGo;
int openCounter;
int closeCounter;
int currentIscPosition;
int nextPosition;
int iscPresetPosToPrint;
unsigned int numberOfAverageRPM;
unsigned int averagedRPM;
volatile unsigned long timeBtwPulse;               // use in frequency calculation, in millisecond.
volatile long pulseHighDuration, pulseLowDuration; // time in microsecond;
unsigned long runningInit;                         // millisecond
unsigned long val_elapTime;                        // to store how long the running status has been TRUE.
float freqInHz;
float timeBtwPulseInSeconds; // time between pulses of input speed signal in second

// ==================== Boolean Parameters ===========================
volatile bool engineRunStatus; // monitor engine startup condition, if speed > 600 rpm = true.
volatile bool tpsIdleFlag;     // wil be = 1 if TPS is at idle position (idle signal on).
volatile bool acIdleFlag;      // wil be = 1 if air compressor is on.
volatile bool engineWarmFlag;  // True when the cooling water temperature is lower than 0.9 volt
bool isNeedIdleControl;
bool iscReachLimited = false;
bool isNeedDelayAC;
volatile bool val_isProgrammingMode_Mode; // 0 = ProgrammingMode (digital pin, LOW), 1 = RunningMode (digial pin, HIGH)

//******************************* FUNCTIONS *****************************//
// ==== Delay function ========
void pauseMillis(unsigned long pauseTimeMs)
{
  unsigned long now = millis();
  while (millis() - now < pauseTimeMs)
  {
    ;
  }
} // end pauseMillis

// todo -  read RPM ========================
int getAveragedRPM()
{
  int rpmAccumulate = 0;
  int freqInRPM = 0;
  for (byte i = 0; i < numberOfAverageRPM; i++) // default= 3 averages
  {
    pulseHighDuration = pulseIn(pin_pulseSignal, HIGH);
    pulseLowDuration = pulseIn(pin_pulseSignal, LOW);
    freqInHz = 1 / ((pulseHighDuration + pulseLowDuration) / 1000000.0); // float
    freqInHz = freqInHz - (0.0125 * freqInHz);
    freqInRPM = freqInHz * 60;
    rpmAccumulate = rpmAccumulate + freqInRPM;
  }
  averagedRPM = rpmAccumulate / 3;
  // debugln(currentRPM);
  // delay(200);
  return (averagedRPM);
} // end getAveragedRPM

// todo - adjust Idle =======================
//  void setIdleStateTrue() { tpsIdleFlag = true; }
//  void setIdleStateFalse() { tpsIdleFlag = false; }

// void setACState() { acIdleFlag = true; }
// void setACStateInvert() { acIdleFlag = false; }

// todo - check whether the engine has been running or not =================
bool isEngineRunning()
{
  int pulseCounter = 0;
  while (pulseCounter <= 100)
  {
    if (digitalRead(4) == HIGH)
    {
      pulseCounter = pulseCounter + 1;
      // if ((pulseCounter % 50) == 0)
      // {
      // debug("number of pulse = ");
      // debugln(pulseCounter);
      // }
    }
  }

  debug("number of pulse = ");
  debugln(pulseCounter);
  // engineRunStatus = true;
  // If pulse > 100 means engine has started and running, otherwise, just wait more pulse.
  // if (pulseCounter >= 100)
  // {
  runningInit = 0;
  return (true);
  // }
  // return engineRunStatus;
}

// todo - motor One Step move ===================
void oneStep(int dirEnum)
{
  if (dirEnum == 0) // => move ISC valve toward motor side (e.g. "in")
  {
    switch (stepNum)
    {
    case 0:
      digitalWrite(9, 0x00);
      digitalWrite(10, 0x01);
      digitalWrite(11, 0x01);
      digitalWrite(12, 0x01);
      break;
    case 1:
      digitalWrite(9, 0x01);
      digitalWrite(10, 0x00);
      digitalWrite(11, 0x01);
      digitalWrite(12, 0x01);
      break;
    case 2:
      digitalWrite(9, 0x01);
      digitalWrite(10, 0x01);
      digitalWrite(11, 0x00);
      digitalWrite(12, 0x01);
      break;
    case 3:
      digitalWrite(9, 0x01);
      digitalWrite(10, 0x01);
      digitalWrite(11, 0x01);
      digitalWrite(12, 0x00);
      break;
    }
  }
  else if (dirEnum == 1) // => move ISC valve away from motor side (e.g. "out")
  {
    switch (stepNum)
    {
    case 0:
      digitalWrite(9, 0x01);
      digitalWrite(10, 0x01);
      digitalWrite(11, 0x01);
      digitalWrite(12, 0x00);
      break;
    case 1:
      digitalWrite(9, 0x01);
      digitalWrite(10, 0x01);
      digitalWrite(11, 0x00);
      digitalWrite(12, 0x01);
      break;
    case 2:
      digitalWrite(9, 0x01);
      digitalWrite(10, 0x00);
      digitalWrite(11, 0x01);
      digitalWrite(12, 0x01);
      break;
    case 3:
      digitalWrite(9, 0x00);
      digitalWrite(10, 0x01);
      digitalWrite(11, 0x01);
      digitalWrite(12, 0x01);
      break;
    }
  }
  delay(stepInterval);
  stepNum++;
  if (stepNum > 3)
  {
    stepNum = 0;
  }
} // end of oneStep

// todo - Keep all coils at HIGH so, no diff voltage, no magnetic remain, reduce heat ==============
void keepCool()
{
  digitalWrite(9, HIGH);  // reduce heat in motor
  digitalWrite(10, HIGH); //  reduce heat in motor
  digitalWrite(11, HIGH); //  reduce heat in motor
  digitalWrite(12, HIGH); //  reduce heat in motor
} // end of keepCool
// todo - control both direction and steps ========================
void openOrCloseByStep(String open_or_close, int step)
{
  if (open_or_close == "open")
  {
    directionEnum = 0;
  }
  else if (open_or_close == "close")
  {
    directionEnum = 1;
  }

  openCounter = 0;
  closeCounter = 0;

  switch (directionEnum)
  {
  case 0: // valve move in, e.g., ISC open (ground sequence =  pin 9, 10, 11, 12)
    for (int i = 0; i < step; i++)
    {
      oneStep(0); // cw
      openCounter += 1;
      // delay(10);
      currentIscPosition += 1;
      debug("Open Counter = ");
      debugln(openCounter);
      debug("Current Position = ");
      debugln(currentIscPosition);
    }
    keepCool(); // reduce heat in motor
    break;

  case 1: // valve move out, e.g., ISC close (ground sequence =  pin 12, 11, 10, 9)
    for (int i = 0; i < step; i++)
    {
      oneStep(1); // ccw
      closeCounter -= 1;
      // delay(10);
      currentIscPosition -= 1;
      debug("Close Counter = ");
      debugln(closeCounter);
      debug("Current Position = ");
      debugln(currentIscPosition);
    }
    keepCool(); // reduce heat in motor
    break;
  }
} // end of openOrCloseByStep

// todo -  initialized the ISCV by move the valve to fully clsoe position ================
void move_ISCV_to_zero()
{
  openOrCloseByStep("close", maxIscStep);
  currentIscPosition = 0;
  keepCool(); // reduce heat in motor
} // end of move_ISCV_to_zero

// todo -  Move ISC to any position in between 0 to maxISCstep (default = 123)
void moveToPos(int newPos)
{
  // todo - to ensure that the POS is within the possible range.
  if (newPos < 0 || newPos > maxIscStep)
  {
    // --------- check integrity of  newPos --------------------
    if (newPos < 0)
    {
      newPos = 0;
      debug("The next position is lower than 0. So, reset the next position to ==> ");
      debugln(newPos);
    }
    else if (newPos > maxIscStep)
    {
      newPos = maxIscStep;
      debug("The next position is more than Maximum limit. So, reset the next position to the maximum limit ==> ");
      debugln(newPos);
    }
  }
  //------------------ to run after get proper newPos value -------
  if (newPos > currentIscPosition)
  {
    openOrCloseByStep("open", newPos - currentIscPosition);
    // pauseMillis(10);
    currentIscPosition = newPos;
  }
  else if (newPos < currentIscPosition)
  {
    openOrCloseByStep("close", currentIscPosition - newPos);
    // pauseMillis(10);
    currentIscPosition = newPos;
  }
  else
  {
    ;
  } // e.g., newPos = currentISCposition. so do nothing
  debug("current ISCV Position is ===> ");
  debugln(currentIscPosition);
  // keepCool();
} // end of moveToPos()

// //::::::::::: change mode ::::::::::
// void changeMode()
// {
//   val_isProgrammingMode_Mode = !val_isProgrammingMode_Mode;
//   delay(10);
//   if (val_isProgrammingMode_Mode == 1)
//   {
//     debugln("Mode = ProgrammingMode (1)");
//     delay(10);
//   }
//   if (val_isProgrammingMode_Mode == 0)
//   {
//     debugln("Mode = RunningMode (0)");
//     delay(10);
//   }
// }

// //:::::::::::::::::: set new defaulIdle position ::::::::::::::
// void setNewIdlePos()
// {
//   val_presetIscPosAtIdle_acOff = currentIscPosition;
//   debug("set newDefaultIdlePos at :");
//   debugln(val_presetIscPosAtIdle_acOff);
//   delay(10);
// }

// todo - tempo
bool checkRunningStatus()
{
  if (rpmNow > 300)
  {
    return true;
  }
  else
  {
    return false;
  }
}

bool isNeedIdleAdjusment()
{
  // Todo 2 - check whether or not the ISC motor have to run, trigger by following logic True
  /*
  1. Engine is running,
  2. Elap time from swith on is longer than 5 seconds. (can be adjust to longer value)
  3. Idle switch is on (digital input becomes LOW(0))
  4. AC clutch is on (digital input becomes LOW(0))
  5. Cooling water temp voltage < 2 VDC (about 60C)
  5. The RPM is less than 1200, ensure that the Wax valve has almostly closed.
  6. ISC has not reached min or max step boundaary.
  */
  bool isNeeded = (engineRunStatus == true &&
                   // millis() - runningInit > 5000 &&
                   val_Idle_Sw == 0 &&
                   // val_AC_Sw == 0 && // use attachedInterrupt pin D3 when AC is on.
                   val_Analog_Cooling_Water_Temp <= 400 &&
                   rpmNow <= 1200);
  //  iscReachLimited == false);
  return isNeeded;
}

void displayRPMandIscPos(int rpm, int nowPos, int presetPos)
{
  int x = 1;
  oled.setTextSize(x);                         // first we have to decide font size
  oled.setCursor(1, 1*x);                        // then set print location or printing cursor
  oled.println("Md:");                         // print conmmand
  oled.setCursor(17, 1*x);                       // then set print location or printing cursor
  oled.println(val_Selected_Mode);             // print conmmand
  oled.setCursor(27, 1*x);                       // then set print location or printing cursor
  oled.println("RPM: ");                       // print conmmand
  oled.setCursor(52, 1*x);                       // then set print location or printing cursor
  oled.println(rpm);                           // print conmmand
  oled.setCursor(78, 1*x);                       // then set print location or printing cursor
  oled.println("CT. ");                        // print conmmand
  oled.setCursor(100, 1*x);                      // then set print location or printing cursor
  oled.println(val_Analog_Cooling_Water_Temp); // print conmmand
  oled.setCursor(1, 12*x);                       // then set print location or printing cursor for ISC position
  oled.println("ISC:");                        // print conmmand
  oled.setCursor(30, 12*x);                      // then set print location or printing cursor
  oled.println(nowPos);                        // print conmmand
  oled.setCursor(50, 12*x);                      // then set print location or printing cursor
  oled.println("/123");                        // print conmmand

  oled.setCursor(80, 12*x);  // then set print location or printing cursor
  oled.println("set@: ");  // print conmmand
  oled.setCursor(110, 12*x); // then set print location or printing cursor
  oled.println(presetPos); // print conmmand

  oled.setCursor(1, 24*x);                  // then set print location or printing cursor
  oled.println("IdSw:");                  // print conmmand
  oled.setCursor(30, 24*x);                 // then set print location or printing cursor
  oled.println(digitalRead(pin_Idle_Sw)); // print conmmand

  oled.setCursor(45, 24*x);               // then set print location or printing cursor
  oled.println("AcSW:");                // print conmmand
  oled.setCursor(75, 24*x);               // then set print location or printing cursor
  oled.println(digitalRead(pin_AC_Sw)); // print conmmand

  oled.setCursor(87, 24*x);              // then set print location or printing cursor
  oled.println("Adj:");                // print conmmand
  oled.setCursor(115, 24*x);             // then set print location or printing cursor
  oled.println(isNeedIdleAdjusment()); // print conmmand

  oled.display(); // oled display command
  // oled.invertDisplay(true);
}

//  ****************************** SET UP  **************************************************************************************************************

void setup()
{
  oled.begin(SSD1306_SWITCHCAPVCC, 0x3C); // SSD1306_SWITCHCAPPVCC is generate 3.3volt internaly,  0x3c is I2c slave(oled) address
  oled.clearDisplay();                    // Celar display function
  oled.setTextSize(1);                    // first we have to decide font size
  oled.setTextColor(SSD1306_WHITE);       // we have to decide back ground color
  oled.setCursor(10, 10);                 // then set print location or printing cursor
  oled.println("Idle X! Set 0 ISC");      // print conmmand
  oled.display();                         // oled display command
  // oled.invertDisplay(true);
  pauseMillis(10);
  oled.clearDisplay(); // Celar display function

  delay(100);
  Serial.begin(9600);
  delay(900);

  debugln();
  debugln("============ Setup Begin =============");

  //--> setup digital inputs
  pinMode(pin_Idle_Sw, INPUT_PULLUP);                                  // D2 TPS at Idle Position Signal
  pinMode(pin_AC_Sw, INPUT_PULLUP);                                    // D3 AC switch signal (12 V = on)
  pinMode(pin_ModeSelect_ProgrammingModeRunningMode_Sw, INPUT_PULLUP); // D4 HIGH = ProgrammingMode, LOW  = RunningMode
  pinMode(pin_storeIdlePosAsDefault_Sw, INPUT_PULLUP);                 // D5 initial val_presetIscPosAtIdle_acOff = 62. After press the switch, the val_presetIscPosAtIdle_acOff will change based on Pot (variable resister mapped).
  pinMode(pin_pulseSignal, INPUT);                                     // D6 speed signal

  // --> digital output
  pinMode(pin_LED_storedNewIdlePos_Sw, OUTPUT); // D7 led brink after pus D5
  // pinMode(pin_LED_storedNewIdlePos_Sw, LOW);        // D7 led brink after pus D5
  // pinMode(pin_AcRelay_Command, OUTPUT);    // D8 LOW (0) will change relay stae from NO to close, AC clutch engage (delay 1 second after AC swithc on)
  // digitalWrite(pin_AcRelay_Command, HIGH); // keep default LOW state, so AC can operate as OEM.

  //--> Analog input
  pinMode(pin_Cooling_Water_Temp, INPUT); // water temp <0.9  = 80c (about 200/1023 of analogRead)

  // initialized values
  engineRunStatus = false; // the engine has not started, just key pwoer on.
  timeBtwPulseInSeconds = 0.0;
  numberOfAverageRPM = 3;
  averagedRPM = 0;
  rpmNow = 0;
  currentRPM = 0;
  stepNum = 0;
  stepToGo = 0;
  openCounter = 0;
  closeCounter = 0;
  val_presetIscPosAtIdle_acOff = 60; // to store the ISC position that ISC can jump to it immediately to get the proper idle speed when the AC is still off.
  val_presetIscPosAtIdle_acOn = 80;  // to store default isc position when AC on
  isNeedIdleControl = false;
  iscPresetPosToPrint = 999;

  selectedModeText = "ProgrammingMode";

  // setup ISC motor and iniitialize ISC.
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);
  keepCool();                      // make all pins in HIGH state to reduce heat in motor
  maxIscStep = 123;                // ISC motor steps between fully close to fully open = 123 from actual tested motor.
  stepInterval = 10;               // set ISC speed, low interval = high speed. default = 15;
  currentIscPosition = maxIscStep; // positon 0 means ISCV at fully close position. this parameter used in setup function to let ISC to fully close after power on.
  move_ISCV_to_zero();             // step the ICV to fully close position (ISC valve moves out 123 steps)
  currentIscPosition = 0;          // positon 0 means ISCV at fully close position.

  rpmNow = map(analogRead(A6), 0, 1023, 0, 1260); // sim RPM pin A6 (read Pot during testing in Wokwi)
  // rpmNow = getAveragedRPM(); pauseMillis(5); //pin d6 , Use this with actual pulse signal

  displayRPMandIscPos(rpmNow, currentIscPosition, val_presetIscPosAtIdle_acOff);
  oled.clearDisplay(); // Celar display function

  // attachInterrupt(digitalPinToInterrupt(pin_AC_Sw), delayACclutch, FALLING);

  debugln("============ Setup Completed | ISC fully opened =============");
  debugln();
} // end of setup

//   MAIN LOOP  **************************************************************************************************************
void loop()
{
  debugln("==========Void Loop begin ==========");

  debugln("============= Check Inputs ==================");
  // Todo - measure input parameter values
  pauseMillis(10);
  val_Idle_Sw = digitalRead(pin_Idle_Sw);
  pauseMillis(15); // pin 2
  debug("01 - Pin2 Value pin_Idle_Sw = ");
  debugln(val_Idle_Sw);
  val_AC_Sw = digitalRead(pin_AC_Sw);
  pauseMillis(5); // pin 3
  debug("02 - Pin3 Value pin_AC_Sw = ");
  debugln(val_AC_Sw);

  val_Selected_Mode = digitalRead(pin_ModeSelect_ProgrammingModeRunningMode_Sw);
  pauseMillis(5); // pin 4, 0 = ProgrammingMode, 1 = RunningMode
  debug("03 - Pin4 Current Mode = ");
  debugln(val_Selected_Mode);

  // todo - define operating mode
  if (val_Selected_Mode == 1)
  {
    selectedModeText = "ProgrammingMode";
  }
  if (val_Selected_Mode == 0)
  {
    selectedModeText = "RunningMode";
  }
  val_storeIdlePosAsDefault_Sw = digitalRead(pin_storeIdlePosAsDefault_Sw);
  pauseMillis(5); // pin 5
  debug("04 - Pin5 val_storeIdlePosAsDefault_Sw = ");
  debugln(val_storeIdlePosAsDefault_Sw);
  // todo - teporary
  rpmNow = map(analogRead(A6), 0, 1023, 0, 1260); // sim RPM pin A6 (read Pot during testing in Wokwi)
  // rpmNow = getAveragedRPM(); // pin d6 , Use this with actual pulse signal
  pauseMillis(5);
  debug("05 - Pin D6 rpmNow = ");
  debugln(rpmNow);
  val_Mapped_PresetIscPos = map(analogRead(pin_knop_ProgrammingModePreset_ISC), 0, 1023, 0, maxIscStep);
  pauseMillis(5); // pin A1
  debug("06 - PinA1 Mapped PresetIscPos = ");
  debugln(val_Mapped_PresetIscPos);
  // todo - set new idle pos if the push buttom pressed.
  if (digitalRead(pin_storeIdlePosAsDefault_Sw) == 0)
  {
    val_presetIscPosAtIdle_acOff = val_Mapped_PresetIscPos;
    // LED blinks 2 timws
    for (int i = 0; i <= 1; i++)
    {
      digitalWrite(pin_LED_storedNewIdlePos_Sw, HIGH);
      pauseMillis(250);
      digitalWrite(pin_LED_storedNewIdlePos_Sw, LOW);
      pauseMillis(250);
    }
  }
  debug("06.1 - new presetIscPosAtIdle = ");
  debugln(val_presetIscPosAtIdle_acOff);

  val_Analog_Cooling_Water_Temp = analogRead(pin_Cooling_Water_Temp);
  pauseMillis(5); // pin A3 raw
  debug("07 - PinA3 Analog_Cooling_Water_Temp (xxxx/1023) = ");
  debugln(val_Analog_Cooling_Water_Temp);
  // todo - teporary
  engineRunStatus = checkRunningStatus();
  debug("08 - Engine Run Status = ");
  debugln(engineRunStatus);
  // engineRunStatus = isEngineRunning(); pauseMillis(5); // Use with pulse signal
  // val_elapTime = millis() - runningInit;

  isNeedIdleControl = isNeedIdleAdjusment(); // adjustment will not work if the value is false.
  debug("inNeedIdleControl = ");
  debugln(isNeedIdleControl);
  pauseMillis(100);

  // Not at idle
  if (isNeedIdleControl == false)
  {
    moveToPos(0);
    pauseMillis(100);
  }
  // ------------------------------------------ CASE AC OFF --------------------------------

  // at idle with ProgrammingMode MODE : user can will adjust the iSC by knop -------------------
  if (isNeedIdleControl == true && selectedModeText == "ProgrammingMode" && digitalRead(pin_AC_Sw) == 1)
  {
    debugln("xxxxxxxxxProgramming Mode STARTTTTTTTTTTTTTTTTTTT xxxxxxxxxxxx");
    //  rpmNow = getAveragedRPM();
    //  pauseMillis(10);
    val_Mapped_PresetIscPos = map(analogRead(pin_knop_ProgrammingModePreset_ISC), 0, 1023, 0, maxIscStep);
    pauseMillis(10);
    val_presetIscPosAtIdle_acOff = val_Mapped_PresetIscPos;
    moveToPos(val_presetIscPosAtIdle_acOff);
    pauseMillis(100);
    //  delay(1000);
    debugln("xxxxxxxxxProgramming Mode ENDDDDDDDDDDDDDD xxxxxxxxxxxx");
  }
  // --------------------- Running Mode : ISC automatically move based on idle speed --------------------
  if (isNeedIdleControl == true && selectedModeText == "RunningMode" && digitalRead(pin_AC_Sw) == 1)
  {
    // Serial.println("-------------- RUNNING MODE ------------------");
    moveToPos(val_presetIscPosAtIdle_acOff); // first move
    pauseMillis(1000);
    rpmNow = map(analogRead(A6), 0, 1023, 0, 1260); // sim RPM pin A6 (read Pot during testing in Wokwi)
  // rpmNow = getAveragedRPM(); pauseMillis(5); //pin d6 , Use this with actual pulse signal

    if (rpmNow >= 750 && rpmNow <= 900) // Idle OK
    {
      val_presetIscPosAtIdle_acOff = currentIscPosition;
      // val_presetIscPosAtIdle_acOn = currentIscPosition+12;

      pauseMillis(10);
      // Serial.println("-------------- IDLE OK ------------------");
    }
    else if (rpmNow < 750) // too low idle
    {
      val_presetIscPosAtIdle_acOff += 8;
      moveToPos(val_presetIscPosAtIdle_acOff);
      // Serial.print("curent Pos = "); Serial.println(val_presetIscPosAtIdle_acOff + 4);
      pauseMillis(1000);
    }
    else if (rpmNow > 900) // too high idle
    {
      val_presetIscPosAtIdle_acOff -= 8;
      moveToPos(val_presetIscPosAtIdle_acOff);
      // Serial.print("curent Pos = "); Serial.println(val_presetIscPosAtIdle_acOff - 4);
      pauseMillis(1000);
    }
  }

  // ------------------------------------------ CASE AC ON --------------------------------

  // at idle with ProgrammingMode MODE : user can will adjust the iSC by knop -------------------
  if (isNeedIdleControl == true && selectedModeText == "ProgrammingMode" && digitalRead(pin_AC_Sw) == 0)
  {
    debugln("xxxxxxxxxProgramming Mode STARTTTTTTTTTTTTTTTTTTT xxxxxxxxxxxx");
    //  rpmNow = getAveragedRPM();
    //  pauseMillis(10);
    val_Mapped_PresetIscPos = map(analogRead(pin_knop_ProgrammingModePreset_ISC), 0, 1023, 0, maxIscStep);
    pauseMillis(10);
    val_presetIscPosAtIdle_acOn = val_Mapped_PresetIscPos;
    moveToPos(val_presetIscPosAtIdle_acOn);
    pauseMillis(100);
    //  delay(1000);
    debugln("xxxxxxxxxProgramming Mode ENDDDDDDDDDDDDDD xxxxxxxxxxxx");
  }
  // --------------------- Running Mode : ISC automatically move based on idle speed --------------------
  if (isNeedIdleControl == true && selectedModeText == "RunningMode" && digitalRead(pin_AC_Sw) == 0)
  {
    // Serial.println("-------------- RUNNING MODE ------------------");
    moveToPos(val_presetIscPosAtIdle_acOn); // first move
    pauseMillis(1000);
    rpmNow = map(analogRead(A6), 0, 1023, 0, 1260); // sim RPM pin A6 (read Pot during testing in Wokwi)
    // rpmNow = getAveragedRPM(); pauseMillis(5); //pin d6 , Use this with actual pulse signal
    if (rpmNow >= 750 && rpmNow <= 900) // Idle OK
    {
      val_presetIscPosAtIdle_acOn = currentIscPosition;
      // val_presetIscPosAtIdle_acOff = currentIscPosition-12;
          pauseMillis(10);
      // Serial.println("-------------- IDLE OK ------------------");
    }
    else if (rpmNow < 750) // too low idle
    {
      val_presetIscPosAtIdle_acOn += 8;
      moveToPos(val_presetIscPosAtIdle_acOn);
      // Serial.print("curent Pos = "); Serial.println(val_presetIscPosAtIdle_acOn + 4);
      pauseMillis(1000);
    }
    else if (rpmNow > 900) // too high idle
    {
      val_presetIscPosAtIdle_acOn -= 8;
      moveToPos(val_presetIscPosAtIdle_acOn);
      // Serial.print("curent Pos = "); Serial.println(val_presetIscPosAtIdle_acOn - 4);
      pauseMillis(1000);
    }
  }

  if (digitalRead(pin_AC_Sw) == 1)
  {
    iscPresetPosToPrint = val_presetIscPosAtIdle_acOff;
  }
  else if (digitalRead(pin_AC_Sw) == 0)
  {
    iscPresetPosToPrint = val_presetIscPosAtIdle_acOn;
  }

  displayRPMandIscPos(rpmNow, currentIscPosition, iscPresetPosToPrint);
  pauseMillis(5);
  oled.clearDisplay();

} // end of main loop.