/*
  * 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.
  * Relais switching (selection and muting)
  * MQTT topics
*/

#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 sendData();

#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=false;

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

#define CLIENT_ID       "Mixer"
EthernetClient ethClient;
PubSubClient mqttClient;

#define PUBLISH_DELAY 10000
long last_send=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;

boolean button_flag=false; //true if button pressed
boolean button_released=true;
long last_button_released=0; //last time button has been released (for debounce)

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

#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 5 //maximum allowed error. stop when reached this zone
#define POT_MIN 10 //minimum value pot can reach
#define POT_MAX 1010 //maximum value pot can reach
#define POTIFILTER 0.8 //0 to 1. 1 means old value stays forever

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=1.0;
float motorI=0.1;
float potidifference_integral=0;
#define MOTORI_ANTIWINDUP 32 //maximum value for (potidifference_integral*motorI). time depends on INTERVAL_MOTORCHECK

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("Starting");

  leds.begin();
  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("Setting up ethernet connection via DHCP");
    if (Ethernet.begin(mac) == 0) { // setup ethernet communication using DHCP
      useethernet=false;
      //Unable to configure Ethernet using DHCP
      Serial.println("Unable to configure Ethernet using DHCP");
      delay(200);
      //for (;;);

      
    }else{
      useethernet=true;
      Serial.println("Ethernet configured via DHCP");
      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("Configuring MQTT client");
      mqttClient.setClient(ethClient);
      mqttClient.setServer("10.0.0.1", 1883);
      Serial.println("MQTT client configured");
      mqttClient.setCallback(callback);
    }
  }else{
    Serial.println("Ethernet disabled");
  }


  poti_set=analogRead(PIN_POT);
  Serial.print("Poti value="); Serial.println(poti_set);

  Serial.println("Ready");
  last_send = millis();
  


}

void reconnect() {
  // Loop until reconnected
  while (!mqttClient.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (mqttClient.connect(CLIENT_ID)) {
      Serial.println("connected");
      mqttClient.publish("audiomixer/ip", ip.c_str()); //Publish own ip
      mqttClient.subscribe("audiomixer/main/volume/set"); //subscribe to /set, republish without /set
    } else {
      Serial.print("failed, rc=");
      Serial.print(mqttClient.state());
      Serial.println(" try again in 5 seconds");
      delay(5000);// Wait 5 seconds before retrying
    }
  }
}

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

  if (useethernet){
    if (!mqttClient.connected()) {
      Serial.println("Reconnecting to mqtt");
      reconnect();
    }
    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;
  }


  //Motor Movement Routine #################
  /*
  if (error==0){ //no errors
      
    if (!poti_reachedposition && abs(poti_read-poti_set)>DEADZONE_POTI){ //difference too high
      if (poti_read-poti_set < 0){
        MOTOR_LEFT();
      }else{
        MOTOR_RIGHT();
      }
    }else if(!poti_reachedposition){ //position reached but flag not set
      MOTOR_STOP();
      Serial.print("reached:");
      Serial.print(" set=");
      Serial.print(poti_set);
      Serial.print(" is=");
      Serial.print(poti_read);
      Serial.print(" vel=");
      Serial.println();
      poti_reachedposition=true; //position reached
    }
  
    
    if ( loopmillis > last_motorcheck+INTERVAL_MOTORCHECK){
      last_motorcheck=loopmillis;
  
      motor_vel=(poti_read-poti_read_last)*1000 /INTERVAL_MOTORCHECK ; //calculate current motor velocity
      poti_read_last=poti_read;
  
      //motor fail check
      if (MOTOR_TURNING() && abs(motor_vel)<MINIMUM_MOTORVEL){ //motor is turning too slow
        if (last_motorTooSlow==0){ //first time slow motor recognized
          last_motorTooSlow=loopmillis; 
        }else if (loopmillis-last_motorTooSlow > MOTOR_FAILTIME){
          error=MOTORDIDNOTTURN;
          Serial.println("MOTORDIDNOTTURN");
        }
        
      }else if (last_motorTooSlow>0){ //was recognized too slow but is now turning fast again
        last_motorTooSlow=0; //reset
      }
      
    }
  }else{ //an error occured. error!=0
    MOTOR_STOP();
  }
  */

  //Test
  /*if (poti_set == 512){
    MOTOR_STOP();
  }else if (poti_set < 512){
    motorspeed=(512-poti_set)/2;
    MOTOR_LEFT_PWM();
  }else if (poti_set > 512){
    motorspeed=(poti_set-512)/2;
    MOTOR_RIGHT_PWM();
  }*/

  

  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;
        _motormove=potidifference*motorP+potidifference_integral;
        motorspeed=constrain(abs(_motormove), 0,255);
        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(" motorspeed=");
    Serial.print(motorspeed);
    Serial.print(" iValue=");
    Serial.print(potidifference_integral);
    Serial.println("");

    if (button_flag){ //TODO: remove hier if correct behaviour implemented
      Serial.println("BUTTON Pressed");
      button_flag=false; //clear flag to reenable button triggering.
    }

    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();

    
    
  }

  
}

void sendData() {
  char msgBuffer[20];
  float h = 50;
  float testvalue = 42;
  /*
  Serial.print("testvalue: ");
  Serial.print(testvalue);
  Serial.println();
  
  if (mqttClient.connect(CLIENT_ID)) {
    mqttClient.publish("audiomixer/messwert/parameter", dtostrf(testvalue, 6, 2, msgBuffer));
    //mqttClient.publish(DEVICENAME+"/br/nb/deur", (statusBD == HIGH) ? "OPEN" : "CLOSED");
  }*/
}

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

 
  //if (strncmp((const char*)payload, "ON", 2) == 0) {
  //}
  
  if (strncmp((const char*)topic, "audiomixer/main/volume/set",sizeof(topic)) == 0) {
    //Serial.println("republish");
    
    String s = String((char*)payload);
    Serial.print("Stringreceived=");
    Serial.println(s);
    float _floatvalue = s.toFloat();
    Serial.print("setvalue=");
    Serial.println(_floatvalue);
    
    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
    mqttClient.publish("audiomixer/main/volume", payload, length );
  }
  
  
}



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
  }
  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);
}