// Raspberry Pi LPC1114 I/O Processor Expansion Board SPI Agent Firmware
// RC servo output test

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

using System;
using System.Collections.Generic;

using SPIAgent;

namespace test_servo
{
    class Program
    {
        static void Main(string[] args)
        {
            Servo s;
            int channel;
            float position;

            Console.WriteLine("\nRaspberry Pi LPC1114 I/O Processor Expansion Board\nServo Output Test\n");

            if (args.Length != 1)
            {
                Console.WriteLine("Usage: test_servo <hostname>");
                Environment.Exit(1);
            }

            // Connect to the SPIAgent Firmware RPC server

            ITransport spiagent = new Transport(args[0]);

            // Configure the servo outputs, and save them in a Dictionary object

            Dictionary<int, Servo> Servos = new Dictionary<int, Servo>();

            Servos.Add(1, new Servo(spiagent, Pins.LPC1114_PWM1));
            Servos.Add(2, new Servo(spiagent, Pins.LPC1114_PWM2));
            Servos.Add(3, new Servo(spiagent, Pins.LPC1114_PWM3));
            Servos.Add(4, new Servo(spiagent, Pins.LPC1114_PWM4));

            for (;;)
            {
                Console.Write("Enter PWM channel number (1-4):      ");

                try
                {
                    channel = int.Parse(Console.ReadLine());
                    if (channel == 0) break;
                    if ((channel < 1) || (channel > 4)) continue;
                }
                catch
                {
                    continue;
                }

                Console.Write("Enter servo position (-1.0 to +1.0): ");

                try
                {
                    position = float.Parse(Console.ReadLine());
                    if (position < Servo.SERVO_MIN_POSITION) continue;
                    if (position > Servo.SERVO_MAX_POSITION) continue;
                }
                catch
                {
                    continue;
                }

                Servos.TryGetValue(channel, out s);
                s.position = position;
            }

            Environment.Exit(0);
        }
    }
}