/* This is an example for any emulators of circuits and the library code
 * was attached to the demo code because some emulators don't have the
 * ability to include external lib.
 *
 * You can use `make split` to split the code into *.cpp and *.h files.
 * Don't remove the `split::file` and `split::include` tags from the code.
 * It's necessary for system of automatic file splitting.
 */

// split::file angle.h
/** ANGLE LIBRARY
 *
 * This library is designed to simplify the work with servos that
 * allows you to set the angle of rotation of the servo drive.
 *
 * When using a group of servo drives, it is possible to control
 * in the blocking and asynchronous modes for them.
 *
 * Information about the license is located in the LICENSE file for details.
 * Development repository:
 * Examples of use:
 */
#ifndef _ANGLE_H
#define _ANGLE_H

#include <limits.h>

/** NULL VALUES
 *
 * NULL_PIN      - value indicates that the pin is not specified;
 * NULL_ANGLE    - value indicates that the angle is not specified;
 * NULL_INTERVAL - value indicates that the interval is not specified
 *                 and must be determined relative to the specified angle.
 */
#define NULL_PIN UCHAR_MAX
#define NULL_ANGLE USHRT_MAX
#define NULL_INTERVAL 0

/** MIN AND MAX SERVO ANGLE
 *
 * Minimum and maximum angle of rotation of the servo drive.
 * For most servos, this indicator is from 0 to 180 degrees.
 *
 * The value is given in degrees units.
 */
#define MIN_ANGLE 0
#define MAX_ANGLE 180

/** PWM
 *   ┌┈┈┐           ┌┈┈┐           ┌┈┈┐
 *   ┊  ┊           ┊  ┊           ┊  ┊
 * ┄┄┘  └┄┄┄┄┄┄┄┄┄┄┄┘  └┄┄┄┄┄┄┄┄┄┄┄┘  └┄┄┄
 *   ┊▓▓┊ - pulse (min to 0 degrees, max to 180/360 degrees);
 *   ┊░░░░░░░░░░░░░░┊ - PWM frequency (40-200Hz);
 *      ┊▒▒▒▒▒▒▒▒▒▒▒┊ - timeout (period - pause).
 */

/** MIN AND MAX PWM PULSE
 *
 * The minimum value indicates the pulse length at which the rotation
 * angle is MIN_ANGLE degrees. The maximum magnitude is the angle of
 * rotation in MAX_ANGLE degrees. For most servos, this range is between
 * 500 and 2500 microseconds.
 *
 * The value is given in microseconds.
 */
#define MIN_PULSE 500
#define MAX_PULSE 2500

/** SERVO PWM FREQUENCY
 *
 * The recommended PWM frequency for servos is usually in
 * the range of 40-200Hz, with most servos using 50Hz.
 *
 * The value is given in Hz units.
 */
#define PWM_FREQ 50

/** MAXIMUM OPERATING SPEED OF ROTATION
 *
 * Most servos has value in the range of 0.1 to 0.12 seconds,
 * i.e. 100 - 120 milliseconds.
 *
 * The value is given in milliseconds.
 */
#define MAX_SPEED 100

namespace Angle {
/** Config
 *
 * Servo drive configuration
 */
struct Config {
    uint16_t minAngle;  // min angle of rotation
    uint16_t maxAngle;  // max angle of rotation
    uint16_t minPulse;  // min pulse length
    uint16_t maxPulse;  // max pulse length
    uint16_t maxSpeed;  // max speed of rotation
    uint16_t pwmFreq;   // PWM frequency
};

/** Servo
 *
 * Servo drive class.
 * A special type of data whose objects allow you to set the angle of rotation
 * of the servo drive.
 */
class Servo {
   private:
    uint16_t angle;     // current angle
    unsigned char pin;  // pin number

    // Servo drive default configuration.
    Config config = {MIN_ANGLE, MAX_ANGLE,
                     MIN_PULSE, MAX_PULSE,
                     MAX_SPEED, PWM_FREQ};

    unsigned long delta = 0;
    unsigned long interval = 0;
    unsigned long timestamp = 0;
    unsigned long timeout = long(1.0 / PWM_FREQ * 1000000.0);

    // Returns time to rotate on the one degree.
    unsigned long t();

    // Turn the servo drive to the specified angle in async mode.
    void syncTurn(uint16_t angle, uint16_t pulse);

    // Turn the servo drive to the specified angle in sync mode.
    void asyncTurn(uint16_t angle, uint16_t pulse);

   public:
    /** Constructors
     *
     * Servo() - default constructor;
     * Servo(unsigned char, bool) - constructor with pin number
     *    and refresh flag;
     * Servo(unsigned char, Config, bool) - constructor with pin number,
     *    configuration and refresh flag.
     *
     * The refresh flag indicates whether to set the angle of rotation
     * to the minimum value after initialization.
     */
    Servo();
    Servo(unsigned char pin, bool refresh = true);
    Servo(unsigned char pin, Config config, bool refresh = true);

    /** Sets the config of the servo drive.
     *
     * The refresh flag indicates whether to set the angle of rotation
     * to the minimum value after config update.
     */
    void setConfig(Config config, bool refresh = true);

    /** Returns the current config of the servo drive.
     */
    Config getConfig();

    /** Detaches the servo drive from the pin.
     */
    void detach();

    /** Attaches the servo drive to the pin.
     */
    void attach(unsigned char pin);

    /** Turns the servo drive to the specified angle.
     *
     * The asyncMode flag indicates whether to turn the servo drive
     * in the blocking or asynchronous mode.
     */
    void turn(uint16_t angle, bool asyncMode = false);

    /** Returns true if the servo drive is busy.
     *
     * This indicator can be used in asynchronous mode to understand
     * whether the servo has finished rotating.
     *
     * Always returns False in blocking mode.
     */
    bool isBusy();
};
}  // namespace Angle
#endif  // _SERVO_MOTOR_H

// split::file angle.cpp
// split::include #include "angle.h"

/** Constructors
 *
 * Servo() - default constructor;
 * Servo(unsigned char, bool) - constructor with pin number
 *    and refresh flag;
 * Servo(unsigned char, Config, bool) - constructor with pin number,
 *    configuration and refresh flag.
 *
 * The refresh flag indicates whether to set the angle of rotation
 * to the minimum value after initialization.
 */
Angle::Servo::Servo() {
    this->attach(NULL_PIN);
}

Angle::Servo::Servo(unsigned char pin, bool refresh) {
    this->attach(pin);
    if (refresh) {
        this->turn(this->config.minAngle, true);
    }
}

Angle::Servo::Servo(unsigned char pin, Angle::Config config, bool refresh) {
    this->attach(pin);
    this->setConfig(config, refresh);
}

/** Sets the config of the servo drive.
 *
 * The refresh flag indicates whether to set the angle of rotation
 * to the minimum value after config update.
 */
void Angle::Servo::setConfig(Angle::Config config, bool refresh) {
    this->angle = NULL_ANGLE;
    this->interval = NULL_INTERVAL;

    this->config = config;
    this->timeout = long(1.0 / float(this->config.pwmFreq) * 1000000.0);

    if (refresh) {
        this->turn(config.minAngle, true);
    }
}
/** Returns the current config of the servo drive.
 */
Angle::Config Angle::Servo::getConfig() {
    return this->config;
}

/** Attaches the servo drive to the pin.
 */
void Angle::Servo::attach(unsigned char pin) {
    if (pin == NULL_PIN) {
        return;
    }

    this->pin = pin;
    pinMode(pin, OUTPUT);
}

/** Detaches the servo drive from the pin.
 */
void Angle::Servo::detach() {
    pinMode(this->pin, INPUT);
    this->pin = NULL_PIN;
}

/** Turns the servo drive to the specified angle.
 *
 * The asyncMode flag indicates whether to turn the servo drive
 * in the blocking or asynchronous mode.
 */
void Angle::Servo::turn(uint16_t angle, bool asyncMode) {
    // Normalize and check angle.
    angle = angle < this->config.minAngle ? this->config.minAngle : angle;
    angle = angle > this->config.maxAngle ? this->config.maxAngle : angle;
    if (this->angle == angle)
        return;

    uint16_t pulse = map(angle, this->config.minAngle, this->config.maxAngle,
                         this->config.minPulse, this->config.maxPulse);

    if (asyncMode) {
        this->asyncTurn(angle, pulse);
    } else {
        this->syncTurn(angle, pulse);
    }
}

/** Returns true if the servo drive is busy.
 *
 * This indicator can be used in asynchronous mode to understand
 * whether the servo has finished rotating.
 *
 * Always returns False in blocking mode.
 */
bool Angle::Servo::isBusy() {
    return this->interval != NULL_INTERVAL;
}

// Returns time to rotate on the one degree.
unsigned long Angle::Servo::t() {
    return long(float(this->config.maxSpeed) / 60.0 * 1000.0);
}

// Async turn.
void Angle::Servo::asyncTurn(uint16_t angle, uint16_t pulse) {
    if (this->interval == NULL_INTERVAL) {
        this->interval = abs(int(angle) - int(this->angle)) * this->t();
        this->delta = this->timeout;
        this->timestamp = micros();
    }

    long delta = micros() - this->timestamp < 0
                     ? ULONG_MAX - this->timestamp + micros()
                     : micros() - this->timestamp;

    if (delta >= timeout - pulse) {
        digitalWrite(this->pin, HIGH);
        delayMicroseconds(pulse);
        digitalWrite(this->pin, LOW);
        this->delta += this->timeout;
        this->timestamp = micros();
    }

    if (this->delta >= this->interval + this->timeout) {
        this->angle = angle;
        this->interval = NULL_INTERVAL;
    }
}

// Blocking turn.
void Angle::Servo::syncTurn(uint16_t angle, uint16_t pulse) {
    this->interval = abs(int(angle) - int(this->angle)) * this->t();
    this->delta = 0;

    do {
        digitalWrite(this->pin, HIGH);
        delayMicroseconds(pulse);
        digitalWrite(this->pin, LOW);
        delayMicroseconds(this->timeout - pulse);
        this->delta += this->timeout;
    } while (this->delta < this->interval);

    this->angle = angle;
    this->interval = NULL_INTERVAL;
}

// split::file example.ino
// split::include #include "angle.h"

#define ASYNC_MODE false
#define WOKWI
// #define TINKERCAD

using namespace Angle;

#ifdef TINKERCAD  // tinkercad simulation
int pins[] = {4, 3, 5, 2, 1, 0};
const Config config = {0, 360, 460, 5000, 100, 200};
#endif

#ifdef WOKWI  // wokwi simultaion
int pins[] = {4, 3, 5, 2, 1, 0};
const Config config = {0, 177, 544, 2466, 120, 50};
#endif

#if !defined(TINKERCAD) && !defined(WOKWI)  // real servo
int pins[] = {5, 9};
const Config config = {0, 180, 500, 2500, 100, 50};
#endif

Servo *servos = new Servo(sizeof(pins) / sizeof(int));
unsigned long interval, timestamp;
bool isOpen;

void setup() {
    for (int i = 0; i < sizeof(pins) / sizeof(int); i++) {
        servos[i] = Servo(pins[i]);
        servos[i].setConfig(config);
        servos[i].turn(0);
    }

    isOpen = true;
    timestamp = millis();
}

void loop() {
    if (ASYNC_MODE) {
        if (millis() - timestamp >= 1000) {
            for (int i = 0; i < sizeof(pins) / sizeof(int); i++) {
                servos[i].turn(isOpen ? 180 : 0, true);
            }
            if (!servos[0].isBusy()) {
                isOpen = !isOpen;
                timestamp = millis();
            }
        }
    } else {
        for (int i = 0; i < sizeof(pins) / sizeof(int); i++) {
            servos[i].turn(isOpen ? 180 : 0, false);
        }
        isOpen = !isOpen;
        //delay(1000);
    }
}