/* Demo Sw for LED Semaphore board and ESp32 DevKitV1, communication over MQTT
 *  
 *  Version 1.0
 *  
 *  Demo sw controls 3xRGB LEDs (color and intensity)
 *  
 *  Demo sw reads messages 
 *  "setLedColor channel color;" - channel 1-3, color names see code below
 *  "setLedIntensity channel intensity;" - channel 1-3, intensity 0-255 dec
 *  "reset;" - just board reset  
 *  "getSwVersion;" - get sw version
 *  note! - semicolon at the end is mandatory
 *  
 *  In Arduino IDE, set board as "ESP32 Wrover module"
 */

#include <WiFi.h>
#define MQTT_MAX_PACKET_SIZE 2048 //128
#include <PubSubClient.h>

#define LED1R 23
#define LED1G 22
#define LED1B 21

#define LED2R 19
#define LED2G 18
#define LED2B 17

#define LED3R 13
#define LED3G 14
#define LED3B 27

#define LED_BUILD_IN 2

#define    PWM_FREQ   5000

// Replace the next variables with your SSID/Password combination
const char* ssid = "spza1";
const char* password = "spza11++";

// Add your MQTT Broker IP address, example:
const char* mqtt_server = "192.168.0.99";

#define TOPIC_IN "LedSemaphore1/dataIn"
#define TOPIC_OUT "LedSemaphore1/dataOut"

#define SW_VERSION "LedSemaphore V1.0"

WiFiClient espClient;
PubSubClient client(espClient);

typedef struct {
  bool      bChange;
  uint32_t  u32ColorCode;
  uint8_t   u8Intensity;
} led_t;

led_t led[3];

uint32_t u32LedActivity;

bool bReportSwVersion;
bool bForceRestart;

// the setup function runs once when you press reset or power the board
void setup() {

  ledcSetup(0, PWM_FREQ, 8); ledcAttachPin(LED1R, 0);
  ledcSetup(1, PWM_FREQ, 8); ledcAttachPin(LED1G, 1);
  ledcSetup(2, PWM_FREQ, 8); ledcAttachPin(LED1B, 2);
  ledcSetup(3, PWM_FREQ, 8); ledcAttachPin(LED2R, 3);
  ledcSetup(4, PWM_FREQ, 8); ledcAttachPin(LED2G, 4);
  ledcSetup(5, PWM_FREQ, 8); ledcAttachPin(LED2B, 5);
  ledcSetup(6, PWM_FREQ, 8); ledcAttachPin(LED3R, 6);
  ledcSetup(7, PWM_FREQ, 8); ledcAttachPin(LED3G, 7);
  ledcSetup(8, PWM_FREQ, 8); ledcAttachPin(LED3B, 8);

  //force initial light
  bSetLed(1, 0xFFFFFF, 255); //white
  bSetLed(2, 0xFFFFFF, 255); //white
  bSetLed(3, 0xFFFFFF, 255); //white

  for (int i=0; i < 3; i++)
  {
    led[i].bChange = true;
    led[i].u32ColorCode = 0xFFFFFF;
    led[i].u8Intensity = 0xFF; 
  }

  pinMode(LED_BUILD_IN,OUTPUT);

  Serial.begin(115200);
  
  setup_wifi();
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);

  u32LedActivity = 0;

  bReportSwVersion = true;
  bForceRestart = false;
}

// the loop function runs over and over again forever
void loop() {

  if (!client.connected()) {
    digitalWrite(LED_BUILD_IN,HIGH);
    reconnect();
    digitalWrite(LED_BUILD_IN,LOW);
  }
  client.loop();

  if (bReportSwVersion == true)
  {
    bReportSwVersion = false;
    Serial.print("Reporting sw version ");
    Serial.println(SW_VERSION);
    client.publish(TOPIC_OUT,SW_VERSION);
    u32LedActivity = 1000;
  }

  if (bForceRestart == true)
  {
    Serial.println("Restart in 1 sec");
    delay(1000);
    ESP.restart();
  }

  for (int i=0; i < 3; i++)
  {
    if (led[i].bChange == true)
    {
       bSetLed(i+1, led[i].u32ColorCode, led[i].u8Intensity); 
       led[i].bChange = false;
    }
  }
  
  if (u32LedActivity != 0)
  {
    u32LedActivity--;
    digitalWrite(LED_BUILD_IN, HIGH); 
  }
  else
  {
    digitalWrite(LED_BUILD_IN, LOW); 
  }
}

void setup_wifi() {
  delay(10);
  // We start by connecting to a WiFi network
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
}

int checkChannelNr(int channel)
{
int ret = 0;

  if ((channel >= 1) && (channel <= 3))
  {
    ret = channel;
  }

  return ret;
}

uint32_t checkColor(char * in)
{
uint32_t ret;

  //https://www.rapidtables.com/web/color/RGB_Color.html
  if (strstr(in, "black") != NULL ) ret = 0x000000;
  else if (strstr(in, "white") != NULL ) ret = 0xFFFFFF;
  else if (strstr(in, "red") != NULL ) ret = 0xFF0000;
  else if (strstr(in, "lime") != NULL ) ret = 0x00FF00;
  else if (strstr(in, "blue") != NULL ) ret = 0x0000FF;
  else if (strstr(in, "yellow") != NULL ) ret = 0xFFFF00;
  else if (strstr(in, "cyan") != NULL ) ret = 0x00FFFF;
  else if (strstr(in, "magenta") != NULL ) ret = 0xFF00FF;
  else if (strstr(in, "silver") != NULL ) ret = 0xC0C0C0;
  else if (strstr(in, "gray") != NULL ) ret = 0x808080;
  else if (strstr(in, "maroon") != NULL ) ret = 0x800000 ;
  else if (strstr(in, "olive") != NULL ) ret = 0x808000;
  else if (strstr(in, "green") != NULL ) ret = 0x008000;
  else if (strstr(in, "purple") != NULL ) ret = 0x800080;
  else if (strstr(in, "teal") != NULL ) ret = 0x008080;
  else if (strstr(in, "navy") != NULL ) ret = 0x000080;
  else ret = 0xFFFFFFFF;

  return ret;
}

void parseCmd(char * input)
{
//    Serial.println(input);

    if (strstr(input, "setLedColor") != NULL)
    {
      char cmd[200];
      int channel;
      char clr[200];
      uint32_t u32Clr; 

      if (3 == sscanf(input, "%s %d %s", cmd, &channel, clr))
      {
        if (checkChannelNr(channel) != 0)
        {
          u32Clr = checkColor(clr);
          if (u32Clr != 0xFFFFFFFF)
          {
            led[channel-1].u32ColorCode = u32Clr;
            led[channel-1].bChange = true;
            Serial.print("channel "); Serial.print(channel); Serial.print(" Color code "); Serial.println(u32Clr, HEX);
          }
          else
          {
            Serial.println("setLedColor unknown color");
          }
        }
        else
        {
          Serial.println("setLedColor wrong channel number");
        }
      }
      else
      {
        Serial.println("setLedColor wrong format");
      }
      
    }
    else if (strstr(input, "setLedIntensity") != NULL)
    {
      char cmd[200];
      int channel;
      int intensity;
      if (3 == sscanf(input, "%s %d %d", cmd, &channel, &intensity))
      {
        if (checkChannelNr(channel) != 0)
        {
          if ((intensity >= 0) && (intensity <= 255))
          {
            led[channel-1].u8Intensity = intensity;
            led[channel-1].bChange = true;
            Serial.print("channel "); Serial.print(channel); Serial.print(" intensity "); Serial.println(intensity);
          }
        }
        else
        {
          Serial.println("setLedIntensity wrong channel number");
        }
      }
      else
      {
        Serial.println("setLedIntensity wrong format");
      }
    }
    else if (strstr(input, "getSwVersion") != NULL)
    {
      bReportSwVersion = true;
    }
    else if (strstr(input, "reset") != NULL)
    {
      bForceRestart = true;
    }
    else
    {
      Serial.println("Unknown command");
    } 
}

void callback(char* topic, byte* message, unsigned int length) {
  Serial.print("Message arrived on topic: ");
  Serial.print(topic);
  Serial.print(". Message: ");
  String messageTemp;
  
  for (int i = 0; i < length; i++) {    
    Serial.print((char)message[i]);
    messageTemp += (char)message[i];
  }
  Serial.println();

  bool bRepeat = true;
  char * pSemicolon;
  char * pBuff = (char *)message;
  while(bRepeat == true)
  {
      pSemicolon = strchr(pBuff,';');
      if (pSemicolon != NULL)
      {
          *pSemicolon = 0;
          parseCmd(pBuff);
          pBuff = pSemicolon + 1;
      }
      else
      {
          bRepeat = false;
      }
  }

  u32LedActivity = 1000;
}


void reconnect() {  
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (client.connect("Esp32LedSemaphore")) {
      Serial.println("connected");
      // Subscribe
      client.subscribe(TOPIC_IN);
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

bool bSetLed(uint8_t led, uint32_t color, uint8_t intensity)
{
uint8_t red, green, blue;
bool bRet = true;

  red = (uint8_t)(color >> 16);
  green = (uint8_t)(color >> 8);
  blue = (uint8_t)(color >> 0);

  if (led == 1)
  {
    ledcWrite(0, ((red * intensity) >> 8));
    ledcWrite(1, ((green * intensity) >> 8));
    ledcWrite(2, ((blue * intensity) >> 8));
  } 
  else if (led == 2)
  {
    ledcWrite(3, ((red * intensity) >> 8));
    ledcWrite(4, ((green * intensity) >> 8));
    ledcWrite(5, ((blue * intensity) >> 8));
  }
  else if (led == 3)
  {
    ledcWrite(6, ((red * intensity) >> 8));
    ledcWrite(7, ((green * intensity) >> 8));
    ledcWrite(8, ((blue * intensity) >> 8));
  } 
  else 
  {
    bRet = false;
    Serial.println("Unsupported LED number");
  }

  return bRet;
}