/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008-2017. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#include "Task.h"

#include <signal.h>

#include <cerrno>

#include "WPIErrors.h"

namespace frc {

const int Task::kDefaultPriority;

Task& Task::operator=(Task&& task) {
  m_thread.swap(task.m_thread);
  m_taskName = std::move(task.m_taskName);

  return *this;
}

Task::~Task() {
  if (m_thread.joinable()) {
    std::cout << "[HAL] Exited task " << m_taskName << std::endl;
  }
}

bool Task::joinable() const noexcept { return m_thread.joinable(); }

void Task::join() { m_thread.join(); }

void Task::detach() { m_thread.detach(); }

std::thread::id Task::get_id() const noexcept { return m_thread.get_id(); }

std::thread::native_handle_type Task::native_handle() {
  return m_thread.native_handle();
}

/**
 * Verifies a task still exists.
 *
 * @return true on success.
 */
bool Task::Verify() { return VerifyTaskId() == TASK_OK; }

/**
 * Gets the priority of a task.
 *
 * @return task priority or 0 if an error occured
 */
int Task::GetPriority() {
  int priority;
  if (HandleError(GetTaskPriority(&priority)))
    return priority;
  else
    return 0;
}

/**
 * This routine changes a task's priority to a specified priority.
 * Priorities range from 1, the lowest priority, to 99, the highest priority.
 * Default task priority is 60.
 *
 * @param priority The priority at which the internal thread should run.
 * @return true on success.
 */
bool Task::SetPriority(int priority) {
  return HandleError(SetTaskPriority(priority));
}

/**
 * Returns the name of the task.
 *
 * @return The name of the task.
 */
std::string Task::GetName() const { return m_taskName; }

Task::TASK_STATUS Task::VerifyTaskId() {
  auto task = m_thread.native_handle();
  if (pthread_kill(task, 0) == 0) {
    return TASK_OK;
  } else {
    return TASK_ERROR;
  }
}

Task::TASK_STATUS Task::GetTaskPriority(int32_t* priority) {
  auto task = m_thread.native_handle();
  int32_t policy = 0;
  struct sched_param param;

  if (VerifyTaskId() == TASK_OK &&
      pthread_getschedparam(task, &policy, &param) == 0) {
    *priority = param.sched_priority;
    return TASK_OK;
  } else {
    return TASK_ERROR;
  }
}

Task::TASK_STATUS Task::SetTaskPriority(int32_t priority) {
  auto task = m_thread.native_handle();
  int32_t policy = 0;
  struct sched_param param;

  if (VerifyTaskId() == TASK_OK &&
      pthread_getschedparam(task, &policy, &param) == 0) {
    param.sched_priority = priority;
    if (pthread_setschedparam(task, SCHED_FIFO, &param) == 0) {
      return TASK_OK;
    } else {
      return TASK_ERROR;
    }
  } else {
    return TASK_ERROR;
  }
}

/**
 * Handles errors generated by task related code.
 */
bool Task::HandleError(TASK_STATUS results) {
  if (results != TASK_ERROR) return true;
  int errsv = errno;
  if (errsv == TaskLib_ILLEGAL_PRIORITY) {
    wpi_setWPIErrorWithContext(TaskPriorityError, m_taskName.c_str());
  } else {
    std::printf("ERROR: errno=%i", errsv);
    wpi_setWPIErrorWithContext(TaskError, m_taskName.c_str());
  }
  return false;
}

}  // namespace frc