#include <Arduino.h>
#include <Homie.h>

/*
 * Wemos d1 mini
 * Flash Size: 4M (1M SPIFFS)
 */

//Upload config: platformio run --target uploadfs

#include <TM1637Display.h>

#define TM1637_CLK D5
#define TM1637_DIO D6

TM1637Display display(TM1637_CLK, TM1637_DIO);

const uint8_t SEG_DONE[] = {
	SEG_B | SEG_C | SEG_D | SEG_E | SEG_G,           // d
	SEG_A | SEG_B | SEG_C | SEG_D | SEG_E | SEG_F,   // O
	SEG_C | SEG_E | SEG_G,                           // n
	SEG_A | SEG_D | SEG_E | SEG_F | SEG_G            // E
	};

const uint8_t SEG_POINT[] = { SEG_DP};

uint8_t display_data[] = { 0xff, 0xff, 0xff, 0xff };
uint8_t display_blank[] = { 0x00, 0x00, 0x00, 0x00 };

unsigned long last_displayupdate=0;
#define DISPLAYUPDATEINTERVAL 100 //maximum time to update display
#define DISPLAYUPDATEINTERVAL_MIN 10 //minimum display update time
bool update_display=true;


#include "HX711.h"
//#define SCALE_CALIBRATION 23805 //calibrated with 2.25kg weight. devide adc reading by calibration weight (in kg) to get this value (or other way around)
#define SCALE_CALIBRATION 23960 //8 club mate 0.5L bottles weight 7.097kg.   scale returns units=340090 with 16 bottles -> 340090/(2*7.097kg) = 23960

// HX711 circuit wiring
const int LOADCELL_DOUT_PIN = D2;
const int LOADCELL_SCK_PIN = D3;

const int PIN_SELFENABLE = D1;

HX711 scale;

float weight_current=0; //last weight reading

#define MEASURE_INTERVAL 100 //ms
#define READING_FILTER_SIZE 40 //latency is about READING_FILTER_SIZE/2*MEASURE_INTERVAL
float weight_read[READING_FILTER_SIZE] = {0};
uint8_t weight_read_pos=0;
#define MEANVALUECOUNT 4 //0<= meanvaluecount < READING_FILTER_SIZE/2

float weight_tare=0; //minimal filtered weight
#define MIN_WEIGHT_DIFFERENCE 50 //minimum weight
float weight_max=0; //max filtered weight

bool weight_sent=false;
unsigned long weight_sent_time=0;

#define MAXONTIME 60000*2 //turn off after ms

#define MQTT_SENDINTERVALL 500 //ms
unsigned long last_mqtt_send=0;
bool livesend=false; //if true, sends continuous data over mqtt

#define FW_NAME "scale"
#define FW_VERSION "0.0.1"

void loopHandler();

HomieNode scaleNode("weight", "Scale", "scale"); //paramters: topic, $name, $type

int sort_desc(const void *cmp1, const void *cmp2);
float getFilteredWeight();
float getWeightSpread();
void sendWeight(float w);
bool cmdHandler(const HomieRange& range, const String& value);
void powerOff();
void displayNumber(float numberdisplay);


void setup() {
  pinMode(PIN_SELFENABLE,OUTPUT);
  digitalWrite(PIN_SELFENABLE, HIGH);
  pinMode(LED_BUILTIN,OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);

  Serial.begin(115200);


  display.setBrightness(7, true); //brightness 0 to 7
  

  Homie.disableResetTrigger(); //disable config reset if pin 1 (D3) is low on startup


  Homie_setFirmware(FW_NAME, FW_VERSION);
  Homie_setBrand(FW_NAME);
  Homie.setLoopFunction(loopHandler);

  scaleNode.advertise("cmd").settable(cmdHandler);  //function inputHandler gets called on new message on topic/input/set  
  scaleNode.advertise("human");
  scaleNode.advertise("spread");
  scaleNode.advertise("raw");
  scaleNode.advertise("max");

  

  Homie.setup();


  scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);


  //calibration
  Serial.println("setup");
  
  scale.set_scale(SCALE_CALIBRATION);
  delay(500);
  scale.tare();
  delay(2000);
  Serial.println("tared. measuring...");
  //after this taring put known weight on scale and get value from scale.get_units(10). then devide this value by the weight and use this number for set_scale(NUMBER)

}

void loop() {
  Homie.loop();  
}


void loopHandler() {
  unsigned long loopmillis=millis();
 
  static unsigned long last_measure=0;
  if (loopmillis>last_measure+MEASURE_INTERVAL) {
    last_measure=loopmillis;

    //Serial.print("reading=");
    weight_current=0;
    if (scale.wait_ready_timeout(1000)) { //for non blocking mode      
      weight_read_pos++;
      weight_read_pos%=READING_FILTER_SIZE;
      weight_current=scale.get_units(1);
      weight_read[weight_read_pos]=weight_current; //one reading takes 91ms
    } else {
        Serial.println("HX711 not found.");
        scaleNode.setProperty("cmd").send("HX711 not found"); //can be done in main loop
    }
      
    float weight_filtered=getFilteredWeight();
    float spread=getWeightSpread();

    //Serial.println(weight_current);
    //Serial.print("spread="); Serial.println(spread,3);

    
    

    #define MAXSPREAD_TARE 0.1 //in kg, for tare can be lower than for measuring
    if (spread<MAXSPREAD_TARE) { //if reading is stable
      if (weight_filtered<weight_tare) { //new min
        weight_tare=weight_filtered; 
        update_display=true;
        Serial.print("new tare="); Serial.println(weight_tare,3);
      }
    }
    #define MAXSPREAD_MEASURE 0.5 //in kg
    if (spread<MAXSPREAD_MEASURE) { //if reading is stable
      if (weight_filtered>weight_max) { //new max
        weight_max=weight_filtered; 
        update_display=true;
        Serial.print("new max="); Serial.println(weight_max,3);
      }
    }


    if (!weight_sent) {
      if (weight_max-weight_tare>MIN_WEIGHT_DIFFERENCE) {
        sendWeight(weight_max-weight_tare);
        update_display=true;
      }
    }
  }



  if (livesend && (millis()>last_mqtt_send+MQTT_SENDINTERVALL)) {
    last_mqtt_send=millis();

    //float weight_filtered=getFilteredWeight();
    float spread=getWeightSpread();

    char charBuf[10];
    dtostrf(weight_current,4, 3, charBuf);
    scaleNode.setProperty("raw").send(charBuf);

    dtostrf(spread,4, 3, charBuf);
    scaleNode.setProperty("spread").send(charBuf);

    dtostrf(weight_max-weight_tare,4, 3, charBuf);
    scaleNode.setProperty("max").send(charBuf); //filtered and auto tared

  }

  #define STAYONTIME_AFTER_SENT 5000
  if (millis() > MAXONTIME || (weight_sent && millis()>weight_sent_time+STAYONTIME_AFTER_SENT)) {
    powerOff();
  }

  if ( (loopmillis > last_displayupdate + DISPLAYUPDATEINTERVAL) | (update_display && (loopmillis > last_displayupdate + DISPLAYUPDATEINTERVAL_MIN) )) {
    last_displayupdate=loopmillis;
    update_display=false; //reset flag
    displayNumber(weight_current);    
  }

  /*
  scale.power_down();			        // put the ADC in sleep mode
  delay(5000);
  scale.power_up();*/
  
}

int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
{
  float a = *((float *)cmp1);
  float b = *((float *)cmp2);
  return a > b ? -1 : (a < b ? 1 : 0);
}

float getFilteredWeight() {
  float copied_values[READING_FILTER_SIZE];
  for(int i=0;i<READING_FILTER_SIZE;i++) {
    copied_values[i] = weight_read[i]; //TODO: maybe some value filtering/selection here
  }
  float copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
  qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);

  float mean=copied_values[READING_FILTER_SIZE/2];
  for (uint8_t i=1; i<=MEANVALUECOUNT;i++) {
    mean+=copied_values[READING_FILTER_SIZE/2-i]+copied_values[READING_FILTER_SIZE/2+i]; //add two values around center
  }
  mean/=(1+MEANVALUECOUNT*2);

  return mean;
}

float getWeightSpread() { //absolute difference between lowest and highest value in buffer
  float copied_values[READING_FILTER_SIZE];
  for(int i=0;i<READING_FILTER_SIZE;i++) {
    copied_values[i] = weight_read[i]; //TODO: maybe some value filtering/selection here
  }
  float copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
  qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);

  float diff=copied_values[0]-copied_values[READING_FILTER_SIZE-1];

  if (diff<0) { //abs for float
    diff*=-1;
  }
  return diff;
}



void sendWeight(float w) {
  char charBuf[10];
  dtostrf(w,4, 3, charBuf);
  scaleNode.setProperty("human").send(charBuf);
  weight_sent=true;
  weight_sent_time=millis();
  Serial.print("Weight sent="); Serial.println(w,3);
}


bool cmdHandler(const HomieRange& range, const String& value) {
  if (range.isRange) {
    return false; //if range is given but index is not in allowed range
  }
  Homie.getLogger() << "cmd" << ": " << value << endl;
  
  
  //boolean value
  if (value=="reset") { //tares and resets calibration value. needed to prepare for calibration
    if (scale.wait_ready_timeout(1000)) { //for non blocking mode
      scale.set_scale();
      scale.tare();
      scaleNode.setProperty("cmd").send("tared");
    } else {
      Serial.println("HX711 not found.");
      scaleNode.setProperty("cmd").send("HX711 not found");
    }
    
  }else if (value=="calibrate") { //get raw value. use "reset" first. then adc to get value for calibration to enter in SCALE_CALIBRATION
    if (scale.wait_ready_timeout(1000)) { //for non blocking mode
      long _units=scale.get_units(10); //if set_scale was called with no parameter before, get_units has not decimals
      char charBuf[13];
      dtostrf(_units,2, 1, charBuf);
      scaleNode.setProperty("cmd").send(charBuf);
    } else {
      Serial.println("HX711 not found.");
      scaleNode.setProperty("cmd").send("HX711 not found");
    }
  }else if (value=="live") {
    livesend=!livesend;
    if (livesend) {
      scaleNode.setProperty("cmd").send("live data enabled");
    }else{
      scaleNode.setProperty("cmd").send("live data disabled");
    }
  }else if (value=="off") {
    powerOff();    
    scaleNode.setProperty("cmd").send("shutting down");
  } else {
    Homie.getLogger() << "cmd not recognized" << endl;
    return false;  
  }

  return true;
}

void powerOff() {
  Serial.println("Turning Off");
  Serial.flush();
  delay(100);
  digitalWrite(PIN_SELFENABLE, LOW);
}


void displayNumber(float numberdisplay) {
  uint8_t displayresolution=3; //how many digits after dot
    bool _negative=false;
    if (numberdisplay<0) {
      numberdisplay*=-1;
      _negative=true;
    }
    if ((numberdisplay>999.9) | (displayresolution==0)) {
      display.showNumberDec((int)(numberdisplay+0.5), false); //just diplay number
    }else{
      uint8_t d1=0;
      uint8_t d2=0;
      uint8_t d3=0;
      uint8_t d4=0;
      if(numberdisplay<10 && displayresolution>=3) { // 5.241,   0.005 etc.
        int _number=(int)(numberdisplay*1000+0.5); //in 1000th kg rounded
        d1=_number%10;
        d2=(_number/10)%10;
        d3=(_number/100)%10;
        d4=(_number/1000)%10;
        display_data[3] = display.encodeDigit(d1); //rightmost digit
        display_data[2] = display.encodeDigit(d2);
        display_data[1] = display.encodeDigit(d3);
        display_data[0] = display.encodeDigit(d4); //leftmost digit
        display_data[0] |= SEG_DP; //add decimal point after left most digit
      }else if(numberdisplay<100 && displayresolution>=2) { //10.24,  99.20
        int _number=(int)(numberdisplay*100+0.5); //in 100th kg rounded
        d1=_number%10;
        d2=(_number/10)%10;
        d3=(_number/100)%10;
        d4=(_number/1000)%10;
        display_data[3] = display.encodeDigit(d1); //rightmost digit
        display_data[2] = display.encodeDigit(d2);
        display_data[1] = display.encodeDigit(d3);
        display_data[1] |= SEG_DP; //add decimal point after second digit from the left
        display_data[0] = display.encodeDigit(d4); //leftmost digit
        if (d4==0) { //number smaller than 1000
          display_data[0]={0}; //turn off left segment
        }
      }else if (numberdisplay<1000 && displayresolution>=1) //100.0,  999.9
      {        
        int _number=(int)(numberdisplay*10+0.5); //in 10th kg rounded
        d1=_number%10;
        d2=(_number/10)%10;
        d3=(_number/100)%10;
        d4=(_number/1000)%10;
        display_data[3] = display.encodeDigit(d1); //rightmost digit
        display_data[2] = display.encodeDigit(d2);
        display_data[2] |= SEG_DP; //add decimal point after second digit from the right
        display_data[1] = display.encodeDigit(d3);
        display_data[0] = display.encodeDigit(d4); //leftmost digit
        if (d4==0) { //number smaller than 1000
          display_data[0]={0}; //turn off left segment
          if (d3==0) { //number smaller than 100
            display_data[1]={0}; //turn off 2nd from left segment
          }
        }
      }

      if (_negative) { //show negative number by using rightmost dot
        display_data[3] |= SEG_DP;
      }

      Serial.println();
      display.setSegments(display_data);
      
    }
}