#include "rotaryencoder.h"

#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <vector>

#include <QDebug>
#include <QElapsedTimer>

#include <wiringPi.h>

// got from https://gist.github.com/ast/a19813fce9d34c7240091db11b8190dd
// https://gist.github.com/ast
// Inspired by Paul Stoffregen's excellent Arduino library Encoder:
// https://github.com/PaulStoffregen/Encoder

constexpr int gpioA = 17;
constexpr int gpioB = 27;
// constexpr int gpioA = 0;
// constexpr int gpioB = 2;
// constexpr int gpioA = 11;
// constexpr int gpioB = 13;

const std::vector<int> encoderTable = {
    0, 1, -1, 0, -1, 0, 0, 1, 1, 0, 0, -1, 0, -1, 1, 0
};

volatile int64_t positionSteps;
volatile uint8_t state;

void pin_isr(void) {
    uint8_t p1val = digitalRead(gpioA);
    uint8_t p2val = digitalRead(gpioB);
    uint8_t s = state & 3;

    if (p1val) s |= 4;
    if (p2val) s |= 8;

    state = (s >> 2);

    switch (s) {
    case 1: case 7: case 8: case 14:
        positionSteps = positionSteps + 1;
        return;
    case 2: case 4: case 11: case 13:
        positionSteps = positionSteps - 1;
        return;
    case 3: case 12:
        positionSteps = positionSteps + 2;
        return;
    case 6: case 9:
        positionSteps = positionSteps - 2;
        return;
    }
}

rotaryencoder::rotaryencoder()
{
    QElapsedTimer t;
    t.start();

    if (wiringPiSetupGpio()) {
        perror("wiringPiSetupGpio");
        exit(EXIT_FAILURE);
    }

    qDebug() << "msecs to setup wiringPi:" << t.elapsed();

    if ( wiringPiISR (gpioA, INT_EDGE_BOTH, &pin_isr) < 0 ) {
        perror("wiringPiISR");
        exit(EXIT_FAILURE);
    }

    qDebug() << "msecs to register interruption A:" << t.elapsed();

    if ( wiringPiISR (gpioB, INT_EDGE_BOTH, &pin_isr) < 0 ) {
        perror("wiringPiISR");
        exit(EXIT_FAILURE);
    }

    qDebug() << "msecs to register interruption B:" << t.elapsed();

    // pinMode (gpioA,  INPUT) ;
    // pinMode (gpioB,  INPUT) ;
    // pullUpDnControl(gpioA, PUD_UP);
    // pullUpDnControl(gpioB, PUD_UP);

    qDebug() << "encoder is ok";

    // // Show position every second
    // while ( 1 ) {
    //     constexpr double stepsPerMm = 200;
    //     const double positionMm = ::positionSteps / stepsPerMm;
    //     qDebug() << ::positionSteps
    //              << '-'
    //              << QString::number(positionMm, 'f', 3)
    //              << "(mm)";
    //     // printf( "%ld\n", ::position);
    //     usleep( 100 * 1000 ); // wait 1 second
    // }
}

int64_t rotaryencoder::position() const
{
    return ::positionSteps;
}