#include <Arduino.h>
//use arduino 1.0.5, otherwise display library does not work properly

#include <Servo.h>
#include <HX711.h>
#include <EEPROM.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
#include <ArduinoSort.h>

#define EEPROMVERSION 1

Servo esc;



#define PIN_BACK 8
#define PIN_DOWN 10
#define PIN_UP 11
#define PIN_SET 12

#define PIN_CURRENT A3
#define PIN_VOLTAGE A2

#define PIN_ESC 9 //D9 = PB1, pin 15

#define PIN_RPM 1 //Interrupt 0 = D2 = PD2, pin 4,Interrupt 1 = D3 = PD3, pin 5
uint16_t rotationcounter=0; //counts up on rotation interrupt
uint16_t rpm=0;

#define ESC_MIN 1000
#define ESC_MAX 2000 //2000
uint16_t esc_value=ESC_MIN; //esc_value 
uint8_t maxcurrent=10;
uint8_t minvoltage=105; // *0.1

uint32_t btn_back_tdown=0;
uint32_t btn_down_tdown=0;
uint32_t btn_up_tdown=0;
uint32_t btn_set_tdown=0;

uint32_t btn_back_trelease=0;
uint32_t btn_down_trelease=0;
uint32_t btn_up_trelease=0;
uint32_t btn_set_trelease=0;

uint8_t btn_back_press=0; //0=not press, 1=down, 2=short press (on release), 3=long press(on release)
uint8_t btn_down_press=0;
uint8_t btn_up_press=0;
uint8_t btn_set_press=0;

#define BTN_BOUNCETIME 50
#define BTN_HOLDTIME 1000


//tutorial: set Multiplier to 1, apply known voltage/current, divide applied voltage/current by LCD Reading = multiplier
#define VOLTAGEMULTIPLIER 0.02009 //0.0011792 //5*0.2415/1024
#define CURRENTMULTIPLIER 0.02083 // 

#define PIN_LED 13

#define MAXDOUBLEVALUE 32767

//DEFAULT EEPROM SETTINGS
#define DEFAULT_FPS 5
#define DEFAULT_SCALECALIBRATION 5875 // (72500/12.34) <- rough estimation
#define DEFAULT_ADCMEDIANVALUES 15
#define DEFAULT_SPSADC 10

//        DOUT, SCK
//HX711 scale(A1, A0);		// parameter "gain" is ommited; the default value 128 is used by the library
HX711 scale;

//A+= green, A-=white, E-=Black, E+=Red
LiquidCrystal_I2C lcd(0x27,16,2);  // set the LCD address to 0x27 for a 16 chars and 2 line display
//lcd sda an pin 27 (A4)
//lcd sck an pin 28 (A5)

double scalecalibration=DEFAULT_SCALECALIBRATION; //default  //eeprom

String SerialMessage = "";
String receivedString ="";

char tempstring[16]; //for dtostrf

unsigned long time_lastlcd=0;
unsigned long looptimelcd_margin=0; //if >0 loop took longer than expected
uint8_t fps=DEFAULT_FPS; //EEPROM
unsigned long time_lcdwait=1000/fps;
String lcd0=""; //lcd line 0
String lcd1=""; // "       1
#define LCD_BACKLIGHT_TIME 20000
uint32_t lcd_backlight_offtimer=0;
#define LCD_BACKLIGHT_WEIGHT 2 //if weightchange lower than x, turn backlight off after LCD_BACKLIGHT_TIME ms


String lcd0_buffer="";
String lcd1_buffer="";

unsigned long time_lastadc=0;
unsigned long looptimeadc_margin=0;
uint8_t spsadc=DEFAULT_SPSADC; //eeprom
unsigned long time_adcwait=1000/spsadc;
int adc_readings=1; //ca. 700ms for 5 readings, ca 75ms for 1 reading, readings=1 10times = 747ms

double weight=0;
#define ADCMEDIANVALUES_MAX 91 //needs to be uneven, maximum number, for array declaration
float weightseries[ADCMEDIANVALUES_MAX]; //last n values for median filter
uint16_t adcmedianvalues=DEFAULT_ADCMEDIANVALUES; //needs to be uneven //eeprom
float adcFilterKeepMedianvaluesFactor=0.8; //how many lowest and highest values will be removed from the array before avaraging (as factor). 0.0 means only median is used. 1.0 means all values are used.
uint16_t adcmedianposition=0;

float calibrationWeight=100; //gramms

#define N_CURRENTREADINGS 8
uint16_t currentreadings[N_CURRENTREADINGS];
uint8_t currentreading_pos=0;


//STATES
#define S_SCALEDISPLAY 0
#define S_MENU 1
  uint8_t state_menu=0;
  #define SM_scale 0
  #define SM_thrusttest 1
  #define SM_calibration 2
  #define SM_fps 3
  #define SM_medianfilter 4
  #define SM_spsadc 5
  #define SM_maxcurrent 6
  #define SM_minvoltage 7
  #define SM_thrusttest_steptime 8
  #define SM_save 9
  #define SM_loaddefaults 10
  String menu_entry[]={"Scale","Thrusttest","Calibration","FPS","Medianfilter","ADC Speed","Max A","Min V","Ramptime","Save","Load Defaults"};
  #define MENU_ENTRIES 10
#define S_MENUENTRY 2
#define S_CALIBRATION 3
  uint8_t state_calibration=0; //0=wait (for press), 1=measure, 2=show
    #define DELAY_CALIBRATIONWAIT 1000 //delay after key press for measuring
    #define DELAY_CALIBRATIONSHOW 5000
#define S_THRUSTDISPLAY 10
  uint8_t state_thrusttest=0;
  unsigned long thrusttest_timer=0;
  #define THRUSTTEST_STARTDELAY 5000
  uint8_t thrusttest_steptime=3; //time in ms for one step esc value. 1ms=>1sec from 0%-100%. --> time in seconds for 0-100% //EEPROM
  unsigned long thrusttest_holdmax=1000; //hold maximum value for x ms
  unsigned long thrusttest_waitend=5000;
  float test_maxamps=0;
  float test_maxthrust=0;
  unsigned long testtime=0;
  
uint8_t state=S_SCALEDISPLAY;

uint32_t statetimer=0; //for state delays



void rpm_interrupt();
float StringToFloat(String s);
void EEPROMWriteDouble(int ee, double value);
double EEPROMReadDouble(int ee);
uint16_t EEPROMReadInt(uint8_t paddr);
void EEPROMWriteInt(uint8_t paddr,uint16_t pdata);
void loadDefaults();
void writeEEPROMsettings();
void updateFPS();
void updateSPSADC();
void loadEEPROMsettings();
String toStringBar(double val,double minimum,double maximum);
String toString(double w,uint8_t dec);
String toString(double w);
String toWeightString(double w,uint8_t dec,uint8_t len);
String toWeightString(double w);
float getWeightSeriesMax();
float getWeightSeriesMin();
double getWeightMedian();
double getWeightFiltered();
float getVoltage();
float getCurrent();
String menuentry_string();
void menuentry_set(uint8_t presstype);
void menuentry_back(uint8_t presstype);
void menuentry_up(uint8_t presstype);
void menuentry_down(uint8_t presstype);
void resetBTNFlag(uint8_t *btnpress);
void buttonHandler(uint8_t pin,uint32_t *btntdown,uint32_t *btntrelease,uint8_t *btnpress);
void buttonCheck();
void updateLCD();


void setup() {                
  Serial.begin(115200);
  
  pinMode(PIN_LED, OUTPUT); 
  digitalWrite(PIN_LED, HIGH);

  esc.attach(PIN_ESC,1000,2000);

  
  esc_value=ESC_MIN;
  esc.writeMicroseconds(esc_value);
  
  pinMode(PIN_UP, INPUT); 
  digitalWrite(PIN_UP, HIGH);
  pinMode(PIN_DOWN, INPUT); 
  digitalWrite(PIN_DOWN, HIGH);
  pinMode(PIN_SET, INPUT); 
  digitalWrite(PIN_SET, HIGH);
  pinMode(PIN_BACK, INPUT); 
  digitalWrite(PIN_BACK, HIGH);
  
  attachInterrupt(PIN_RPM, rpm_interrupt, RISING);
  
  lcd.init();
  lcd.backlight();
  lcd.noAutoscroll();
 
  //Serial.println("Multiscale"); 
  lcd.clear();
  lcd.print("Multiscale");
  
  if(!digitalRead(PIN_BACK)) //if back pressed, load defaults
    loadDefaults();
  else
    loadEEPROMsettings();
  
  //Serial.print("calib="); Serial.println(scalecalibration,10); 
  lcd.setCursor(0,1);
  
  dtostrf(scalecalibration,4,5,tempstring);
  lcd.print("cal="+String(tempstring));

  scale.begin(A1, A0);
  
  scale.set_scale(scalecalibration);
  scale.tare();
  
  /*delay(500);
  lcd.clear();
  long t_teststart=millis();
  for (int i=0;i<10;i++)
    scale.get_units(1);
  long t_testend=millis();
  dtostrf((t_testend-t_teststart),10,0,tempstring);
  lcd.print("t="+String(tempstring));
  delay(2000);*/
  
  
  digitalWrite(PIN_LED, LOW);
  lcd.clear();
}


void loop() {
  
  buttonCheck();

  //checkSerial();
  
  
  /*
  if (receivedString.length()>0){
    if (receivedString.equals("tare")){
      double weightTareBefore=scale.get_units(5);
      scale.tare();
      double weightTareAfter=scale.get_units(5);
      Serial.print("Tared. Difference=");
      Serial.println((weightTareAfter-weightTareBefore),4);
    }
    else if (receivedString.substring(0,11).equals("calibration")){
      calibrationWeight=StringToFloat(receivedString.substring(12)+"0");
      
      Serial.print("Calibration, set weight="); Serial.println(calibrationWeight,10);
      
      scale.set_scale();
      scale.tare();
      Serial.println("Reset Scale, place known weight now! ...");
      
      delay(5000);
      
      Serial.println("Do not touch scale!");
      double calibrationWeight_scale=scale.get_units(10);
      Serial.println(calibrationWeight_scale, 100);
      scalecalibration=calibrationWeight_scale/calibrationWeight;
      
      Serial.print("Done. Scalescalibration="); Serial.println(scalecalibration,10); 
      scale.set_scale(scalecalibration);
      writeEEPROMsettings();
      Serial.println("Saved to EEPROM");
      
    }else if(receivedString.equals("g")){
      Serial.println(scale.get_units(5), 10);
    }else{
      Serial.print("unknown command: "); Serial.println(receivedString);
    }
  }*/
  
  currentreadings[currentreading_pos]=analogRead(PIN_CURRENT);
  currentreading_pos++;
  currentreading_pos%=N_CURRENTREADINGS;
  
  double showweight=getWeightFiltered();
  switch(state){
  case S_SCALEDISPLAY:
    if ((getWeightSeriesMax()-getWeightSeriesMin())<LCD_BACKLIGHT_WEIGHT){
      if (lcd_backlight_offtimer==0)
        lcd_backlight_offtimer=millis();
      if ((lcd_backlight_offtimer+LCD_BACKLIGHT_TIME)<millis())
        lcd.noBacklight();
    }else{
      lcd_backlight_offtimer=0;
      lcd.backlight();
    }
    lcd0=toStringBar(weight,0,1000);
    //lcd1=toWeightString(weight)+"g";
    //lcd1=toWeightString(getWeightMedian())+"g";
    
    lcd1=toWeightString(showweight)+"g";
    Serial.print(showweight,3); Serial.println("g");
    
    //___
    if (btn_back_press==2){ //press BACK to tare
      scale.tare();
    }
    
    if (btn_set_press==2){
      state=S_MENU;
      state_menu=0;
    }
    break;
    
  case S_MENU:
    lcd.backlight();
    lcd0=menu_entry[state_menu];
    lcd1="---";
  
    //___
    if (btn_back_press==2){
      state=S_SCALEDISPLAY;
    }
    
    if (btn_down_press==2){
      if (state_menu>0)
        state_menu--;
    }
    if (btn_up_press==2){
      if (state_menu<(MENU_ENTRIES-1))
        state_menu++;
    }
    
    if (btn_set_press==2){ //Menu entry selected
      state=S_MENUENTRY;
    }
    break;
  
  case S_MENUENTRY: //menu entry selected
    lcd0="  #"+menu_entry[state_menu]+"#";
    lcd1=menuentry_string();
    
    //___
    if (btn_back_press==2){
      menuentry_back(btn_down_press);
      state=S_MENU;
    }
    
    if (btn_down_press>=2){
      menuentry_down(btn_down_press);
    }
    if (btn_up_press>=2){
      menuentry_up(btn_up_press);
    }
    if (btn_set_press>=2){
      menuentry_set(btn_set_press);
    }
    break;
    
  case S_CALIBRATION: //menu entry selected
    
    if (btn_back_press==2){
      state=S_MENUENTRY; //abort
    }
    
    if (state_calibration==0){ //wait for press
      lcd0="Remove weight";
      lcd1="then press SET";
      if (btn_set_press>=2){
        state_calibration=1;
        statetimer=millis();
      }
    }else if (state_calibration==1){ //taring
      if (millis()<(statetimer+DELAY_CALIBRATIONWAIT)){ //wait some time
        lcd0="Wait";
        lcd1="";
      }else{
        lcd0="Taring";
        lcd1="";
        scale.set_scale();
        scale.tare();
        state_calibration=2;
      }
    }else if (state_calibration==2){ // wait for press
      lcd0="Place "+toString(calibrationWeight,0)+"g";
      lcd1="press SET";
      if (btn_set_press>=2){
        state_calibration=3;
        statetimer=millis();
      }
    }else if (state_calibration==3){ // measuring
      if (millis()<(statetimer+DELAY_CALIBRATIONWAIT)){ //wait some time
        lcd0="Please wait ...";
        lcd1="";
      }else if (millis()<(statetimer+DELAY_CALIBRATIONWAIT+500)){ //print new text
        lcd0="";
        lcd1="Measuring ...";
      }else{
        double calibrationWeight_scale=scale.get_units(30);
        scalecalibration=calibrationWeight_scale/calibrationWeight;
        
        scale.set_scale(scalecalibration);
        writeEEPROMsettings();
        state_calibration=4;
        statetimer=millis();
      }
    }else if (state_calibration==4){ //show data
      if (millis()<(statetimer+DELAY_CALIBRATIONSHOW)){ //wait some time
        lcd0=""+toString(scalecalibration);
        lcd1="Saved to eeprom";
      }else{
        lcd0="";
        lcd1="";
        state_calibration=0;
        state=S_SCALEDISPLAY;
      }
    }
    
    break;

    case S_THRUSTDISPLAY:
      {
      
      if (state_thrusttest>0){ //only check if test started
        if (getCurrent()>maxcurrent ){
          esc_value=ESC_MIN;
          esc.writeMicroseconds(esc_value); //esc off
          state_thrusttest=10; //10=amp max error
        }
        if (getVoltage()*10<minvoltage){
          esc_value=ESC_MIN;
          esc.writeMicroseconds(esc_value); //esc off
          state_thrusttest=11; //11=undervoltage
        }
      }
      
      if (btn_down_press==1 || btn_back_press==1){
        esc_value=ESC_MIN;
        esc.writeMicroseconds(esc_value); //esc off
        state_thrusttest=0;
      }
      
      if (btn_set_press==2){ //goto menu
        state_thrusttest=0;
        esc_value=ESC_MIN;
        esc.writeMicroseconds(esc_value); //esc off
        state=S_MENU;
        state_menu=0;
      }

      //look for new max values
      if (test_maxamps<getCurrent()){
        test_maxamps=getCurrent();
      }
      if (test_maxthrust<weight){
        test_maxthrust=weight;
      }
      
      if (state_thrusttest==0 || (state_thrusttest>=2 && state_thrusttest<=4)){
        lcd0=toWeightString(getCurrent(),1,2)+"A "+toWeightString(getVoltage(),2,1)+"V";
        lcd1=toWeightString(weight,0,2)+"g " + String(esc_value)+" "+String(rpm);
      }

      if (state_thrusttest==0){ //wait to start
        //lcd0=toWeightString(getCurrent(),2,1)+"A "+toWeightString(getVoltage(),2,1)+"V";
        //lcd1=toWeightString(weight,2,1)+"g v=" + toWeightString(esc_value,2,0);

        if (btn_up_press==3){ //long press
          scale.tare();
          state_thrusttest=1; //start
          thrusttest_timer=millis();
        }
        
      }else if (state_thrusttest==1){ //initialize thrusttest
        lcd0="Starting Test ...";
        lcd1="Max "+toWeightString(maxcurrent,2,1)+"A";
        esc_value=ESC_MIN;
        esc.writeMicroseconds(esc_value);
        if (millis()-thrusttest_timer>THRUSTTEST_STARTDELAY){ //start ramping
          Serial.println("Time;Value;Current;Voltage;Thrust;RPM"); //Print CSV Head
          test_maxamps=0; //reset statistics
          test_maxthrust=0;
          state_thrusttest=2;
          thrusttest_timer=millis(); //use timer for ramping 
          testtime=millis();
        }
      }else if (state_thrusttest==2){ //ramping up
        if (millis()-thrusttest_timer>thrusttest_steptime){
          esc_value++;
          if (esc_value>ESC_MAX){
            esc_value=ESC_MAX;
            state_thrusttest=3; //next state
          }
          esc.writeMicroseconds(esc_value);
          thrusttest_timer=millis();
        }
        
        //lcd0=toWeightString(getCurrent(),2,1)+"A "+toWeightString(getVoltage(),2,1)+"V";
        //lcd1=toWeightString(weight,2,1)+"g v=" + toWeightString(esc_value,2,0);
        
      }else if (state_thrusttest==3){ //hold
        if (millis()-thrusttest_timer>thrusttest_holdmax){
          state_thrusttest=4;
          thrusttest_timer=millis();
        }
        
        //lcd0=toWeightString(getCurrent(),2,1)+"A "+toWeightString(getVoltage(),2,1)+"V";
        //lcd1=toWeightString(weight,2,1)+"g v=" + toWeightString(esc_value,2,0);
        
      }else if (state_thrusttest==4){ //ramp down
        if (millis()-thrusttest_timer>thrusttest_steptime){
          esc_value--;
          if (esc_value<ESC_MIN){
            esc_value=ESC_MIN;
            state_thrusttest=5; //next state
          }
          esc.writeMicroseconds(esc_value);
          thrusttest_timer=millis();
        }
        
        //lcd0=toWeightString(getCurrent(),2,1)+"A "+toWeightString(getVoltage(),2,1)+"V";
        //lcd1=toWeightString(weight,2,1)+"g v=" + toWeightString(esc_value,2,0);
        
      }else if (state_thrusttest==5){ //end, wait
        if (millis()-thrusttest_timer>thrusttest_waitend){
          state_thrusttest=0;
          esc_value=ESC_MIN;
          esc.writeMicroseconds(esc_value); //make shure esc is off
          thrusttest_timer=millis();
        }
        
        lcd0="Max "+toWeightString(test_maxamps,2,1)+"A";
        lcd1=toWeightString(test_maxthrust,2,1)+"g";
        
      }else if(state_thrusttest==10){ //amp max error
        lcd0="Max Amp";
      }else if(state_thrusttest==11){ //min voltage error
        lcd0="Undervolt.";
      }
    
    break;
    }
  }
  
  //reset button flags
  resetBTNFlag(&btn_back_press);
  resetBTNFlag(&btn_down_press);
  resetBTNFlag(&btn_up_press);
  resetBTNFlag(&btn_set_press);
  
  
  //read scale
  if (millis() >= time_lastadc+time_adcwait)
  {
    weight=scale.get_units(adc_readings);
    adcmedianposition++;
    adcmedianposition%=adcmedianvalues;
    weightseries[adcmedianposition]=weight; //save weight to series for medianfilter
    looptimeadc_margin=millis()-time_lastadc-time_adcwait;
    
    uint16_t time_passed=millis()-time_lastadc;
    uint16_t _rotmultiplier=60000/time_passed;
    rpm=rotationcounter*_rotmultiplier; //rotationcounter*60000/(millis()-time_lastadc)
    
    rotationcounter=0;
    
    time_lastadc=millis();
    
    if (state==S_THRUSTDISPLAY && state_thrusttest>=2 && state_thrusttest<=5){ //in thrusttest
      Serial.print(String(millis()-testtime)+";");
      Serial.print(String(esc_value)+";");
      Serial.print(toWeightString(getCurrent(),2,1)+";");
      Serial.print(toWeightString(getVoltage(),2,1)+";");
      Serial.print(toWeightString(weight,2,1)+";");
      Serial.println(String(rpm));
    }
  }
  
  //DISPLAY
  
  if (millis() >= time_lastlcd+time_lcdwait)
  {
    updateLCD();
    looptimelcd_margin=millis()-time_lastlcd-time_lcdwait;
    time_lastlcd=millis();
    
    
  }
  
  //Status led lights up if atmega cannot keep up
  if (looptimelcd_margin>time_lcdwait || looptimeadc_margin>time_adcwait){
    digitalWrite(PIN_LED,HIGH);
  }else{
    digitalWrite(PIN_LED,LOW);
  }
  
  
}

void updateLCD(){

  /*switch(state){
  case S_SCALEDISPLAY:
    if ((getWeightSeriesMax()-getWeightSeriesMin())<LCD_BACKLIGHT_WEIGHT){
      if (lcd_backlight_offtimer==0)
        lcd_backlight_offtimer=millis();
      if ((lcd_backlight_offtimer+LCD_BACKLIGHT_TIME)<millis())
        lcd.noBacklight();
    }else{
      lcd_backlight_offtimer=0;
      lcd.backlight();
    }
    lcd0=toStringBar(weight,0,500);
    lcd1=toWeightString(weight)+"g";
    
    break;
  case S_MENU: //entry show
    lcd.backlight();
    lcd0=menu_entry[state_menu];
    lcd1="---";
    
    
    break;
    
  case S_MENUENTRY: //Entry selected/entered
    lcd0="  #"+menu_entry[state_menu]+"#";
    lcd1=menuentry_string();
    
    break;
    
  case S_CALIBRATION:
    if (state_calibration==0){ //wait for press
      lcd0="Place weight and";
      lcd1="press SET";
    }else if (state_calibration==1){ //measuring
      lcd0="Please wait ...";
      lcd1="";
    }else if (state_calibration==2){ 
      lcd0="";
      lcd1="press SET";
    }
    
    break;
  }
  */
  if (!lcd0.equals(lcd0_buffer) || !lcd1.equals(lcd1_buffer)){
      lcd.clear();
      lcd.print(lcd0);
      lcd.setCursor(0,1); 
      lcd.print(lcd1);
      lcd0_buffer=lcd0;
      lcd1_buffer=lcd1;
    }
}

void buttonCheck()
{
  buttonHandler(PIN_BACK,&btn_back_tdown,&btn_back_trelease,&btn_back_press);
  buttonHandler(PIN_DOWN,&btn_down_tdown,&btn_down_trelease,&btn_down_press);
  buttonHandler(PIN_UP,&btn_up_tdown,&btn_up_trelease,&btn_up_press);
  buttonHandler(PIN_SET,&btn_set_tdown,&btn_set_trelease,&btn_set_press);
  
}

void buttonHandler(uint8_t pin,uint32_t *btntdown,uint32_t *btntrelease,uint8_t *btnpress)
{
  if (!digitalRead(pin)){
    if (*btntdown==0 && (*btntrelease+BTN_BOUNCETIME)<millis()){
      *btntdown=millis();
      *btnpress=1; //is down
    }
    
  }else{
    if (*btntdown!=0){
      if (*btnpress==1){ //if down press not already handled, ie set to 0
        if ((*btntdown+BTN_HOLDTIME)>millis()){ //short
          *btnpress=2;
        }else{ //long
          *btnpress=3;
        }
      }
      if ((*btntdown+BTN_BOUNCETIME)<millis()){ //wait if debouncetime over
        *btntdown=0;
        *btntrelease=millis(); 
      }
    }
  }
}




void resetBTNFlag(uint8_t *btnpress){
  if (*btnpress==2 || *btnpress==3)
    *btnpress=0; //reset if press or hold
}

void menuentry_down(uint8_t presstype){ //DOWN
  switch(state_menu){
    case SM_calibration: // Calibration
      if (presstype==2){
        if (calibrationWeight>0)
          calibrationWeight--;
      }else if(presstype==3){
        if (calibrationWeight>50)  
          calibrationWeight-=50;
        else
          calibrationWeight=0;
      }
      break;
    case SM_fps:
      if (fps>1)
          fps--;
      break;
    case SM_medianfilter:
      if (adcmedianvalues>3)
          adcmedianvalues-=2;
      break;
      
    case SM_spsadc:
      if (spsadc>1)
          spsadc--;
      break;
      
    case SM_maxcurrent:
      if (maxcurrent>1)
          maxcurrent--;
      break;
      
    case SM_minvoltage:
      if (minvoltage>1)
          minvoltage--;
      break;
      
    case SM_thrusttest_steptime:
      if (thrusttest_steptime>1)
          thrusttest_steptime--;
      break;
      
      
  }
}
void menuentry_up(uint8_t presstype){ //UP
  switch(state_menu){
    case SM_calibration: // Calibration
      if (presstype==2)
        calibrationWeight++;
      if (presstype==3)
        calibrationWeight+=50;
      break;
    case SM_fps:
        if (fps<25)
          fps++;
      break;
    case SM_medianfilter:
        if (adcmedianvalues<(ADCMEDIANVALUES_MAX-1))
          adcmedianvalues+=2;
      break;
      
    case SM_spsadc:
        if (spsadc<100)
          spsadc++;
      break;
    case SM_maxcurrent:
      if (maxcurrent<50)
          maxcurrent++;
      break;
      
    case SM_minvoltage:
      if (minvoltage<255)
          minvoltage++;
      break;
      
    case SM_thrusttest_steptime:
      if (thrusttest_steptime<255)
          thrusttest_steptime++;
      break;
  }
}

void menuentry_back(uint8_t presstype){ //BACK, only additional function
  switch(state_menu){
    case SM_fps: // FPS
      updateFPS(); //calculate waittime from fps variable
      break;

    case SM_spsadc:
      updateSPSADC(); //calculate waittime
      state=S_MENU;
      break;  

  }
}
void menuentry_set(uint8_t presstype){ //SET
  switch(state_menu){
    case SM_scale: //Scale
      state=S_SCALEDISPLAY; //switch to scale display
      
      break;
    case SM_thrusttest: //Thrusttest
      state=S_THRUSTDISPLAY;
      lcd.backlight();
      break;
    case SM_calibration: // Calibration
      state=S_CALIBRATION;
      state_calibration=0;
      break;
      
    case SM_fps: // FPS
      updateFPS(); //calculate waittime from fps variable
      state=S_MENU;
      break;
    
    case SM_medianfilter: // medianfilter
      state=S_MENU;
      break;  
      
    case SM_spsadc:
      updateSPSADC(); //calculate waittime
      state=S_MENU;
      break;  
     
    case SM_save: // Save EEPROM
      writeEEPROMsettings();
      state=S_MENU;
      break;
    case SM_loaddefaults: // Load defaults
      loadDefaults();
      state=S_MENU;
      break;
  }
}


String menuentry_string(){ //second line string if entry selected
  String s="";
  switch(state_menu){
    case SM_scale: //Scale
    case SM_thrusttest: case SM_save: case SM_loaddefaults:
      s="Press SET";
      break;
    case SM_calibration: // Calibration
      s=toString(calibrationWeight,0)+"g";
      break;
    case SM_fps:
      s=toString(fps,0);
      break;
    case SM_medianfilter:
      s=toString(adcmedianvalues,0);
      break;
      
    case SM_spsadc:
      s=toString(spsadc,0)+"Hz";
      break;
      
    case SM_maxcurrent:
      s=toString(maxcurrent,0)+"A";
      break;
      
    case SM_minvoltage:
      s=toString(minvoltage/10.0,1)+"V";
      break;
      
    case SM_thrusttest_steptime:
      s=toString(thrusttest_steptime,0)+"s";
      break;
      
    
      
    
  }
  return s;
}

float getCurrent()
{
  uint16_t _current=0;
  for (uint8_t i=0;i<N_CURRENTREADINGS;i++){
    _current+=currentreadings[i]/N_CURRENTREADINGS;
  }
  
  return _current*CURRENTMULTIPLIER+0.00001;
}

float getVoltage()
{
  return analogRead(PIN_VOLTAGE)*VOLTAGEMULTIPLIER+0.00001;
}

double getWeightMedian() //return median weight from weightseries
{
  boolean mask[adcmedianvalues]; //true=disabled value
  for (uint16_t i=0;i<adcmedianvalues;i++)
    mask[i]=false;
  float cmin=MAXDOUBLEVALUE; uint16_t cmin_i=0;
  float cmax=0; uint16_t cmax_i=0;
  while(cmin!=cmax){ //stop when only one value left
    cmin=MAXDOUBLEVALUE;
    cmax=-MAXDOUBLEVALUE-1;
    for (uint16_t i=0;i<adcmedianvalues;i++){
      if (!mask[i]){
        if (weightseries[i]<cmin){
          cmin=weightseries[i];
          cmin_i=i;
        }
        if (weightseries[i]>cmax){
          cmax=weightseries[i];
          cmax_i=i;
        }
      }
      
    }
    mask[cmin_i]=true;
    mask[cmax_i]=true;
  }
  return cmax;
}

double getWeightFiltered() {
  boolean mask[adcmedianvalues]; //true=disabled value
  for (uint16_t i=0;i<adcmedianvalues;i++)
    mask[i]=false;
  float cmin=MAXDOUBLEVALUE; uint16_t cmin_i=0;
  float cmax=0; uint16_t cmax_i=0;
  //while(cmin!=cmax){ //stop when only one value left
  uint16_t _valuesLeft=adcmedianvalues;


  while (_valuesLeft>2 && _valuesLeft>(uint16_t)(adcmedianvalues*adcFilterKeepMedianvaluesFactor)) 
  {
    cmin=MAXDOUBLEVALUE;
    cmax=-MAXDOUBLEVALUE-1;
    for (uint16_t i=0;i<adcmedianvalues;i++){
      if (!mask[i]){
        if (weightseries[i]<cmin){
          cmin=weightseries[i];
          cmin_i=i;
        }
        if (weightseries[i]>cmax){
          cmax=weightseries[i];
          cmax_i=i;
        }
      }
      
    }
    mask[cmin_i]=true;
    mask[cmax_i]=true;
    _valuesLeft-=2; //2 values have been masked out
  }



  double sum=0;
  uint16_t meanvalues=0;
  for (uint16_t i=0;i<adcmedianvalues;i++){
    if (!mask[i]) { //not masked out
      sum+=weightseries[i];
      meanvalues++;
    }
  }
  return sum/meanvalues;
}

float getWeightSeriesMin()
{
  float cmin=MAXDOUBLEVALUE;
  for (uint16_t i=0;i<adcmedianvalues;i++){
    if (weightseries[i]<cmin)
     cmin=weightseries[i]; 
  }
  return cmin;
}
float getWeightSeriesMax()
{
  float cmax=0;
  for (uint16_t i=0;i<adcmedianvalues;i++){
    if (weightseries[i]>cmax)
     cmax=weightseries[i]; 
  }
  return cmax;
}

String toWeightString(double w){
  return toWeightString(w,3,6);
}
String toWeightString(double w,uint8_t dec,uint8_t len){
  char outstring[16];
  char vz;
  if(w<0) {
    vz='-';
  }else{
    vz='+';
  }
  dtostrf(abs(w),len,dec,outstring);
  return String(vz)+""+String(outstring);
}

String toString(double w){
  return toString(w,5);
}
String toString(double w,uint8_t dec){
  char outstring[16];
  dtostrf(w,1,dec,outstring);
  return String(outstring);
}



String toStringBar(double val,double minimum,double maximum){
  String s="";
  if (val<minimum){
    s="<";
  }else if(val>maximum){
    s="               >";
  }else{
    uint8_t pos=map(val,minimum,maximum,0,15);
    for (uint8_t i=0;i<pos;i++){
      s+=" ";
    }
    s+="|";
  }
  return s;
}





/*
void checkSerial(){
  receivedString="";
  
  while (Serial.available())
  {
      if (Serial.peek() == '\n' || Serial.peek() == '\r')          // Ignore newlines.
      {
        Serial.read();
        if (SerialMessage.length()>0)
          receivedString=SerialMessage; //Save the string bevore clearing
        SerialMessage=""; 
        Serial.println();
      }
      else  // For all other characters,
      {
        char c=Serial.read();
        SerialMessage.concat(c); //concat all received characters
        Serial.print(c);

      }
  }
}*/

void updateFPS(){
  time_lcdwait=1000/fps;
}
void updateSPSADC(){
  time_adcwait=1000/spsadc;
}

void loadEEPROMsettings() //load diversity settings
{
  uint8_t addr=0;
  uint8_t checkbyte=EEPROM.read(addr++); //checkbyte //0
  if (checkbyte==EEPROMVERSION){ //if checkbyte correct
    scalecalibration=EEPROMReadDouble(addr);  addr+=4; //uint32_t
    fps=EEPROM.read(addr++); //uint8_t
    adcmedianvalues=EEPROM.read(addr++); //uint8_t
    spsadc=EEPROM.read(addr++); //uint8_t
    maxcurrent=EEPROM.read(addr++); //uint8_t
    minvoltage=EEPROM.read(addr++); //uint8_t
    thrusttest_steptime=EEPROM.read(addr++); //uint8_t
    
    updateFPS();
    updateSPSADC();

  }else{
    Serial.println("Wrong EEPROM Vers.");
  }
  
}

void writeEEPROMsettings() //save diversity settings
{
  uint8_t addr=0;
  EEPROM.write(addr++,EEPROMVERSION);
  EEPROMWriteDouble(addr, scalecalibration); addr+=4;
  EEPROM.write(addr++, fps);  
  EEPROM.write(addr++, adcmedianvalues);  
  EEPROM.write(addr++, spsadc);  
  EEPROM.write(addr++, maxcurrent);  
  EEPROM.write(addr++, minvoltage);  
  EEPROM.write(addr++, thrusttest_steptime);
    //EEPROM.write(addr++, scalecalibration);  
    //EEPROMWriteInt(addr+=2, switchdeadtime);
  
  
}



void loadDefaults(){
  scalecalibration=DEFAULT_SCALECALIBRATION;
  fps=DEFAULT_FPS;
  updateFPS();
  adcmedianvalues=DEFAULT_ADCMEDIANVALUES;
  spsadc=DEFAULT_SPSADC;
  updateSPSADC();
}

void EEPROMWriteInt(uint8_t paddr,uint16_t pdata){
  EEPROM.write(paddr, (uint8_t)(pdata%256));
  EEPROM.write(++paddr, (uint8_t)(pdata/256));
}

uint16_t EEPROMReadInt(uint8_t paddr){
  uint16_t data=EEPROM.read(paddr);
  data+=EEPROM.read(++paddr)*256;
  return data;
}


void EEPROMWriteDouble(int ee, double value)
{
   byte* p = (byte*)(void*)&value;
   for (uint16_t i = 0; i < sizeof(value); i++)
       EEPROM.write(ee++, *p++);
}

double EEPROMReadDouble(int ee)
{
   double value = 0.0;
   byte* p = (byte*)(void*)&value;
   for (uint16_t i = 0; i < sizeof(value); i++)
       *p++ = EEPROM.read(ee++);
   return value;
}

float StringToFloat(String s){
  char buf[s.length()];
  s.toCharArray(buf,s.length());
  return atof(buf); 
}


void rpm_interrupt(){
   rotationcounter++;
}