/*
 * TODO:
 * bestehende steuerung aus espcontroller übernehmen und testen
 * topics und handler implementieren
 */

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

#define LEDPIN 8 //PB0 = D8
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
#define PIN_BUTTON A3 //A3 = PC3,  defining PCx as pin doesnt work
#define PIN_ENCA A2 //A2 = PC2
#define PIN_ENCB A1 //A1 = PC1
#define BUTTON_RELEASE_DEBOUNCE 10 //minimum time after button release to reenable triggering

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

#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=512; //set value
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_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
#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

//error
uint8_t error=0;
#define NOERROR 0
#define MOTORDIDNOTTURN 1

void setup() {
  pinMode(PIN_BUTTON,INPUT_PULLUP);

  pinMode(PIN_POT,INPUT);

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

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

  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 (Ethernet.begin(mac) == 0) { // setup ethernet communication using DHCP
    //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
    mqttClient.setClient(ethClient);
    mqttClient.setServer("10.0.0.1", 1883);
    Serial.println("MQTT client configured");
    mqttClient.setCallback(callback);
    
  
    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()) {
      reconnect();
    }
    mqttClient.loop();
  }

  

  //Serial Input ##############################################
  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
  
  if (!digitalRead(PIN_BUTTON)){ //button pressed
    if (button_released){
      button_released=false; //flag: not released
      if(loopmillis-last_button_released > BUTTON_RELEASE_DEBOUNCE){
        button_flag=true;
      }
    }
  }else if(!button_flag && !button_released){ //button released and flag has been cleared
    last_button_released=loopmillis;
    button_released=true;
  }

  //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){ //error 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();
  }
  
  

  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(" vel=");
    Serial.print(motor_vel);
    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 pin, boolean state){
  if (state==true){
    srbits |= 1UL << pin; //set bit
  }else{
    srbits &= ~(1UL << pin); //clear bit
  }
  digitalWrite(SRLATCH, LOW);
  shiftOut(SRDATA, SRCLOCK, MSBFIRST, srbits>>8);
  shiftOut(SRDATA, SRCLOCK, MSBFIRST, srbits);
  digitalWrite(SRLATCH, HIGH);
}
boolean srRead(uint8_t pin){
  return (srbits >> pin) & 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);
}