#include <include/stepper.h>
#include <thread>
#include <functional>
#include <iostream>
#include <fstream>
#include <memory>
#include <cmath>

namespace simon {
  namespace stepper {
    c_stepper::c_stepper(const std::string& axis, const std::string& fname)
    {
      auto file_content = read_file(fname);
      auto j_set = json::parse(file_content);
      if (!j_set["axis_configurations"].is_array()) {
        std::cout << "Expected an array in the configuration below \"axis_configurations\"." << "\n";
        // TODO: Throw exception
      }
      extract_settings(j_set, axis);
      m_driver_thread = std::thread(std::bind(&a4988::c_allegro_4988::run, &(*m_ptr_a4988)));
    }

    c_stepper::~c_stepper()
    {
      m_ptr_a4988->kill();
      m_driver_thread.join();
    }

    void c_stepper::extract_settings(const json& j_set, const std::string& axis)
    {
      int ind = 0;
      for (const auto& j_axis_conf : j_set["axis_configurations"]) {
        ind++;
        if (j_axis_conf["id"].is_string()) {
          if (j_axis_conf["id"] == axis) {
            auto a_ptr = std::make_unique<a4988::a4988_settings_t>(
              a4988::a4988_settings_t ({
                // TODO: Do more sanitizing before retrieving this data
                .gpio_en = j_axis_conf["a4988"]["gpio_en"],
                .gpio_step = j_axis_conf["a4988"]["gpio_step"],
                .gpio_dir = j_axis_conf["a4988"]["gpio_dir"],
                .gpio_microsteps = j_axis_conf["a4988"]["microsteps"]
              })
            );
            m_ptr_a4988 = std::make_unique<a4988::c_allegro_4988>(std::move(a_ptr));
            m_stepper = {
               // TODO: Do more sanitizing before retrieving this data
              .gear_reduction = j_axis_conf["stepper"]["gear_reduction"],
              .steps_per_revolution = j_axis_conf["stepper"]["steps_per_revolution"],
              .clockwise = j_axis_conf["stepper"]["clockwise"]
            };
            break;
          }
        } else {
          // TODO:Throw exception
          std::cerr << "Expected a string in the configuration below [\"axis_configurations\"][" << ind << "][\"id\"]\n";
        }
      }
    }

    std::string c_stepper::read_file(const std::string& fname) const
    {
      std::ifstream input_stream(fname);
      if (!input_stream) {
        // TODO: Throw exception here
        std::cerr << "Can't open input file!";
      }
      std::string line;
      std::string jstring;
      while (getline(input_stream, line)) {
          jstring += line;
      }
      return jstring;
    }

    float c_stepper::rotate(const float& degrees)
    {
      auto steps_360 =  m_stepper.gear_reduction
                      * m_stepper.steps_per_revolution
                      * static_cast<float>(m_ptr_a4988->get_microsteps());
      auto steps = static_cast<unsigned>((std::abs(degrees)/360.0f) * steps_360);
      bool direction = (degrees < 0) ? !m_stepper.clockwise : m_stepper.clockwise;
      m_ptr_a4988->post_move(steps, direction, 100);
      return steps;
    }
  }
}