#include "Serial.h"

#ifdef ENABLE_SERIAL

SerialCommand SCmd;

void sendOK()
{
  Serial.println("OK"); /* Send "OK" message */
}

void readHandler()
{
  Serial.print(__FUNCTION__);
  char *arg = SCmd.next(); /* Get the next argument from the SerialCommand object buffer */
  int i = 0;
  while (arg != NULL)
  {
    Serial.print("argument #");
    Serial.print(++i);
    Serial.print(": ");
    Serial.println(arg);
    arg = SCmd.next(); /* Get the next argument from the SerialCommand object buffer */
  }

  sendOK(); /* Send "OK" message */
}

void writeHandler()
{
  Serial.print(__FUNCTION__);
  sendOK(); /* Send "OK" message */
}
void executeHandler()
{
  Serial.print(__FUNCTION__);
  sendOK(); /* Send "OK" message */
}

void initSerial()
{

  Serial.begin(19200);
  SCmd.addCommand((char *)"AT", serialGet, readHandler, writeHandler, executeHandler);

  /*
    SCmd.addCommand("G", serialGet);
    SCmd.addCommand("P", serialSetKp);
    SCmd.addCommand("I", serialSetKi);
    SCmd.addCommand("D", serialSetKd);
    SCmd.addCommand("T", serialSetTemp);
    SCmd.addCommand("F", serialSetFan);
    SCmd.addCommand("AUTO", serialAutoPID);
    SCmd.addCommand("OFF", offHeater);

    SCmd.addDefaultHandler(serialPardon);
    */

  Serial.println(F(SERIAL_READY));
}

void parseSerial()
{
  // SCmd.readSerial();
  SCmd.loop();
}
void serialGet()
{
  for (int i = 0; i < 2; i++)
  {
    printPIDSetting(i);
  }
}
void serialSetTemp()
{
  int idxHeater, newSetpoint;
  if (!getCmdArg(idxHeater) || !getCmdArg(newSetpoint))
  {
    serialPardon();
  }
  else if (newSetpoint > HEATER_MAX_TEMP)
  {
    Serial.println((SERIAL_ERR_MAX_TEMP));
  }
  else if (newSetpoint < 0)
  {
    Serial.println((SERIAL_ERR_NEG_TEMP));
  }
  else if (idxHeater > 1 || idxHeater < 0)
  {
    Serial.println((SERIAL_ERR_NO_HEATER));
  }
  else
  {
    setHeaterTemp(idxHeater, newSetpoint);
  }
}

void serialSetFan()
{
  int fanStatus;
  if (!getCmdArg(fanStatus))
  {
    serialPardon();
  }
  else
  {
    setFan(fanStatus);
  }
}

void serialSetKp()
{
  int idxHeater;
  double newval;
  if (!getCmdArg(idxHeater) || !getCmdArg(newval))
  {
    serialPardon();
  }
  else if (newval < 0)
  {
    Serial.println(F(SERIAL_ERR_NEG_VAL));
  }
  else
  {
    PIDKp[idxHeater] = newval;
    PIDHeater[idxHeater].SetTunings(PIDKp[idxHeater], PIDKi[idxHeater], PIDKd[idxHeater]);
    printPIDSetting(idxHeater);
  }
}

void serialSetKi()
{
  int idxHeater;
  double newval;
  if (!getCmdArg(idxHeater) || !getCmdArg(newval))
  {
    serialPardon();
  }
  else if (newval < 0)
  {
    Serial.println(F(SERIAL_ERR_NEG_VAL));
  }
  else
  {
    PIDKi[idxHeater] = newval;
    PIDHeater[idxHeater].SetTunings(PIDKp[idxHeater], PIDKi[idxHeater], PIDKd[idxHeater]);
    printPIDSetting(idxHeater);
  }
}

void serialSetKd()
{
  int idxHeater;
  double newval;
  if (!getCmdArg(idxHeater) || !getCmdArg(newval))
  {
    serialPardon();
  }
  else if (newval < 0)
  {
    Serial.println(F(SERIAL_ERR_NEG_VAL));
  }
  else
  {
    PIDKd[idxHeater] = newval;
    PIDHeater[idxHeater].SetTunings(PIDKp[idxHeater], PIDKi[idxHeater], PIDKd[idxHeater]);
    printPIDSetting(idxHeater);
  }
}

void serialAutoPID()
{
  double temp;
  int hotend, ncycles;
  if (!getCmdArg(hotend) || !getCmdArg(temp) || !getCmdArg(ncycles))
  {
    serialPardon();
  }
  else if (hotend >= 2)
  {
    Serial.println((SERIAL_ERR_NO_HEATER));
  }
  else if (temp > HEATER_MAX_TEMP)
  {
    Serial.println((SERIAL_ERR_MAX_TEMP));
  }
  else if (temp < 0)
  {
    Serial.println((SERIAL_ERR_NEG_TEMP));
  }
  else if (ncycles < 2)
  {
    Serial.println((SERIAL_ERR_AT_CYC));
  }
  else
  {
    AT(temp, hotend, ncycles, false);
  }
}

void serialPardon()
{
  Serial.println(F(SERIAL_PARDON));
}

bool getCmdArg(int &output)
{
  char *arg;
  arg = SCmd.next();
  if (arg != NULL)
  {
    output = atoi(arg);
    return true;
  }
  else
  {
    output = -1;
    return false;
  }
}

bool getCmdArg(double &output)
{
  char *arg;
  arg = SCmd.next();
  if (arg != NULL)
  {
    output = atof(arg);
    return true;
  }
  else
  {
    output = -1.0f;
    return false;
  }
}
#endif