using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using VRepProxy;

namespace VRepProxy.Objects
{
	public class Robot:VRepObject
    {
		private Wheels wheels;
		private Perceptions perceptions;

		public Robot(){
			Category = CATEGORY_ROBOT;
		}
			
		public int GetCategory(){
			return CATEGORY_ROBOT;
		}

		public Wheels GetWheels(){
			if (wheels == null) {
				wheels = new Wheels ();
			}
			return wheels;
		}

		public Perceptions GetPerceptions(){
			if (perceptions == null) {
				perceptions = new Perceptions ();
			}
			return perceptions;
		}
    }

	public class Perceptions{

		private ProximitySensor proximitySensor;
		private VisionSensor visionSensor;

		public Perceptions(){
			proximitySensor = new ProximitySensor ();
			visionSensor = new VisionSensor ();
		}

		public String getPerceivedColor(){
			return (visionSensor.GetColor ());
		}

		public double[] getPerceivedDistance(){
			double [] ss1 = proximitySensor.Get ();
			double[] ss2 = new double[ss1.Length];
			for (int i = 0; i < ss1.Length; i++) {
				if (ss1 [i] >= 0) {
					ss2 [i] = 1 - ss1 [i];
				} else {
					ss2 [i] = ss1 [i];
				}
			}
			return (ss2);
		}
	}
}
