/*- AdvancedServo simple -
0****************************************************************************************
 * This example simply displays the Phidget AdvancedServo serial number when it is 
 * attached and sets the servo positions, velocity, and acceleration to 0.  Then we will 
 * do a simple simulation of a basic movement of a servo motor at 100.00 velocity and 
 * 100.00 Acceleration.  I decided to leave out the current change event handler for 
 * easier readability. For a more detailed example, see the Servo-full example.
 * 
 * Please note that this example was designed to work with only one Phidget AdvanceServo 
 * connected. 
 * For an example showing how to use two Phidgets of the same time concurrently, please see the
 * Servo-multi example in the Servo Examples.

 * Copyright 2007 Phidgets Inc.  All rights reserved.*/

using System;
using System.Collections.Generic;
using System.Text;
//Needed for the AdvancedServo class, phidget base classes, and PhidgetException class

//Needed for the event handling classes 
using Phidgets.Events; 
//Using this simply for the sleep() method so that the for loop will wail for the motor 
//to finish moving to the previous new position before setting a new position
using System.Threading;



using ebbits.Robots;
using ebbits.Arduino;

namespace ebbits {
  class Program {
    static void Main(string[] args) {
      Logger.LogSerial = false;
      MainArduino a = new MainArduino("COM50");
      //AbstractRobot l = new LaserBot(169861, a);
      AbstractRobot p = new PaintBot(169889, a);
      //AbstractRobot g = new GlassBot(169887);

      Console.WriteLine("All Engaged. Press Key to Start.");
      Console.ReadLine();

      while(true) {
        a.clearStack();
        a.trainOn(1);
        if(a.whereRead() != 5) {
          Logger.Warn("Achtung! Steht nicht auf Start!");
          //Console.ReadLine();
          if(!a.findStart()) {
            Logger.Warn("ABBRUCH! Konnte die Bahn nicht Finden oder einstellen!");
            //Console.ReadLine();
            continue;
          }
        }
        while(a.trainSwitchIsOff()) {
          Thread.Sleep(100);
        }

        Logger.Message("Move from Start to Welding Pos");
        a.trainMove(70);
        a.measureAndStop('r', 1);

        Logger.Message("Welding");
        Thread.Sleep(1000);//l.run();
        
        Logger.Message("Move from Welding to Paint Pos");
        a.trainMove(68);
        a.measureAndStop('r', 2);

        Logger.Message("Paint");
        p.run();
        Console.ReadLine();

        Logger.Message("Move from Paint to Glas Pos");
        a.trainMove(68);
        a.measureAndStop('r', 3);

        Logger.Message("Glas");
        Thread.Sleep(1000);//g.run();

        Logger.Message("Move from Glas to Turn around Pos");
        a.trainMove(78);
        a.measureAndStop('r',6);

        Logger.Message("Turn aroud and move from Turn around to Waiting Pos");
        System.Threading.Thread.Sleep(500);
        a.changeDir(0);
        a.trainMove(90);
        a.readStop('r',4);

        Logger.Message("Move from Waiting Pos to Start Pos");
        a.trainMove(90);
        a.readStop('r', 5);

        Logger.Info("Runde zuende!");
      }
    }
  }
}