/*************************************************************************************************************************************************
 * EEPROM
 *   stores "Triplets",
 *   each Triplet sets a parameter of a servo
 *   there can be up to 341 Triplets (341*3 = 1023) 
 *   Triplets can be "executed" to run the stored "program"
 *   a "Target" Triplet sets the target position of a servo and hence is specially important
 *     "Target" Triplets are numbered from 0..n
 *   
 *   memory layout is little-endian
 *   memory layout of Triplets is (in binary)
 *     ssss0ttttttttnnnnnnnnnnn   (ekTarget)   set target of servo ssss to nnnnnnnnnnn + 500 microseconds (500..2547) then wait tttttttt/10.0 seconds
 *     ssss1000pppp0nnnnnnnnnnn   (ekMinPos)   set min position = nnnnnnnnnnn + 500 microseconds (500..2547) ; pin = pppp
 *     ssss1010gggg0nnnnnnnnnnn   (ekMaxPos)   set max position = nnnnnnnnnnn + 500 microseconds (500..2547) ; group = gggg;
 *     ssss1100vvvvvvvvaaaaaaaa   (ekVmaxAmax) Vmax[ssss] = vvvvvvvvvv uS per interval; Amax[ssss] = aaaaaaaaaa/10.0 uS per interval;
 *     111111111111111111111111   (ekEOF)      end of memory
 *   memory has 1024 bytes so the last byte cannot be used
 *
 *     initially there is assumed to be 1 servo
 *     a servo is added for each different ssss that is seen
 *     each new servo is assumed
 *       max velocity      = 0
 *       max acceleration  = 0
 *       min position      = 500 microseconds
 *       max position      = 2500 microseconds
 *
 *     tttttttt = 0..100 means wait for tttttttt/10.0 seconds
 *     tttttttt = 128..228 means wait for completion then wait (tttttttt-128)/10.0 seconds
 *************************************************************************************************************************************************/

#ifndef servoedit3_h
#define servoedit3_h

#include "servoedit4.h"
#include <EEPROM.h>

typedef uint32_t TTriplet;
enum TEEPROMkind {ekTarget,ekMinPos,ekMaxPos,ekVmaxAmax,ekEOF};

TTriplet GetEEPROMTriplet(uint16_t addr);
TTriplet UnpackEEPROMTarget(uint16_t TargetNum);
void InitialiseEEPROM();
uint16_t EEPROMnumTargets();
void EEPROMDeleteTriplet(uint16_t TargetNum);
void EEPROMDeleteServos(uint8_t servomin, uint8_t servomax, uint16_t addr);
bool EEPROMappendTarget(uint8_t servo, uint16_t target, uint8_t wait);
bool EEPROMappendMinPos(uint8_t servo, uint16_t pos, uint8_t pin);
bool EEPROMappendMaxPos(uint8_t servo, uint16_t pos, uint8_t group);
bool EEPROMappendVmaxAmax(uint8_t servo, float Vmax, float Amax);
void EEPROMsaveTriplet(TTriplet t, uint16_t addr);
uint16_t EEPROMpercentFull();
TEEPROMkind UnpackEEPROMkind(TTriplet t);
void UnpackEEPROMparams(TTriplet t, TServo *Params);
uint8_t UnpackEEPROMservo(TTriplet t);
uint8_t UnpackEEPROMdelay(TTriplet t);
bool EEPROMhasServo(uint8_t servo);
void CheckEEPROMGood();
uint16_t EEPROMTop();
void SendEEPROMplain();
void SendEEPROM();
void RecvEEPROM(); 
TTriplet EEPROMTargetToTriplet(uint32_t servo, uint32_t target, uint32_t wait);
TTriplet EEPROMMinPosToTriplet(uint32_t servo, uint32_t pos, uint32_t pin);
TTriplet EEPROMMaxPosToTriplet(uint32_t servo, uint32_t pos, uint32_t group);
TTriplet EEPROMVmaxAmaxToTriplet(uint32_t servo, float Vmax, float Amax);

struct TTriplet3 {
  byte a,b,c;
};

//==============================================================
// GetEEPROMTriplet
//   returns the Triplet at an address
//==============================================================
TTriplet GetEEPROMTriplet(uint16_t addr)
{
  TTriplet result;
  TTriplet3 t3;
  if (addr > 1020)
    return 0xFFFFFF; else
  {
    result = 0;
    EEPROM.get(addr,t3);
    memcpy(&result,&t3,sizeof(t3));
    return result;
  }
}

//==============================================================
// GetEEPROMTriplet3
//   returns the Triplet3 at an address
//==============================================================
TTriplet3 GetEEPROMTriplet3(uint16_t addr)
{
  TTriplet3 t3;
  if (addr > 1020) 
    t3.a = t3.b = t3.c = 0xFF; else
    EEPROM.get(addr,t3);
  return t3;
}

//==============================================================
// PutEEPROMTriplet
//   updates a Triplet to the EEPROM
//==============================================================
void PutEEPROMTriplet(int16_t addr, TTriplet t)
{
  if (addr <= 1020) {
    TTriplet3 t3;
    memcpy(&t3,&t,sizeof(t3));
    EEPROM.put(addr,t3);
  }
}

//==============================================================
// PutEEPROMTriplet3
//   updates a Triplet to the EEPROM
//==============================================================
void PutEEPROMTriplet3(int16_t addr, TTriplet3 t)
{
  if (addr <= 1020) {
    EEPROM.put(addr,t);
  }
}

//==============================================================
// UnpackEEPROMkind
//   returns kind of triplet
//     ekTarget    set target of servo
//     ekMinPos    set min position and pin num
//     ekMaxPos    set max position and group
//     ekVmaxAmax  set Vmax and Amax
//     ekEOF       end of memory
//==============================================================
TEEPROMkind UnpackEEPROMkind(TTriplet t)
{
  if (t == 0xFFFFFF)
    return ekEOF; else
  {
    if ((t & 0x080000) == 0)
      return ekTarget; else
    switch (t & 0xF0000) {      
      case 0x80000:  return ekMinPos;
      case 0xA0000:  return ekMaxPos;
      case 0xC0000:  return ekVmaxAmax;
      default:        return ekEOF;
    }
  }
}

//==============================================================
// GetEEPROMkind
//   gets a triplet from the EEPROM
//   returns kind of triplet
//==============================================================
TEEPROMkind GetEEPROMkind(uint16_t addr) {
  return UnpackEEPROMkind( GetEEPROMTriplet(addr));
}

//==============================================================
// EEPROMTargetToAddr
//   converts a "Target" Triplet number to an EEPROM address
//   "Target" Triplets are numbered from 0..n
//   returns EEPROM address
//==============================================================
int16_t EEPROMTargetToAddr(uint16_t TargetNum)
{
  for (int16_t result = 0; GetEEPROMkind(result) != ekEOF; result += sizeof(TTriplet3))
    if (GetEEPROMkind(result) == ekTarget)
    {
      if (TargetNum == 0)
        return result;
      TargetNum--;
    } 
  return -1;
}

//==============================================================
// EEPROMTop
//   returns the addr of the first EOF byte in the EEPROM
//==============================================================
uint16_t EEPROMTop()
{
  int16_t result;
  for (result = 0; GetEEPROMkind(result) != ekEOF; result += sizeof(TTriplet3)) {}
  return result;
}

//==============================================================
// UnpackEEPROMTarget
//   returns a "Target" Triplet 
//   "Target" Triplets are numbered from 0..n
//==============================================================
TTriplet UnpackEEPROMTarget(uint16_t TargetNum)
{
  int16_t addr;
  addr = EEPROMTargetToAddr(TargetNum);
  if (addr >= 0)
    return GetEEPROMTriplet(addr); else
    return 0xFFFFFF;
}

//==============================================================
// CheckEEPROMGood
//   if there are errors in the EEPROM then initialise it
//==============================================================
void CheckEEPROMGood()
{
  TTriplet t;
  uint8_t srv;
  bool ok = true;

  for (int16_t addr = 0; GetEEPROMkind(addr) != ekEOF; addr += sizeof(TTriplet3))
  {
    t = GetEEPROMTriplet(addr);
    if (UnpackEEPROMkind(t) == ekEOF)
      return; 
  
    srv = UnpackEEPROMservo(t);
    switch (UnpackEEPROMkind(t)) {
      case ekTarget:
      case ekMinPos:
      case ekMaxPos:
      case ekVmaxAmax: ok = ok && (srv < NumServosMax); break;
      ekEOF: return; 
      default: ok = false;
    }
  }
  if (not ok)
    InitialiseEEPROM();
}

//==============================================================
// InitialiseEEPROM
//   prt an EOF mark at the start of the EEPROM
//==============================================================
void InitialiseEEPROM()
{
  PutEEPROMTriplet(0,0xFFFFFF);
//  Serial.println("Initialising EEPROM");
}

//==============================================================
// EEPROMnumTargets
//   returns number of "target" Triplets in the EEPROM
//==============================================================
uint16_t EEPROMnumTargets()
{
  uint16_t result = 0;
  for (int16_t addr = 0; GetEEPROMkind(addr) != ekEOF; addr += sizeof(TTriplet3))
  {
    if (GetEEPROMkind(addr) == ekTarget)
      result++;
  }
  return result;
}

//==============================================================
// EEPROMhasServo
//   returns whether servo mentioned in EEPROM 
//==============================================================
bool EEPROMhasServo(uint8_t servo)
{
  for (int16_t addr = 0; GetEEPROMkind(addr) != ekEOF; addr += sizeof(TTriplet3))
    if (GetEEPROMkind(addr) == servo)
      return true;
  return false;
}

//==============================================================
// EEPROMDeleteServos
//   delete all the Triplets which refer to servos 
//     which reference a servo between servomin and servomax
//     and >= addr
//==============================================================
void EEPROMDeleteServos(uint8_t servomin, uint8_t servomax, uint16_t addr)
{
  int16_t srv,dst;
  dst = addr;
  servomin = servomin << 4;
  servomax = servomax << 4;

  for (int16_t src = addr; GetEEPROMkind(src) != ekEOF; src += sizeof(TTriplet3))
  {
    srv = GetEEPROMkind(src);
    if ((srv < servomin) or (srv > servomax))
    {
      EEPROM.update(dst,EEPROM.read(src  )); dst++;
      EEPROM.update(dst,EEPROM.read(src+1)); dst++;
      EEPROM.update(dst,EEPROM.read(src+2)); dst++;
    }
  }
  PutEEPROMTriplet(dst,0xFFFFFF);
}

//==============================================================
// EEPROMDeleteTriplet
//   delete a Triplet
//==============================================================
void EEPROMDeleteTriplet(uint16_t dst)
{ 
  while (GetEEPROMkind(dst+sizeof(TTriplet3)) != ekEOF)         
  {
    EEPROM.update(dst,EEPROM.read(dst+3)); dst++;
    EEPROM.update(dst,EEPROM.read(dst+3)); dst++;
    EEPROM.update(dst,EEPROM.read(dst+3)); dst++;
  }
  PutEEPROMTriplet(dst,0xFFFFFF);
}

//==============================================================
// EEPROMsaveTriplet
//   writes a triplet to the EEPROM
//==============================================================
void EEPROMsaveTriplet(TTriplet t, uint16_t addr)
{
  PutEEPROMTriplet(addr,t);
}

//==============================================================
// EEPROMappendTriplet
//   appends a triplet to the EEPROM
//   puts an EOF byte after it
//   returns true if there was enough space
//==============================================================
bool EEPROMappendTriplet(TTriplet t)
{
//Serial.print(F("EEPROMappendTriplet ")); Serial.println(t,HEX); 
  uint16_t addr;
  addr = EEPROMTop();
  if (addr < EEPROM.length()-1)
  {
    PutEEPROMTriplet(addr,t);
    PutEEPROMTriplet(addr+sizeof(TTriplet3),0xFFFFFF);
    return true;
  } else
    return false;
}

//==============================================================
// EEPROMTargetToTriplet
//   packs a servo number, target position and wait time into a triplet
//==============================================================
TTriplet EEPROMTargetToTriplet(uint32_t servo, uint32_t target, uint32_t wait)
{
  return
    ((servo & 0x0F) << 20) +
    ((wait & 0xFF) << 11) +
    ((target-500) & 0x7FF);
}

//==============================================================
// EEPROMMinPosToTriplet
//   packs a servo number, min position and pin number into a triplet
//   ssss1000 pppp0nnn nnnnnnnn   set min position == nnnnnnnnnnn + 500 microseconds (500..2547) ; pin == pppp
//==============================================================
TTriplet EEPROMMinPosToTriplet(uint32_t servo, uint32_t pos, uint32_t pin)
{
  return
    ((servo & 0x0F) << 20) +
    0x80000+
    ((pin & 0x0F) << 12) +
    ((pos-500) & 0x7FF);
}

//==============================================================
// EEPROMMaxPosToTriplet
//   packs a servo number, max position and group number into a triplet
//   ssss1010 gggg0nnn nnnnnnnn   set max position == nnnnnnnnnnn + 500 microseconds (500..2547) ; group == gggg;
//==============================================================
TTriplet EEPROMMaxPosToTriplet(uint32_t servo, uint32_t pos, uint32_t group)
{
  return 
    ((servo & 0x0F) << 20) +
    0xA0000+
    ((group & 0x0F) << 12) +
    ((pos-500) & 0x7FF);
}

//==============================================================
// EEPROMVmaxAmaxToTriplet
//   packs a servo number, Vmax and Amax into a triplet
//   ssss1100 vvvvvvvv aaaaaaaa   Vmax[ssss] == vvvvvvvvvv uS per interval; Amax[ssss] == aaaaaaaaaa/10.0 uS per interval;
//==============================================================
TTriplet EEPROMVmaxAmaxToTriplet(uint32_t servo, float Vmax, float Amax)
{
  return
    ((servo & 0x0F) << 20) +
    0xC0000+
    ((round(Vmax) & 0xFF) << 8) +
    (round(Amax*10.0) & 0xFF);
}

//==============================================================
// EEPROMpercentFull
//   returns how full is the EEPROM as a percentage
//==============================================================
uint16_t EEPROMpercentFull()
{
  return EEPROMTop()*100 / (EEPROM.length()-1);
}

//==============================================================
// EEPROMappendTarget
//   appends a servo number, target position and wait time to the EEPROM
//   returns true if there was enough space
//==============================================================
bool EEPROMappendTarget(uint8_t servo, uint16_t target, uint8_t wait)
{
//Serial.print(F("EEPROMappendTarget ")); Serial.print(servo); Serial.print(' '); Serial.print(target); Serial.print(' '); Serial.println(wait); 
  return EEPROMappendTriplet(EEPROMTargetToTriplet(servo,target,wait));
}

//==============================================================
// EEPROMappendMinPos
//   appends a servo number, min position and pin number to the EEPROM
//   returns true if there was enough space
//==============================================================
bool EEPROMappendMinPos(uint8_t servo, uint16_t pos, uint8_t pin)
{
  return EEPROMappendTriplet(EEPROMMinPosToTriplet(servo,pos,pin));
}

//==============================================================
// EEPROMappendMaxPos
//   appends a servo number, max position and group number to the EEPROM
//   returns true if there was enough space
//==============================================================
bool EEPROMappendMaxPos(uint8_t servo, uint16_t pos, uint8_t group)
{
  return EEPROMappendTriplet(EEPROMMaxPosToTriplet(servo,pos,group));
}

//==============================================================
// EEPROMappendVmaxAmax
//   appends a servo number, Vmax and Amax to the EEPROM
//   returns true if there was enough space
//==============================================================
bool EEPROMappendVmaxAmax(uint8_t servo, float Vmax, float Amax)
{
  return EEPROMappendTriplet(EEPROMVmaxAmaxToTriplet(servo,Vmax,Amax));
}

//==============================================================
// UnpackEEPROMservo
//   returns which servo a triplet refers to
//==============================================================
uint8_t UnpackEEPROMservo(TTriplet t)
{
  return (t >> 20) & 0x0F;
}

//==============================================================
// UnpackEEPROMdelay
//   the "wait" part of a "target" triplet
//   returns 
//==============================================================
uint8_t UnpackEEPROMdelay(TTriplet t)
{
  return (t >> 11) & 0xFF;
}

//==============================================================
// UnpackEEPROMparams
//   uses a triplet to update some of the Params of a servo
//==============================================================
void UnpackEEPROMparams(TTriplet t, TServo *Params)
{
  switch (UnpackEEPROMkind(t)) {
    case ekTarget:   Params->Target = (t & 0x7FF)+500; break;
    case ekMinPos:   Params->ServoMin = (t & 0x7FF)+500; Params->PinNum = (t >> 12) & 0x0F; break;
    case ekMaxPos:   Params->ServoMax = (t & 0x7FF)+500; Params->Group = (t >> 12) & 0x0F; break;
    case ekVmaxAmax: Params->Vmax = (t >> 8) & 0xFF; Params->Amax = (t & 0xFF)/10.0; break;
  }
}

//==============================================================
// SendEEPROM
//   send EEPROM contents to PC over serial
//     SendEEPROM: plain bytes as binary
//     SendEEPROMbytes(): bytes as hex digits
//     SendEEPROMtext(): with translation
//==============================================================
void SendEEPROM() {
  int16_t addr = 0;

  while (GetEEPROMkind(addr) != ekEOF)
  {
    Serial.write(EEPROM.read(addr)); addr++;
    Serial.write(EEPROM.read(addr)); addr++;
    Serial.write(EEPROM.read(addr)); addr++;
  }
  Serial.write((byte)0xFF); 
  Serial.write((byte)0xFF); 
  Serial.write((byte)0xFF); 
}

void SendEEPROMbytes() {
  int16_t addr = 0;
  while (GetEEPROMkind(addr) != ekEOF)
  {
    Serial.print(EEPROM.read(addr) / 16,HEX); Serial.print(EEPROM.read(addr) % 16,HEX); Serial.print(' '); addr++;
    Serial.print(EEPROM.read(addr) / 16,HEX); Serial.print(EEPROM.read(addr) % 16,HEX); Serial.print(' '); addr++;
    Serial.print(EEPROM.read(addr) / 16,HEX); Serial.print(EEPROM.read(addr) % 16,HEX); Serial.print(' '); addr++;
    if (addr % 18 == 0) Serial.println(' ');
  }
  Serial.println("FF");
}

void SendEEPROMtext() {  
  for (int16_t addr = 0; GetEEPROMkind(addr) != ekEOF; addr += sizeof(TTriplet3))
  {
    TServo Params;
    SerialPrintHex(addr,3);  Serial.print(' '); 
    TTriplet t = GetEEPROMTriplet(addr);    
    SerialPrintHex(t,6);  Serial.print(F(" servo="));
    Serial.print(UnpackEEPROMservo(t)); Serial.print(' '); 
    UnpackEEPROMparams(t, &Params);

    switch (UnpackEEPROMkind(t)) {
      case ekTarget:    Serial.print(F("Target "));   Serial.print(Params.Target);    Serial.print(F(" delay=")); if (UnpackEEPROMdelay(t) >= 128) Serial.print("Fin+"); Serial.print((UnpackEEPROMdelay(t) & 0x7F)/10.0); break;
      case ekMinPos:    Serial.print(F("MinPos "));   Serial.print(Params.ServoMin);  Serial.print(F(" PinNum="));  Serial.print(Params.PinNum); break;
      case ekMaxPos:    Serial.print(F("MaxPos "));   Serial.print(Params.ServoMax);  Serial.print(F(" Group="));   Serial.print(Params.Group); break;
      case ekVmaxAmax:  Serial.print(F("Vmax="));     Serial.print(Params.Vmax);      Serial.print(F(" Amax="));    Serial.print(Params.Amax); break;
    }
    Serial.println(""); 
  }
  SerialPrintHex(addr,3);  Serial.println(" FF");
}

void SendEEPROMplain() {
  Serial.println(F("EEPROM"));
  SendEEPROMtext();
}

//==============================================================
// RecvEEPROM
//   receive EEPROM from PC to Nano
//   next bytes received are n EEPROM triplets followed by FF
//   Nano replies with "OK" (byte 0xAA) after each byte from PC
//==============================================================
void RecvEEPROM() {
  TTriplet3 t;
  
  for (int16_t addr = 0; addr <= 1020; addr += sizeof(TTriplet3)) {
    Serial.readBytes(&t.c,1);
    Serial.readBytes(&t.b,1);
    Serial.readBytes(&t.a,1);
    PutEEPROMTriplet3(addr, t);
  }
  Serial.write(0xAA);
}

#endif