import controlP5.*;

ControlP5 cp5;
int myColor = color(0,0,0);




long lastReadADC=0;
long lastSend=0;
long lastHBLoop=0;

int LOOPTIME_CONTROLLER=10; //controller looptime
long lastLoopController=0;
int ADC_CALIB_THROTTLE_MIN=2000; //controller
int ADC_CALIB_THROTTLE_MAX=3120; //controller
int ADC_READTIME=10; //controller. in ms
int adc_throttle_raw=ADC_CALIB_THROTTLE_MIN; //controller. raw value read from adc (here slider)
float adc_throttle=adc_throttle_raw; //controller. internally filtered value
float ADC_THROTTLE_FILTER=0.2; //controller

int speedvalue=0;
int MAXBRAKERATE=7; //Controller

int SENDPERIOD=50; //controller. in ms
int sendSpeed=0; //transmittet speed value over usart

int LOOPTIME=5; //hoverboard.
int RATE=30; //hoverboard. rate value, see config.h. default 30
float FILTER=0.1; //hoverboard. filter value, see config.h default 0.1
float speedRate=0; //hoverboard.implemented as fixdt. what comes out of rate limiting.
int outSpeed=0; //hoverboard. speedvalue for pwm. filtered speedRate

Slider SinputValue;
Slider SfilteredInputValue;
Slider SspeedValue;
Slider SspeedRate;
Slider SoutSpeed;

int inputValue = ADC_CALIB_THROTTLE_MIN;
int maxvalue=1000;

int timetestMode=0;
long timeteststarted_time=0;
int outSpeedThresholdIncrease=int(1000*0.707); //threshold for timing stop for increasing speed
int outSpeedThresholdDecrease=int(1000*(1-0.707)); //threshold for timing stop for decreasing speed
long timingtest_start=0; //for timing testing
long timingtest_end=0; //for timing testing

void setup() {
  frameRate(int(1000.0/5));
  size(700,400);
  noStroke();
  cp5 = new ControlP5(this);
  
  // add a horizontal sliders, the value of this slider will be linked
  // to variable 'sliderValue' 
  SinputValue=cp5.addSlider("inputValue")
     .setPosition(10,50)
     .setSize(500,30)
     .setRange(ADC_CALIB_THROTTLE_MIN,ADC_CALIB_THROTTLE_MAX)
     ;
  SfilteredInputValue=cp5.addSlider("filteredInputValue")
     .setPosition(10,50+30+10)
     .setSize(500,20)
     .setRange(ADC_CALIB_THROTTLE_MIN,ADC_CALIB_THROTTLE_MAX)
     ;
     
  SspeedValue=cp5.addSlider("speedValue") //after brake slowing
     .setPosition(10,50+30+10+20+10)
     .setSize(500,20)
     .setRange(0,1000)
     ;
     
  SspeedRate=cp5.addSlider("speedRate")
     .setPosition(10,50+30+10+20+10+20+10)
     .setSize(500,20)
     .setRange(0,1000)
     ;
     
  SoutSpeed=cp5.addSlider("outSpeed")
     .setPosition(1,50+30+10+20+10+20+10+20+10)
     .setSize(500,30)
     .setRange(0,1000)
     ;
  

}

void draw() {
  long loopmillis=millis();
  
  //Time testing
  
  switch (timetestMode) {
    case 0: //started
      println("Starting Timing Test 'Increase'. Startvalue="+ADC_CALIB_THROTTLE_MIN); 
      SinputValue.setValue(ADC_CALIB_THROTTLE_MIN); //reset values to minimum
      SfilteredInputValue.setValue(ADC_CALIB_THROTTLE_MIN);
      SspeedRate.setValue(0);
      SoutSpeed.setValue(0);
      timetestMode++;
      timeteststarted_time=loopmillis;
      break;
    case 1: //wait some time
      if (loopmillis-timeteststarted_time>1000){
        println("Set input to value="+ADC_CALIB_THROTTLE_MAX); 
        timetestMode++;
        SinputValue.setValue(ADC_CALIB_THROTTLE_MAX); //Jump value
        timingtest_start=loopmillis;
      }
      break;
    case 2: //wait until threshold reached
      if (outSpeed>=outSpeedThresholdIncrease){
        println("Threshold "+outSpeed+" (outSpeed) reached"); 
        timetestMode++;
        timingtest_end=loopmillis;
        println("Increase Time="+(timingtest_end-timingtest_start)+"ms");
        timeteststarted_time=loopmillis;
      }
      break;
      
    case 3: //wait some time in between test
      if (loopmillis-timeteststarted_time>1000){
        println("Waiting between tests"); 
        timetestMode++;
        timingtest_start=loopmillis;
      }
      break;
      
    case 4://started
      println("Starting Timing Test 'Decrease'. Startvalue="+ADC_CALIB_THROTTLE_MAX); 
      SinputValue.setValue(ADC_CALIB_THROTTLE_MAX); //reset values to maximum
      SfilteredInputValue.setValue(ADC_CALIB_THROTTLE_MAX);
      SspeedRate.setValue(1000);
      SoutSpeed.setValue(1000);
      timetestMode++;
      break;
    case 5: //wait some time
      if (loopmillis-timeteststarted_time>1000){
        println("Set input to value="+ADC_CALIB_THROTTLE_MIN); 
        timetestMode++;
        SinputValue.setValue(ADC_CALIB_THROTTLE_MIN); //Jump value
        timingtest_start=loopmillis;
      }
      break;
    case 6: //wait until threshold reached
      if (outSpeed<=outSpeedThresholdDecrease){
        println("Threshold "+outSpeed+" (outSpeed) reached"); 
        timetestMode++;
        timingtest_end=loopmillis;
        println("Decrease Time="+(timingtest_end-timingtest_start)+"ms");
      }
      break;
  }
    
  
  
  if (loopmillis-lastReadADC>ADC_READTIME){ //frequency of controller adc read/filtering
    lastReadADC=millis();
    adc_throttle_raw = inputValue; //adc_throttle_raw = analogRead(PIN_THROTTLE);
    adc_throttle = adc_throttle*(1-ADC_THROTTLE_FILTER) + adc_throttle_raw*ADC_THROTTLE_FILTER; //adc_throttle = adc_throttle*(1-ADC_THROTTLE_FILTER) + adc_throttle_raw*ADC_THROTTLE_FILTER;
    SfilteredInputValue.setValue(adc_throttle);
  }
  if (loopmillis-lastLoopController>LOOPTIME_CONTROLLER){ //frequency of controller LOOPTIME
    lastLoopController=millis();
    int lastSpeed=speedvalue;
    speedvalue=int(constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, 1000 ) ,0, 1000));
    
    if (speedvalue<=lastSpeed) { //braking
      speedvalue = (lastSpeed-speedvalue)>MAXBRAKERATE ? (lastSpeed-MAXBRAKERATE):speedvalue;
    }
    SspeedValue.setValue(speedvalue);
  }
    
  if (loopmillis-lastSend>SENDPERIOD){ //Frequency of transmitting new values to the hoverboard
    lastSend=millis();
    sendSpeed=speedvalue;
  }
  
  if (loopmillis-lastHBLoop>LOOPTIME){ //frequency of hoverboard mainboard (looptime)
    lastHBLoop=millis();
    int cmd=sendSpeed; //commanded value
    
    //rateLimiter16 function
    float q0;  //int16_t q0;
    float q1;  //int16_t q1;
    
    q0 = (cmd-speedRate);  //q0 = (u << 4)  - *y;
    
    if (q0 > RATE) {  //if (q0 > rate) {
      q0=RATE; //  q0 = rate;
    }else{ //} else {
      q1=-RATE;  //  q1 = -rate;
      if (q0<q1) {//  if (q0 < q1) {
        q0=q1;//    q0 = q1;
      }//  }
    }  //}
    
    speedRate = q0 + speedRate; //*y = q0 + *y;
    SspeedRate.setValue(speedRate);
    
    outSpeed= int(outSpeed*(1-FILTER) + speedRate*FILTER); //filtLowPass16 function
    SoutSpeed.setValue(outSpeed);
  }
  
  //Breathing led test
  int _b=int((loopmillis/10)%(255*2));
  fill( _b<255 ? _b : (255*2)-_b );
  rect(0,0,width,10);  
  
  
}

/*
void slider(float theColor) {
  myColor = color(theColor);
  println("a slider event. setting background to "+theColor);
}

*/