#include "Parms_ADI.h"

ParmsLibClass::~ParmsLibClass()
{
    _fileLib.~FileLibClass();
}

int32_t ParmsLibClass::read(uint16_t pnum)
{
    _printMessage("Read parm %d.\n", pnum);
    return parm[pnum];
}

void ParmsLibClass::factoryReset(void)
{
    //_printMessage("Setting factory parms.\n");
    displayText("Parm Restore");
    Serial.println("Setting factory parms.\n");
    
    
    uint16_t parmLimitSize = sizeof(parmlimits) / sizeof(parmlimits[0]);
    // skip 0 for checksum
    for (int i = 1; i < parmLimitSize; i++)
    {
        // skip serial number
        if (i != 64)
        {
            if (parmlimits[i][3] == 1)
            {
                parm[i] = parmlimits[i][0];
            }
        }
    }

    parm[0] = _calcChecksum();

    // save to FS
    saveToFS();

    _printMessage("CheckSum: %d\n", parm[0]);
    delay(2000);
}

bool ParmsLibClass::write(uint16_t pnum, int32_t pdat)
{
  _printMessage("Write parm %d = %d.\n", pnum, pdat);
  int csi, csi2;
  if ((pnum == 1) && (pdat == 255))
  {
    factoryReset();
    return true;
  }

  if ((pnum == 1) && (pdat == 253))
  {
    // esp_task_wdt_init(180, true); //enable panic so ESP32 restarts 180 seconds
    updateFromFS();
    return true;
  }

  if ((pnum == 1) && (pdat == 1234)) // v509
  {
    parm[76] = getEpochRtc();
    saveToFS();
    return true;
  }

  if ((pnum == 1) && (pdat == 252)) // v509
  {
    displayText("WDT TEST LONG");
    delay(6000);
    displayText("WDT TEST SHORT");
    tone(HEARTBEATLED, 100, 500); // set wdog timeout to 2 sec
    delay(5000);
    displayText("WDT FAILED!!!!");
    delay(4000);
    return true;
  }

  if ((pnum == 1) && (pdat == 250)) // Lidar test mode
  {
    displayText("LIDAR TEST");
    lidarTest = 1;
    return true;
  }

  if ((pnum == 1) && (pdat == 6014))
  {
    displayText("TOGGLE VB");
    if (VB)
    {
      VB = 0;
      Serial.println("Verbose OFF");
    }
    else
    {
      VB = 1;
      Serial.println("Verbose ON");
    }
    return true;
  }

  if (pnum == 18) // calibrate battery voltage
  {
    Serial.printf("Raw Battery A2D: %d\n", analogRead(BATTERYINPUTPIN));
    pdat *= 1000; // voltage * 1000

    csi = 0;
    for (csi2 = 0; csi2 < 20; csi2++)
      csi += analogRead(BATTERYINPUTPIN);
    csi /= 20;

    if (csi == 0)
      csi = 1;
    pdat /= csi;
  }

  // don't write parm 33(exercise mode) to SD, its set to 0 on boot
  if (pnum == 33)
  {
    parm[pnum] = pdat;
    return true;
  }

  if (!_qualify(pnum, pdat))
  {
      _printMessage("Parm %d with value %d is out of range.\n", pnum, pdat);
      return false;
  }
  if (pnum == 1)
  	if ((pdat < 5) || (pdat == 100))
  	{}
  	else
  	return true;
  	
  parm[pnum] = pdat;
  // update checksum
  parm[0] = _calcChecksum();
  // now write to SD
  saveToFS();
  return true;
  
}

//*******************************************************
//
//*******************************************************

bool ParmsLibClass::saveToFS(char *filename)
{
    _printMessage("Writing parms to FS.\n");
    _qualifyAll();
    if (!_fileLib.write(filename, reinterpret_cast<uint8_t *>(parm), sizeof(parm)))
    {
        _printMessage("Failed to write parms to FS.\n");
        return false;
    }
    return true;
}

bool ParmsLibClass::loadFromFS(char *filename)
{
    _printMessage("Setting parms from FS.\n");
    memset(parm, 0, sizeof(parm));
    if (!_fileLib.read(filename, reinterpret_cast<uint8_t *>(parm), sizeof(parm)))
    {
        _printMessage("Failed to read parms from FS.\n");
        factoryReset();
        return false;
    }

    int32_t checkSum = _calcChecksum();
    if (checkSum != parm[0])
    {
        _printMessage("Error checksum mismatch. Expected %d, got %d.\n", parm[0], checkSum);
        factoryReset();
        return false;
    }

    return true;
}

bool ParmsLibClass::_qualify(uint16_t pnum, int32_t pdat)
{
    _printMessage("Qualifying parm %d with value %d.\n", pnum, pdat);
    uint16_t parmLimitSize = sizeof(parmlimits) / sizeof(parmlimits[0]);
    // qualify parms min/max
    if(pnum > PARMSIZE)  // disallow write to an non-existant parm
        return false;

    if (pnum > parmLimitSize)    // allow any data if parm number is outside of limit table
    {
        return true;
    }

    if (pdat >= parmlimits[pnum][1])
    {
        if (pdat <= parmlimits[pnum][2])
        {
            return true;
        }
    }

    return false;
}

void ParmsLibClass::_qualifyAll(void)
{
    uint16_t parmLimitSize = sizeof(parmlimits) / sizeof(parmlimits[0]);
    for (uint16_t i = 1; i < parmLimitSize; i++)
    {
        if (!_qualify(i, parm[i]))
        {
            parm[i] = parmlimits[i][0];
        }
    }
}

int32_t ParmsLibClass::_calcChecksum(void)
{
    int32_t checkSum = 0;
    uint16_t parmLimitSize = sizeof(parmlimits) / sizeof(parmlimits[0]);
    // skip 0 for checksum
    for (uint16_t i = 1; i < parmLimitSize; i++)
    {
        checkSum += parm[i];
    }
    return checkSum;
}

void ParmsLibClass::_printMessage(const char *message, ...)
{
    if (_verbose)
    {
        char buffer[256];

        va_list args;
        va_start(args, message);
        vsnprintf(buffer, sizeof(buffer), message, args);
        va_end(args);
        Serial.printf("ParmsLibClass:: %s", buffer);
    }
}
    





void SD_LogWrite(char *str)
{
  return;

  char lstr[80];
  char tstr[80];
  String St;

  Serial.println("Writing Log");
  displayText("Writing Log");

  St = makeTimeStr();
  St.toCharArray(tstr, 80);
  sprintf(lstr, "%s -> %s\n", tstr, str);

  Serial.print("Log: ");
  Serial.println(lstr);

}

void qualifyParms(void)
{
  int i;
  // qualify parms min/max
  for (i = 1; i < (PARMSIZE - 1); i++)
  {
    if (parm[i] < parmlimits[i][1])
      parm[i] = parmlimits[i][1];
    if (parm[i] > parmlimits[i][2])
      parm[i] = parmlimits[i][2];
  }
}