// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2015/05/27
//  filename:   Controller.cpp
//
//  author:     Gildas Bayard
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Utility classes for controllers
//
//
/*********************************************************************/
#include "Controller.h"

using namespace flair::core;

namespace flair {
namespace sensor {

RumbleMessage::RumbleMessage(unsigned int leftForce, unsigned int leftTimeout,
                             unsigned int rightForce, unsigned int rightTimeout)
    : Message(sizeof(ControllerAction) + sizeof(leftForce) +
              sizeof(leftTimeout) + sizeof(rightForce) + sizeof(rightTimeout)) {
  SetLeftForce(leftForce);
  SetLeftTimeout(leftTimeout);
  SetRightForce(rightForce);
  SetRightTimeout(rightTimeout);
  buffer[0] = (char)ControllerAction::Rumble;
}

unsigned int RumbleMessage::GetLeftForce() const {
  unsigned int leftForce;
  memcpy(&leftForce, buffer + leftForceOffset, sizeof(leftForce));
  return leftForce;
}

unsigned int RumbleMessage::GetLeftTimeout() const {
  unsigned int leftTimeout;
  memcpy(&leftTimeout, buffer + leftTimeoutOffset, sizeof(leftTimeout));
  return leftTimeout;
}

unsigned int RumbleMessage::GetRightForce() const {
  unsigned int rightForce;
  memcpy(&rightForce, buffer + rightForceOffset, sizeof(rightForce));
  return rightForce;
}

unsigned int RumbleMessage::GetRightTimeout() const {
  unsigned int rightTimeout;
  memcpy(&rightTimeout, buffer + rightTimeoutOffset, sizeof(rightTimeout));
  return rightTimeout;
}

void RumbleMessage::SetLeftForce(unsigned int leftForce) {
  memcpy(buffer + leftForceOffset, &leftForce, sizeof(leftForce));
}

void RumbleMessage::SetLeftTimeout(unsigned int leftTimeout) {
  memcpy(buffer + leftTimeoutOffset, &leftTimeout, sizeof(leftTimeout));
}

void RumbleMessage::SetRightForce(unsigned int rightForce) {
  memcpy(buffer + rightForceOffset, &rightForce, sizeof(rightForce));
}

void RumbleMessage::SetRightTimeout(unsigned int rightTimeout) {
  memcpy(buffer + rightTimeoutOffset, &rightTimeout, sizeof(rightTimeout));
}

SwitchLedMessage::SwitchLedMessage(bool isOn, unsigned int ledId)
    : Message(sizeof(ControllerAction) + sizeof(isOn) + sizeof(ledId)) {
  if (isOn)
    SetOn();
  else
    SetOff();
  SetLedId(ledId);
}

bool SwitchLedMessage::IsOn() const {
  bool isOn;
  memcpy(&isOn, buffer + isOnOffset, sizeof(isOn));
  return isOn;
}

unsigned int SwitchLedMessage::GetLedId() const {
  unsigned int ledId;
  memcpy(&ledId, buffer + ledIdOffset, sizeof(ledId));
  return ledId;
}

void SwitchLedMessage::SetOn() {
  bool isOn = true;
  memcpy(buffer + isOnOffset, &isOn, sizeof(isOn));
}

void SwitchLedMessage::SetOff() {
  bool isOn = false;
  memcpy(buffer + isOnOffset, &isOn, sizeof(isOn));
}

void SwitchLedMessage::SetLedId(unsigned int ledId) {
  memcpy(buffer + ledIdOffset, &ledId, sizeof(ledId));
}

FlashLedMessage::FlashLedMessage(unsigned int ledId, unsigned int onTime,
                                 unsigned int offTime)
    : Message(sizeof(ControllerAction) + sizeof(ledId) + sizeof(onTime) +
              sizeof(offTime)) {
  SetLedId(ledId);
  SetOnTime(onTime);
  SetOffTime(offTime);
}

unsigned int FlashLedMessage::GetLedId() const {
  unsigned int ledId;
  memcpy(&ledId, buffer + ledIdOffset, sizeof(ledId));
  return ledId;
}

unsigned int FlashLedMessage::GetOnTime() const {
  unsigned int onTime;
  memcpy(&onTime, buffer + onTimeOffset, sizeof(onTime));
  return onTime;
}

unsigned int FlashLedMessage::GetOffTime() const {
  unsigned int offTime;
  memcpy(&offTime, buffer + offTimeOffset, sizeof(offTime));
  return offTime;
}

void FlashLedMessage::SetLedId(unsigned int ledId) {
  memcpy(buffer + ledIdOffset, &ledId, sizeof(ledId));
}

void FlashLedMessage::SetOnTime(unsigned int onTime) {
  memcpy(buffer + onTimeOffset, &onTime, sizeof(onTime));
}

void FlashLedMessage::SetOffTime(unsigned int offTime) {
  memcpy(buffer + offTimeOffset, &offTime, sizeof(offTime));
}

} // end namespace sensor
} // end namespace flair