// ================================================================
//  PWM PRESCALER OPTIMISER
//
//  The ESP32 has two MCPWM units and each unit has ONE clock
//  prescaler (clk_cfg.clk_prescale) shared by both of its timers:
//
//    Gen 1: MCPWM0, Timer 0, Operator 0 -> GPIO 32  \_ share MCPWM0 clk
//    Gen 2: MCPWM0, Timer 1, Operator 1 -> GPIO 33  /
//    Gen 3: MCPWM1, Timer 0, Operator 0 -> GPIO 27  \_ share MCPWM1 clk
//    Gen 4: MCPWM1, Timer 1, Operator 1 -> GPIO 14  /
//
//  The output frequency of one channel is
//      f = 160 MHz / (clkPre + 1) / (timerPre + 1) / (period + 1)
//  with clkPre and timerPre each 0..255 and period 1..65535.
//
//  For a given clkPre the reachable band is therefore
//      lowest  f = 160 MHz / ((clkPre + 1) * 256 * 65536)
//      highest f = 160 MHz / ((clkPre + 1) * 2)
//  At clkPre = 0 the lowest reachable frequency is 9.5367 Hz.
//
//  BUG (fixed here): the old code chose the shared prescaler from the
//  MASTER channel alone (Gen1 for MCPWM0, Gen3 for MCPWM1) and then
//  made the secondary live with it. Any master above ~10 Hz drove its
//  unit to clkPre = 0, which put every secondary frequency below
//  9.5367 Hz outside the reachable band. The locked-clock search then
//  found no legal period, silently returned period = 0, and that zero
//  was written straight into the timer -- so Gen2 / Gen4 went dead.
//  Reported case: Gen1 = 100 Hz, Gen2 = 7.87 Hz -> no output on Gen2.
//
//  The prescaler is now chosen for BOTH channels of a unit at once,
//  and the search can no longer return a period of zero.
// ================================================================

// Frequency error is scored in whole tenths of a ppm. Two candidates that agree
// to that grid are called equal, and the search then spends its remaining
// freedom on duty-cycle resolution (a bigger period) rather than chasing
// accuracy far below what the ESP32 crystal itself holds. Scoring on a grid
// also keeps the comparisons exact -- scoring raw doubles let a difference in
// the last bit masquerade as a real accuracy win.
//
// SPEED: the ESP32 has no hardware double unit, so every double divide here is
// a software routine of a few hundred cycles. The search is written to do as
// few of them as possible -- see NOTE 28 in the main .ino. Three things do the
// work: the timer prescaler is stepped only across the range that can actually
// produce a legal period, the scan stops the moment it lands exactly on the
// requested frequency, and each candidate costs two divides instead of five.

// How many timer-prescaler steps to try per clock prescaler. The period is
// largest at the first step and falls away as roughly 1/step, so both the
// accuracy on offer and the duty-cycle resolution are concentrated at the
// start of the range -- scanning the whole 256 costs eight times as much and
// buys about a ppm. Measured over 15000 random frequency pairs: median added
// error 0.000 ppm, 99th percentile 1.1 ppm, and no channel ever became
// unreachable that the full scan could reach.
#define TIMER_SCAN_SPAN 32

// ===== ONE CHANNEL AT A FIXED CLOCK PRESCALER =====
// divTotal is 160000000 / freq, worked out once by the caller. cp is the clock
// prescaler PLUS ONE. Returns true if freq is inside the reachable band for cp;
// when it is not, the nearest legal registers are still written back -- never a
// period of zero -- so a channel degrades instead of dying.
static bool bestForCp(double divTotal, uint16_t cp,
                      uint16_t &timerPre, uint32_t &period, double &score)
{
  double remaining = divTotal / (double)cp;      // divide still owed by timer x period

  // period+1 has to land in [2, 65536], which confines timerPre+1 to this range.
  // Stepping only across it skips every candidate the old scan used to compute
  // and then throw away.
  long tpLo = (long)ceil(remaining * (1.0 / 65536.0));
  long tpHi = (long)(remaining * 0.5);
  if (tpLo < 1)   tpLo = 1;
  if (tpHi > 256) tpHi = 256;
  if (tpHi > tpLo + (TIMER_SCAN_SPAN - 1)) tpHi = tpLo + (TIMER_SCAN_SPAN - 1);

  double   bestScore  = 0.0;
  uint32_t bestPeriod = 0;
  uint16_t bestTimer  = 0;
  bool     found      = false;
  double   reject     = 0.0;   // see the cheap test below

  for (long tp = tpLo; tp <= tpHi; tp++) {
    double q = remaining / (double)tp;           // the ideal period+1, not yet rounded
    uint32_t pp = (uint32_t)(q + 0.5);           // nearest period+1 the register can hold
    if (pp < 2 || pp > 65536) continue;          // rounding can nudge us over an edge
    double d = fabs(q - (double)pp);             // how far off, in period counts
    // Relative error is d/pp, so this candidate can only win if d/pp*1e7 lands
    // under the score to beat. Testing it as a multiply first means the divide
    // is only paid for by candidates that stand a chance -- and since the score
    // to beat drops quickly, most candidates never reach it.
    if (found && d * 1e7 >= reject * (double)pp) continue;
    double sc = floor(d / (double)pp * 1e7 + 0.5);
    if (!found || sc < bestScore) {
      bestScore = sc; bestPeriod = pp - 1; bestTimer = (uint16_t)(tp - 1); found = true;
      reject = bestScore + 0.5;
      // tp only grows from here, so the period only shrinks -- an exact hit at
      // this point is already the best frequency AND the best resolution.
      if (sc == 0.0) break;
    }
  }

  if (!found) {
    // Out of band for this prescaler. Clamp to the nearest legal corner so the
    // channel still produces something and the group search can score it.
    if (remaining > 65536.0) { bestTimer = 255; bestPeriod = 65535; }  // freq too low
    else                     { bestTimer = 0;   bestPeriod = 1;     }  // freq too high
    double q = remaining / (double)(bestTimer + 1);
    double pp = (double)(bestPeriod + 1);
    bestScore = floor(fabs(q - pp) / pp * 1e7 + 0.5);
  }

  timerPre = bestTimer; period = bestPeriod; score = bestScore;
  return found;
}

// ===== GROUP OPTIMISER (both timers of one MCPWM unit) =====
// Picks the single shared clock prescaler that serves both channels, scoring
// each candidate by, in order:
//    1. whether BOTH channels are actually reachable,
//    2. the worst frequency error of the two,
//    3. the coarser of the two periods (duty-cycle resolution).
// Pass 0 for a channel that is not configured yet and it is ignored.
// Returns false if some channel could not be reached at any prescaler, which
// only happens outside the 0.0373 Hz .. 80 MHz the hardware can produce.
//
// The answer is cached per group. Start/Stop and the Suspend/Resume in
// checkTime() call setFrequency() twice with the SAME four frequencies and only
// the duty cycles changed, and duty has no bearing on the prescalers -- so the
// second pass, and the second of the two applyGroup calls inside each pass,
// costs nothing at all.
struct PrescalerCache {
  bool     valid;
  float    fA, fB;
  uint16_t clkPre, timerPreA, timerPreB;
  uint32_t periodA, periodB;
  bool     ok;
};
static PrescalerCache pcache[2] = {{false,0,0,0,0,0,0,0,false},
                                   {false,0,0,0,0,0,0,0,false}};

bool findBestPrescalersPair(int group, float fA, float fB,
                            uint16_t &clkPre,
                            uint16_t &timerPreA, uint32_t &periodA,
                            uint16_t &timerPreB, uint32_t &periodB)
{
  PrescalerCache &cc = pcache[group];
  if (cc.valid && cc.fA == fA && cc.fB == fB) {
    clkPre = cc.clkPre;
    timerPreA = cc.timerPreA; periodA = cc.periodA;
    timerPreB = cc.timerPreB; periodB = cc.periodB;
    return cc.ok;
  }

  double dA = (fA > 0.0) ? (double)BASE_CLK / (double)fA : 0.0;
  double dB = (fB > 0.0) ? (double)BASE_CLK / (double)fB : 0.0;

  // Narrow the clock prescaler to the window where BOTH channels are reachable.
  // A prescaler inside the window always beats one outside it, so when the
  // window exists the rest of the range cannot win and is never examined.
  long cpLo = 1, cpHi = 256;
  if (dA > 0.0) {
    long lo = (long)ceil(dA * (1.0 / 16777216.0));   // 256 * 65536
    long hi = (long)(dA * 0.5);
    if (lo > cpLo) cpLo = lo;
    if (hi < cpHi) cpHi = hi;
  }
  if (dB > 0.0) {
    long lo = (long)ceil(dB * (1.0 / 16777216.0));
    long hi = (long)(dB * 0.5);
    if (lo > cpLo) cpLo = lo;
    if (hi < cpHi) cpHi = hi;
  }
  if (cpLo < 1) cpLo = 1;
  if (cpHi > 256) cpHi = 256;
  if (cpLo > cpHi) { cpLo = 1; cpHi = 256; }   // no window: best effort over the lot

  int      bestMiss  = 3;
  double   bestScore = 0.0;
  uint32_t bestRes   = 0;
  bool     have      = false;

  for (long cp = cpLo; cp <= cpHi; cp++) {
    uint16_t tA = 0, tB = 0; uint32_t pA = 1, pB = 1;
    double   scA = 0.0, scB = 0.0;
    int      miss = 0;
    uint32_t res  = 65535;    // coarser of the two periods

    if (dA > 0.0) {
      if (!bestForCp(dA, (uint16_t)cp, tA, pA, scA)) miss++;
      if (pA < res) res = pA;
    }
    if (dB > 0.0) {
      if (!bestForCp(dB, (uint16_t)cp, tB, pB, scB)) miss++;
      if (pB < res) res = pB;
    }
    double sc = (scA > scB) ? scA : scB;   // worst of the two channels

    bool better = false;
    if (!have)                 better = true;
    else if (miss < bestMiss)  better = true;
    else if (miss == bestMiss) {
      if (sc < bestScore)                        better = true;
      else if (sc == bestScore && res > bestRes) better = true;
    }

    if (better) {
      bestMiss = miss; bestScore = sc; bestRes = res; have = true;
      clkPre = (uint16_t)(cp - 1);
      timerPreA = tA; periodA = pA; timerPreB = tB; periodB = pB;
    }
  }

  cc.valid = true; cc.fA = fA; cc.fB = fB;
  cc.clkPre = clkPre;
  cc.timerPreA = timerPreA; cc.periodA = periodA;
  cc.timerPreB = timerPreB; cc.periodB = periodB;
  cc.ok = (bestMiss == 0);
  return cc.ok;
}

// ===== REPORT HELPER =====
void reportChannel(const char* name, int gpio, float reqFreq, float reqDuty,
                   uint16_t clkPre, uint16_t timerPre, uint32_t period, uint32_t compare)
{
  double tickRate = (double)BASE_CLK / (clkPre + 1) / (timerPre + 1);
  float actualFreq = tickRate / (period + 1);
  float actualDuty = (float)compare / (period + 1) * 100.0;
if(DEBUG == 2){
  Serial.println("------------------------------------");
  Serial.print(name); Serial.print(" on GPIO "); Serial.println(gpio);
  Serial.print("  Requested: "); Serial.print(reqFreq, 4); Serial.print(" Hz, ");
  Serial.print(reqDuty, 2); Serial.println(" %");
  Serial.print("  Actual:    "); Serial.print(actualFreq, 4); Serial.print(" Hz, ");
  Serial.print(actualDuty, 2); Serial.println(" %");
  Serial.print("  Registers: clkPre="); Serial.print(clkPre);
  Serial.print("  timerPre="); Serial.print(timerPre);
  Serial.print("  period="); Serial.print(period);
  Serial.print("  compare="); Serial.println(compare);
  }
}

// NOTE: retuneGen1() used to live here -- a cut-down register poke that kept the
// clock prescaler locked and re-searched only the timer prescaler, roughly fifty
// times cheaper than setGen1(). It never worked, across three variations, and is
// deliberately not being reintroduced without hardware evidence. Adjust Mode now
// calls setFrequency(1) directly, the same call Protocol and Sweep modes use.
// See NOTE 26 in the main .ino for the full account.

// ================================================================
//  TIMER REGISTER WRITES
//
//  From the ESP32 register description of PWM_TIMERx_CFG0_REG:
//
//     timer_prescale ... "takes effect when PWM timer stops and starts again"
//
//  The timer prescaler is NOT a live register. Written to a timer that is
//  still turning it is simply ignored, and the channel carries on using the
//  divisor it was last started with. timer_period sits in the same register
//  but is a shadow with an update method of "immediate", and clk_prescale is
//  a unit-level register that applies at once -- which is why a botched write
//  produced a WRONG frequency rather than no frequency at all: two of the
//  three values changed and the third did not.
//
//  Nor is it enough to ask the timer to stop and assume it has. timer_start
//  = 0 does not mean "stop now" -- it means "stop at TEZ", the next time the
//  counter passes zero, which coming out of a 2 Hz channel is up to half a
//  second away. Freezing the counter (timer_mod = 0) stops it dead, but a
//  frozen counter never REACHES zero, so that pending stop never completes,
//  the timer was never really stopped, and the new prescaler is ignored all
//  over again. That is what NOTE 34 chased down: out of a fast channel the
//  counter passed zero by itself while the prescaler search was running and
//  everything worked, so only the step FOLLOWING a low frequency came out
//  wrong -- 2 Hz to 52 Hz gave 4.70 Hz, the old divisor with the new period.
//
//  So bring the zero crossing forward rather than waiting for it. The period
//  register updates immediately (unlike the prescaler), so dropping it to just
//  above wherever the counter has already reached makes TEZ arrive within a
//  couple of ticks. Then the stop is real and the next start latches.
//
//  See NOTEs 30 and 34 in the main .ino for how this surfaced.
// ================================================================
static inline void haltTimer(mcpwm_dev_t &unit, int t)
{
  unit.timer[t].timer_cfg1.timer_start = 0;   // stop at the next zero crossing...

  uint32_t v = unit.timer[t].timer_status.timer_value;   // ...and cause one now
  unit.timer[t].timer_cfg0.timer_period = (v < 65528) ? (v + 8) : 1;

  // Wait for it. Bounded, because a timer that is already stopped sits at a
  // value that will never change: the guard costs well under a millisecond and
  // simply leaves the old behaviour in place rather than hanging.
  uint32_t guard = 0;
  while(unit.timer[t].timer_status.timer_value != 0 && ++guard < 20000) { }

  unit.timer[t].timer_cfg1.timer_mod = 0;     // freeze what is now genuinely
}                                             // a stopped timer

// One 32-bit store rather than two read-modify-writes. Assigning the bitfields
// separately reads this register back, modifies it and writes it again, twice,
// with the period shadow live in the middle of it -- there is no reason to give
// the hardware that window when the whole register is being replaced anyway.
static inline void writeTimerCfg0(mcpwm_dev_t &unit, int t,
                                  uint16_t timerPre, uint32_t period)
{
  mcpwm_timer_cfg0_reg_t cfg0;
  cfg0.val = 0;
  cfg0.timer_prescale        = timerPre;
  cfg0.timer_period          = period;
  cfg0.timer_period_upmethod = 0;             // 0 = update the active period now
  unit.timer[t].timer_cfg0.val = cfg0.val;
}

// Passed to applyGroup0/applyGroup1 as "starting" to mean BOTH channels of the
// unit are being loaded together. 1/2 and 3/4 still name a single channel.
#define GEN_BOTH 9

// ================================================================
//  applyGroup0() — programs the whole MCPWM0 unit (Gen1 + Gen2)
//  applyGroup1() — programs the whole MCPWM1 unit (Gen3 + Gen4)
//
//  Both timers of a unit have to be written together because they
//  share one clock prescaler: moving it for one channel changes the
//  tick rate under the other, so the other's timer prescaler and
//  period have to be recomputed at the same time.
//
//  "starting" is the channel the caller just set; it is always
//  (re)started. The partner keeps its own run state -- it is
//  reprogrammed either way, but only restarted if it was running.
// ================================================================
void applyGroup0(int starting)
{
  uint16_t clkPre = 0, tPre1 = 0, tPre2 = 0;
  uint32_t per1 = 1, per2 = 1;

  bool ok = findBestPrescalersPair(0, gen1_Frequency, gen2_Frequency,
                                   clkPre, tPre1, per1, tPre2, per2);
  savedClkPre0 = clkPre;
  MCPWM0.clk_cfg.clk_prescale = clkPre;

  if (!ok && DEBUG == 2)
    Serial.println("MCPWM0: requested frequency outside 0.0373 Hz - 80 MHz, clamped");

  if (gen1_Frequency > 0) {
    // Only force a stop on a timer that is about to be started again. A
    // channel left stopped must NOT be frozen here: stopGen() drives its pin
    // low by way of the generator's "low at TEZ" action, so the counter has to
    // be allowed to reach zero one last time or the output stays where it was.
    bool start1 = (starting == 1 || starting == GEN_BOTH || genRunning[1]);
    if (start1) haltTimer(MCPWM0, 0);       // guarantee the stop->start that
    writeTimerCfg0(MCPWM0, 0, tPre1, per1);   // latches the new prescaler
    uint32_t compare = (uint32_t)(((per1 + 1) * gen1_DutyCycle / 100.0) + 0.5);
    MCPWM0.operators[0].timestamp[0].gen = compare;
    if (start1) {
      MCPWM0.operators[0].generator[0].gen_utez = 2;  // high on zero
      MCPWM0.operators[0].generator[0].gen_utea = 1;  // low on compare
      MCPWM0.timer[0].timer_cfg1.timer_mod   = 1;     // count-up
      MCPWM0.timer[0].timer_cfg1.timer_start = 2;     // run continuously -- and this
      genRunning[1] = true;                           // 0->2 is what latches tPre1
    }
    reportChannel("setGen1 (MCPWM0-T0)", GEN1_PIN, gen1_Frequency, gen1_DutyCycle,
                  clkPre, tPre1, per1, compare);
  }

  if (gen2_Frequency > 0) {
    // Only force a stop on a timer that is about to be started again. A
    // channel left stopped must NOT be frozen here: stopGen() drives its pin
    // low by way of the generator's "low at TEZ" action, so the counter has to
    // be allowed to reach zero one last time or the output stays where it was.
    bool start2 = (starting == 2 || starting == GEN_BOTH || genRunning[2]);
    if (start2) haltTimer(MCPWM0, 1);       // guarantee the stop->start that
    writeTimerCfg0(MCPWM0, 1, tPre2, per2);   // latches the new prescaler
    uint32_t compare = (uint32_t)(((per2 + 1) * gen2_DutyCycle / 100.0) + 0.5);
    MCPWM0.operators[1].timestamp[0].gen = compare;
    if (start2) {
      MCPWM0.operators[1].generator[0].gen_utez = 2;
      MCPWM0.operators[1].generator[0].gen_utea = 1;
      MCPWM0.timer[1].timer_cfg1.timer_mod   = 1;
      MCPWM0.timer[1].timer_cfg1.timer_start = 2;
      genRunning[2] = true;
    }
    reportChannel("setGen2 (MCPWM0-T1)", GEN2_PIN, gen2_Frequency, gen2_DutyCycle,
                  clkPre, tPre2, per2, compare);
  }
}

void applyGroup1(int starting)
{
  uint16_t clkPre = 0, tPre3 = 0, tPre4 = 0;
  uint32_t per3 = 1, per4 = 1;

  bool ok = findBestPrescalersPair(1, gen3_Frequency, gen4_Frequency,
                                   clkPre, tPre3, per3, tPre4, per4);
  savedClkPre1 = clkPre;
  MCPWM1.clk_cfg.clk_prescale = clkPre;

  if (!ok && DEBUG == 2)
    Serial.println("MCPWM1: requested frequency outside 0.0373 Hz - 80 MHz, clamped");

  if (gen3_Frequency > 0) {
    // Only force a stop on a timer that is about to be started again. A
    // channel left stopped must NOT be frozen here: stopGen() drives its pin
    // low by way of the generator's "low at TEZ" action, so the counter has to
    // be allowed to reach zero one last time or the output stays where it was.
    bool start3 = (starting == 3 || starting == GEN_BOTH || genRunning[3]);
    if (start3) haltTimer(MCPWM1, 0);       // guarantee the stop->start that
    writeTimerCfg0(MCPWM1, 0, tPre3, per3);   // latches the new prescaler
    uint32_t compare = (uint32_t)(((per3 + 1) * gen3_DutyCycle / 100.0) + 0.5);
    MCPWM1.operators[0].timestamp[0].gen = compare;
    if (start3) {
      MCPWM1.operators[0].generator[0].gen_utez = 2;
      MCPWM1.operators[0].generator[0].gen_utea = 1;
      MCPWM1.timer[0].timer_cfg1.timer_mod   = 1;
      MCPWM1.timer[0].timer_cfg1.timer_start = 2;
      genRunning[3] = true;
    }
    reportChannel("setGen3 (MCPWM1-T0)", GEN3_PIN, gen3_Frequency, gen3_DutyCycle,
                  clkPre, tPre3, per3, compare);
  }

  if (gen4_Frequency > 0) {
    // Only force a stop on a timer that is about to be started again. A
    // channel left stopped must NOT be frozen here: stopGen() drives its pin
    // low by way of the generator's "low at TEZ" action, so the counter has to
    // be allowed to reach zero one last time or the output stays where it was.
    bool start4 = (starting == 4 || starting == GEN_BOTH || genRunning[4]);
    if (start4) haltTimer(MCPWM1, 1);       // guarantee the stop->start that
    writeTimerCfg0(MCPWM1, 1, tPre4, per4);   // latches the new prescaler
    uint32_t compare = (uint32_t)(((per4 + 1) * gen4_DutyCycle / 100.0) + 0.5);
    MCPWM1.operators[1].timestamp[0].gen = compare;
    if (start4) {
      MCPWM1.operators[1].generator[0].gen_utez = 2;
      MCPWM1.operators[1].generator[0].gen_utea = 1;
      MCPWM1.timer[1].timer_cfg1.timer_mod   = 1;
      MCPWM1.timer[1].timer_cfg1.timer_start = 2;
      genRunning[4] = true;
    }
    reportChannel("setGen4 (MCPWM1-T1)", GEN4_PIN, gen4_Frequency, gen4_DutyCycle,
                  clkPre, tPre4, per4, compare);
  }
}

// ================================================================
//  setGens() — load all four channels in one pass.
//
//  setFrequency() calls this rather than setGen1..setGen4 in turn. Doing them
//  one at a time meant Gen1 was programmed AND STARTED against whatever Gen2's
//  frequency happened to be at that moment -- zero on the first call after a
//  reset -- and then had to be corrected once Gen2 arrived and moved the unit's
//  shared clock. Correcting a timer that is already running cannot change its
//  prescaler (see the note above haltTimer), so Gen1 went on dividing by the
//  number it was started with while its period and its unit clock both moved
//  underneath it. Setting all four frequencies before programming anything
//  means each unit is worked out once, from final values, and started once.
//  It also halves the searching.
// ================================================================
void setGens(float f1, float d1, float f2, float d2,
             float f3, float d3, float f4, float d4)
{
  gen1_Frequency = f1; gen1_DutyCycle = d1;
  gen2_Frequency = f2; gen2_DutyCycle = d2;
  gen3_Frequency = f3; gen3_DutyCycle = d3;
  gen4_Frequency = f4; gen4_DutyCycle = d4;
  applyGroup0(GEN_BOTH);
  applyGroup1(GEN_BOTH);
}

// ================================================================
//  setGen1..setGen4 — unchanged call signatures. Each stores its own
//  frequency and duty, then reprograms its whole MCPWM unit so the
//  shared clock prescaler suits both channels of that unit.
// ================================================================
void setGen1(float freq, float duty)
{
  gen1_Frequency = freq;
  gen1_DutyCycle = duty;
  applyGroup0(1);
}

void setGen2(float freq, float duty)
{
  gen2_Frequency = freq;
  gen2_DutyCycle = duty;
  applyGroup0(2);
}

void setGen3(float freq, float duty)
{
  gen3_Frequency = freq;
  gen3_DutyCycle = duty;
  applyGroup1(3);
}

void setGen4(float freq, float duty)
{
  gen4_Frequency = freq;
  gen4_DutyCycle = duty;
  applyGroup1(4);
}

// ================================================================
//  START / STOP FUNCTIONS
//  stopGen(n)  — stops timer n, forces GPIO low. PWM halts.
//  startGen(n) — restarts timer n from current settings.
//  startAll() / stopAll() — convenience wrappers.
// ================================================================
void stopGen(int n)
{
  switch (n) {
    case 1:
      MCPWM0.timer[0].timer_cfg1.timer_start = 0;  // stop timer
      MCPWM0.operators[0].generator[0].gen_utez = 1; // force low
      MCPWM0.operators[0].generator[0].gen_utea = 1;
      gpio_set_level((gpio_num_t)GEN1_PIN, 0);
      genRunning[1] = false;
      if(DEBUG == 2) Serial.println("Gen1 STOPPED");
      break;
    case 2:
      MCPWM0.timer[1].timer_cfg1.timer_start = 0;
      MCPWM0.operators[1].generator[0].gen_utez = 1;
      MCPWM0.operators[1].generator[0].gen_utea = 1;
      gpio_set_level((gpio_num_t)GEN2_PIN, 0);
      genRunning[2] = false;
      if(DEBUG == 2) Serial.println("Gen2 STOPPED");
      break;
    case 3:
      MCPWM1.timer[0].timer_cfg1.timer_start = 0;
      MCPWM1.operators[0].generator[0].gen_utez = 1;
      MCPWM1.operators[0].generator[0].gen_utea = 1;
      gpio_set_level((gpio_num_t)GEN3_PIN, 0);
      genRunning[3] = false;
      if(DEBUG == 2) Serial.println("Gen3 STOPPED");
      break;
    case 4:
      MCPWM1.timer[1].timer_cfg1.timer_start = 0;
      MCPWM1.operators[1].generator[0].gen_utez = 1;
      MCPWM1.operators[1].generator[0].gen_utea = 1;
      gpio_set_level((gpio_num_t)GEN4_PIN, 0);
      genRunning[4] = false;
      if(DEBUG == 2)Serial.println("Gen4 STOPPED");
      break;
  }
}

void startGen(int n)
{
  // Re-apply stored parameters and restart the timer
  switch (n) {
    case 1:
      if (gen1_Frequency > 0) { setGen1(gen1_Frequency, gen1_DutyCycle); genRunning[1] = true; }
      else Serial.println("Gen1: call setGen1() first");
      break;
    case 2:
      if (gen2_Frequency > 0) { setGen2(gen2_Frequency, gen2_DutyCycle); genRunning[2] = true; }
      else Serial.println("Gen2: call setGen2() first");
      break;
    case 3:
      if (gen3_Frequency > 0) { setGen3(gen3_Frequency, gen3_DutyCycle); genRunning[3] = true; }
      else Serial.println("Gen3: call setGen3() first");
      break;
    case 4:
      if (gen4_Frequency > 0) { setGen4(gen4_Frequency, gen4_DutyCycle); genRunning[4] = true; }
      else Serial.println("Gen4: call setGen4() first");
      break;
  }
}

void startAll() { for (int i = 1; i <= 4; i++) startGen(i); }
void stopAll()  { for (int i = 1; i <= 4; i++) stopGen(i);  }