#include "DigitalInput.h"

#include <Arduino.h>

namespace RoboidControl {
namespace Arduino {

#pragma region Digital input

DigitalInput::DigitalInput(unsigned char pin, Thing& parent) : Thing(parent) {
  this->pin = pin;
  pinMode(this->pin, INPUT);
  std::cout << "digital input start\n";
}

void DigitalInput::Update(bool recursive) {
  this->isHigh = digitalRead(this->pin);
  this->isLow = !this->isHigh;
  Thing::Update(recursive);
}

#pragma endregion Digital input

#pragma region Touch sensor

DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing& parent)
    : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}

void DigitalInput::TouchSensor::Update(bool recursive) {
  this->touchedSomething = digitalInput.isLow;
}

#pragma endregion Touch sensor

#pragma region Relative encoder

int DigitalInput::RelativeEncoder::interruptCount = 0;
volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0;
volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0;

DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config,
                                               Thing& parent)
    : RoboidControl::RelativeEncoder(parent),
      digitalInput(config.pin, parent),
      pulsesPerRevolution(config.pulsesPerRevolution) {}

void DigitalInput::RelativeEncoder::Start() {
  // We support up to 2 pulse counters
  if (interruptCount == 0)
    attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0,
                    RISING);
  else if (interruptCount == 1)
    attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt1,
                    RISING);
  else {
    // maximum interrupt count reached
    std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached"
              << std::endl;
    return;
  }
  interruptIx = interruptCount;
  interruptCount++;
  std::cout << "pin ints. " << interruptCount << std::endl;
}

int DigitalInput::RelativeEncoder::GetPulseCount() {
  if (interruptIx == 0) {
    int pulseCount = pulseCount0;
    pulseCount0 = 0;
    return pulseCount;
  } else if (interruptIx == 1) {
    int pulseCount = pulseCount1;
    pulseCount1 = 0;
    return pulseCount;
  }
  return 0;
}

void DigitalInput::RelativeEncoder::Update(bool recursive) {
  unsigned long currentTimeMs = GetTimeMs();
  if (this->lastUpdateTime == 0) {
    this->Start();
    this->lastUpdateTime = currentTimeMs;
    return;
  }

  float timeStep = (float)(currentTimeMs - this->lastUpdateTime) / 1000.0f;
  int pulseCount = GetPulseCount();
  netPulseCount += pulseCount;

  this->pulseFrequency = pulseCount / timeStep;
  this->rotationSpeed = pulseFrequency / pulsesPerRevolution;

  // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
  //           << " timestep " << timeStep << std::endl;

  this->lastUpdateTime = currentTimeMs;
}

#if defined(ESP8266) || defined(ESP32)
void ICACHE_RAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt0() {
  pulseCount0++;
}
#else
void DigitalInput::RelativeEncoder::PulseInterrupt0() {
  pulseCount0++;
}
#endif

#if defined(ESP8266) || defined(ESP32)
void IRAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt1() {
  pulseCount1++;
}
#else
void DigitalInput::RelativeEncoder::PulseInterrupt1() {
  pulseCount1++;
}
#endif

#pragma endregion Relative encoder

}  // namespace Arduino
}  // namespace RoboidControl