#include "pen.h"
#include "config.h"
#include <ESP32Servo.h>

static Servo   s_servo;
static bool    s_penDown   = false;
static uint8_t s_angleUp   = SERVO_ANGLE_UP;
static uint8_t s_angleDown = SERVO_ANGLE_DOWN;

void pen_init() {
    s_servo.attach(PIN_SERVO);
    pen_up();
}

void pen_up() {
    s_servo.write(s_angleUp);
    delay(SERVO_SETTLE_MS);
    s_penDown = false;
}

void pen_down() {
    s_servo.write(s_angleDown);
    delay(SERVO_SETTLE_MS);
    s_penDown = true;
}

bool pen_is_down() {
    return s_penDown;
}

void pen_set_down_angle(uint8_t angle) {
    s_angleDown = angle;
}

void pen_set_up_angle(uint8_t angle) {
    s_angleUp = angle;
}