/*
 * Detector.cpp
 *
 *  Created on: Apr 5, 2018
 *      Author: Andreas Schneider, Johannes Wendel, Jonas Zeunert, Rafael Vinci, Dr. Franca Rupprecht
 */

#include "Detector.h"

#include <iostream>
#include <math.h>

#include "../lib/wiringPi/wiringPi.h"

namespace Input
{

Detector::Detector(std::map<std::string, char> input_config, std::map<char, InputEvent> events) :
  gpio(input_config), input_events(events), is_running(true)
{
  detect_thread = std::thread(&Detector::detect, this);
  notify_thread = std::thread(&Detector::notify_handlers, this);
}

Detector::~Detector()
{
  is_running = false;

  notify_thread.join();
  detect_thread.join();
}

void Detector::register_input_event_handler(InputEventHandler* handler)
{
  LOCK_EVENT_HANDLER();
  event_handler.insert(handler);
}

void Detector::unregister_input_event_handler(InputEventHandler* handler)
{
  LOCK_EVENT_HANDLER();
  event_handler.erase(handler);
}

void Detector::notify_handlers()
{
  while(is_running)
  {
    // return if no event in queue
    if(event_queue.empty())
    {
      std::this_thread::yield();
      return;
    }

    InputEvent event = event_queue.front();
    event_queue.pop();

    // getting a guard and calling all registered handlers
    LOCK_EVENT_HANDLER();
    for(auto* handler : event_handler)
    {
      // todo timeout!
      handler->handle(event);
    }
  }
}

// Cycles over all inputs and enqueues an input event if detected.
void Detector::detect()
{
  while(is_running)
  {
    char address;
    if(check_inputs(address))
    {
      try
      {
        InputEvent& event = input_events.at(address);
        event_queue.emplace(event);
      }
      catch(std::out_of_range& e)
      {
        // todo log exception!
      }
    }
  }
}

// todo work with gpiointerface
bool Detector::check_inputs(char& address)
{
  for(int row = 0; row < 8; row++)
  {
    digitalWrite(gpio["ROW_A"], row & 0b001);
    digitalWrite(gpio["ROW_B"], row & 0b010);
    digitalWrite(gpio["ROW_C"], row & 0b100);

    for(int col = 0; col < 8; col++)
    {
      digitalWrite(gpio["COL_A"], col & 0b001);
      digitalWrite(gpio["COL_B"], col & 0b010);
      digitalWrite(gpio["COL_C"], col & 0b100);

      // wait for mux to set address
      std::this_thread::sleep_for(std::chrono::nanoseconds(SLEEP_DURATION_NANO));
      if(digitalRead(gpio["INPUT"]))
      {
        address = pow(2, row) + col;
        return true;
      }
    }
  }
  return false;
}

}