#include <EduBox.h>
#include "bt_lib.h"

#define MOTOR_SPEED 0.1

EduBox eduBox;

void setup() {
  Serial.begin(115200);
  eduBox.begin();
}

void MrugajRGB(float x) {
  if( x < 1e-6 )
    return;

  static uint32_t prev = 0;
  uint32_t now = millis();
  if (now - prev > 1000.0f*(1.0f/x) ) {
    prev = now;

    static bool isRGBON = false;    

    if(isRGBON)
      eduBox.eduRGB_SetValues(25,0,0);
    else
      eduBox.eduRGB_SetValues(0,25,0);

    isRGBON = !isRGBON;
  }
}

void WyswietlLiczbyKlikniec(void) {
  static uint32_t prev = 0;
  uint32_t now = millis();
  if (now - prev > 100 ) {
    prev = now;

    eduBox.edu7Segment_DisplayNumber(liczba_1klikow);
    eduBox.edu7Segment_DisplayNumber(liczba_nklikow,true);
  }
}

void loop() {
  ObslugaPrzycisku(eduBox.eduButton_GetState());

  float position = eduBox.eduStepperMotor_GetPosition();

  float position_set = liczba_nklikow - liczba_1klikow;

  float speed = 0;
  if( position > position_set )
      speed = (-1.0f)*MOTOR_SPEED;
  else
      speed = MOTOR_SPEED;

  eduBox.eduStepperMotor_SetSpeed(speed);

  eduBox.edu7Segment_DisplayNumber(abs(position));
  Serial.println(position);

  delay(10);
}