#include "PHApp.h"
#include "features.h"

// <<1;2;64;printRegisters:1:0>>
// <<1;2;64;print:1:0>>

#if defined(MB_RELAY_0) || defined(MB_RELAY_1) || defined(MB_RELAY_2) || defined(MB_RELAY_3)
#include "./components/Relay.h"
#endif

#if (defined(MB_ANALOG_3POS_SWITCH_0) && defined(MB_ANALOG_3POS_SWITCH_1) || defined(MB_ANALOG_3POS_SWITCH_3))
#include "./components/3PosAnalog.h"
#endif
#ifdef HAS_STATUS
#include "./components/StatusLight.h"
#endif

#include <Ethernet.h>
#include <macros.h>

#include "./config.h"
#include "./config_adv.h"
#include "./enums.h"
#include "./features.h"
#include "./debug_utils.h"
#include "ModbusBridge.h"

#define XD 1

#define ADD_RELAY(relayNum, relayPin, relayKey, relayRegister)           \
  relay_##relayNum = new Relay(this, relayPin, relayKey, relayRegister); \
  components.push_back(relay_##relayNum);                                \
  relay_##relayNum->setGateway(modbusBridge->getMudbus())

#define ADD_POT(potNum, potPin, potKey, potRegister)         \
  pot_##potNum = new POT(this, potPin, potKey, potRegister); \
  components.push_back(pot_##potNum);                        \
  pot_##potNum->setGateway(modbusBridge->getMudbus())

#define ADD_POS3ANALOG(posNum, switchPin1, switchPin2, switchKey, switchRegister)                \
  pos3Analog_##posNum = new Pos3Analog(this, switchPin1, switchPin2, switchKey, switchRegister); \
  components.push_back(pos3Analog_##posNum);                                                     \
  pos3Analog_##posNum->setGateway(modbusBridge->getMudbus())

short PHApp::print(short arg1, short arg2)
{
  uchar s = components.size();
  for (uchar i = 0; i < s; i++)
  {
    Component *component = components[i];
    component->info();
  }
  return E_OK;
}

void PHApp::printRegisters()
{

#if ENABLED(HAS_MODBUS_REGISTER_DESCRIPTIONS)
  Log.setShowLevel(false);
  Serial.print("| Name | ID  | Address | RW |  Function Code | Number Addresses |Register Description| \n");
  Serial.print("|------|----------|----|----|----|----|-------|\n");
  short size = components.size();
  for (int i = 0; i < size; i++)
  {
    Component *component = components[i];
    if (!(component->nFlags & 1 << OBJECT_NET_CAPS::E_NCAPS_MODBUS))
    {
      continue;
    }
    Log.verbose("| %s | %d | %d | %s | %d | %d | %s |\n",
                (Component *)component->name.c_str(),
                component->id,
                component->getAddress(),
                component->getRegisterMode(),
                component->getFunctionCode(),
                component->getNumberAddresses(),
                component->getRegisterDescription().c_str());
  }
  Log.setShowLevel(true);
#endif
}

short PHApp::reset()
{
#ifdef ARDUINO
  // Reset code for Arduino
  asm volatile("jmp 0");
#endif
#ifdef ESP8266
  ESP.restart();
#endif
  return E_NOT_IMPLEMENTED;
}

short PHApp::list(short val0, short val1)
{
  uchar s = components.size();
  for (uchar i = 0; i < s; i++)
  {
    Component *component = components[i];
    Log.verboseln("PHApp::list - %d | %s", component->id, component->name.c_str());
  }
  return E_OK;
}
short PHApp::setup()
{
  Serial.begin(SERIAL_BAUD_RATE);
  while (!Serial && !Serial.available())
  {
  }
  Log.begin(LOG_LEVEL_VERBOSE, &Serial);
  setFlag(OBJECT_RUN_FLAGS::E_OF_DEBUG);
  Ethernet.begin(mac, ip, gateway, subnet);
  bridge = new Bridge(this);
  com_serial = new SerialMessage(Serial, bridge);
  components.push_back(com_serial);
  components.push_back(bridge);

#ifdef MODBUS_BRIDGE
  modbusBridge = new ModbusBridge(this);
  components.push_back(modbusBridge);
#endif

#ifdef OMRON_PID_SLAVE_START
  pids = new OmronPID(modbusBridge, OMRON_PID_SLAVE_START);
  components.push_back(pids);
#endif

#ifdef MB_RELAY_0
  ADD_RELAY(0, MB_RELAY_0, COMPONENT_KEY_MB_RELAY_0, MB_R_RELAY_0);
#endif

#ifdef MB_RELAY_1
  ADD_RELAY(1, MB_RELAY_1, COMPONENT_KEY_MB_RELAY_1, MB_R_RELAY_1);
#endif

#ifdef MB_ANALOG_0
  ADD_POT(0, MB_ANALOG_0, COMPONENT_KEY_MB_ANALOG_0, MB_R_ANALOG_0);
#endif

#ifdef MB_ANALOG_1
  ADD_POT(1, MB_ANALOG_1, COMPONENT_KEY_MB_ANALOG_1, MB_R_ANALOG_1);
#endif

#if (defined(MB_ANALOG_3POS_SWITCH_0) && (defined(MB_ANALOG_3POS_SWITCH_1)))
  ADD_POS3ANALOG(0, MB_ANALOG_3POS_SWITCH_0, MB_ANALOG_3POS_SWITCH_1, COMPONENT_KEY_MB_ANALOG_3POS_SWITCH_0, MB_R_SWITCH_0);
#endif

#if (defined(MB_ANALOG_3POS_SWITCH_2) && (defined(MB_ANALOG_3POS_SWITCH_3)))
  ADD_POS3ANALOG(1, MB_ANALOG_3POS_SWITCH_2, MB_ANALOG_3POS_SWITCH_3, COMPONENT_KEY_MB_ANALOG_3POS_SWITCH_1, MB_R_SWITCH_1);
#endif

#ifdef HAS_OMRON_VFD_MODBUS
  omronVFD = new OmronVFD(modbusBridge, OMRON_MX2_SLAVE_ID, this);
  components.push_back(omronVFD);
  omronVFD->mode = pos3Analog_0;
  omronVFD->dir = pos3Analog_1;
  omronVFD->speed = pot_1;
  omronVFD->owner = this;
#endif

#ifdef HAS_FEED_SERVO
  stepperController_0 = new StepperController(this,
                                              MB_STEPPER_DIR_0, MB_STEPPER_PULSE_0, MB_STEPPER_FEEDBACK_0, MB_STEPPER_OVERLOAD_0, MB_STEPPER_ENABLED_0,
                                              STEPPER_DEFAULT_SPEED_0,
                                              STEPPER_PULSE_WIDTH_0,
                                              STEPPER_DEFAULT_DIR_0,
                                              COMPONENT_KEY_STEPPER_0,
                                              MB_RW_STEPPER_SPEED_0);
  components.push_back(stepperController_0);
  stepperController_0->setGateway(modbusBridge->getMudbus());
#endif

#ifdef MOTOR_LOAD_PIN
  mLoad = new MotorLoad(this, MOTOR_LOAD_PIN, COMPONENT_KEY_VFD_LOAD, MB_R_VFD_LOAD);
  components.push_back(mLoad);
  mLoad->setGateway(modbusBridge->getMudbus());
#ifdef OMRON_MX2_SLAVE_ID
  if (omronVFD)
  {
    omronVFD->motorLoad = mLoad;
  }
#endif

#endif

#ifdef HAS_STATUS
  statusLight_0 = new StatusLight(this,
                                  STATUS_WARNING_PIN,
                                  COMPONENT_KEY_FEEDBACK_0,
                                  MB_MONITORING_STATUS_FEEDBACK_0);
  components.push_back(statusLight_0);
  statusLight_0->setGateway(modbusBridge->getMudbus());

  statusLight_1 = new StatusLight(this,
                                  STATUS_ERROR_PIN,
                                  COMPONENT_KEY_FEEDBACK_1,
                                  MB_MONITORING_STATUS_FEEDBACK_1);
  components.push_back(statusLight_1);
  statusLight_1->setGateway(modbusBridge->getMudbus());
#else
  statusLight_0, statusLight_1 = NULL;
#endif

#ifdef HAS_FEED_SERVO
  if (stepperController_0)
  {
    setExtruderFeed(stepperController_0);
  }
#endif

  _state = APP_STATE::RESET;
  _error = E_OK;

  App::setup();
  registerComponents(bridge);
  onRegisterMethods(bridge);

  STATUS_ERROR_ON;
  STATUS_ERROR_BLINK;
  STATUS_WARNING_ON;
  STATUS_WARNING_BLINK;
  delay(500);
  STATUS_ERROR_OFF;
  STATUS_WARNING_OFF;
  STATUS_ERROR_BLINK_OFF;
  STATUS_WARNING_BLINK_OFF;

  print(0, 0);

#ifdef PRINT_MODBUS_REGISTERS
  printRegisters();
#endif

#ifdef MB_RELAY_0
  relay_0->set(MB_RELAY_0_DEFAULT);
#endif

#ifdef MB_RELAY_1
  relay_1->set(MB_RELAY_1_DEFAULT);
#endif

#if defined(HAS_FEED_SERVO) && defined(MB_RW_FEED_VFD_DEFAULT_SPEED_RATIO)
  modbusBridge->mb->R[MB_W_FEED_VFD_RATIO] = MB_RW_FEED_VFD_DEFAULT_SPEED_RATIO;
#endif

  return E_OK;
}

short PHApp::onRegisterMethods(Bridge *bridge)
{
  Log.verbose(F("* App:onRegisterMethods *" CR));
  bridge->registerMemberFunction(COMPONENT_KEY::COMPONENT_KEY_APP, this, C_STR("print"), (ComponentFnPtr)&PHApp::print);
  bridge->registerMemberFunction(COMPONENT_KEY::COMPONENT_KEY_APP, this, C_STR("list"), (ComponentFnPtr)&PHApp::list);
  bridge->registerMemberFunction(COMPONENT_KEY::COMPONENT_KEY_APP, this, C_STR("printRegisters"), (ComponentFnPtr)&PHApp::printRegisters);
  return E_OK;
}

short PHApp::onWarning(short code)
{
  Log.warning(F("* App:onWarning - code=%d" CR), code);
  if (statusLight_0)
  {
    statusLight_0->setBlink(true);
  }
  STATUS_WARNING_ON;
  STATUS_WARNING_BLINK;

  return E_OK;
}
short PHApp::onRun(short code)
{
  STATUS_WARNING_ON;
  STATUS_WARNING_BLINK_OFF;
  return E_OK;
}
short PHApp::onError(short id, short code)
{
  if (code == getLastError())
  {
    return code;
  }

  Log.error(F("* App:onError - component=%d (%s) code=%d" CR), byId(id)->name.c_str(), id, code);
  switch (code)
  {
  case E_VFD_TIMEOUT:
  case E_PID_TIMEOUT:
  {
    Log.errorln("App::onError  Timeout %d", id);
    break;
  }
  case E_FEED_OVERLOAD:
  case E_VFD_OVERLOAD:
  {
    Log.errorln("App::onError Overload %d", id);
    break;
  }

  default:
    break;
  }

  STATUS_WARNING_ON;
  STATUS_WARNING_BLINK;
  STATUS_ERROR_ON;
  STATUS_ERROR_BLINK;
  setLastError(code);
  return code;
}
short PHApp::onStop(short val)
{
  Log.verboseln(F("* App:onStop - " CR));
  STATUS_WARNING_OFF;
  STATUS_WARNING_BLINK_OFF;
}

short PHApp::clearError()
{
  Log.verboseln(F("* App:clearError - " CR));
  STATUS_ERROR_OFF;
  STATUS_ERROR_BLINK_OFF;
  STATUS_WARNING_OFF;
  STATUS_WARNING_BLINK_OFF;
  setLastError(E_OK);
}

short PHApp::loopExtruderFeed()
{

#ifdef HAS_FEED_SERVO
  if (!extruderFeed)
  {
    return E_INVALID_PARAMETERS;
  }

  if (extruderFeed->isOverloaded())
  {
    extruderFeed->speed(0);
    if (omronVFD)
    {
      omronVFD->stop();
    }
    onError(extruderFeed->id, E_FEED_OVERLOAD);
    return E_FEED_OVERLOAD;
  }

#if defined(MB_W_FEED_VFD_RATIO) && defined(MB_RW_FEED_VFD_DEFAULT_SPEED_RATIO)
  OmronVFDState *state = omronVFD->getVFDState();
  if (omronVFD->direction != OmronVFD::E_VFD_DIR::E_VFD_DIR_FORWARD)
  {
    extruderFeed->speed(0);
    return E_OK;
  }
  int netRatio = modbusBridge->mb->R[MB_W_FEED_VFD_RATIO];
  int vfdState = state->state;
  int vfdSpeed = state->FC;
  int t0 = ((OMRON_VFD_MAX_FREQ * 100 / netRatio) / 100);
  int t1 = t0 * STEPPER_MAX_SPEED_0 / 100;
  int newSpeed = abs(t1 * vfdSpeed / 100);
  switch (vfdState)
  {
  case OMRON_STATE_RUNNING:
  case OMRON_STATE_DECELERATING:
  case OMRON_STATE_ACCELERATING:
  {
    extruderFeed->speed(newSpeed);
    break;
  }
  default:
    extruderFeed->speed(0);
    break;
  }
#endif
#endif
}
short PHApp::loopModbus()
{
  if (!modbusBridge)
  {
    return E_OK;
  }
  Mudbus *mb = modbusBridge->mb;

  mb->R[MB_R_SYSTEM_ERROR] = getLastError();
  if (mb->R[MB_R_APP_STATE_REG] == MB_R_SYSTEM_CMD_PRINT_REGS)
  {
    printRegisters();
    mb->R[MB_R_APP_STATE_REG] = 0;
  }
  if (mb->R[MB_R_APP_STATE_REG] == MB_R_SYSTEM_CMD_PRINT_QUEUE)
  {
    modbusBridge->print();
    mb->R[MB_R_APP_STATE_REG] = 0;
  }
  if (mb->R[MB_R_APP_STATE_REG] == MB_R_SYSTEM_CMD_PRINT_MEMORY)
  {
    printMemory();
    mb->R[MB_R_APP_STATE_REG] = 0;
  }
  if (mb->R[MB_R_APP_STATE_REG] == MB_R_SYSTEM_CMD_PRINT_VFD)
  {
    omronVFD->getVFDState()->print();
    mb->R[MB_R_APP_STATE_REG] = 0;
  }
  if (pids && mb->R[MB_R_APP_STATE_REG] == MB_R_SYSTEM_CMD_PRINT_PIDS)
  {
    mb->R[MB_R_APP_STATE_REG] = 0;
    pids->printStates();
  }
  if (mb->R[MB_W_RESET_REG] == 1)
  {
    reset();
  }
  return E_OK;
}
short PHApp::loop()
{
  App::loop();
  loopExtruderFeed();
  loopModbus();
  return E_OK;
}
short PHApp::getAppState(short val)
{
  return E_OK;
}