// #define USE_INTERRUPT
#include <ESPMegaPRO.h>
#include <ETH.h>
#include <PubSubClient.h>
#include <PubSubClientTools.h>
#include <Thread.h>
#include <StaticThreadController.h>

// Network Connectivity
#define HOSTNAME "espmega-pro-r3"
const IPAddress IP(192, 168, 0, 210);
const IPAddress SUBNET(255, 255, 255, 0);
const IPAddress GATEWAY(192, 168, 0, 1);
const IPAddress DNS(10, 192, 1, 1);
const IPAddress MQTT_SERVER(192, 168, 0, 26);
const int MQTT_PORT = 1883;
// #define MQTT_USE_AUTH
#ifdef MQTT_USE_AUTH
const char MQTT_USERNAME[] = "username";
const char MQTT_PASSWORD[] = "password";
#endif

// Inputs
const int DEBOUNCE_TIME_MS = 50;
const int virtual_interrupt_pins[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
int virtual_interupt_state[16];
unsigned long virtual_interupt_timer[16];

// Outputs
#define PWM_COUNT 16
const int pwm_pins[PWM_COUNT] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
int pwm_states[PWM_COUNT];
int pwm_values[PWM_COUNT];
const float pwm_linear_scaling_m[PWM_COUNT] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
const float pwm_linear_scaling_c[PWM_COUNT] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
#define PWM_CYCLE_VALUES_COUNT 3
const int PWM_CYCLE_VALUES[PWM_CYCLE_VALUES_COUNT] = {50, 125, 255};

// Forward declaration
void virtual_interrupt_loop();
void virtual_interrupt_callback(int pin, int state);
void network_begin();
void mqtt_connect();
void mqtt_subscribe();
void thread_initialization();
void mqtt_callback(String topic, String message);
void io_begin();

void publish_pwm_states();
void publish_pwm_state(int id);
void pwm_set_state(int id, int state);
void pwm_set_value(int id, int value);
void pwm_toggle(int id);
void pwm_toggle(int id1, int id2);
void pwm_cycle_value(int id);
boolean pwm_group_state(int id1, int id2);

WiFiClient eth;
PubSubClient mqtt_client(MQTT_SERVER, 1883, eth);
PubSubClientTools mqtt(mqtt_client);

Thread mqtt_reconnector = Thread();
StaticThreadController<1> thread_controller(&mqtt_reconnector);

void setup()
{
  Serial.begin(115200);
  Serial.println("ESPMega R3 Initializing . . .");
  ESPMega_begin();
  network_begin();
  Serial.println("Initializing MQTT . . .");
  mqtt_connect();
  thread_initialization();
}

void loop()
{
  virtual_interrupt_loop();
  mqtt_client.loop();
  ESPMega_loop();
  thread_controller.run();
}

void io_begin()
{
  Serial.println("Initializing I/O . . .");
  memset(pwm_states, 0, PWM_COUNT);
  memset(pwm_values, 255, PWM_COUNT);
}

void network_begin()
{
  Serial.print("Initializing Network ");
  ETH.begin();
  ETH.setHostname(HOSTNAME);
  ETH.config(IP, GATEWAY, SUBNET, DNS, DNS);
  for (int i = 0; i < 3; i++)
  {
    delay(2500);
    Serial.print(" .");
  }
  Serial.println();
}

void mqtt_connect()
{
  if (!mqtt_client.connected())
  {
    Serial.print("MQTT not connected, connecting . . .\n");
#ifdef MQTT_USE_AUTH
    mqtt_client.connect(HOSTNAME, MQTT_USERNAME, MQTT_PASSWORD);
#else
    mqtt_client.connect(HOSTNAME);
#endif
    if (mqtt_client.connected())
    {
      mqtt_subscribe();
      Serial.print("MQTT connected\n");
    }
    else
    {
      Serial.print("MQTT not connected, continuing in standalone mode\n");
    }
  }
}

void mqtt_subscribe()
{
  mqtt.subscribe("/espmega", mqtt_callback);
}

void thread_initialization()
{
  Serial.println("Initializing Threads . . .");
  Serial.println("Initializing MQTT Thread . . .");
  mqtt_reconnector.onRun(mqtt_connect);
  mqtt_reconnector.setInterval(30000);
}

void mqtt_callback(String topic, String message)
{
}

void virtual_interrupt_callback(int pin, int state)
{
  Serial.printf("Pin %d changed to %d\n", pin, state);
}

void virtual_interrupt_loop()
{
  for (int i = 0; i < 16; i++)
  {
    int current_pin_value = ESPMega_digitalRead(virtual_interrupt_pins[i]);
    if (virtual_interupt_state[i] != current_pin_value)
    {
      if (millis() - virtual_interupt_timer[i] > DEBOUNCE_TIME_MS)
      {
        virtual_interupt_state[i] = current_pin_value;
        virtual_interrupt_callback(i, current_pin_value);
      }
    }
    else
    {
      virtual_interupt_timer[i] = millis();
    }
    yield();
  }
}

void publish_pwm_states()
{
  for (int i = 0; i < PWM_COUNT; i++)
  {
    publish_pwm_state(i);
  }
}

void publish_pwm_state(int id)
{
  int state = pwm_states[id - 1];
  int value = pwm_values[id - 1];
  String topic = "/iotplc/pwm";
  topic += (id);
  topic += "/state";
  if (state == 1)
  {
    mqtt_client.publish((char *)topic.c_str(), "on");
  }
  else if (state == 0)
  {
    mqtt_client.publish((char *)topic.c_str(), "off");
  }
  topic = "/iotplc/pwm";
  topic += (id);
  topic += "/value";
  mqtt_client.publish((char *)topic.c_str(), (char *)String(value).c_str());
}

void pwm_set_state(int id, int state)
{
  if (state != pwm_states[id - 1])
  {
    pwm_states[id - 1] = state;
    int pwm_value = pwm_values[id - 1];
    analogWrite(pwm_pins[id - 1], state * (int)(pwm_linear_scaling_m[id - 1] * pwm_value + pwm_linear_scaling_c[id - 1]));
    publish_pwm_state(id);
  }
}

void pwm_set_value(int id, int value)
{
  pwm_values[id - 1] = value;
  int pwm_state = pwm_states[id - 1];
  analogWrite(pwm_pins[id - 1], pwm_state * (int)(pwm_linear_scaling_m[id - 1] * value + pwm_linear_scaling_c[id - 1]));
  publish_pwm_state(id);
}

void pwm_toggle(int id)
{
  int state = !pwm_states[id - 1];
  pwm_set_state(id, state);
}

void pwm_toggle(int id1, int id2)
{
  boolean state = pwm_group_state(id1, id2);
  if (state)
  {
    pwm_set_state(id1, 0);
    pwm_set_state(id2, 0);
  }
  else
  {
    pwm_set_state(id1, 1);
    pwm_set_state(id2, 1);
  }
}

boolean pwm_group_state(int id1, int id2)
{
  int state1 = pwm_states[id1 - 1], state2 = pwm_states[id2 - 1];
  if (state1 || state2)
    return true;
  return false;
}

void pwm_cycle_value(int id)
{
  int state = pwm_states[id - 1];
  int value = pwm_values[id - 1];
  if (state == 1)
    for (int i = 0; i < PWM_CYCLE_VALUES_COUNT; i++)
    {
      if (PWM_CYCLE_VALUES[i] == value)
      {
        if (i > 0)
        {
          pwm_set_value(id, PWM_CYCLE_VALUES[i - 1]);
          return;
        }
        else
        {
          pwm_set_state(id, 0);
          return;
        }
      }
    }
  pwm_set_state(id, 1);
  pwm_set_value(id, PWM_CYCLE_VALUES[PWM_CYCLE_VALUES_COUNT - 1]);
}