package adaptive.ports;

import adaptive.support.*;

//sonar data structure for the new robots
public class PioneerSonarData implements RobotSonarData{

  static final int NUM_SONAR = 16;

  static final boolean VERBOSE = false;


  private SonarData ma_sonar[];
  private PositionData ma_pos[];

  public PioneerSonarData () {
    ma_sonar = new SonarData[NUM_SONAR];
    ma_pos=new PositionData[NUM_SONAR];
    for (int i=0; i<ma_sonar.length; i++) {
      ma_sonar[i] = new SonarData(-1.0f);
    }
    setupPositions();

  }

  public PioneerSonarData (float val) {
    ma_sonar = new SonarData[NUM_SONAR];
    ma_pos=new PositionData[NUM_SONAR];
    for (int i=0; i<ma_sonar.length; i++) {
      ma_sonar[i] = new SonarData(val);
    }
    setupPositions();
  }


  public PioneerSonarData (PioneerSonarData tsd) {
    ma_sonar = new SonarData[NUM_SONAR];
    ma_pos = new PositionData[NUM_SONAR];
    for (int i=0; i<ma_sonar.length; i++) {
      ma_sonar[i] = new SonarData(tsd.getSonarData(i));
      ma_pos[i] = new PositionData(tsd.getPosition(i));
    }    
  }




  public float getReading (int i) {
    return ma_sonar[i].getReading();
  }
  public float getReading (float angle){
    return getReading(findClosestSonar(angle));
  }
  public int findClosestSonar(float angle){
    PositionData pd=getPosition(0);
    float mindiff=Math.abs(pd.getT()-angle);
    float temp;
    int minIndex=0;
    for (int i=1;i<this.length();i++){
      pd=getPosition(i);
      temp=Math.abs(pd.getT()-angle);
      if (temp<mindiff){
	mindiff=temp;
	minIndex=i;
      }
    }
    if (VERBOSE) System.out.println("Closest to angle "+angle+" is "+minIndex);
    return minIndex;
  }

  public boolean isValid(int i) {
    return ma_sonar[i].isValid();
  }

  public int length() {
    return ma_sonar.length;
  }



  //gets entire PositionData object for element i
  public PositionData getPosition (int i) {
    return ma_pos[i].getCurrent();
  }
  //gets entire SonarData object for element i
  public SonarData getSonarData (int i) {
    return ma_sonar[i].getCurrent();
  }


  public void update (float val, int i) {
    ma_sonar[i].update(val);
  }

  //only works for same length arrays
  public void update (float val[]) {
    if (val.length == ma_sonar.length) {
      for (int i=0; i<ma_sonar.length; i++) 
	ma_sonar[i].update(val[i]);
    }
  }

  public void update (PioneerSonarData tsd) {
    if (tsd.length() == ma_sonar.length) {
      for (int i=0; i<ma_sonar.length; i++) 
	ma_sonar[i].update(tsd.getReading(i));
    }
  }

  private void setupPositions(){
    //sonar positions taken from asbestos.txt
    ma_pos[0] = new PositionData(0.1143f, 0.1397f, 90);
    ma_pos[1] = new PositionData(0.1651f, 0.1207f, 50);
    ma_pos[2] = new PositionData(0.2032f, 0.0762f, 30);
    ma_pos[3] = new PositionData(0.2159f, 0.0254f, 10);
    ma_pos[4] = new PositionData(0.2156f,-0.0254f, -10);
    ma_pos[5] = new PositionData(0.2032f,-0.0762f, -30);
    ma_pos[6] = new PositionData(0.1651f, -0.1207f,-50);
    ma_pos[7] = new PositionData(0.1143f,-0.1397f, -90);
    ma_pos[8] = new PositionData(-0.1143f, -0.1397f,-90);
    ma_pos[9] = new PositionData(-0.1651f, -0.1207f,-130);
    ma_pos[10] = new PositionData(-0.2032f,-0.0762f,-150);
    ma_pos[11] = new PositionData(-0.2159f, -0.0254f,-170);
    ma_pos[12] = new PositionData(-0.2159f,  0.0254f, 170);
    ma_pos[13] = new PositionData(-0.2032f,  0.0762f, 150);
    ma_pos[14] = new PositionData(-0.1651f,  0.1207f, 130);
    ma_pos[15] = new PositionData(-0.1143f,  0.1397f, 90);
  }


  public String toString () {
    String s = new String();
    
    s = "Pioneer sonar readings (0.."+this.length()+")= {\n\t";
    for (int i=0; i<(int)(this.length()/2); i++)
      s += ma_sonar[i] + " ";
    s += "\n\t";
    for (int i=(int)(this.length()/2); i<this.length(); i++)
      s += ma_sonar[i] + " ";
    s += "\n\t} ";
    
    return s;
  }


} //end class PioneerSonarData