#include <Arduino.h>
#include <Wire.h>
#include "SimpleSH1106.h"
#define SerialPrintHex(i,digits) {for(int8_t n=(digits-1)*4;n>=0;n-=4) Serial.print((i>>n)&15,HEX);}

#include "servoedit4.h"
#include "servoedit3.h"

//#define HasRunBtn

enum TMyEvent {
    meNone,
    meClick,
    meWheelMove,
#ifdef HasRunBtn
    meRunBtn,
#endif
    meCommand};
TMyEvent MyEvent = meNone;
int8_t WheelDelta = +1;

uint8_t CurServo = 0;
float KnobSpeed = 1;

enum TParamValue {pvTarget,pvDelay,pvMinMax,pvPinGrpVmax,pvAmax};

const int pinBtn = A2;
const int pinCCW = A0;
const int pinCW  = A1;
#ifdef HasRunBtn
const int pinRunBtn = A3;
#endif

//==============================================================
// DrawTrackBar
//   draws a "trackbar" at the bottom of the screen
//   a: the value
//   amin: min value
//   amax: max value
//==============================================================
void DrawTrackBar(float a, float amin, float amax)
{
  const int8_t xmin = 5;
  const int8_t xmax = 123;
  int8_t ax = round(((xmax-3)-(xmin+3))*(a-amin) / (amax-amin) + (xmin+3));

  setupPageCol(6,xmin);
  DrawBarSH1106(0xE0);
  for (int8_t x=xmin+1;x<=xmax-1;x++)
    if ((x >= ax-2) && (x <= ax+2))
      DrawBarSH1106(0xFF); else
      DrawBarSH1106(0xA0);
  DrawBarSH1106(0xE0);

  setupPageCol(7,xmin);
  for (int8_t x=xmin;x<=xmax;x++)
    if ((x >= ax-2) && (x <= ax+2))
      DrawBarSH1106(0x1F); else
      DrawBarSH1106(0);
}

//==============================================================
// DrawOLEDStrF
//   draws a string that has been declared in PROGMEM using the F() macro
//   replace a $ char with the integer number in f
//   replace a & char with the f with one digit after the d.p.
//   ss: the string 
//   f: a numeric value to be printed
//   x: x-coord of topleft of string
//   page: y-coord of topleft of string = page*8
//   Font: the font defined in servoedit3.h
//==============================================================
uint8_t DrawOLEDStrF(const __FlashStringHelper* ss, float f, uint8_t x, int8_t page, word Font)
{
  uint16_t p = ss;
  while (1) {
    char c  =  pgm_read_byte_near(p);

    switch (c) {
      case 0: return x;
      case '$': x += DrawIntSH1106(round(f),x,page,Font); break;
      case '&': {
          int i = round(f*10);
          x += DrawIntSH1106(i / 10,x,page,Font);
          x += DrawCharSH1106('.',x, page, Font);
          x += DrawIntSH1106(i % 10,x,page,Font);
        } break;
      default: x += DrawCharSH1106(c,x, page, Font);
    }
    p++;
  }  
}

//==============================================================
// DrawOLEDStr1
//   draws a string at coords 2,0 using SmallFont
//   calls DrawOLEDStrF - same rules apply
//   s: the string 
//   f: a numeric value to be printed
//==============================================================
uint8_t DrawOLEDStr1(const __FlashStringHelper* s, float f)
{
  clearSH1106();
  return DrawOLEDStrF(s,f,2,0,SmallFont);
}

//==============================================================
// DrawOLEDStr2
//   draws a string at coords 2,16 using LargeCondensedFont
//   calls DrawOLEDStrF - same rules apply
//   s: the string 
//   f: a numeric value to be printed
//==============================================================
uint8_t DrawOLEDStr2(const __FlashStringHelper* s, float f)
{
  return DrawOLEDStrF(s,f,2,2,LargeCondensedFont);
}

//==============================================================
// DrawOLEDStr3a
//   draws a string at coords 2,40 using SmallFont
//   calls DrawOLEDStrF - same rules apply
//   s: the string 
//   f: a numeric value to be printed
//==============================================================
uint8_t DrawOLEDStr3a(const __FlashStringHelper* s, float f)
{
  return DrawOLEDStrF(s,f,2,4+1,SmallFont);
}

//==============================================================
// DrawOLEDStr3b
//   draws a string at coords 64,40 using SmallFont
//   calls DrawOLEDStrF - same rules apply
//   s: the string 
//   f: a numeric value to be printed
//==============================================================
uint8_t DrawOLEDStr3b(const __FlashStringHelper* s, float f)
{
  return DrawOLEDStrF(s,f,64,4+1,SmallFont);
}

//==============================================================
// DrawOLEDStr4a
//   draws a string at coords 2,48 using SmallFont
//   calls DrawOLEDStrF - same rules apply
//   s: the string 
//   f: a numeric value to be printed
//==============================================================
uint8_t DrawOLEDStr4a(const __FlashStringHelper* s, float f)
{
  return DrawOLEDStrF(s,f,2,5+1,SmallFont);
}

//==============================================================
// DrawOLEDStr4b
//   draws a string at coords 65,48 using SmallFont
//   calls DrawOLEDStrF - same rules apply
//   s: the string 
//   f: a numeric value to be printed
//==============================================================
uint8_t DrawOLEDStr4b(const __FlashStringHelper* s, float f)
{
  return DrawOLEDStrF(s,f,64,5+1,SmallFont);
}

//==============================================================
// DrawOLEDStr5a
//   draws a string at coords 2,56 using SmallFont
//   calls DrawOLEDStrF - same rules apply
//   s: the string 
//   f: a numeric value to be printed
//==============================================================
uint8_t DrawOLEDStr5a(const __FlashStringHelper* s, float f)
{
  return DrawOLEDStrF(s,f,2,7,SmallFont);
}

//==============================================================
// DrawOLEDStr5b
//   draws a string at coords 65,56 using SmallFont
//   calls DrawOLEDStrF - same rules apply
//   s: the string 
//   f: a numeric value to be printed
//==============================================================
uint8_t DrawOLEDStr5b(const __FlashStringHelper* s, float f)
{
  return DrawOLEDStrF(s,f,64,7,SmallFont);
}

//==============================================================
// DrawOLEDParams
//   draws the parameters of the current servo at the bottom of the screen
//==============================================================
void DrawOLEDParams() {
  DrawOLEDStr3a(F("Pos $"),Servos[CurServo].Target);
  uint8_t x = DrawOLEDStr3b(F("($"),Servos[CurServo].ServoMin);
  DrawOLEDStrF(F(" - $)"),Servos[CurServo].ServoMax,x,4+1,SmallFont);
  
  DrawOLEDStr4a(F("Vmax $"),Servos[CurServo].Vmax);
  DrawOLEDStr4b(F("Amax &"),Servos[CurServo].Amax);
  DrawOLEDStr5a(F("Group $"),Servos[CurServo].Group);
  DrawOLEDStr5b(F("Pin $"),Servos[CurServo].PinNum);
}

//==============================================================
// ShowError
//   show error message for 1 sec
//   s: error message
//   f: numeric value
//==============================================================
void ShowError(const __FlashStringHelper* s, float f)
{
  DrawOLEDStr1(F("Error"),0);
  DrawOLEDStr2(s,f);
  delay(1000);
}

//==============================================================
// RelaxAll
//   detach all Servos 
//==============================================================
void RelaxAll() {
  for (int8_t servo=0; servo<NumServos; servo++) 
    Servos[servo].aServo.detach();
  DrawOLEDStr1(F("Button held down"),0);
  DrawOLEDStr2(F("Relax All"),CurServo+1);
  DrawOLEDStr4a(F("Relax all servos"),CurServo+1);
  while (Quadrature() >= 4) {} // wait for up
  clearSH1106();
}

//==============================================================
// ADCQuadrature
//   gets a value from the rotary encoder
//   encoder uses resistors to convert signals into an analogue voltage
//   returns 4; else // button pressed
//   returns 0,1,2,3 quadrature signals in binary
//   one notch is one whole cycle
//   3 is the rest state 
//   does a little bebouncing
//==============================================================
int8_t ADCQuadrature() {
  byte n = 0;
  int8_t state;

  do {
    static int8_t prevState = -1;
    int16_t i = analogRead(A7);
    
    if (i < 104) state = 4; else // button pressed
    if (i < 358) state = 2; else
    if (i < 632) state = 3; else // rest state
    if (i < 846) state = 0; else
                 state = 1;     
  
    if (prevState == state)
      n++; else
      n = 0;
    prevState = state;
  } while (n < 3); // debouncing count
  return state;
}

//==============================================================
// Quadrature
//   gets a value from the rotary encoder
//   encoder uses 3 digital pins
//   returns 4; else // button pressed
//   returns 0,1,2,3 quadrature signals in binary
//   one notch is one whole cycle
//   3 is the rest state 
//   does a little bebouncing
//==============================================================
int8_t Quadrature() {
  byte n = 0;
  int8_t state;

  do {
    static int8_t prevState = -1;
    if (digitalRead(pinBtn) == LOW) state = 4; else // button pressed
    if (digitalRead(pinCW) == LOW) {
      if (digitalRead(pinCCW) == LOW) 
        state = 3; else // rest state
        state = 1;       
    } else {
      if (digitalRead(pinCCW) == LOW) 
        state = 2; else 
        state = 0;       
    }
  
    if (prevState == state)
      n++; else
      n = 0;
    prevState = state;
  } while (n < 3); // debouncing count
  return state;
}

//==============================================================
// CheckEncoder
//   runs a FSM to decode the quadrature signals into up/down counts
//   FSM is from eevblog.com
//     https://www.eevblog.com/forum/projects/easy-rotary-encoder-implementation-good-easy-to-implement-code/?action=dlattach;attach=107187
//   returns an event state
//     meNone: nothing happened
//     meClick: button clicked
//     meWheelMove: wheel moved
//==============================================================
TMyEvent CheckEncoder() {
  enum TEncState {Idle11,CW_01,CW_00,CW_10,ERR01,ERR00,ERR10,CCW10,CCW00,CCW01, EncStateHigh };
  struct TEncStateRec {
    TEncState next;
    int8_t incr;};  
  TEncStateRec NextEncState[EncStateHigh][4] = {
      /*Idle11*/   {{ERR00,  0},    {CW_01,  0},    {CCW10,  0},    {Idle11,  0}},
      /*CW_01 */   {{CW_00,  0},    {CW_01,  0},    {ERR10,  0},    {Idle11,  0}},
      /*CW_00 */   {{CW_00,  0},    {CW_01,  0},    {CW_10,  0},    {Idle11, +1}},
      /*CW_10 */   {{CW_00,  0},    {CW_01, +1},    {CW_10,  0},    {Idle11, +1}},
      /*ERR01 */   {{CW_00,  0},    {ERR01,  0},    {CCW10,  0},    {Idle11, -1}},
      /*ERR00 */   {{ERR00,  0},    {CCW01,  0},    {CW_10,  0},    {Idle11,  0}},
      /*ERR10 */   {{CCW00,  0},    {CW_01,  0},    {ERR10,  0},    {Idle11, +1}},
      /*CCW10 */   {{CCW00,  0},    {ERR01,  0},    {CCW10,  0},    {Idle11,  0}},
      /*CCW00 */   {{CCW00,  0},    {CCW01,  0},    {CCW10,  0},    {Idle11, -1}},
      /*CCW01 */   {{CCW00,  0},    {CCW01,  0},    {CCW10, -1},    {Idle11, -1}}};
  static TEncState EncState = Idle11;
  static int8_t count = 0;  
  static int32_t downTime = 0;  

  do {  
    int8_t state = Quadrature();     
    static bool prevbtn = false;  
  
    if (state >= 4) { // button down;
      if (!prevbtn) {
        prevbtn = true;
        downTime = millis();
        return meClick;
      }
      
      if (millis() - downTime > 1000) 
        RelaxAll();
      
      return meNone;
    } else {
      prevbtn = false;

      count += NextEncState[EncState][state].incr;  
      EncState = NextEncState[EncState][state].next;
    }
#ifdef HasRunBtn
  } while ((EncState != Idle11) && (Serial.available() == 0) && (digitalRead(pinRunBtn) == HIGH));
#else
  } while ((EncState != Idle11) && (Serial.available() == 0));
#endif
      
  if (count) {
    WheelDelta = sign(count); 
    count = 0;
    return meWheelMove;
  }
  return meNone;
}

//==============================================================
// WaitForEvent
//   waits until the encoder has 
//   returns an event state
//     meClick: button clicked
//     meWheelMove: wheel moved
//       +1 or -1 is in WheelDelta
//       KnobSpeed is bigger if the wheel is turning faster
//     meCommand: a serial command arrived
//     meRunBtn: the Run button was pressed
//==============================================================
TMyEvent WaitForEvent(bool bBreakOnCommand) {
  static uint32_t prevwd = 0;
  const int tmax = 500;
  const int tmin = 300;
  const float kmax = 100.0;
  const float kmin = 0.1;
  float t;
  static float t1 = 0;
  uint32_t prevt = millis();

  do {
    MyEvent = CheckEncoder();
    
    switch (MyEvent) {
      case meClick: KnobSpeed = 1; break;        
      case meWheelMove:         
        if (prevwd != WheelDelta)
          t1 = tmax;
        prevwd = WheelDelta;
        t = millis();
        t = t-prevt+1;
        t1 = t1 + (t-t1)*0.1;
        t1 = max(t1,t);
        t1 = constrain(t1,tmin,tmax);
        KnobSpeed = ((tmax - t1)*tmin*(kmax - kmin) / (t1*(tmax - tmin))) + kmin;
        KnobSpeed = constrain(KnobSpeed,1,kmax);
        break;
    }
    
    if (bBreakOnCommand && (Serial.available() > 0))
      MyEvent = meCommand;
      
    #ifdef HasRunBtn
      if (bBreakOnCommand && (digitalRead(pinRunBtn) == LOW))
        MyEvent = meRunBtn;
    #endif
  } while (MyEvent == meNone);
  return MyEvent;
}

//==============================================================
// ExecMenu_Position_Set_Value
//   displays and executes the menu screen to set the target position of the current servo
//==============================================================
void ExecMenu_Position_Set_Value()
{
  DrawOLEDStr1(F("Position $ Value"),CurServo+1);
  do {
    DrawOLEDStr2(F("$  "),round(Servos[CurServo].Target));
    DrawTrackBar(Servos[CurServo].Target,Servos[CurServo].ServoMin,Servos[CurServo].ServoMax);

    switch (WaitForEvent(false)) {
      case meWheelMove:
        SetServoPosition(CurServo, Constrain(Servos[CurServo].Target+round(WheelDelta*KnobSpeed),Servos[CurServo].ServoMin,Servos[CurServo].ServoMax));
        break;
    }
  } while (MyEvent != meClick);
}

//==============================================================
// ServoSetDefaults
//   sets the default parameter values for a servo
//==============================================================
void ServoSetDefaults(uint8_t srv, bool setPos)
{
  Servos[srv].Group = 0;
  Servos[srv].PinNum = srv+2;
  Servos[srv].ServoMax = 2500;
  Servos[srv].ServoMin = 500;
  Servos[srv].Velocity = 0;
  Servos[srv].Vmax = 10.0;
  Servos[srv].Amax = 0.1;

  Servos[srv].Target = 1500;
  if (setPos)
  {
    Servos[srv].Position = 1500;
  }
}

//==============================================================
// SetAllDefaults
//   sets the default parameter values for all servos
//==============================================================
void SetAllDefaults()
{
  for (int8_t srv=0; srv<NumServosMax; srv++)
    ServoSetDefaults(srv,false);
}

//==============================================================
// MoveServosToFinish
//   execute MoveServos until all are at targets
//   returns true if user cancelled by holding the button down
//==============================================================
bool MoveServosToFinish() {
  uint32_t prevt = millis();
  while (!MoveServos()) {
    if (digitalRead(pinBtn) == HIGH) 
      prevt = millis(); else
    if (millis()-prevt > 1000) {
      RelaxAll();
      return true;
    }
  } 
  return false;
}

//==============================================================
// EEPROMexec
//   executes the command triplets in the EEPROM
//     changes are written into the Params array
//   servo: 
//     servo>=0: sets Params for one servo
//   "target" command triplets tell a servo to move
//     "target" triplets are numbered from 0 to (number of "target" triplets)-1
//   bDoMoves: for any "target" command, wait to finish its move
//   bStopOnFirstMove: return when the first ekTarget is executed
//
//   typical calls are
//      EEPROMexec(-1,false,false);
//        set the parameters and targets for all servos
//      EEPROMexec(-1,true,false);
//        play the whole file
//      EEPROMexec(servo,false,false);
//        set up the parameters for 'servo'
//      EEPROMexec(servo,false,true);
//        initialise the servo
//==============================================================
void EEPROMexec(int8_t servo, bool bDoMoves, bool bStopOnFirstMove)
{
  uint16_t addr;
  TTriplet t;
  uint8_t srv,dly;
  TEEPROMkind epk;

  addr = 0;
  if (servo < 0)
    SetAllDefaults(); else
    ServoSetDefaults(servo,false);

  while (true)
  {
    t = GetEEPROMTriplet(addr);
    epk = UnpackEEPROMkind(t);
    if (epk == ekEOF)
      break;
    srv = UnpackEEPROMservo(t);
    srv = Min(srv,NumServosMax-1);
    NumServos = max(srv+1,NumServos);

    if ((srv == servo) || (servo < 0))
      UnpackEEPROMparams(t,&(Servos[srv]));

    if (epk == ekTarget) {
      if ((srv == servo) || (servo < 0))
      {
        dly = UnpackEEPROMdelay(t);

        if (bDoMoves) {
          if (dly > 2)
          {
            DrawOLEDStr1(F("Play Memory"),0);
            DrawOLEDStr2(F("Playing $"),addr/3);
          }

          if (dly < 128) {
            uint32_t prevt = millis();
            uint32_t prevdown = millis();
            do {
              MoveServos();
              if (digitalRead(pinBtn) == HIGH) 
                prevdown = millis(); else
              if (millis()-prevdown > 1000) {
                RelaxAll(); // emergency stop
                return;
              }
            } while (millis()-prevt < dly*100.0); 
          } else
          {
            if (MoveServosToFinish()) 
              return; // emergency stop
            delay((dly-128)*100);
          }
        }
        
        if (bStopOnFirstMove)
          break;
      }
    }
    addr += 3;
  }
  if (bDoMoves)
    MoveServosToFinish();
}

//==============================================================
// TargetHasChanged
//   returns: is the current target value of the servo different from that predicted by the commands in the EEPROM?
//==============================================================
bool TargetHasChanged(int8_t servo)
{
  TServo temp;
  temp = Servos[servo];
  EEPROMexec(servo,false,false);
  bool result = Servos[servo].Target != temp.Target;
  Servos[servo] = temp;
  return result;
}

//==============================================================
// MaxChangedServo
//   if the current target value of any different from that prodicted by the commands in the EEPROM
//     returns the servo number of the maximum changed servo
//   else returns -1
//==============================================================
int8_t MaxChangedServo()
{
  uint8_t servo;
  int8_t result = -1;

  for (servo=0; servo<NumServos; servo++)
    if (TargetHasChanged(servo))
      result = servo;
  return result;
}

//==============================================================
// AppendOneServoToMemory
//   servo: the servo to save
//   aDelay: whether to wait after a servo move
//     aDelay = 0..100 means wait for aDelay/10.0 seconds
//     aDelay = 128..228 means wait until all outstanding are finished) wait (aDelay-128)/10.0 seconds
// 
//   save the current params of servo s
//   "run" the EEPROM for servo s (but don"t exec movements)
//   if (the servo position or params have changed) save them
// 
//   if (the servo is not anywhere in the EEPROM) save at least one parameter
//     so that the EEPROM specifies how many servos there are
//   
//   returns true = succeeded, i.e. the EEPROM wasn't full
//==============================================================
bool AppendOneServoToMemory(int8_t servo, uint8_t aDelay)
{
  TServo temp = Servos[servo];
  EEPROMexec(servo,false,false);

  bool result = true;
  if ((Servos[servo].ServoMin != temp.ServoMin) || (Servos[servo].PinNum != temp.PinNum)) result = result && EEPROMappendMinPos(servo,temp.ServoMin,temp.PinNum);
  if ((Servos[servo].ServoMax != temp.ServoMax) || (Servos[servo].Group   != temp.Group)) result = result && EEPROMappendMaxPos(servo,temp.ServoMax,temp.Group);
  if ((Servos[servo].Vmax != temp.Vmax)         || (Servos[servo].Amax    != temp.Amax) ) result = result && EEPROMappendVmaxAmax(servo,temp.Vmax,temp.Amax);
  if (Servos[servo].Target != temp.Target                                               ) result = result && EEPROMappendTarget(servo,round(temp.Target),aDelay);
  Servos[servo] = temp;
  if (! EEPROMhasServo(servo))
    result = result && EEPROMappendMinPos(servo,temp.ServoMin,temp.PinNum);
  return result;
}

//==============================================================
// SaveAllToMemory
//   save all servos to memory
//   aDelay: whether to wait after a servo move
//     aDelay = 0..100 means wait for aDelay/10.0 seconds
//     aDelay = 128..228 means wait until finished then wait (aDelay-128)/10.0 seconds
//==============================================================
void SaveAllToMemory(uint8_t aDelay)
{
  int8_t servo,maxservo;
  uint16_t Top;
  bool b;


  b = true;
  Top = EEPROMTop();
  maxservo = MaxChangedServo();
  for (servo=0; servo<NumServos; servo++)
    if (servo == maxservo)
      b = b && AppendOneServoToMemory(servo,aDelay); else
      b = b && AppendOneServoToMemory(servo,0);
  if (Top == EEPROMTop())
    ShowError(F("No Change"),0);
  if (! b)
    ShowError(F("Memory Full"),0); 
}

//==============================================================
// ExecMenu_Clear
//   displays and executes the menu screen to query clearing the EEPROM
//==============================================================
void ExecMenu_Clear()
{
  enum TClearMode {cmClearEEPROM, cmClearServos, cmCancel};
  TClearMode ClearMode  = cmClearServos;

  do {
    DrawOLEDStr1(F("Clear Memory"),0);
    switch (ClearMode) {
      case cmClearEEPROM: DrawOLEDStr2(F("Clr Memory"),0); DrawOLEDStr3a(F("Clear Memory"),0); break;
      case cmClearServos: DrawOLEDStr2(F("Clr Servos"),0); DrawOLEDStr3a(F("Clear Memory"),0); DrawOLEDStr4a(F("Clear Servos"),0); break;
      case cmCancel:      DrawOLEDStr2(F("Cancel"),0); break;
    }

    switch (WaitForEvent(false)) {
      case meWheelMove:
        if (((WheelDelta > 0) && (ClearMode < cmCancel)) || ((WheelDelta < 0) && (ClearMode > cmClearEEPROM)))
          ClearMode = (TClearMode)(((byte)ClearMode)+WheelDelta);
        break;  
    }
  } while (MyEvent != meClick);


  switch (ClearMode) {
    case cmClearServos:
        CurServo = 0;
        NumServos = 1;
        ServoSetDefaults(0,true);
        break;
    case cmCancel:      return;
  }
  InitialiseEEPROM();
}

//==============================================================
// EditParamValue
//   displays and executes the menu screen to edit the EEPROM
//==============================================================
float EditParamValue(const __FlashStringHelper* s1, const __FlashStringHelper* s2, float a, float amin, float amax,
  uint8_t servo, uint8_t addr, TParamValue ParamValue)
{
  float d;

  while (true) {
    DrawOLEDStr1(s1,0);
    if (ParamValue == pvDelay) {
      if (a >= 128)
        DrawOLEDStr2(F("Fin+&s"),(a-128)/10.0); else
        DrawOLEDStr2(F("& sec"),a/10.0);
    } else
      DrawOLEDStr2(s2,a);    
    DrawOLEDStr5a(F("Servo = $"),  servo);
    DrawOLEDStr5b(F("Addr = $"),  addr);
    switch (WaitForEvent(false)) 
      case meWheelMove: {
        if ((ParamValue == pvTarget) || (ParamValue == pvMinMax) || (ParamValue == pvDelay))
          d = round(WheelDelta*KnobSpeed); else
          d = WheelDelta;          
        if (ParamValue == pvAmax)
          d /= 10.0;
        a = constrain(a+d,amin,amax);
        if (ParamValue == pvTarget)
          SetServoPosition(servo, a);
        break;
      case meClick: return a;
    }
  }
}

//==============================================================
// ExecMenu_EditMemory
//   displays and executes the menu screen to edit the EEPROM
//==============================================================
void ExecMenu_EditMemory(bool bDelete)
{
  int16_t addr = 0;
  TServo Params;
  TTriplet t;
  uint8_t servo,dly;
  int16_t top;
        
  while (true) {
    do {
      uint8_t x;
      top = EEPROMTop() / 3;
      if (bDelete) 
        x = DrawOLEDStr1(F("Delete from memory "),0); else
        x = DrawOLEDStr1(F("Edit Memory "),0);
      if ((addr == -2) || (addr == top))
        DrawOLEDStr2(F("Quit"),0); else
      if (((addr == -1) || (addr == top+1)) && bDelete)
        DrawOLEDStr2(F("Clear"),0); else
      {
        DrawOLEDStrF(F("$"),addr,x,0,SmallFont);
        t = GetEEPROMTriplet(addr*3);
        servo = UnpackEEPROMservo(t);
        UnpackEEPROMparams(t, &Servos[servo]);
        Params = Servos[servo];
        DrawOLEDStr5a(F("Servo = $"), servo);

        switch (UnpackEEPROMkind(t)) {
          case ekTarget:    DrawOLEDStr2(F("Target"),0);  DrawOLEDStr3a(F("Target = $"),      Params.Target);   DrawOLEDStr4a(F("Delay = &"),UnpackEEPROMdelay(t)/10.0); break;
          case ekMinPos:    DrawOLEDStr2(F("Min Pos"),0); DrawOLEDStr3a(F("Min Position = $"),Params.ServoMin); DrawOLEDStr4a(F("Pin = $"),  Params.PinNum);   break;
          case ekMaxPos:    DrawOLEDStr2(F("Max Pos"),0); DrawOLEDStr3a(F("Max Position = $"),Params.ServoMax); DrawOLEDStr4a(F("Group = $"),Params.Group);    break;
          case ekVmaxAmax:  DrawOLEDStr2(F("V/A max"),0); DrawOLEDStr3a(F("Vmax = $"),        Params.Vmax);     DrawOLEDStr4a(F("Amax = &"), Params.Amax);     break;
          default:          DrawOLEDStr2(F("Error"),0); break;
        }

        if (MoveServosToFinish())
          return; // emergency stop
      }
  
      switch (WaitForEvent(false)) {
        case meWheelMove: 
          if (WheelDelta > 0) {
            if (addr == top+1)
              addr = 0; else
              addr++;
          } else {
            if (addr == 0)
              addr = top+1; else
              addr--;
          }
      }
    } while (MyEvent != meClick);

    if ((addr == -2) || (addr == top))
      return;
    if ((addr == -1) || (addr == top+1)) {
      ExecMenu_Clear();
      return;
    }

    if (bDelete) {
      EEPROMDeleteTriplet(addr*3);
    } else {
      switch (UnpackEEPROMkind(t)) {
        case ekTarget:
          Params.Target = EditParamValue(F("Target Position"),F("$"),Params.Target,Params.ServoMin,Params.ServoMax,servo,addr,pvTarget);
          dly = EditParamValue(F("Delay"),F(""),UnpackEEPROMdelay(t),0,255,servo,addr,pvDelay);
          EEPROMsaveTriplet(EEPROMTargetToTriplet(servo, Params.Target,dly), addr*3);
          break;

        case ekMinPos:
          Params.ServoMin = EditParamValue(F("Minimum Position"),F("Min=$"),Params.ServoMin,200,2500,servo,addr,pvMinMax);
          Params.PinNum = EditParamValue(F("Pin Number"),F("Pin=$"),Params.PinNum,2,12,servo,addr,pvPinGrpVmax);
          EEPROMsaveTriplet(EEPROMMinPosToTriplet(servo, Params.ServoMin, Params.PinNum), addr*3);
          break;

        case ekMaxPos:
          Params.ServoMax = EditParamValue(F("Maximum Position"),F("Max=$"),Params.ServoMax,500,2500,servo,addr,pvMinMax);
          Params.Group = EditParamValue(F("Group"),F("Group=$"),Params.Group,0,11,servo,addr,pvPinGrpVmax);
          EEPROMsaveTriplet(EEPROMMaxPosToTriplet(servo, Params.ServoMax, Params.Group), addr*3);
          break;

        case ekVmaxAmax:
          Params.Vmax = EditParamValue(F("Vmax"),F("Vmax=$"),Params.Vmax,0,255,servo,addr,pvPinGrpVmax);
          Params.Amax = EditParamValue(F("Amax"),F("Amax=&"),Params.Amax,0,25.5,servo,addr,pvAmax);
          EEPROMsaveTriplet(EEPROMVmaxAmaxToTriplet(servo, Params.Vmax, Params.Amax), addr*3);
          break;
      }
    }
  }
}

//==============================================================
// ExecMenu_Learn_Wait
//   displays and executes the menu screen to 
//     save the servos to memory with/without a wait
//     edit the EEPROM
//       edit step(s)
//       delete step(s)
//       clear 
//==============================================================
void ExecMenu_Learn_Wait()
{
  enum TWaitMode {wcFinish, wcFinishDelay, wcDelay, wcNoDelay, wcCancel, wcEdit, wcDelete};
  static TWaitMode WaitMode = wcFinish;
  static uint8_t aDelay = 0;


  if (MaxChangedServo() < 0) 
    WaitMode = wcNoDelay;
  
  do {
    if (EEPROMTop() == 0) 
      WaitMode = min(WaitMode,wcCancel);
    switch (WaitMode) {
      case wcFinish:
      case wcFinishDelay:
      case wcDelay: if (MaxChangedServo() < 0) WaitMode = wcNoDelay;
    }

    switch (WaitMode) {
      case wcNoDelay:
      case wcFinish:
      case wcFinishDelay:
      case wcDelay:     DrawOLEDStr1(F("Learn ($% full) Wait"),EEPROMpercentFull()); break;
      default:          DrawOLEDStr1(F("Learn ($% full)"),EEPROMpercentFull());
    }

    switch (WaitMode) {
      case wcNoDelay:        DrawOLEDStr2(F("No Wait"),0); DrawOLEDStr4a(F("Set target and continue"),0); break;
      case wcFinish:         DrawOLEDStr2(F("Until Finish"),0); DrawOLEDStr4a(F("Wait until reaches target"),0); break;
      case wcFinishDelay:    DrawOLEDStr2(F("Finish+Dly"),0); DrawOLEDStr4a(F("Reach target then wait more"),0); break;
      case wcDelay:          DrawOLEDStr2(F("Fixed Delay"),0); DrawOLEDStr4a(F("Wait for a fixed time"),0); break;
      case wcCancel:         DrawOLEDStr2(F("Cancel"),0); break;
      case wcEdit:           DrawOLEDStr2(F("Edit"),0); DrawOLEDStr4a(F("Change the stored program"),0); break;
      case wcDelete:         DrawOLEDStr2(F("Delete"),0); DrawOLEDStr4a(F("Delete line(s)"),0); break;
    }

    switch (WaitForEvent(false)) {
      case meWheelMove:
        if (((WheelDelta > 0) && (WaitMode < wcDelete)) || ((WheelDelta < 0) && (WaitMode > wcFinish)))
          WaitMode = (TWaitMode)(((byte)WaitMode)+WheelDelta);
        break;
    }
  } while (MyEvent != meClick);

  switch (WaitMode) {
    case wcFinishDelay:
    case wcDelay:
      do {
        if (WaitMode == wcFinishDelay)
          DrawOLEDStr1(F("Learn: Finished+Delay"),0); else
          DrawOLEDStr1(F("Learn: Wait Delay"),0);
        if (aDelay == 0)
          DrawOLEDStr2(F("No Wait"),0); else
          DrawOLEDStr2(F("Wait &sec"),aDelay/10.0);

        switch (WaitForEvent(false)) {
          case meWheelMove: aDelay = Constrain(aDelay+WheelDelta,0,100); break;
        }
      } while (MyEvent != meClick);
      break;
  }

  switch (WaitMode) {
    case wcNoDelay:        SaveAllToMemory(0); break;
    case wcFinish:         SaveAllToMemory(128); break;
    case wcFinishDelay:    SaveAllToMemory(aDelay+128); break;
    case wcDelay:          SaveAllToMemory(aDelay); break;
    case wcEdit:           ExecMenu_EditMemory(false); break;
    case wcDelete:         ExecMenu_EditMemory(true); break;
  }
}

//==============================================================
// ExecMenu_Setup_Servo
//   displays and executes the menu screen to set the parameters of a servo
//   bSelectPin=true: show the "Pin" menu
//==============================================================
void ExecMenu_Setup_Servo(bool bSelectPin)
{
  enum TState {
        stSetup_Pin,
        stSetup_Min,
        stSetup_Max,
        stSetup_Vmax,
        stSetup_Amax,
        stSetup_Group};
  static TState state = stSetup_Pin;
  uint8_t srv;
  bool more;

  if (bSelectPin)
  {
    state = stSetup_Pin;
  } else {
    more = true;
    while (more)
    {
      DrawOLEDStr1(F("Setup Servo $"),CurServo+1);
      switch (state) {
        case stSetup_Pin:   DrawOLEDStr2(F("Pin"),0); break;
        case stSetup_Min:   DrawOLEDStr2(F("Min"),0); break;
        case stSetup_Max:   DrawOLEDStr2(F("Max"),0); break;
        case stSetup_Vmax:  DrawOLEDStr2(F("Vmax"),0); break;
        case stSetup_Amax:  DrawOLEDStr2(F("Amax"),0); break;
        case stSetup_Group: DrawOLEDStr2(F("Group"),0); break;
      }

      switch (WaitForEvent(false)) {
        case meClick: more = false; break;
        case meWheelMove: 
          if (((WheelDelta > 0) && (state < stSetup_Group)) || ((WheelDelta < 0) && (state > stSetup_Pin)))
            state = (TState)(((byte)state)+WheelDelta);
          break;
      }
    }
  }

  if (state == stSetup_Pin)
    Servos[CurServo].aServo.detach();

  do {
    switch (state) {
      case stSetup_Pin:    DrawOLEDStr1(F("Servo $ Pin"),  CurServo+1); DrawOLEDStr2(F("Pin = $"),  Servos[CurServo].PinNum); break;
      case stSetup_Min:    DrawOLEDStr1(F("Servo $ Min"),  CurServo+1); DrawOLEDStr2(F("Min = $"),  Servos[CurServo].ServoMin);  DrawTrackBar(Servos[CurServo].ServoMin,500,2500); break;
      case stSetup_Max:    DrawOLEDStr1(F("Servo $ Max"),  CurServo+1); DrawOLEDStr2(F("Max = $"),  Servos[CurServo].ServoMax);  DrawTrackBar(Servos[CurServo].ServoMax,500,2500); break;
      case stSetup_Vmax:   DrawOLEDStr1(F("Servo $ Vmax"), CurServo+1); DrawOLEDStr2(F("Vmax=$"),   Servos[CurServo].Vmax);      DrawTrackBar(Servos[CurServo].Vmax,0,255); break;
      case stSetup_Amax:   DrawOLEDStr1(F("Servo $ Amax"), CurServo+1); DrawOLEDStr2(F("Amax=&"),   Servos[CurServo].Amax);      DrawTrackBar(Servos[CurServo].Amax,0,25.5); break;
      case stSetup_Group:  DrawOLEDStr1(F("Servo $ Group"),CurServo+1); DrawOLEDStr2(F("Group = $"),Servos[CurServo].Group); break;
    }

    switch (WaitForEvent(false)) {
      case meWheelMove:
        switch (state) {
          case stSetup_Pin:   Servos[CurServo].PinNum =   Constrain(Servos[CurServo].PinNum   +WheelDelta,2,13); break;
          case stSetup_Min:   Servos[CurServo].ServoMin = Constrain(Servos[CurServo].ServoMin +round(WheelDelta*KnobSpeed),500,2500); break;
          case stSetup_Max:   Servos[CurServo].ServoMax = Constrain(Servos[CurServo].ServoMax +round(WheelDelta*KnobSpeed),500,2500); break;
          case stSetup_Vmax:  Servos[CurServo].Vmax =     Constrain(Servos[CurServo].Vmax     +WheelDelta*Max(KnobSpeed/5,1),0,255); break;
          case stSetup_Amax:  Servos[CurServo].Amax =     constrain(Servos[CurServo].Amax     +WheelDelta*Max(KnobSpeed/5,1)*0.1,0,25.5); break;
          case stSetup_Group: Servos[CurServo].Group =    Constrain(Servos[CurServo].Group    +WheelDelta,0,11); break;
        }
        break;
    }
  } while (MyEvent != meClick);

  if (state == stSetup_Pin)
  {
    for (srv=0; srv<NumServos; srv++)
      if ((srv != CurServo) && (Servos[CurServo].PinNum == Servos[srv].PinNum))
        ShowError(F("Dup Pin (0x)"),srv);
  }
}

//==============================================================
// ExecMenu_NumServos
//   displays and executes the menu screen to add or delete servos from the list of servos
//==============================================================
void ExecMenu_NumServos()
{
  uint8_t n = Min(NumServosMax,NumServos+1);
  do {
    DrawOLEDStr1(F("Number of servos"),0);
    DrawOLEDStr2(F("Total = $"),n);
    if (n > NumServos)
      DrawOLEDStr4a(F("Add servo"),0); else
    if (n < NumServos)
      DrawOLEDStr4a(F("Delete servos"),0); 

    switch (WaitForEvent(false)) {
      case meWheelMove: n = Constrain(n+WheelDelta,1,Min(NumServosMax,NumServos+1)); break;
    }
  } while (MyEvent != meClick);

  if (n > NumServos)
  {
    NumServos = n;
    ServoSetDefaults(NumServos-1,true);
    CurServo = NumServos-1;
    ExecMenu_Setup_Servo(true);
  } else
  {
    for (int8_t srv=n; srv<NumServos; srv++)
      Servos[srv].aServo.detach();
    EEPROMDeleteServos(n-1,0xFF,0);
    NumServos = n;
    CurServo = NumServos-1;
  }
}

//==============================================================
// ExecMenu_Setup_Choose_Servo
//   displays and executes the menu screen to choose which servo to modify
//==============================================================
void ExecMenu_Setup_Choose_Servo()
{
  do {
    if (CurServo == NumServos+1) 
    {
      DrawOLEDStr1(F("Setup Relax Servos"),0);
      DrawOLEDStr2(F("Relax All"),CurServo+1);
      DrawOLEDStr4a(F("Relax all servos"),CurServo+1);
    } else
    if (CurServo == NumServos)
    {
      DrawOLEDStr1(F("Setup num servos"),0);
      if (NumServos == NumServosMax)
        DrawOLEDStr2(F("Del servo"),0); else
        DrawOLEDStr2(F("Add servo"),0);
      DrawOLEDStr4a(F("Num servos $"),NumServos);
    } else {
      DrawOLEDStr1(F("Setup Choose Servo"),0);
      DrawOLEDStr2(F("Servo $"),CurServo+1);
      DrawOLEDParams();
    }

    switch (WaitForEvent(false)) {
      case meWheelMove: CurServo = Constrain(CurServo+WheelDelta,0,NumServos+1); break;
    }
  } while (MyEvent != meClick);

  if (CurServo == NumServos+1) {
    for (int8_t servo=0; servo<NumServos; servo++) 
      Servos[servo].aServo.detach();
    CurServo = 0;
  } else
  if (CurServo == NumServos)
    ExecMenu_NumServos(); else
    ExecMenu_Setup_Servo(false);
}

//==============================================================
// PollSerial
//   poll while waiting
//   returns true if a complete command was received
//   
//   serial command from PC
//       1nnnnnnn   n = low 7 bits
//       0000ssss   cur servo = ssss (0..n-1)
//       00010mmm   target[ssss] = mmmnnnnnnn0 + 500 microseconds (500..2547)
//       00011mmm   min[ssss] = mmmnnnnnnn0 + 500 microseconds (500..2547)
//       00100mmm   max[ssss] = mmmnnnnnnn0 + 500 microseconds (500..2547)
//       0100000m   Vmax[ssss] = mnnnnnnn uS per interval
//       0100001m   Amax[ssss] = mnnnnnnn/10.0 uS per interval;
//       01000100   pin[ssss] = nnnnnnn
//       01000110   group[ssss] = nnnnnnn
//       01100000   send EEPROM from Nano to PC; next bytes sent are EEPROM triplets followed by FF
//       01100010   receive EEPROM from PC to Nano; next bytes recv'd are EEPROM triplets followed by FF
//       01100100   send EEPROM from Nano to PC as plain text
//       01100110   run EEPROM
//       0110100m   Learn; mnnnnnnn = aDelay; 0..100 = wait for aDelay/10.0 seconds; 128..228 means wait until finished then wait (aDelay-128)/10.0 seconds
//
//   Nano replies to PC with "OK" (byte 0xAA) after each byte from PC
//     except for the 01100010 command (send EEPROM) 
//     with the 01100000 command (recv EEPROM), Nano replies with "OK" after each byte from PC
//     PC has to wait for the "OK" 'cos the Nano serial buffer is so small
//==============================================================
bool PollSerial() {
  static uint16_t lowbits = 0;
  static uint8_t servo = 0;
  uint16_t mn;
  bool result = false;
  
  if (Serial.available() > 0) {
    uint16_t b = Serial.read();
    if (b & 0x80)
      lowbits = b & 0x7F; else      
    if ((b & 0xF0) == 0) {
      servo = min(b,NumServosMax-1);
      while (servo >= NumServos) {
        NumServos++;
        ServoSetDefaults(NumServos-1,false);
      }
    } else      
    {
      result = true;
      mn = ((b & 7) << 7) + lowbits;
      mn = mn*2 + 500;
      switch (b & 0xF8) {
        case 0x10:
          SetServoPosition(servo,mn);
          Servos[servo].Target = mn; 
          Servos[servo].Position = mn; 
          break;

        case 0x18: Servos[servo].ServoMin  = mn; 
        break;

        case 0x20: Servos[servo].ServoMax  = mn; 
        break;
        
        default:
          mn = ((b & 1) << 7) + lowbits; 
          switch (b & 0xFE) {
            case 0x40: Servos[servo].Vmax = mn;
            break;

            case 0x42: Servos[servo].Amax = mn/10.0;
            break;

            case 0x44: Servos[servo].PinNum = lowbits;
            break;

            case 0x46: Servos[servo].Group = lowbits;
            break;

            case 0x60: SendEEPROM(); break;
            case 0x62: RecvEEPROM(); break;
            case 0x64: SendEEPROMplain(); break;
            case 0x66: EEPROMexec(-1,true,false); break;
            case 0x68: SaveAllToMemory(mn); break;
        }
      }
    }
    Serial.write(0xAA);
  }
  return result;
}

//==============================================================
// ExecMenu_Main
//   displays and executes the main menu screen 
//     Position: set the postion of a setvo
//     Learn: execute the Learn menu
//     Play: execute the Play menu
//     Setup: execute the Setup menu
//==============================================================
void ExecMenu_Main()
{
  enum TState {
    stPosition,
    stLearn,
    stPlay,
    stSetup};
  TState state = stPosition;

  while (1)
  {
    DrawOLEDStr1(F("Select function"),0);
    switch (state) {
      case stPosition:    DrawOLEDStr2(F("Position $"),CurServo+1); DrawOLEDParams(); break;
      case stLearn:       DrawOLEDStr2(F("Learn"),0); DrawOLEDStr3a(F("Store current positions"),0); break;
      case stPlay:        DrawOLEDStr2(F("Play"),0); DrawOLEDStr3a(F("Play stored positions"),0); break;
      case stSetup:       DrawOLEDStr2(F("Setup"),0); DrawOLEDStr3a(F("Servo parameters"),0); break;
    }

    switch (WaitForEvent(true)) {
      case meClick:
        switch (state) {
          case stPosition:    ExecMenu_Position_Set_Value(); break;
          case stLearn:       ExecMenu_Learn_Wait(); break;
          case stPlay:        EEPROMexec(-1,true,false); break;
          case stSetup:       ExecMenu_Setup_Choose_Servo(); break;
        }
        break;

      case meCommand: {
          uint32_t prevt = millis();
          do {
            if (PollSerial())
              prevt = millis();
          } while (millis()-prevt < 300);
        } break;

      #ifdef HasRunBtn
        case meRunBtn: EEPROMexec(-1,true,false); break;
      #endif

      case meWheelMove:
        switch (state) {
          case stPosition:
            if (WheelDelta > 0)
            {
              if (CurServo == NumServos-1)
                state = stLearn; else
                CurServo++;
            } else
            {
              if (CurServo > 0)
                CurServo--;
            }
            break;

          default:
            if (WheelDelta > 0)
            {
              if (state < stSetup)
                state = (TState)(((byte)state)+WheelDelta);
            } else
            if (state == stLearn)
            {
              CurServo = NumServos-1;
              state = stPosition;
            } else
              state = (TState)(((byte)state)+WheelDelta);
        }
        if ((state == stPlay) && (EEPROMnumTargets() == 0))
          state = (TState)(((byte)state)+WheelDelta);
        break;
    }
  }
}

//==============================================================
// setup
//   initialise the servos, etc.
//==============================================================
void setup() {
  Serial.begin(115200); 
//  TestServo();  
  
  pinMode(pinCW, INPUT_PULLUP);
  pinMode(pinCCW, INPUT_PULLUP);
  pinMode(pinBtn, INPUT_PULLUP);
  #ifdef HasRunBtn
    pinMode(pinRunBtn, INPUT_PULLUP);
  #endif

  Wire.begin(); 
  initSH1106();
  SetAllDefaults(); 
  for (byte servo=0; servo < NumServosMax; servo++) {
    EEPROMexec(servo,false,true);
    Servos[servo].Position = Servos[servo].Target;
  }
//  EEPROMexec(-1,false,false);
  Serial.println("Initialised");
}

//==============================================================
// loop
//   execute the main menu
//==============================================================
void loop() {
  ExecMenu_Main(); // never exits from this call
}