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

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

//Upload config: platformio run --target uploadfs

#define PIN_VBAT A0
unsigned long vbat_calib1_adc=581; //adc value at vbat_calib_voltage (voltage at battery)
float vbat_calib1_voltage=6.0; //voltage used for calibration

unsigned long vbat_calib2_adc=782; //second calibration point (higher voltage)
float vbat_calib2_voltage=8.0;

int vbat_raw_filtered=-1; //-1 flags it as initialized
bool vbatSent=false;

bool weight_updated=false;

unsigned long last_weightchange=10000; //give some headstart if startup takes a bit longer

#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 };
uint8_t display_custom[] = { 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;

uint8_t displaybrightness = 7; //0 to 7

unsigned long last_new_display_custom=0;
unsigned long display_custom_duration=1000*3;

unsigned long last_display_blink=0; //for turning off shortly
unsigned long display_blink_duration=100;

#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
float weight_filtered=0;
float spread=0;

#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 5 //0<= meanvaluecount < READING_FILTER_SIZE/2. how many values will be used from sorted weight array from the center region. abour double this values reading are used

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
HomieNode displayNode("display", "Display", "scale"); //paramters: topic, $name, $type
HomieNode hardwareNode("hardware", "Hardware", "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);
bool displayNodeHandler(const HomieRange& range, const String& value);
void powerOff();
void displayNumber(float numberdisplay);
float getVBat();
void updateVBat();
void sendVBat();
float mapFloat(float x, float in_min, float in_max, float out_min, float out_max);

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

  Serial.begin(115200);
  Serial.println("Hello");


  display.setBrightness(displaybrightness, 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);
  Serial.println("setLoopFunction");
  Homie.setLoopFunction(loopHandler);
  

  
  scaleNode.advertise("human");
  scaleNode.advertise("spread");
  scaleNode.advertise("raw");
  scaleNode.advertise("max");

  displayNode.advertise("segments").settable(displayNodeHandler);  

  hardwareNode.advertise("cmd").settable(cmdHandler);  //function inputHandler gets called on new message on topic/input/set  
  hardwareNode.advertise("vbat");
  hardwareNode.advertise("vbatraw");

  Serial.println("homie setup");

  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() {
  unsigned long loopmillis=millis();
 
  static unsigned long last_measure=0;
  if (loopmillis>last_measure+MEASURE_INTERVAL) {
    last_measure=loopmillis;

    updateVBat(); //also update vbat reading

    //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.");
        hardwareNode.setProperty("cmd").send("HX711 not found"); //can be done in main loop
    }
      
    weight_filtered=getFilteredWeight();
    spread=getWeightSpread();

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

    #define MAXSPREAD 0.2 //in kg

    if (spread<MAXSPREAD) { //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);
        last_display_blink=loopmillis; //blink
      }

      if (weight_filtered>weight_max) { //new max
        weight_max=weight_filtered; 
        update_display=true;
        Serial.print("new max="); Serial.println(weight_max,3);
        last_display_blink=loopmillis; //blink
      }
    }else{
      last_weightchange=loopmillis;
    }

    
    weight_updated=true;

    

  }

  
  #define STAYONTIME_IDLE 20000 //how long to stay on (ms) after no change in weight detected

  //#define STAYONTIME_AFTER_SENT 5000
  //if (millis() > MAXONTIME || (weight_sent && millis()>weight_sent_time+STAYONTIME_AFTER_SENT) || millis()>last_weightchange+STAYONTIME_IDLE) {
  if (millis() > MAXONTIME || millis()>last_weightchange+STAYONTIME_IDLE) {
    powerOff();
  }
  
  if ( (loopmillis > last_displayupdate + DISPLAYUPDATEINTERVAL) | (update_display && (loopmillis > last_displayupdate + DISPLAYUPDATEINTERVAL_MIN) )) {
    last_displayupdate=loopmillis;
    update_display=false; //reset flag
    if ((loopmillis >= last_new_display_custom) && (loopmillis < last_new_display_custom+display_custom_duration)) { //custom display message
      display.setBrightness(displaybrightness,true); //enable display
      display.setSegments(display_custom);
    }else{ //normale weight display
      displayNumber(weight_filtered-weight_tare);
      display.setSegments(display_data);

      if ((loopmillis >= last_display_blink) && (loopmillis < last_display_blink+display_blink_duration)) {
        display.setBrightness(2,true); //dimmed
      }else{
        display.setBrightness(displaybrightness,true);
      }
    }
  }

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

  Homie.loop();  
}


void loopHandler() {
  unsigned long loopmillis=millis();

  if (weight_updated) {
    weight_updated=false;
    


    if (!weight_sent) {
      if (weight_max-weight_tare>MIN_WEIGHT_DIFFERENCE) {
        if (Homie.getMqttClient().connected()) {
          sendWeight(weight_max-weight_tare);
        }else{
          Serial.println("Cannot send weight because mqtt not connected!");
        }
        update_display=true;
      }
    }

    
    if (!vbatSent && loopmillis>10000) { //send voltage some time after turn on
      if (Homie.getMqttClient().connected()) {
        sendVBat();
        vbatSent=true;
      }else{
        Serial.println("Cannot send vbat because mqtt not connected!");
      }
    }
  }

  if (Homie.getMqttClient().connected() && 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
  }
  
}

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();
      hardwareNode.setProperty("cmd").send("tared");
    } else {
      Serial.println("HX711 not found.");
      hardwareNode.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);
      hardwareNode.setProperty("cmd").send(charBuf);
    } else {
      Serial.println("HX711 not found.");
      hardwareNode.setProperty("cmd").send("HX711 not found");
    }
  }else if (value=="live") {
    livesend=!livesend;
    if (livesend) {
      hardwareNode.setProperty("cmd").send("live data enabled");
    }else{
      hardwareNode.setProperty("cmd").send("live data disabled");
    }
  }else if (value=="vbat") {
    sendVBat();
  }else if (value=="off") {
    powerOff();    
    hardwareNode.setProperty("cmd").send("shutting down");
  } else {
    Homie.getLogger() << "cmd not recognized" << endl;
    return false;  
  }

  return true;
}

bool displayNodeHandler(const HomieRange& range, const String& value) {
  if (range.isRange) {
    return false; //if range is given but index is not in allowed range
  }
  Homie.getLogger() << "displayset" << ": " << value << endl;
  
  if (value.length()!=8*4) { //treat as number to show
  
    displayNumber(value.toFloat()); 
    for (uint8_t i=0;i<4;i++) {
      display_custom[i]=display_data[i];
    }
    
  }else{
    uint8_t number = 0;
    for (uint8_t i=0; i< value.length(); i++)  // for every character in the string  strlen(s) returns the length of a char array
    {
      number *= 2; // double the result so far
      if (value[i] == '1'){ 
        number++;  //add 1 if needed
        Serial.print("1("); Serial.print(i); Serial.print(")");
      }
      if ((i+1)%8==0 && i!=0) { //was the last bit
        display_custom[i/8] = number; //use this number
        Serial.print(" ->");Serial.print(i/8); Serial.print("="); Serial.println(number);
        number=0; //reset number for next byte
      }
    }
  }  

  last_new_display_custom=millis(); //save time when message arrived

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

      
      
    }
}

float getVBat() { //get filtered vbat value
  return mapFloat(vbat_raw_filtered,vbat_calib1_adc,vbat_calib2_adc, vbat_calib1_voltage,vbat_calib2_voltage); //2-point mapping  adc -> voltage;
}

void updateVBat() { //continuous update for filtering
  float vbat_filter=0.95; //the closer to 1 the higher the filtering. 0 means no filtering
  if (vbat_raw_filtered<0) {  //at first reading (vbat_raw_filtered is initialized with value <0)
    vbat_filter=0; //use first reading with 100%
  }
  vbat_raw_filtered=vbat_raw_filtered*vbat_filter + analogRead(PIN_VBAT)*(1.0-vbat_filter);
}

void sendVBat() {
  char charBuf[10];
  dtostrf(vbat_raw_filtered,1, 0, charBuf);
  hardwareNode.setProperty("vbatraw").send(charBuf); 
  Serial.print("vbatraw="); Serial.println(vbat_raw_filtered);

  dtostrf(getVBat(),4, 3, charBuf);
  hardwareNode.setProperty("vbat").send(charBuf);   
  Serial.print("vbat="); Serial.println(getVBat(),3);
}

float mapFloat(float x, float in_min, float in_max, float out_min, float out_max)
{
 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}