/*- 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
using Phidgets;
//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 System.IO.Ports;

namespace AdvancedServo_simple {
  class Program {
    private static AdvancedServo advServo;
    private static SerialPort led;
    static void Main(string[] args) {
      connectLed("COM50");
      trainOn(1);
      Console.WriteLine("Train Engaged. Press Key");
      Console.ReadLine();
      trainMove();
      readStop();
      //Console.WriteLine("Wait to its stop. Press Key, then it Laser and Move");
      //Console.ReadLine();
      controlLaser(169861);
      trainMove();
      readStop();
      //Console.WriteLine("Wait to its stop. Press Key, then it Paint and Move");
      //Console.ReadLine();
      controlPaint(169889);
      trainMove();
      readStop();
      //Console.WriteLine("Wait to its stop. Press Key, then it put the Glas and Move");
      //Console.ReadLine();
      controlGlass(169887);
      trainMove();
      readStop();
      //Console.WriteLine("Wait to its stop. Press Key, then it moves Back");
      //Console.ReadLine();
      changeDir(0);
      trainMove();
      readStop();
      //Console.WriteLine("Wait to its stop. Press Key, then it moves Back");
      //Console.ReadLine();
      trainMove();
      led.Close();
    }

    private static void readStop() {
      string r = led.ReadLine();
      while(!System.Text.RegularExpressions.Regex.Match(r, "\\*r[0-9]=[0-9A-Fa-f]*\\$.*#").Success) {
        r = led.ReadLine();
      }
    }

    private static void changeDir(byte dir) {
      serialSend('d', 2, dir); //Richtung = 0
    }

    private static void trainMove() {
      serialSend('b', 2, 0); //Bremse aus
    }

    private static void trainOn(byte dir) {
      serialSend('p', 2, 1); //Speed = 1
      serialSend('d', 2, dir); //Richtung = 1
      serialSend('b', 2, 1); //Bremse = an
    }

    private static void setLaser(byte p) {
      serialSend('l', 1, p);
    }

    private static void setColor(byte p) {
      serialSend('c', 1, p);
    }

    private static int serialReadCurrent() {
      serialSend('u', 1, 0, false);
      string r = led.ReadLine();
      return Int32.Parse(r.Substring(3, (r.Length - 7)));
    }

    private static void serialSend(char key, byte pos, byte val, bool loud = true) {
      string cdata = key + pos + "=" + val;
      char c = '$';
      foreach(char t in cdata) {
        c = (char)(c ^ t);
      }
      string s = "*"+key+pos.ToString()+"="+val.ToString()+"$"+c+"#";
      if(loud) {
        Console.WriteLine("Serial: " + s);
      }
      led.WriteLine(s);
    }

    private static void connectLed(string led_com) {
      led = new SerialPort(led_com, 57600);
      led.Open();
      led.WriteLine("");
    }

    private static void controlLaser(int serial) {
      connectServo(serial);
      if(!advServo.Attached) { return; }

      setStartPosition(new int[] { 0, 77, 175, 155, 84 });
      Thread.Sleep(500);
      //Console.WriteLine("Servos are in Start!");

      setPosition(new int[] { -1, 77, 157, 134, 84 }, 400);
      setPosition(new int[] { -1, 83, 97, 116, 44 }, 200);
      setLaser(1);
      setPosition(new int[] { -1, 102, 90, 108, 42 }, 20);
      setPosition(new int[] { -1, 98, 79, 90, 48 }, 20);
      setPosition(new int[] { -1, 83, 82, 95, -1 }, 20);
      setPosition(new int[] { -1, 83, 97, 116, 44 }, 20);
      setLaser(0);
      setPosition(new int[] { -1, 77, 157, 134, 84 }, 400);
      setPosition(new int[] { -1, 77, 175, 155, 84 }, 400);
      //Thread.Sleep(100);
      shutdownServo();
    }

    private static void controlPaint(int serial) {
      connectServo(serial);
      if(!advServo.Attached) { return; }
      /* not needed, turn, */
      setStartPosition(new int[] { 0, 87, 175, 160, 92 });
      Thread.Sleep(500);
      //Console.WriteLine("Servos are in Start!");

      setPosition(new int[] { -1, 87, 153, 138, 92 }, 400);
      setPosition(new int[] { -1, 44, 111, 161, 63 }, 200);
      setColor(2);
      setPosition(new int[] { -1, 90, -1, -1, -1 }, 50);
      setColor(0);
      setPosition(new int[] { -1, -1, 140, -1, -1 }, 200);
      setPosition(new int[] { -1, -1, 71, 55, 110 }, 200);
      setPosition(new int[] { -1, -1, 43, 42, 175 }, 200);
      setColor(2);
      setPosition(new int[] { -1, 60, 33, 22, -1 }, 50);
      setColor(0);
      setPosition(new int[] { -1, -1, 63, 45, 150 }, 200);
      setPosition(new int[] { -1, 50, 68, 76, 132 }, 200);
      setColor(7);
      setPosition(new int[] { -1, -1, -1, -1, 160 }, 100);
      setPosition(new int[] { -1, 60, -1, -1, 172 }, 100);
      setPosition(new int[] { -1, -1, -1, -1, 144 }, 100);
      setPosition(new int[] { -1, 70, -1, -1, 150 }, 100);
      setPosition(new int[] { -1, -1, -1, -1, 175 }, 100);
      setPosition(new int[] { -1, 80, -1, -1, -1 }, 100);
      setPosition(new int[] { -1, -1, -1, -1, 155 }, 100);
      setPosition(new int[] { -1, 90, 74, 84, 148 }, 100);
      setPosition(new int[] { -1, -1, -1, -1, 175 }, 100);
      setPosition(new int[] { -1, 100, -1, -1, 166 }, 100);
      setPosition(new int[] { -1, -1, -1, -1, 146 }, 100);
      setColor(0);
      setPosition(new int[] { -1, -1, 100, -1, -1 }, 200);
      setPosition(new int[] { 0, 87, 175, 160, 92 }, 400);
      //Thread.Sleep(100);
      shutdownServo();
    }

    private static void controlGlass(int serial) {
      connectServo(serial);
      if(!advServo.Attached) { return; }
      
      setStartPosition(new int[] { 20, 68, 175, 175, 20 });
      Thread.Sleep(500);
     //Console.WriteLine("Servos are in Start!");

      setPosition(new int[] { 20, 68, 160, 162, 20 }, 400);
      setPosition(new int[] { -1, 154, 88, 88, -1 }, 400);
      setPosition(new int[] { -1, -1, 86, 116, 42 }, 200);
      setPosition(new int[] { -1, -1, 77, -1, 48 }, 50);
      setPosition(new int[] { 160, -1, -1, -1, -1 }, 200);
      setPosition(new int[] { -1, -1, 85, -1, -1 }, 20);
      setPosition(new int[] { -1, -1, -1, 106, -1 }, 200);
      setPosition(new int[] { -1, -1, 118, 122, 174 }, 200);
      //Console.WriteLine("Picked Up Glas");
      setPosition(new int[] { -1, 13, -1, -1, -1 }, 300);
      setPosition(new int[] { -1, -1, 90, 100, 20 }, 200);

      setPosition(new int[] { -1, -1, 82, 97, 25 }, 20); // Fine Settings
      //Console.WriteLine("Placed Glas!");
      Thread.Sleep(1000);
      setPosition(new int[] { -1, -1, 90, 90, 20 }, 20); // Fine Lift

      setPosition(new int[] { -1, -1, 103, 105, 80 }, 150);
      setPosition(new int[] { -1, -1, -1, -1, 174 }, 300);
      setPosition(new int[] { -1, 154, 118, 122, -1 }, 300);
      setPosition(new int[] { -1, -1, 86, 102, 42 }, 300);
      setPosition(new int[] { -1, -1, 81, 113, 48 }, 100);
      setPosition(new int[] { -1, -1, -1, 117, -1 }, 20);
      setPosition(new int[] { 20, -1, 78, -1, -1 }, 150);
      setPosition(new int[] { -1, -1, 81, 111, -1 }, 20);
      setPosition(new int[] { -1, -1, 90, -1, -1 }, 100);
      //Console.WriteLine("Put Glas back!");
      setPosition(new int[] { 20, 68, 160, 162, 20 }, 400);
      setPosition(new int[] { 20, 68, 175, 175, 20 }, 400);
      //Thread.Sleep(100);
      shutdownServo();
    }

    private static void shutdownServo() {
      servosStop();
      //Console.WriteLine("End. Press Key");
      //Console.ReadLine();
      return;
    }

    private static void connectServo(int serial) {
      advServo = new AdvancedServo();
      advServo.open(serial);
      Console.WriteLine("Waiting for Advanced Servo \"" + serial + "\" to be attached...");
      advServo.waitForAttachment();
      //Console.WriteLine("Pess KEY... To start!");
      //Console.ReadLine();
    }

    private static void setPosition(int[] pos, int speed) {
      for(int i = 0; i < 5; i++) {
        if(pos[i] != -1) {
          AdvancedServoServo s = advServo.servos[i];
          s.Acceleration = speed;
          s.Position = pos[i];
        }
      }
      for(int i = 0; i < 5; i++) {
        if(pos[i] != -1) {
          AdvancedServoServo s = advServo.servos[i];
          try {
            while(Math.Abs(s.Position - pos[i]) > 0.01) { Thread.Sleep(10); s.Position = pos[i]; }
          } catch(PhidgetException) {
            Console.WriteLine("Exeption!");
            setPosition(pos, speed);
          }
        }
      }
    }

    private static void servosStop() {
      for(int i = 0; i < 5; i++) {
        advServo.servos[i].Engaged = false;
      }
    }

    private static void setStartPosition(int[] positions) {
      for(int i = 0; i < 5; i++) {
        AdvancedServoServo s = advServo.servos[i];
        s.Type = ServoServo.ServoType.TOWERPRO_MG90;
        s.Acceleration = 10000;
        s.VelocityLimit = 200;
        s.Position = positions[i];
        s.Engaged = true;
      }
    }
  }
}