public void addValue(long x, double y) {
      // calculate the bar in which this value should go
      int bar = (int) (x / barsize);

      double Y = y;
      if (ysum.containsKey(bar)) {

        double newyvalue = Y + ysum.get(bar);
        long newxvalue = x + xsum.get(bar);

        Integer newcount = count.get(bar) + 1;

        ysum.put(bar, newyvalue);
        xsum.put(bar, newxvalue);
        count.put(bar, newcount);
        Vector<Double> barvalues = values.get(bar);
        barvalues.add(Y);
        values.put(bar, barvalues);

      } else {
        ysum.put(bar, Y);
        xsum.put(bar, x);
        count.put(bar, 1);
        Vector<Double> barvalues = new Vector<Double>();
        barvalues.add(Y);
        values.put(bar, barvalues);
      }
    }
 public YIntervalSeries getYIntervalSeries(String rowname, long factor) {
   YIntervalSeries mydataset = new YIntervalSeries(rowname);
   double sdcount = 0;
   double total = 0;
   Vector<Double> totalvalues = new Vector<Double>();
   double avgtotal = 0;
   double minus = 0;
   double plus = 0;
   double zero = 0;
   for (Integer key : ysum.keySet()) {
     Double value = ysum.get(key) / (double) count.get(key);
     Double point = (double) xsum.get(key) / (double) count.get(key);
     Vector<Double> listofvalues = values.get(key);
     double sumofdiff = 0.0;
     for (Double onevalue : listofvalues) {
       sumofdiff += Math.pow(onevalue - value, 2);
       sdcount++;
       total += Math.pow(onevalue, 2);
       avgtotal += onevalue;
       totalvalues.add(onevalue);
       if (onevalue == 1) {
         plus++;
       }
       ;
       if (onevalue == -1) {
         minus++;
       }
       ;
       if (onevalue == 0) {
         zero++;
       }
       ;
     }
     double sd = Math.sqrt(sumofdiff / count.get(key));
     // mydataset.add(point/factor, value,value+sd,value-sd);
     // mydataset.add(point/factor, value,value,value);
     mydataset.add(
         point / factor,
         value,
         value + 1.96 * (sd / Math.sqrt(count.get(key))),
         value - 1.96 * (sd / Math.sqrt(count.get(key))));
   }
   double sdtotal = 0;
   double avgsd = total / sdcount;
   double test = 0;
   for (Double onevalue : totalvalues) {
     sdtotal += Math.pow(Math.pow(onevalue, 2) - (total / sdcount), 2);
     test += onevalue;
   }
   // System.out.println(rowname+" mean square: "+avgsd+"
   // +/-95%:"+1.96*Math.sqrt(sdtotal/sdcount)/Math.sqrt(sdcount));
   // System.out.println("total -1:"+minus+" total +1:"+plus+" zero: "+zero
   // +" total:"+sdcount);
   return mydataset;
 }
  private void saveToFile() {
    String fileName;
    if (logFileName.contains("."))
      fileName = logFileName.substring(logFileName.lastIndexOf('.')) + "-headingError.txt";
    else fileName = logFileName + "-headingError.txt";

    FileLogger flogger = new FileLogger(fileName, false);
    long startTime = robotData.getExpStartTime();

    log("Time (ms)\tDelta Time (ms)\tActual Heading\tIdeal Heading\tHeading Error", flogger);
    Enumeration<HeadingDivergenceState> e = divergenceData.elements();
    while (e.hasMoreElements()) {
      HeadingDivergenceState currState = e.nextElement();
      log(
          currState.time
              + "\t"
              + (currState.time - startTime)
              + "\t"
              + currState.currHeading
              + "\t"
              + currState.idealHeading
              + "\t"
              + currState.headingError,
          flogger);
    }

    // Print when the robot starts traversing an edge...
    log("Start Times of Edge Traversal:", flogger);
    log("Time (ms)\tDelta Time (ms)\tDummy", flogger);
    Vector<PathEdge> pathEdges = robotData.getPathEdges();
    Enumeration<PathEdge> e2 = pathEdges.elements();
    while (e2.hasMoreElements()) {
      PathEdge currEdge = e2.nextElement();
      long relStartTime = (currEdge.getStartTime() - robotData.getExpStartTime()) / 1000;
      log(currEdge.getStartTime() + "\t" + relStartTime + "\t" + 0, flogger);
    }
  }
  /**
   * The constructor.
   *
   * @param logFileName The experiment log file to analyze.
   * @param samplingInterval The interval at which to calculate the heading error in milliseconds.
   * @param saveToFile whether to save the error calculations to a file.
   */
  public VisualizeHeadingError(String logFileName, long samplingInterval, boolean saveToFile) {
    this.logFileName = logFileName;
    robotData = new RobotExpData(logFileName);

    long startTime = robotData.getExpStartTime();

    // Calculate heading divergence every sampling interval
    for (long time = startTime; time < robotData.getExpStopTime(); time += samplingInterval) {

      // Only do the calculation if the robot has started to move.
      if (time >= robotData.getPathEdge(0).getStartTime())
        divergenceData.add(getHeadingError(time));
    }

    if (saveToFile) saveToFile();
    showPlot();
  }
  /** Displays the data in a graph. */
  private void showPlot() {
    // Create a data series containing the heading error data...
    XYSeries headingErrorSeries = new XYSeries("Heading Error");
    Enumeration<HeadingDivergenceState> e = divergenceData.elements();
    while (e.hasMoreElements()) {
      HeadingDivergenceState currState = e.nextElement();
      headingErrorSeries.add(
          (currState.time - robotData.getExpStartTime()) / 1000, currState.headingError);
    }

    // Create two data series one containing the times when the robot starts heading
    // towards a waypoint, and another containing the times when the robot arrives at
    // a waypoint
    final XYSeries beginEdgeSeries = new XYSeries("Begin Edge Traveral");
    final XYSeries waypointArrivalSeries = new XYSeries("Waypoint Arrival");
    Vector<PathEdge> pathEdges = robotData.getPathEdges();
    Enumeration<PathEdge> e2 = pathEdges.elements();
    while (e2.hasMoreElements()) {
      PathEdge currEdge = e2.nextElement();
      double beginEdgeTime = (currEdge.getStartTime() - robotData.getExpStartTime()) / 1000.0;
      beginEdgeSeries.add(beginEdgeTime, 0);
      double wayPointArrivalTime = (currEdge.getEndTime() - robotData.getExpStartTime()) / 1000.0;
      waypointArrivalSeries.add(wayPointArrivalTime, 0);
    }

    // Create a dataset out of the data series
    XYSeriesCollection dataset = new XYSeriesCollection();
    dataset.addSeries(headingErrorSeries);
    dataset.addSeries(beginEdgeSeries);
    dataset.addSeries(waypointArrivalSeries);

    // Create the chart
    JFreeChart chart =
        ChartFactory.createXYLineChart(
            "Heading Error vs. Time", // chart title
            "Time (s)", // x axis label
            "Heading Error (radians)", // y axis label
            dataset, // the data
            PlotOrientation.VERTICAL, // plot orientation (y axis is vertical)
            true, // include legend
            true, // tooltips
            false // urls
            );

    // Place the legend on top of the chart just below the title.
    LegendTitle legend = chart.getLegend();
    legend.setPosition(RectangleEdge.TOP);

    chart.setBackgroundPaint(Color.white);

    XYPlot plot = chart.getXYPlot();
    //        plot.setBackgroundPaint(Color.lightGray);
    //    //    plot.setAxisOffset(new Spacer(Spacer.ABSOLUTE, 5.0, 5.0, 5.0, 5.0));
    //        plot.setDomainGridlinePaint(Color.white);
    //        plot.setRangeGridlinePaint(Color.white);

    // Display the points and not the lines connecting the points
    XYLineAndShapeRenderer renderer = new XYLineAndShapeRenderer();
    renderer.setSeriesLinesVisible(0, true); // display the heading errors as a line
    renderer.setSeriesShapesVisible(0, false);
    renderer.setSeriesLinesVisible(
        1, false); // display the begin edge traversal points as blue dots
    renderer.setSeriesShapesVisible(1, true);
    renderer.setSeriesPaint(1, Color.BLUE);
    renderer.setSeriesShape(1, new java.awt.geom.Ellipse2D.Double(-3, -3, 6, 6));
    renderer.setSeriesLinesVisible(
        2, false); // display the begin edge traversal points as green dots
    renderer.setSeriesShapesVisible(2, true);
    renderer.setSeriesPaint(2, Color.GREEN.darker());
    renderer.setSeriesShape(2, new java.awt.geom.Ellipse2D.Double(-5, -5, 10, 10));
    plot.setRenderer(renderer);

    //        final NumberAxis domainAxis = (NumberAxis)plot.getDomainAxis();
    //        domainAxis.setRange(new Range(0,140));

    //        final NumberAxis rangeAxis = (NumberAxis) plot.getRangeAxis();
    //     // change the auto tick unit selection to integer units only...
    ////        rangeAxis.setStandardTickUnits(NumberAxis.createIntegerTickUnits());
    //        rangeAxis.setRange(new Range(-Math.PI, Math.PI));

    ChartPanel chartPanel = new ChartPanel(chart);
    chartPanel.setPreferredSize(new java.awt.Dimension(1000, 600));

    // Create a frame for the chart, then display it.
    ApplicationFrame appFrame = new ApplicationFrame("Heading Error for " + logFileName);
    appFrame.setContentPane(chartPanel);
    appFrame.pack();
    RefineryUtilities.centerFrameOnScreen(appFrame);
    appFrame.setVisible(true);
  }