// LabView LINX Remote I/O RC servo services specific to the
// Raspberry Pi LPC1114 I/O Processor Expansion Board

// Copyright (C)2016-2018, Philip Munts, President, Munts AM Corp.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include <string.h>

#include <spi-agent.h>

#include "linx-server/common.h"
#include "lpc1114-servo.h"

//***************************************************************************

// Servo output channel constructor

Servo_LPC1114::Servo_LPC1114(int32_t pin, int32_t *error)
{
  this->pin = -1;

  if (!IS_PWM(pin))
  {
    *error = ENODEV;
    return;
  }

  spiagent_servo_configure(pin, error);
  if (*error) return;

  this->pin = pin;
  this->configured = true;
  *error = 0;
}

//***************************************************************************

// Servo output channel constructor, with deferred configuration flag

Servo_LPC1114::Servo_LPC1114(int32_t pin, int32_t deferconfig, int32_t *error)
{
  this->pin = -1;

  if (!IS_PWM(pin))
  {
    *error = ENODEV;
    return;
  }

  if (deferconfig)
  {
    this->pin = pin;
    this->configured = false;
  }
  else
  {
    spiagent_servo_configure(pin, error);
    if (*error) return;

    this->pin = pin;
    this->configured = true;
  }

  *error = 0;
}

//***************************************************************************

// Servo output channel write() method

void Servo_LPC1114::write(uint16_t pulsewidth, int32_t *error)
{
  float s = pulsewidth/500.0 - 3.0;

  // Configure the servo output,  if necessary

  if (!this->configured)
  {
    spiagent_servo_configure(this->pin, error);
    if (*error) return;

    this->configured = true;
  }

  spiagent_servo_put(this->pin, s, error);
}

//***************************************************************************

void lpc1114_servo_init(void)
{
  int32_t error;

  // Register servo output channel objects

  servo_add_channel(LPC1114_PWM1, new Servo_LPC1114(LPC1114_PWM1, true, &error));
  servo_add_channel(LPC1114_PWM2, new Servo_LPC1114(LPC1114_PWM2, true, &error));
  servo_add_channel(LPC1114_PWM3, new Servo_LPC1114(LPC1114_PWM3, true, &error));
  servo_add_channel(LPC1114_PWM4, new Servo_LPC1114(LPC1114_PWM4, true, &error));
}