#ifndef OMRON_PID_H
#define OMRON_PID_H

#include <macros.h>
#include <Vector.h>
#include <Component.h>
#include <SRegister.h>
#include <ModbusValue.h>

#include "./config.h"
#include "./OmronE5.h"
#include "./components/StatusLight.h"
#include "ModbusBridge.h"
#include "enums.h"


#define E_PID_MB_QUEUE_STATUS 1
#define E_PID_MB_QUEUE_USER_1 2
#define E_PID_MB_QUEUE_USER_2 3
#define E_PID_MB_QUEUE_LENGTH 1

#define E_PID_MB_M_QUEUE_PID_0  OMRON_PID_SLAVE_START
#define E_PID_MB_M_QUEUE_PID_1  OMRON_PID_SLAVE_START + 1
#define E_PID_MB_M_QUEUE_LENGTH NB_OMRON_PIDS

class OmronState
{
public:
  enum FLAGS
  {
    DIRTY = 1,
    UPDATING = 2,
    UPDATED = 3
  };
  short statusHigh;
  short statusLow;
  short pv;
  short sp;
  short flags;
  short slaveID;
  short idx;
  short lastDT;

  millis_t lastUpdated;
  millis_t lastWritten;

  short state;
  ShiftRegister<uchar, E_PID_MB_QUEUE_LENGTH> queue;

  OmronState() : statusHigh(-1),
                 statusLow(-1),
                 pv(-1),
                 sp(-1),
                 flags(DIRTY),
                 lastDT(0),
                 lastUpdated(millis()),
                 lastWritten(millis()),
                 queue({E_PID_MB_QUEUE_STATUS,
                        /*E_PID_MB_QUEUE_USER_1,
                        E_PID_MB_QUEUE_USER_2*/
                        })
  {
  }

  bool isRunning()
  {
    return !OR_E5_STATUS_BIT(statusHigh, statusLow, OR_E5_STATUS_1::OR_E5_S1_RunStop);
  }
  bool isHeating()
  {
    return OR_E5_STATUS_BIT(statusHigh, statusLow, OR_E5_STATUS_1::OR_E5_S1_Control_OutputOpenOutput);
  }
  bool isCooling()
  {
    return OR_E5_STATUS_BIT(statusHigh, statusLow, OR_E5_STATUS_1::OR_E5_S1_Control_OutputCloseOutput);
  }

  bool isAutoTuning()
  {
    return OR_E5_STATUS_BIT(statusHigh, statusLow, OR_E5_STATUS_1::OR_E5_S1_ATExcecute);
  }

  void print()
  {
    Serial.print("PID - ");
    Serial.print(idx);
    Serial.print(" : Slave Addr : ");
    Serial.print(slaveID);
    Serial.print(" | PV : ");
    Serial.print(pv);
    Serial.print(" | SP : ");
    Serial.print(sp);
    Serial.print(" | LastUpdate : ");
    Serial.print(millis() - lastUpdated);
    Serial.print(" | DT : ");
    Serial.print(lastDT);
    Serial.print(" | Flags : ");
    Serial.print(flags, HEX);
    Serial.print(" | Status Low : ");
    Serial.print(statusLow);
    Serial.print(" | Status High : ");
    Serial.print(statusHigh);
    Serial.print("\n");
  }
};

// Addon to deal with multiple Omron PID controllers
class OmronPID : public Component, public ModbusValue<int[]>
{
public:
  OmronPID(ModbusBridge *_bridge,
           short _slaveStart) : Component("OmronPID", COMPONENT_KEY_PID_0, Component::COMPONENT_DEFAULT, NULL),
                                modbus(_bridge),
                                slaveStart(_slaveStart),
                                queue({E_PID_MB_M_QUEUE_PID_0,
                                       E_PID_MB_M_QUEUE_PID_1}),
                                ModbusValue<int[]>(_slaveStart)
  {
    // setFlag(OBJECT_RUN_FLAGS::E_OF_DEBUG);
    SBI(nFlags, OBJECT_NET_CAPS::E_NCAPS_MODBUS);
    d0TS = startTS = millis();
    lastError = E_OK;
    readInterval = OMRON_PID_UPDATE_INTERVAL;
    setFunctionCode(MB_FC::MB_FC_READ_REGISTERS);
    setAddress(MB_REGISTER_OFFSET_TC);
    setNumberAddresses(MB_REGISTER_OFFSET_TC_RANGE * NB_OMRON_PIDS);
    setRegisterMode(MB_REGISTER_MODE::E_MB_REGISTER_MODE_READ);
  }

  virtual short loop();
  virtual short setup();

  short debug();
  short info();

  short onRegisterMethods(Bridge *bridge);

  // PID access
  OmronState *nextToUpdate();
  OmronState *nextToUpdate2();
  
  // Modbus callbacks
  short responseFn(short error);

  short onResponse(short error);
  short queryResponse(short error);
  short onError(short error);
  short onRawResponse(short size, uint8_t rxBuffer[]);

  short modbusLoop();

  // PID programming
  void stopAll();
  void runAll();
  void setAllSP(int sp);

  bool isHeatingUp();
  bool isRunning();
  // StatusLight statusLight;

  ///////////////////////////////////////////
  // Modbus

  Vector<Query> queries;
  void testPIDs();
  void printStates();

private:
  // actual PID states
  OmronState states[NB_OMRON_PIDS];
  ModbusBridge *modbus;

  short slaveStart;  
  short lastError;

  // Modbus query / commands
  int eachPID(short fn, int addr, int value);
  int eachPIDW(int addr, int value);
  int singlePID(int slave, short fn, int addr, int value);
  int singlePIDW(int slave, int addr, int value);
  OmronState *pidBySlave(int slave);
  short read10_16(int slaveAddress, int addr, int prio);
  void updateState();
  
  void updateTCP();
  short fromTCP();
  void print();
  void resetStates();

  millis_t readInterval;
  millis_t startTS;
  millis_t d0TS; 
  

  ShiftRegister<uchar, E_PID_MB_M_QUEUE_LENGTH> queue;

protected:
  // initialize PID states
  void initPIDS();
  // for debugging and testing
};

#endif