using System;
using System.Collections.Generic;
using System.Text;
using Phidgets;
using System.Threading;

namespace ebbits.Robots {
  public abstract class AbstractRobot {
    protected AdvancedServo servo;
    protected int cservos;
    protected string name;
    private Thread t;
    protected void init(int id) {
      servo = new AdvancedServo();
      servo.open(id);
      Logger.Info("Waiting for Servo \"" + id + "\" is attached");
      servo.waitForAttachment();
      if(!servo.Attached) { throw new AccessViolationException("Robot not Found"); }
    }
    public abstract void run();
    protected void setPosition(int[] pos, int speed) {
      for(int i = 0; i < cservos; i++) {
        if(pos[i] != -1) {
          AdvancedServoServo s = servo.servos[i+ 5 - cservos];
          s.Acceleration = speed;
          s.Position = pos[i];
        }
      }

      bool waiting = true;
      while(waiting) {
        waiting = false;
        for(int i = 0; i < cservos; i++) {
          if(pos[i] != -1) {
            AdvancedServoServo s = servo.servos[i + 5 - cservos];
            s.Position = pos[i];
            try {
              if(Math.Abs(s.Position - pos[i]) > 0.01) {
                waiting = true;
              }
            } catch(PhidgetException) {
              Logger.Warn("Exeption!");
              s.Position = pos[i];
            }
            Thread.Sleep(10);
          }
        }
      }
    }
    protected void setStartPosition(int[] positions, int sleep) {
      for(int i = 0; i < cservos; i++) {
        AdvancedServoServo s = servo.servos[i + 5 - cservos];
        s.Type = ServoServo.ServoType.TOWERPRO_MG90;
        s.Acceleration = 10000;
        s.VelocityLimit = 200;
        s.Position = positions[i];
        s.Engaged = true;
      }
      Thread.Sleep(sleep);
    }
    protected void shutdownServo() {
      for(int i = 0; i < cservos; i++) {
        servo.servos[i + 5 - cservos].Engaged = false;
      }
      if(t.IsAlive) {
        t.Abort();
      }
    }

    protected void startPowerMess() {
      this.t = new System.Threading.Thread(current_runnter);
      t.Start();
    }

    private void current_runnter(object obj) {
      DateTime t = DateTime.Now;
      double current = 0;
      while(true) {
        double c = 0;
        for(int i = 0; i < 5; i++) {
          c += this.servo.servos[0].Current;
        }
        current += (c * 12 * (DateTime.Now.Ticks - t.Ticks)) / 1000000;
        t = DateTime.Now;
        Logger.Info(this.name+" Watt: " + current.ToString());
        System.Threading.Thread.Sleep(100);
      }
    }
  }
}