// Servo motor tester by Andrey Samokhin
// ver. 0.0.1 (15 Dec 2023)

// INSTRUCTIONS:
//   - Set the correct number of pulses per detent (i.e., change the following
//     line: '#define PULSES_PER_DETENT 2').
//   - To make an encoder count in the oposite direction swap the respective
//     Arduino pins (i.e., swap '2' and '3' in the following lines:
//     '#define ENC_PIN_A 3' and '#define ENC_PIN_B 2').



#include <Encoder.h>
#include <Servo.h>
#include <TM1637Display.h>

#define PULSES_PER_DETENT 2 // the number of pulses per detent

#define ENC_PIN_A 2
#define ENC_PIN_B 3
#define ENC_BUTTON_PIN 4
#define DISP_CLK_PIN 5
#define DISP_DIO_PIN 6
#define SERVO_PIN 9

#define DISP_BRIGHTNESS 3  // the brightness of the display
#define DEBOUNCE_DELAY 50  // the debounce time

// 'ENC_ROTATION_SPEED_COEF' is used to configure the relationship between the
// encoder rotation speed and the rate of value change.
#define ENC_ROTATION_SPEED_COEF 50

Encoder enc(ENC_PIN_A, ENC_PIN_B);
TM1637Display disp(DISP_CLK_PIN, DISP_DIO_PIN);
Servo serv;

const int16_t preset_positions[] = { 90, 0, 90, 180 };
uint8_t position_idx = 0;
int16_t servo_angle = 90;
int32_t enc_counter = 0;
int32_t enc_counter_prev = 0;
int32_t enc_counter_position_reset = 0;
uint32_t enc_read_time = 0;
uint32_t button_change_state_time = 0;
uint8_t button_state = 0;  // 0 - released; 1 - pressed

void setup() {
  serv.attach(SERVO_PIN);
  serv.write(servo_angle);

  pinMode(ENC_BUTTON_PIN, INPUT_PULLUP);

  disp.setBrightness(DISP_BRIGHTNESS, true);
  uint8_t disp_ini_value[] = { 0b01000000, 0b01000000, 0b01000000, 0b01000000 };
  disp.setSegments(disp_ini_value);
  delay(1000);
  disp.showNumberDec(servo_angle, false);
}

void loop() {
  enc_counter = enc.read();
  if (enc_counter % (PULSES_PER_DETENT * 2) == 0 && enc_counter != enc_counter_prev) {
    uint32_t time_ms = millis(); 
    uint16_t delta = (int16_t)((enc_counter - enc_counter_prev) / (PULSES_PER_DETENT * 2));
    if (ENC_ROTATION_SPEED_COEF > 2 * (time_ms - enc_read_time)) {
      servo_angle += 4 * delta;
    } else if (ENC_ROTATION_SPEED_COEF > time_ms - enc_read_time) {
      servo_angle += 2 * delta;
    } else {
      servo_angle += delta;
    }
    if (servo_angle > 180) {
      servo_angle = 180;
    } else if (servo_angle < 0) {
      servo_angle = 0;
    }
    enc_read_time = time_ms;
    serv.write(servo_angle);
    disp.showNumberDec(servo_angle, false);
    enc_counter_prev = enc_counter;
  }

  uint8_t enc_button_pin_value = digitalRead(ENC_BUTTON_PIN);
  if (enc_button_pin_value == button_state && millis() - button_change_state_time > DEBOUNCE_DELAY) {
    if (enc_button_pin_value == 0) {  // i.e., the button is pressed
      button_state = 1;
      if (enc_counter_position_reset != enc_counter) {
        enc_counter_position_reset = enc_counter;
        position_idx = 0;
      }
      if (position_idx > sizeof(preset_positions) / 2 - 1) {
        position_idx = 0;
      }
      servo_angle = preset_positions[position_idx];
      position_idx++;
      serv.write(servo_angle);
      disp.showNumberDec(servo_angle, false);
    } else { // i.e., the button is released
      button_state = 0;
    }
    button_change_state_time = millis();
  }
}