#ifdef CONFIG_TOP
#include "proffieboard_v3_config.h"
#define NUM_BLADES 0
#define NUM_BUTTONS 2
#define VOLUME 1500
const unsigned int maxLedsPerStrip = 144;
#define CLASH_THRESHOLD_G 1.0
#define ENABLE_AUDIO
#define ENABLE_MOTION
#define ENABLE_WS2811
#define ENABLE_SD
#define SAVE_STATE
#define ENABLE_ALL_EDIT_OPTIONS
#endif

#ifdef CONFIG_PRESETS
Preset presets[] = {
   { "TeensySF", "",
    /*what goes here*/, "Preset 1"},
};
BladeConfig blades[] = {/*does anything go here*/};
#endif

#ifdef CONFIG_BUTTONS
Button PowerButton(BUTTON_POWER, powerButtonPin, "Extend");
Button AuxButton(BUTTON_AUX, auxPin, "Retract");
#endif

#ifdef CONFIG_BOTTOM

#define CPR 230.7           // Counts per revolution of motor encoder                                                                                                                                                                                          
#define REVOLUTIONS 5       // Number of revolutions for max blade extension                                                                                                                                                                                   
#define DEADZONE 10         // Stops PID loop if error is within this many encoder ticks                                                                                                                                                                       
#define PWM_MAX 255         // PWM range                                                                                                                                                                                                                       
#define P 1.0
#define I 0.01
#define D 0.1
#define DEBOUNCE_DELAY 50   // Debounce delay in ms                                                                                                                                                                                                            
#define PID_LOOP_TIME 10    // PID loop time in ms                                                                                                                                                                                                             

#define ENCODER_CH_A data2Pin
#define ENCODER_CH_B data3Pin
#define MOTOR_IN1 8
#define MOTOR_IN2 9

class BladeExtender : public Looper, public RotaryReceiver {
public:
    const char* name() override { return "BladeExtender"; }
    void Setup() override {
        pinMode(MOTOR_IN1, MODE_OUTPUT);
        pinMode(MOTOR_IN2, MODE_OUTPUT);
    }
    void Loop() override {
        float targetCount = SaberBase::IsOn() ? 0 : REVOLUTIONS * CPR;

        // PID Loop                                                                                                                                                                                                                                                
        uint32_t currentTime = millis();
        uint32_t elapsedTime = currentTime - lastTime;

        if (elapsedTime >= PID_LOOP_TIME) {
            error = targetCounts - position_;
            float output = 0.0;
            if (abs(error) > DEADZONE) {
                integral += error * (elapsedTime / 1000.0);
                derivative = (error - previousError) / (elapsedTime / 1000.0);
                output = P * error + I * integral + D * derivative;
            }
            analogWrite(MOTOR_IN1, clamp(output, 0, PWM_MAX));
            analogWrite(MOTOR_IN2, clamp(-output, 0, PWM_MAX));

            previousError = error;
            lastTime = currentTime;
        }
    }
    // RotaryReceiver implementation                                                                                                                                                                                                                             
    void Update(int delta) override {
        position_ += delta;
    }
private:
    float integral = 0.0;
    float previousError = 0.0;
    uint32_t lastTime;
    int position_ = 0;
};

BladeExtender blade_extender;
Rotary<ENCODER_CH_A, ENCODER_CH_B, 1> rotary(&blade_extender);

#endif