/*
  * 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));

#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/";


long last_send=0;
#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;

//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;

#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

//#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 100 //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.1;
float potidifference_integral=0;
#define MOTORI_ANTIWINDUP 90 //maximum value for (potidifference_integral*motorI). time depends on INTERVAL_MOTORCHECK
//Motor starts moving at about speed=80

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

//error
boolean motorerror=false;

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()){
    Serial.println("Button Pressed");
  }else if(button_knob.buttonHold()){
    Serial.println("Button hold");
  }
    
  





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

  //Input Handling
  if (volEncVel!=0){ //knob moved
    poti_set+=volEncVel*encoderMultiplier; //change poti set value
    poti_set=constrain(poti_set, POT_MIN,POT_MAX);
    poti_reachedposition=false;
    publishCurrentSetVolume();
  }


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

  if (!motorerror) //motor not stuck etc
  {
    if (loopmillis-last_motorcheck>INTERVAL_MOTORCHECK) 
    {
      last_motorcheck=loopmillis;
      int 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
        
        int _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;
        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 (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_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(" iVal=");
    Serial.print(potidifference_integral);
    if (poti_reachedposition) {
      Serial.print("!");
    }
    Serial.println("");

    for(uint8_t i=0;i<leds.numPixels();i++){ //set color of all leds
      leds.setPixelColor(i, Wheel(wheelpos+i*10));
    }
    wheelpos+=5;
  
    leds.show();
  }

  if (loopmillis%5001==0) {
    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) {
  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=map(poti_set,POT_MIN,POT_MAX, 0.0,100.0); //get percentage from set poti value
  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");
    }
  }
}