#include <include/a4988.h>
#include <pigpio.h>
#include <queue>
#include <cmath>
#include <thread>
#include <string>
#include <memory>

#include <iostream>

namespace simon {
  namespace a4988 {
  c_allegro_4988::c_allegro_4988(std::unique_ptr<a4988_settings_t> ptr_set)
    : m_ptr_set(std::move(ptr_set)), m_initialized(false), m_alive(true)
  {

  }
  void c_allegro_4988::run(void)
  {
    if (!m_initialized) {
      initialize();
    }
    while(m_alive)
    {
      std::cout << "In thread" << std::endl;
      if (!m_queue.empty()) {
        auto cmd = m_queue.front();
        // Distinguish between different tytpes of commands and derive type
        if (*cmd == "move") {
          auto move = std::static_pointer_cast<c_move>(cmd);
          if (!set_pwm_freq(std::round(1e6/move->period_us))) {
            // TODO: proper error handling
          }
          gpioHardwarePWM(m_ptr_set->gpio_step, m_pwm_freq, PI_HW_PWM_RANGE/2);
          gpioWrite(m_ptr_set->gpio_dir, move->dir);
          std::cout << move->period_us << " - " << move->steps << std::endl;
          auto t_sleep = move->period_us*move->steps;
          std::this_thread::sleep_for(std::chrono::microseconds(t_sleep));
          gpioHardwarePWM(m_ptr_set->gpio_step, m_pwm_freq, 0);
        }
        else if (*cmd == "enable") {
          auto enable = std::static_pointer_cast<c_enable>(cmd);
          gpioWrite(m_ptr_set->gpio_en, enable->enabled);
        }
        else if (*cmd == "kill") {
          m_alive = false;
        }
        else {
          // TODO: Throw an exception here
        }
        m_queue.pop();
      }
      else {
        // Idle
      }
    }
  }
  void c_allegro_4988::post(std::shared_ptr<c_cmd> cmd)
  {
    m_queue.push(cmd);
  }
  void c_allegro_4988::post_move(const unsigned steps, const bool dir,
                                 const unsigned period_us)
  {
    auto move(std::make_shared<c_move>(steps, dir, period_us));
    auto cmd = std::static_pointer_cast<c_cmd>(move);
    m_queue.push(cmd);
  }

  void c_allegro_4988::initialize(void)
  {
    // TODO: proper error management throwing exceptions
    if(m_ptr_set == nullptr) {
      return;
    }
    // Configure clock source for PWM
    gpioCfgClock(CFG_SAMPLE_RATE_US, CFG_PERIPHERAL_SRC_PCM, CFG_DEPRECATED_ARG);
    // Initialize pigpio library
    gpioInitialise();
    // Configure as outputs
    gpioSetMode(m_ptr_set->gpio_en, PI_OUTPUT);
    gpioSetMode(m_ptr_set->gpio_dir, PI_OUTPUT);
    // Initialize outputs
    enable();
    gpioWrite(m_ptr_set->gpio_dir, 0);
    // Set PWM to 0
    m_pwm_freq = 1000;
    gpioHardwarePWM(m_ptr_set->gpio_step, m_pwm_freq, 0);
    m_initialized = true;
  }
  int c_allegro_4988::enable(void)
  {
    // TODO: proper error handling
    auto enable(std::make_shared<c_enable>(true));
    auto cmd = std::static_pointer_cast<c_cmd>(enable);
    m_queue.push(cmd);
    return 0;
  }
  int c_allegro_4988::disable(void)
  {
    // TODO: proper error handling
    auto enable(std::make_shared<c_enable>(false));
    auto cmd = std::static_pointer_cast<c_cmd>(enable);
    m_queue.push(cmd);
    return 0;
  }
  void c_allegro_4988::kill(void)
  {
    // TODO: proper error handling
    auto kill(std::make_shared<c_kill>());
    auto cmd = std::static_pointer_cast<c_cmd>(kill);
    m_queue.push(cmd);
  }
  int c_allegro_4988::set_pwm_freq(const unsigned pwm_freq)
  {
    if (pwm_freq > PI_HW_PWM_MAX_FREQ) {
      return 1;
    }
    m_pwm_freq = pwm_freq;
    return 0;
  }
 } // namespace a4988
} // namespace simon