/*
  * Ideas/TODO:
  * POT_MIN, POT_MAX as variable with calibration procedure. Drive slowly to both ends until value does not get lower.
  * Motor error checking. Timeout overall (if regulation fails or stuck). Timeout movement (motor is tunring but no change in poti value detected). Move right direction.
  * Hardware: motorentstörkondensatoren einbauen direkt an motor. 47nF + zu - und zwei 10nF + zu case und - zu case
  * PI Optimieren. aktuell overshoot
  * Implement knob menu structure
*/

#include <Arduino.h>

#include <Adafruit_NeoPixel.h>
#ifdef __AVR__
  #include <avr/power.h>
#endif

void reconnect();
uint32_t Wheel(byte WheelPos);
boolean srRead(uint8_t pbit);
void srWrite(uint8_t pbit, boolean state);
void callback(char* topic, byte* payload, unsigned int length);
void srShiftOut();
void setMuteInt(uint8_t i);
void setSelectionInt(uint8_t i);
boolean getSelection(uint8_t pbit);
boolean getMute(uint8_t pbit);
void setSelectionChannel(uint8_t i, boolean state);
void setMuteChannel(uint8_t i, boolean state);
void publishCurrentSetVolume();
void publishAllStates(int pn, String pTopicname, boolean (*pgetBit) (uint8_t));
void changeRelaisByNumber(uint8_t pn, String pTopicPrefix, String pTopic, String pspayload, void (*psetXChannel) (uint8_t, boolean));
float getSetVolume();

#define LEDPIN 9  //PB1 = D9 = Pin15
Adafruit_NeoPixel leds = Adafruit_NeoPixel(9, LEDPIN, NEO_GRB + NEO_KHZ800);

uint8_t wheelpos=0;
#include "Ethernet.h"
#include "PubSubClient.h"

boolean useethernet=true;

//Ethernet and MQTT
String ip = "";
uint8_t mac[6] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x06};

#define CLIENT_ID       "Mixer"
EthernetClient ethClient;
PubSubClient mqttClient;
String mqttdevname="audiomixer/";

bool flag_publishCurrentSetVolume=false;


long last_send=0;
#define MAXIMUMMQTTSENDINTERVAL 100
#define MQTTRECONNECTDELAY 5000
unsigned long last_mqttreconnectattempt=0;




//Serial
long last_serialdebug=0;
#define INTERVAL_SERIALDEBUG 200



//Inputs
#include "button.h"

#define PIN_BUTTON A3 //A3 = PC3,  defining PCx as pin doesnt work
#define PIN_ENCA A2 //A2 = PC2
#define PIN_ENCB A1 //A1 = PC1

Button button_knob;

//Shift Register 595
//connections: https://www.arduino.cc/en/tutorial/ShiftOut
#define SRLATCH PD4 //D4 = PD4
#define SRCLOCK PD3 //D3 = PD3
#define SRDATA PD2 //D2 = PD2
uint16_t srbits=0;

#define NUMSELECTCHANNELS 8
#define NUMMUTECHANNELS 8


#include <Encoder.h>
Encoder volEnc(PIN_ENCA,PIN_ENCB);
float encoderMultiplier=4.0;
int volEncVel=0;

//Servo stuff
#define PIN_MOTOR_IN1 PD5 //to L293(pin2) Motor IN1 
#define PIN_MOTOR_IN2 PD6 //to L293(pin7) Motor IN2
//#define SRPIN_MOTOR_IN1 1 //L293(pin2) Motor IN1   -- moved to atmega pin
//#define SRPIN_MOTOR_IN2 2 //L293(pin7) Motor IN2   -- moved to atmega pin
uint8_t motorspeed=0;
int _motormove;

#define PIN_POT A0 //A0 = PC0, reference potentiometer wiper
#define DEADZONE_POTI 10 //maximum allowed error. stop when reached this zone
#define POT_MIN 45 //minimum value pot can reach
#define POT_MAX 950 //maximum value pot can reach
#define POTIFILTER 0.8 //0 to 1. 1 means old value stays forever
#define MAX_MOTOR_PWM 192 //0 to 255. Maximum pwm to output

int poti_set; //set value, initial value will be read from poti
int poti_read=0; //read value from poti
boolean poti_reachedposition=true; //set to true if position reached. after that stop turning

int last_potidifference=0;
int potidifference=0; //gets overwritten at start of each motorcheck

//#define MOTOR_STOP(); srWrite(SRPIN_MOTOR_IN1,LOW); srWrite(SRPIN_MOTOR_IN2,LOW);
//#define MOTOR_LEFT(); srWrite(SRPIN_MOTOR_IN1,LOW); srWrite(SRPIN_MOTOR_IN2,HIGH);
//#define MOTOR_RIGHT(); srWrite(SRPIN_MOTOR_IN1,HIGH); srWrite(SRPIN_MOTOR_IN2,LOW);
//#define MOTOR_TURNING() (srRead(SRPIN_MOTOR_IN1) != srRead(SRPIN_MOTOR_IN2))
#define MOTOR_STOP(); digitalWrite(PIN_MOTOR_IN1,LOW); digitalWrite(PIN_MOTOR_IN2,LOW);
#define MOTOR_LEFT(); digitalWrite(PIN_MOTOR_IN1,LOW); digitalWrite(PIN_MOTOR_IN2,HIGH);
#define MOTOR_RIGHT(); digitalWrite(PIN_MOTOR_IN1,HIGH); digitalWrite(PIN_MOTOR_IN2,LOW);
#define MOTOR_LEFT_PWM(); digitalWrite(PIN_MOTOR_IN1,LOW); analogWrite(PIN_MOTOR_IN2,motorspeed);
#define MOTOR_RIGHT_PWM(); analogWrite(PIN_MOTOR_IN1,motorspeed); digitalWrite(PIN_MOTOR_IN2,LOW);
#define MOTOR_TURNING() (digitalRead(PIN_MOTOR_IN1) != digitalRead(PIN_MOTOR_IN2))


//Motorcheck
long last_motorcheck=0;
#define INTERVAL_MOTORCHECK 50 //check motor movement every x ms
//int poti_read_last=0;
//int motor_vel=0; //analog read units per second //TODO: reintroduce into code or remove
//#define MINIMUM_MOTORVEL 20 //minimum velocity motor should turn wenn active
//#define MOTOR_FAILTIME 500 //in ms. if motor did not turn fox x amount of time at least with MINIMUM_MOTORVEL an error will initiate
//long last_motorTooSlow=0; //typically 0

float motorP=2.0;
float motorI=0.05;
float motorD=1;
float potidifference_integral=0;
#define MOTORI_ANTIWINDUP 100 //maximum value for (potidifference_integral*motorI). time depends on INTERVAL_MOTORCHECK
//Motor starts moving at about speed=80

long last_potidifferenceLow=0;
#define DEADZONETIMEUNTILREACHED 250 //time [ms] poti read value has to be inside of deadzone to set reachedposition flag (and stop regulating)


//Menu system
uint8_t menu_mode=0; //0= volume set mode, 1=mute output selection, 2=output group selection
uint8_t menu_selectedChannel=0;
#define MENU_MAXCHANNEL 7

long last_ledupdate=0;
#define INTERVAL_LEDUPDATE 50

void setup() {
  pinMode(PIN_BUTTON,INPUT_PULLUP);
  button_knob = Button();

  pinMode(PIN_POT,INPUT);
  

  pinMode(SRLATCH, OUTPUT);
  pinMode(SRCLOCK, OUTPUT);
  pinMode(SRDATA, OUTPUT);

  Serial.begin(9600);
  while (!Serial) {};
  Serial.println("Boot");

  leds.begin();
  leds.clear();
  for(uint8_t i=0;i<leds.numPixels();i++){ //set color of all leds
    leds.setPixelColor(i, leds.Color(100,100,100));
  }

  leds.show();

  
  if (useethernet)
  {
    Serial.println("trying DHCP");
    if (Ethernet.begin(mac) == 0) { // setup ethernet communication using DHCP
      useethernet=false;
      //Unable to configure Ethernet using DHCP
      Serial.println("DHCP Err");
      delay(200);
      //for (;;);

      
    }else{
      useethernet=true;
      Serial.print("IP address: ");
      Serial.println(Ethernet.localIP());
      Serial.println();
    
      ip = String (Ethernet.localIP()[0]);
      ip = ip + ".";
      ip = ip + String (Ethernet.localIP()[1]);
      ip = ip + ".";
      ip = ip + String (Ethernet.localIP()[2]);
      ip = ip + ".";
      ip = ip + String (Ethernet.localIP()[3]);
      //Serial.println(ip);
    
      // setup mqtt client
      Serial.println("Conf MQTT");
      mqttClient.setClient(ethClient);
      mqttClient.setServer("10.0.0.1", 1883);
      Serial.println("MQTT ok");
      mqttClient.setCallback(callback);
    }
  }else{
    Serial.println("Eth disabled");
  }


  poti_set=analogRead(PIN_POT);

  #ifdef DEBUG
  Serial.println("Ready");
  #endif
  
  last_send = millis();
  
}

void reconnect() {
  // Loop until reconnected
  if (!mqttClient.connected()) {
    Serial.print("Connect MQTT ..");
    // Attempt to connect
    if (mqttClient.connect(CLIENT_ID)) {
      Serial.println("connected");
      publishCurrentSetVolume();
      mqttClient.publish("audiomixer/ip", ip.c_str()); //Publish own ip
      mqttClient.subscribe("audiomixer/volume/set"); //subscribe to /set, republish without /set
      mqttClient.subscribe("audiomixer/mute/set"); //without range
      for (uint8_t i=0;i<NUMMUTECHANNELS;i++) { //with range
        String sub_topic="audiomixer/mute_"+String(i)+"/set";
        mqttClient.subscribe((char*) sub_topic.c_str());
      }
      mqttClient.subscribe("audiomixer/select/set"); //without range
      for (uint8_t i=0;i<NUMSELECTCHANNELS;i++) { //with range
        String sub_topic="audiomixer/select_"+String(i)+"/set";
        mqttClient.subscribe((char*) sub_topic.c_str());
      }      
      
    } else {
      Serial.print("failed, rc=");
      Serial.print(mqttClient.state());
    }
  }
}

void loop() {
  long loopmillis=millis();

  if (useethernet){
    if (!mqttClient.connected()) {
      if (loopmillis-last_mqttreconnectattempt > MQTTRECONNECTDELAY) {
        Serial.println("Recon. mqtt");
        reconnect();
        last_mqttreconnectattempt=loopmillis;
      }
    }
    mqttClient.loop();
  }

  

  //Serial Input ##############################################
  /*For debugging
  while (Serial.available() > 0) {
    int _value = Serial.parseInt();
    if (Serial.read() == '\n') {
      Serial.print("value=");
      Serial.println(_value);
      //poti_set=_value;
      //poti_reachedposition=false; //aim for new position
      srWrite(_value,!srRead(_value));
     
    }
  }
  */

  //Inputs ###################################################
  poti_read=poti_read*POTIFILTER + (1.0-POTIFILTER)*analogRead(PIN_POT); //read poti
  
  
  button_knob.update(millis(),!digitalRead(PIN_BUTTON)); //Update routine

  

  
  if (button_knob.buttonPressed()){ //short press
    switch(menu_mode) {
      case 0: //volume
        //TODO: implement someting here, muting maybe?
        break;

      case 1: //mute
        if (menu_selectedChannel<=MENU_MAXCHANNEL) { //inside valid range
          //setMuteChannel(menu_selectedChannel,!getMute(menu_selectedChannel)); // mute/unmute menu_selectedChannel
          changeRelaisByNumber(NUMMUTECHANNELS,"audiomixer/mute_", ""+menu_selectedChannel, ""+!getMute(menu_selectedChannel), &setMuteChannel); //toggle
        }else{ //nothing selected
          menu_mode = 0; //return to volume mode
        }
        break;

      case 2: //group selection
        if (menu_selectedChannel<=MENU_MAXCHANNEL) { //inside valid range
          //setSelectionChannel(menu_selectedChannel,!getSelection(menu_selectedChannel)); // toggle selection menu_selectedChannel
          changeRelaisByNumber(NUMSELECTCHANNELS,"audiomixer/select_", ""+menu_selectedChannel, ""+!getSelection(menu_selectedChannel), &setSelectionChannel); //toggle
        }else{ //nothing selected
          menu_mode = 0; //return to volume mode
        }
        break;
    }
  }else if(button_knob.buttonHold()){ //long press
    switch(menu_mode) {
      case 0: //volume
        menu_mode = 1; //change to mute select mode
        break;

      case 1: //mute
        menu_mode = 2; //change to output group select mode. (hold button a second time)
        break;

      case 2: //group selection
        menu_mode = 1; //change back to mute select mode
        break;
    }
    
  }
    
  


  //Read Encoder to velocity "volEncVel"
  
  int _volEnc=volEnc.read();
  if (_volEnc!=0){ //encoder moved
    volEncVel+=_volEnc;
    volEnc.write(0); //reset value
  }
  

  //Input Handling
  if (volEncVel!=0){ //knob moved
    switch(menu_mode) {
      case 0: //volume
        poti_set+=volEncVel*encoderMultiplier; //change poti set value
        poti_set=constrain(poti_set, POT_MIN,POT_MAX);
        poti_reachedposition=false;
        flag_publishCurrentSetVolume=true;
        volEncVel=0; //reset vel for next loop
        break;

      case 1: case 2: //mute or group selection 
        menu_selectedChannel+=127; //offset to compensate negative values
        menu_selectedChannel+=volEncVel/2; //every encoder detend is +-2
        if (menu_selectedChannel<127){
          menu_selectedChannel=127; //lower limit (0)
        }else if (menu_selectedChannel > 127+MENU_MAXCHANNEL+1) { //max channel and one extra for "nothing selected"
          menu_selectedChannel=127+MENU_MAXCHANNEL+1; //upper limit
        }
        menu_selectedChannel-=127; //take out offset

        if (volEncVel/2 != 0) { //if value change was at least one detend
          volEncVel=0; //reset vel for next loop
        }
        
        break;
    }
    
  }


  if (flag_publishCurrentSetVolume && loopmillis-last_send>MAXIMUMMQTTSENDINTERVAL){
    flag_publishCurrentSetVolume=false;
    last_send = loopmillis;
    publishCurrentSetVolume();
  }


  //Motor Movement Routine ################# 

  if (loopmillis-last_motorcheck>INTERVAL_MOTORCHECK) 
  {
    last_motorcheck=loopmillis;

    last_potidifference = potidifference; //save last difference
    potidifference=poti_set-poti_read; //positive means poti needs to be moved higher. max poti value is 1023
    
    
    
    if (poti_reachedposition) {
      motorspeed=0;
      potidifference_integral=0;
      MOTOR_STOP();
    }else{ //not reached position
      _motormove=0; //negative: move left, positive: move right. abs value: speed. 0 <= abs(_motormove) <= 255

      potidifference_integral+=potidifference*motorI;
      potidifference_integral=constrain(potidifference_integral,-MOTORI_ANTIWINDUP,MOTORI_ANTIWINDUP); //constrain
      _motormove=potidifference*motorP + potidifference_integral + motorD*(last_potidifference-potidifference);
      motorspeed=constrain(abs(_motormove), 0,MAX_MOTOR_PWM);
      
      if (poti_read<=POT_MIN && _motormove<0) { //stop motor if soft endstops reached and wants to turn that way
        MOTOR_STOP();
        potidifference_integral=0;
        _motormove=0;
      }else if (poti_read>=POT_MAX && _motormove>0){  //stop motor if soft endstops reached and wants to turn that way
        MOTOR_STOP();
        potidifference_integral=0;
        _motormove=0;
      }else{ //no endstop reached
        if (_motormove<0) {
          MOTOR_LEFT_PWM();
        }else if (_motormove>0) {
          MOTOR_RIGHT_PWM();
        }else{
          MOTOR_STOP();
        }
      }

      if ( (potidifference>0 && last_potidifference<0) || (potidifference<0 && last_potidifference>0) ) { //different signs. potidifference has crossed 0. set value overshoot
        potidifference_integral=0; //reset integral to stop further overshoot
      }


      /*
      Serial.print(" diff=");
      Serial.print(potidifference);
      Serial.print(" iVal=");
      Serial.print(potidifference_integral);
      Serial.print(" dVal=");
      Serial.print((last_potidifference-potidifference)*motorD);
      Serial.print(" motormove=");
      Serial.print(_motormove);
      if (poti_reachedposition) {
        Serial.print("!");
      }
      Serial.println("");
      */
      

      
      

      if (abs(potidifference)<DEADZONE_POTI) {
        if (last_potidifferenceLow==0){
          last_potidifferenceLow=loopmillis; //save millis first time entered deadzone
        }
        if (loopmillis-last_potidifferenceLow>DEADZONETIMEUNTILREACHED) {
          poti_reachedposition=true;
        }
        
      }else{
        last_potidifferenceLow = 0;
      }

    }
  }
  

  

  if ( loopmillis > last_ledupdate+INTERVAL_LEDUPDATE){
    last_ledupdate=loopmillis;

    switch(menu_mode) {
      case 0: //volume
        if (poti_reachedposition) { //idle
          for(uint8_t i=0;i<leds.numPixels();i++){ //set color of all leds
            leds.setPixelColor(i, Wheel(wheelpos+i*10));
          }
        }else{ //motor is currently moving
          leds.clear();
          leds.setPixelColor(min(getSetVolume()*8/100, 7), Wheel(wheelpos)); //show volume as bargraph
        }

        wheelpos+=1;
        break;

      case 1: case 2: //group selection //mute
        //show selected channel
        leds.clear();
        uint8_t _r=0;
        uint8_t _g=0;
        uint8_t _b=0;

        for(uint8_t i=0;i<=MENU_MAXCHANNEL;i++){ //set color of all channel leds
          if (menu_mode==1) { //in mute mode
            if (getMute(i)) { //not muted
              _r = 0; _g = 50; _b = 0;
            }else{ //muted
              _r = 50; _g = 0; _b = 50;
            }            
          }else if(menu_mode==2) { //in selection mode
            if (!getSelection(i)) { //Selected A
              _r = 50; _g = 50; _b = 50;
            }else{ //Selected B
              _r = 50; _g = 0; _b = 0;
            }
          }
          if (menu_selectedChannel==i) { //this item is currently selected
            _r*=4; //make color brighter
            _g*=4;
            _b*=4;
          }
          if (menu_selectedChannel==MENU_MAXCHANNEL+1) { //nothing selected
            _r*=4; //make color brighter
            _g*=4;
            _b*=4;
          }
          leds.setPixelColor(i, leds.Color(_r,_g,_b));
        }
        

        break;
    }

    leds.show();
  }
  

  /*
  if ( loopmillis > last_serialdebug+INTERVAL_SERIALDEBUG){
    last_serialdebug=loopmillis;

    Serial.print(" set=");
    Serial.print(poti_set);
    Serial.print(" is=");
    Serial.print(poti_read);
    //Serial.print(" mspeed=");
    //Serial.print(motorspeed);
    Serial.print(" motormove=");
    Serial.print(_motormove);
    Serial.print(" iVal=");
    Serial.print(potidifference_integral);
    if (poti_reachedposition) {
      Serial.print("!");
    }
    Serial.println("");
  }
  */

  if (loopmillis%5001==0) { //TODO: remove when working
    Serial.println(loopmillis); //alive print. for debugging
  }

  
}


void callback(char* topic, byte* payload, unsigned int length) {
  payload[length] = '\0'; //add end of string character
  String spayload = String((char*)payload);
  Serial.print("Message arrived:");
  Serial.print(topic);
  for (unsigned int i = 0; i < length; i++) {
    Serial.print((char)payload[i]);
  }
  Serial.println();
  



  if (String(topic).equals("audiomixer/volume/set")){
    
    float _floatvalue = spayload.toFloat();
    _floatvalue=constrain(_floatvalue,0.0,100.0);
    
    poti_set=constrain(map(_floatvalue,0.0,100.0,POT_MIN,POT_MAX),POT_MIN,POT_MAX); //set new poti position
    poti_reachedposition=false; //aim for new position

    publishCurrentSetVolume();

  }else if (String(topic).equals("audiomixer/mute/set")) { //withouth range
    //Serial.print("Mute string="); Serial.println(spayload);
    uint16_t ipayload=spayload.toInt();
    if (spayload.equalsIgnoreCase("false")) {
      setMuteInt(0); //all unmuted
    }else if (spayload.equalsIgnoreCase("true")) {
      setMuteInt(pow(2,NUMMUTECHANNELS)-1); //all muted
    }else if (ipayload>=0 && ipayload<((uint16_t)1<<NUMMUTECHANNELS)){ //in range
      setMuteInt(ipayload); //set bits directly
    }
    
    publishAllStates(NUMMUTECHANNELS,"mute_", &getMute);
    
  }else if (String(topic).equals("audiomixer/select/set")) { //withouth range
    //Serial.print("Select string="); Serial.println(spayload);
    uint16_t ipayload=spayload.toInt();
    if (spayload.equalsIgnoreCase("false")) {
      setSelectionInt(0); //all select to NC
    }else if (spayload.equalsIgnoreCase("true")) {
      setSelectionInt(pow(2,NUMSELECTCHANNELS)-1); //all select to NO
    }else if (ipayload>=0 && ipayload<((uint16_t)1<<NUMSELECTCHANNELS)){ //in range
      setSelectionInt(ipayload); //set bits directly
    }

    publishAllStates(NUMSELECTCHANNELS,"select_", &getSelection);
    
  }else if (String(topic).startsWith("audiomixer/mute_")) { //with range
    changeRelaisByNumber(NUMMUTECHANNELS,"audiomixer/mute_", topic, spayload, &setMuteChannel);

  }else if (String(topic).startsWith("audiomixer/select_")) {
    changeRelaisByNumber(NUMSELECTCHANNELS,"audiomixer/select_", topic, spayload, &setSelectionChannel);
  }

  
  
}



void srWrite(uint8_t pbit, boolean state){ //change bit to state
  if (state==true){
    srbits |= 1UL << pbit; //set bit
  }else{
    srbits &= ~(1UL << pbit); //clear bit
  }
  srShiftOut();
}
void srShiftOut(){
  digitalWrite(SRLATCH, LOW);
  shiftOut(SRDATA, SRCLOCK, MSBFIRST, srbits>>8);
  shiftOut(SRDATA, SRCLOCK, MSBFIRST, srbits);
  digitalWrite(SRLATCH, HIGH);
}
boolean srRead(uint8_t pbit){ //get state at bit
  return (srbits >> pbit) & 1U;
}


uint32_t Wheel(byte WheelPos) {
  WheelPos = 255 - WheelPos;
  if(WheelPos < 85) {
    return leds.Color(255 - WheelPos * 3, 0, WheelPos * 3);
  }
  if(WheelPos < 170) {
    WheelPos -= 85;
    return leds.Color(0, WheelPos * 3, 255 - WheelPos * 3);
  }
  WheelPos -= 170;
  return leds.Color(WheelPos * 3, 255 - WheelPos * 3, 0);
}

void setMuteInt(uint8_t i) {
  uint16_t mask=(( (uint16_t)1<<(NUMMUTECHANNELS))-1 )<<NUMSELECTCHANNELS;
  srbits = ((i<<NUMSELECTCHANNELS) & mask) | (srbits & ~mask);
  srShiftOut();
}
boolean getMute(uint8_t pbit) { //low is muted
  return srbits & (1<<(pbit+NUMSELECTCHANNELS)); //check bit at position
}
void setSelectionInt(uint8_t i) {
  uint16_t mask=(( (uint16_t)1<<(NUMMUTECHANNELS))-1 )<<NUMSELECTCHANNELS;
  srbits = (srbits & mask) | (i & ~mask);
  srShiftOut();
}
boolean getSelection(uint8_t pbit) {
  return srbits & (1<<pbit); //check bit at position
}


void setSelectionChannel(uint8_t i, boolean state){
  if (i<NUMSELECTCHANNELS) {
    srWrite(i, state);
  }
}
void setMuteChannel(uint8_t i, boolean state){
  if (i<NUMMUTECHANNELS) {
    srWrite(i+NUMSELECTCHANNELS, state); //offset. selection is first shift register
  }
}

void publishCurrentSetVolume()
{
  float _setpercentage=getSetVolume();
  char pub_payload[8]; // Buffer big enough for 7-character float
  dtostrf(_setpercentage, 1, 2, pub_payload);
  mqttClient.publish("audiomixer/volume", pub_payload);
  Serial.print("pub="); Serial.println(_setpercentage);
}

void publishAllStates(int pn, String pTopicname, boolean (*pgetBit) (uint8_t)){
  for (uint8_t i=0;i<pn;i++) {
    String pub_topic = mqttdevname+pTopicname+String(i);
    boolean _state=pgetBit(i);
    if (_state) {
      mqttClient.publish((char*) pub_topic.c_str(), "true");
    }else{
      mqttClient.publish((char*) pub_topic.c_str(), "false");
    }
  }
}

void changeRelaisByNumber(uint8_t pn, String pTopicPrefix, String pTopic, String pspayload, void (*psetXChannel) (uint8_t, boolean)) 
{
  uint8_t _index=255;
  for (uint8_t i=0; i<pn && _index==255; i++) {
    if (String(pTopic).equals(pTopicPrefix+String(i)+"/set")) {
      _index=i;
    }
  }
  if (_index==255) { 
    Serial.println("Index OOR");
  }else{ //index ok
    String pub_topic = pTopicPrefix+String(_index);
    if (pspayload.equalsIgnoreCase("false")) {
      psetXChannel(_index,false);
      mqttClient.publish((char*) pub_topic.c_str(), "false");
    }else if (pspayload.equalsIgnoreCase("true")) {
      psetXChannel(_index,true);
      mqttClient.publish((char*) pub_topic.c_str(), "true");
    }
  }
}

float getSetVolume() {
  return map(poti_set,POT_MIN,POT_MAX, 0.0,100.0); //get percentage from set poti value
}