// #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_BASE_TOPIC "/espmega/ProR3"
// #define MQTT_USE_AUTH
#ifdef MQTT_USE_AUTH
const char MQTT_USERNAME[] = "username";
const char MQTT_PASSWORD[] = "password";
#endif

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

// 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 pwm_state_callback(String topic, String message);
void pwm_value_callback(String topic, String message);
void state_request_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);

void publish_input_states();
void publish_input_state(int id);
void publish_input_state(int id, int state);

char PWM_STATE_TOPIC[75] = MQTT_BASE_TOPIC "/pwm/00/state";
char PWM_VALUE_TOPIC[75] = MQTT_BASE_TOPIC "/pwm/00/value";
char INPUTS_TOPIC[75] = MQTT_BASE_TOPIC "/input/00";

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();
  io_begin();
  network_begin();
  Serial.println("Initializing MQTT . . .");
  mqtt_connect();
  thread_initialization();
  Serial.println("Initialization Completed.");
  Serial.println("Jumping to User Code.");
}

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);
  for(int i=0;i<PWM_COUNT;i++) {
    pwm_values[i] = 4095;
  }
}

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");
      publish_pwm_states();
      publish_input_states();
    }
    else
    {
      Serial.print("MQTT not connected, continuing in standalone mode\n");
    }
  }
}

void mqtt_subscribe()
{
  char PWM_SET_STATE_TOPIC[75] = MQTT_BASE_TOPIC "/pwm/00/set/state";
  char PWM_SET_VALUE_TOPIC[75] = MQTT_BASE_TOPIC "/pwm/00/set/value";
  char STATE_REQUEST_TOPIC[75] = MQTT_BASE_TOPIC "/requeststate";
  for (int i = 0; i < PWM_COUNT; i++)
  {
    PWM_SET_VALUE_TOPIC[sizeof(MQTT_BASE_TOPIC) + 4] = ((i - i % 10) / 10) + '0';
    PWM_SET_VALUE_TOPIC[sizeof(MQTT_BASE_TOPIC) + 5] = (i % 10) + '0';
    PWM_SET_STATE_TOPIC[sizeof(MQTT_BASE_TOPIC) + 4] = ((i - i % 10) / 10) + '0';
    PWM_SET_STATE_TOPIC[sizeof(MQTT_BASE_TOPIC) + 5] = (i % 10) + '0';
    mqtt.subscribe(PWM_SET_STATE_TOPIC, pwm_state_callback);
    mqtt.subscribe(PWM_SET_VALUE_TOPIC, pwm_value_callback);
  }
  mqtt.subscribe(STATE_REQUEST_TOPIC, state_request_callback);
}

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

void pwm_state_callback(String topic, String message)
{
  Serial.println("Recieved PWM State Message");
  Serial.println(topic);
  Serial.println(message);
  int a = topic.charAt(sizeof(MQTT_BASE_TOPIC)+4) - '0';
  int b = topic.charAt(sizeof(MQTT_BASE_TOPIC)+5) - '0';
  int id = 10 * a + b;
  if (message.compareTo("on")==0)
  {
    Serial.printf("Setting pwm %d to on\n", id);
    pwm_set_state(id, true);
  }
  else if (message.compareTo("off")==0)
  {
    Serial.printf("Setting pwm %d to off\n", id);
    pwm_set_state(id, false);
  }
}

void pwm_value_callback(String topic, String message)
{
  Serial.println("Recieved PWM Value Message");
  Serial.println(topic);
  Serial.println(message);
  int a = topic.charAt(sizeof(MQTT_BASE_TOPIC)+4) - '0';
  int b = topic.charAt(sizeof(MQTT_BASE_TOPIC)+5) - '0';
  int id = 10 * a + b;
  int value = message.toInt();
  pwm_set_value(id, value);
}

void virtual_interrupt_callback(int pin, int state)
{
  publish_input_state(pin,state);
  Serial.printf("Pin %d changed to %d\n", pin, state);
  if(pin==15) {
    pwm_toggle(4);
  }
}

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];
  int value = pwm_values[id];
  PWM_STATE_TOPIC[sizeof(MQTT_BASE_TOPIC) + 4] = ((id - id % 10) / 10) + '0';
  PWM_STATE_TOPIC[sizeof(MQTT_BASE_TOPIC) + 5] = (id % 10) + '0';
  if (state == 1)
  {
    mqtt_client.publish(PWM_STATE_TOPIC, "on");
  }
  else if (state == 0)
  {
    mqtt_client.publish(PWM_STATE_TOPIC, "off");
  }
  mqtt.publish(String(PWM_VALUE_TOPIC), String(value));
}

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

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

void pwm_toggle(int id)
{
  int state = !pwm_states[id];
  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];
  int value = pwm_values[id];
  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]);
}

void publish_input_states()
{
  for (int i = 0; i < VINT_COUNT; i++)
  {
    publish_input_state(i);
  }
}
void publish_input_state(int id)
{
  int state = ESPMega_digitalRead(virtual_interrupt_pins[id]);
  publish_input_state(id, state);
}

void publish_input_state(int id, int state)
{
  char INPUTS_TOPIC[75] = MQTT_BASE_TOPIC "/input/00";
  INPUTS_TOPIC[sizeof(MQTT_BASE_TOPIC) + 6] = ((id - id % 10) / 10) + '0';
  INPUTS_TOPIC[sizeof(MQTT_BASE_TOPIC) + 7] = (id % 10) + '0';
  mqtt.publish(String(INPUTS_TOPIC), state ? "1" : "0");
}

void state_request_callback(String topic, String message)
{
  publish_input_states();
  publish_pwm_states();
}