#include "Arduino.h"
#include "ezButton.h"
#include "Vrekrer_scpi_parser.h"
#include <string>

/*===================================================
              Konfigurations Parameter
===================================================*/
auto csVendorString = F("VENDOR,USB SCPI Gpio,#0001," VREKRER_SCPI_VERSION);
const uint32_t cu32DebounceTime = 50UL;
ezButton tInputArray[] =
{
  ezButton(0),
  ezButton(1),
  ezButton(2),
  ezButton(3),
  ezButton(4),
  ezButton(5),
  ezButton(6),
  ezButton(7)
};
const uint8_t cu8InputArrayCnt = sizeof(tInputArray) / sizeof(ezButton);
static_assert(!(cu8InputArrayCnt>8), "More than 8 Inputs defined!");

const uint8_t cu8OutputArray[] =
{
  9,
  10,
  11,
  12,
  13,
  14,
  15,
  16
};
const uint8_t cu8OutputArrayCnt = sizeof(cu8OutputArray) / sizeof(uint8_t);
static_assert(!(cu8OutputArrayCnt>8), "More than 8 Outputs defined!");
const uint8_t cu8OutputDefault[cu8OutputArrayCnt] =
{
  0,
  0,
  0,
  0,
  0,
  0,
  0,
  0
};

/*===================================================
              Globale Variablen
===================================================*/
uint8_t u8ActInState = 0U;
uint8_t u8ActOutState = 0U;
uint8_t u8InputInvert = 0U;
SCPI_Parser scpi_instr;
auto last = millis();

/*===================================================
              Helper
===================================================*/
char GetBase(SCPI_C commands, SCPI_P parameters)
{
  char base = DEC;
  if (parameters.Size() > 0)
  {
    String format = String(parameters[0]);
    if (format == "b" || format == "B")
    {
      base = BIN;
    }
    else if (format == "h" || format == "H")
    {
      base = HEX;
    }
  }
  return base;
}

String GetBaseString(char base, uint8_t u8Val)
{
  char buffer[12] = {'0', 'b'}; // Fuer BIN setzen. Ansonsten wird es ueberschrieben
  switch (base)
  {
  default:
  case DEC:
    snprintf(buffer, sizeof(buffer), "%d", u8Val);
    break;

  case BIN:
    for (int i = 0; i < 8 ; i++)
    {
      buffer[2 + 7 - i] = (u8Val & (1 << i)) ? '1' : '0';
    }
    break;

  case HEX:
    snprintf(buffer, sizeof(buffer), "0x%02X", u8Val);
    break;
  }
  return String(buffer);
}

/*===================================================
              SCPI Funktionen
===================================================*/
void Identify(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
  //*IDN? Suggested return string should be in the following format:
  // "<vendor>,<model>,<serial number>,<firmware>"
  interface.println(csVendorString);
}

void SetOutput(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
  // For simplicity no bad parameter check is done.
  if (parameters.Size() > 0)
  {
    String lower = String(parameters[0]);
    lower.toUpperCase();
    if(lower.startsWith("0X"))
    {
      uint32_t u32 = std::stoul(lower.substring(2).c_str(), nullptr, 16);
      u8ActOutState = constrain((int32_t)u32, 0, 255);
    }
    else if(lower.startsWith("0B"))
    {
      uint32_t u32 = std::stoul(lower.substring(2).c_str(), nullptr, 2);
      u8ActOutState = constrain((int32_t)u32, 0, 255);
    }
    else
    {
      //Kein Hex, Kein Bin --> es wird eine normale Zahl angenommen
      u8ActOutState = constrain(String(parameters[0]).toInt(), 0, 255);
    }
  }
}

void GetOutput(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
  char base = GetBase(commands, parameters);
  String out = GetBaseString(base, u8ActOutState);
  interface.println(out);
}

void GetInput(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
  char base = GetBase(commands, parameters);
  String out = GetBaseString(base, u8ActInState);
  interface.println(out);
}

void SetInputInvert(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
  // For simplicity no bad parameter check is done.
  if (parameters.Size() > 0)
  {
    u8InputInvert = constrain(String(parameters[0]).toInt(), 0, 1);
  }
}

void GetInputInvert(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
  interface.println(String(u8InputInvert,10));
}

/*=========================================================================
                              Setup / Loop
===========================================================================*/
void setup()
{
  SerialUSB.begin(115200);

  // Input Entprellzeit setzen fuer alle definierten Inputs
  for (uint8_t i = 0U; i < cu8InputArrayCnt; i++)
  {
    tInputArray[i].setDebounceTime(cu32DebounceTime);
  }
  // Richtung und Default Wert setzen fuer alle definierten Outputs
  for (uint8_t i = 0U; i < cu8OutputArrayCnt; i++)
  {
    pinMode(cu8OutputArray[i], OUTPUT);
    digitalWrite(cu8OutputArray[i], cu8OutputDefault[i]);
  }
  pinMode(PIN_LED, OUTPUT); //Nur damit man sieht dass sich was tut

  // SCPI Commands setzen
  scpi_instr.RegisterCommand(F("*IDN?"), &Identify);
  scpi_instr.SetCommandTreeBase(F("SYSTem:IO"));
    scpi_instr.RegisterCommand(F(":Output"), &SetOutput);
    scpi_instr.RegisterCommand(F(":Output?"), &GetOutput);
    scpi_instr.RegisterCommand(F(":Input?"), &GetInput);
  scpi_instr.SetCommandTreeBase(F("SYSTem:Cfg"));
    scpi_instr.RegisterCommand(F(":INVert"), &SetInputInvert);
    scpi_instr.RegisterCommand(F(":INVert?"), &GetInputInvert);
}

void loop()
{
  // Inputs lesen
  uint8_t u8NewInState = 0;
  for (uint8_t i = 0U; i < cu8InputArrayCnt; i++)
  {
    tInputArray[i].loop();
    if(tInputArray[i].getState())
    {
      u8NewInState |= (1<<i);
    }
  }
  if(u8InputInvert > 0)
  {
    u8ActInState = u8NewInState ^ 0xFF;
  }
  else
  {
    u8ActInState = u8NewInState;
  }

  //SCPI Instrument zyklisch aufrufen
  scpi_instr.ProcessInput(SerialUSB, "\r\n");

  // Outputs setzen
  for (uint8_t i = 0U; i < cu8OutputArrayCnt; i++)
  {
    uint8_t u8NewOutPinState = (u8ActOutState >> i) & 0x1;
    digitalWrite(cu8OutputArray[i], u8NewOutPinState);
  }

  // Nur um zu sehen dass sich noch was bewegt die LED toggeln
  auto now = millis();
  if ((now - last) > 200)
  {
    digitalWrite(PIN_LED, digitalRead(PIN_LED) ^ 1);
    last = now;
  }
}