-- Raspberry Pi LPC1114 I/O Processor Expansion Board Ada servo test program

-- Copyright (C)2013-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.

WITH Ada.Command_Line;      USE Ada.Command_Line;
WITH Ada.Strings.Unbounded; USE Ada.Strings.Unbounded;
WITH Ada.Text_IO;           USE Ada.Text_IO;

WITH spi_agent.exceptions;  USE spi_agent.exceptions;
WITH spi_agent.pins;        USE spi_agent.pins;
WITH spi_agent.pwm;         USE spi_agent.pwm;
WITH spi_agent.transport;   USE spi_agent.transport;

PROCEDURE test_servo IS

  servername : Unbounded_String;
  error      : Integer;
  Outputs    : ARRAY (pwm_channel_t) OF Servo;
  Channel    : Integer;
  Position   : servo_position_t;

  PACKAGE int_io IS NEW Integer_IO(Integer);
  USE int_io;

  PACKAGE servoposition_io IS NEW Float_IO(servo_position_t);
  USE servoposition_io;

BEGIN
  New_Line;
  Put_Line("Raspberry Pi LPC1114 I/O Processor Expansion Board Servo Output Test");
  New_Line;

  -- Analyze command line parameters

  IF Argument_Count = 0 THEN
    servername := To_Unbounded_String("localhost");
  ELSIF Argument_Count = 1 THEN
    servername := To_Unbounded_String(Argument(1));
  ELSE
    Put_Line("Usage: " & Command_Name & " [hostname]");
    RETURN;
  END IF;

  -- Initialize the SPI Agent Firmware transport library

  spi_agent.transport.open(To_String(servername) & ASCII.NUL, error);

  IF error /= 0 THEN
    RAISE SpiAgentError WITH
      "spi_agent.transport.open() failed, error:" & Integer'IMAGE(error);
  END IF;

  -- Configure servo outputs

  FOR c IN pwm_channel_t LOOP
    Outputs(c) := Create(c);
    Outputs(c).Put(SERVO_NEUTRAL);
  END LOOP;

  -- Set servo positions

  Mainloop : LOOP
    Put("Enter servo channel number (1-4):      ");
    Get(Channel, 0);

    IF Channel = 0 THEN
      EXIT Mainloop;
    ELSIF Channel >= 1 AND Channel <= 4 THEN
      Put("Enter servo position (-1.0 to +1.0): ");
      Get(Position, 0);
      Outputs(pwm_channel_t(Channel)).Put(Position);
    END IF;
  END LOOP Mainloop;

  -- Deinitialize the SPI Agent Firmware transport library

  spi_agent.transport.close(error);

  IF error /= 0 THEN
    RAISE SpiAgentError WITH
      "spi_agent.transport.close() failed, error:" & Integer'IMAGE(error);
  END IF;
END test_servo;