/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package flightcontroller;

import java.awt.Color;
import java.awt.EventQueue;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;
import java.awt.geom.Point2D;
import java.lang.reflect.InvocationTargetException;
import java.text.DecimalFormat;
import java.text.NumberFormat;
import java.util.Collections;
import java.util.Iterator;
import java.util.Map;
import java.util.NoSuchElementException;
import java.util.Set;
import java.util.TreeSet;
import java.util.Vector;
import java.util.logging.Level;
import java.util.logging.Logger;

/**
 *
 * @author jar
 */
public class Sector extends Thread 
    implements Runnable {
   // data
   private double size = 300.0; // the width of a Sector side (assumed square)
   private int nFlights = 20;   // The number of flights in the sector
   // Make the HashMap thread safe because we will be accessing it from
   // both this class and the GUI
   private FlightMap<Color, Flight> fltMap = new FlightMap();
   // An array of the interfering flights
   public static final int NANGLES = 360;
   private Color[] hitAngles = new Color[3*NANGLES];
   public static final int DT = 30;  // seconds per time step
   private int lookAheadTime = 600;  // look-ahead time in seconds
   private static final double TOO_CLOSE = 10.0;
   private static final double r1 = 5.0;  // The minimun legal spacing
   private static final double PI180 = Math.PI/180.0;
   public int nHits = 0;   // Number of hits
   private Vector<Double> results = new Vector();
   private int flightTotal = 0;  // total number of fliughts so far
   private SectorPanel sectorPanel = null;
   private CoursePanel coursePanel = null;
   private FlightControllerView fcv = null;
   private int time = 0;
   // 0 = started, 1 = stepping, 2 = pause, 3 = resume, 4 = exit, 5=manual
   private int stepState = 0;
   private transient Flight controlledFlight = null;  // The flight we control
   private int manualControl = 0;  // 0 = false, 1 = true, -1 = need one
   double manualHeading = -5000.0;

   public Sector() {
         ;
   }

   public Sector(double size, int nFlights, int lookahead, SectorPanel fltPanel,
       CoursePanel crsPanel, FlightControllerView fcv) {
      this.size = size;
      this.nFlights = nFlights;
      this.sectorPanel = fltPanel;
      this.coursePanel = crsPanel;
      coursePanel.addMouseListener(new CourseListener());
      this.lookAheadTime = lookahead;
      this.fcv = fcv;
         // Create the initial flights
      for(int i = 0; i < nFlights; i++) {
         Flight f = newFlight(0);
         /*
         if(i == 0) {
            // fcv not yet visible, so OK to call it directly
            if (fcv.getManualControlState()) {
               controlledFlight = f;
               f.setIsControlled(true);
            }
         }
          * */
         fltMap.put(f.getFltID(), f);      }
   }


   /**
    * Implements run for the Thread
    */
   public void run() {

       time = 0;
       while(true) {
          switch(stepState) {
             case 0:  // initialize
                stepState = 1;
                controlFlights();
                break;
             case 1:  // run
                controlFlights();
                break;
             case 2:  // pause
               try {
                  // pause
                  sleep(800);
               } catch (InterruptedException ex) {
                  Logger.getLogger(Sector.class.getName()).log(Level.SEVERE, null, ex);
               }
                break;
             case 3:  //resume
                stepState = 1;
                controlFlights();
                break;
             case 4:  //stop
                return;
          }
         try {
            Sector.sleep(100);
         } catch (InterruptedException ex) {
            Logger.getLogger(Sector.class.getName()).log(Level.SEVERE, null, ex);
         }
       }
   }

   public void setStepState(int state) {
      stepState = state;
   }

   /**
    * Control the Flights for one time step
    * @return a synchronized copy of the FlightMap
    */
   public Map controlFlights() {
      Set<Color> keys = fltMap.keySet();  // all flights
      Iterator<Color> itr = keys.iterator();
      int numberNew = 0;
      while(itr.hasNext()) {
         Flight f = fltMap.get(itr.next());
         double success = flightStep(f);
         if(success > 360.0) {   // Left sector
            itr.remove();
            numberNew++;  // Add a new Flight
         }
         if(success < -360.0) {  // failures
            nHits++;
            EventQueue.invokeLater(new Runnable() {
               public void run() {
                  fcv.updateHits(Integer.toString(nHits));
               }
            });
            // Check controlled flight
            if(controlledFlight != null) {
               if(controlledFlight.equals(f)) {
                  controlledFlight = null;
                  manualControl = -1;
               }
            }
            itr.remove();
            f = null;
         }
      }
      for(int i = 0; i < numberNew; i++) {  // replace flights
         Flight f = newFlight(time);
         if(manualControl == -1) {
            manualControl = 1;
            controlledFlight = f;
            f.setIsControlled(true);
         }

         fltMap.put(f.getFltID(), f);

      }
      FlightMap dupMap = fltMap.clone();
      final Map<Color,Flight> fmMap = Collections.synchronizedMap(dupMap);
      // Update the SectorPanel done in SwingWorker

      time += DT;
      int hours = time/3600;
      int minutes = (time - hours*3600)/60;
      int seconds = time%60;
      String hstring = Integer.toString(hours);
      if(hstring.length() ==1) {
         hstring = "0"+hstring;
      }
      String mstring = Integer.toString(minutes);
      if(mstring.length() ==1) {
         mstring = "0"+mstring;
      }
      String sstring = Integer.toString(seconds);
      if(sstring.length() ==1) {
         sstring = "0"+sstring;
      }
      final String hms = hstring + ":" + mstring + ":" + sstring;
      try {
         // Notify GUI of update
         EventQueue.invokeAndWait(new Runnable() {

            public void run() {
               fcv.updateTime(hms);
               sectorPanel.recalc(fmMap);
            }
         });
      } catch (InterruptedException ex) {
         Logger.getLogger(Sector.class.getName()).log(Level.SEVERE, null, ex);
      } catch (InvocationTargetException ex) {
         Logger.getLogger(Sector.class.getName()).log(Level.SEVERE, null, ex);
      }
       return fmMap;
   }

   /**
    * Calculate where to go next. Return the heading in degrees wr North
    * @param f the flight
    * @return the heading or -5000.0 if no path found; +5000.0 if exited Sector
    */
   public double flightStep(final Flight f) {
      double heading = 0.0;
      double bearToGoal = f.bearingToGoal();  // in degrees
      int tNext = f.getPositions().last().getSecs() + DT;
      double startAngle = Math.rint(bearToGoal);
      FlightPosition fp = null;
      flightInterferences(f);  // Fill the hitAngles array
      int goodBin = -5000;
      // start headed to goal and pick the closest heading
      if(f.isIsControlled()) {
         final FlightInfo fi = new FlightInfo(hitAngles.clone(), f.getFltID(),
             f.bearingToGoal(), -5000.);
         try {
            // Notify GUI of update
            EventQueue.invokeAndWait(new Runnable() {
               public void run() {
                  fcv.getFlightColorPanel().setBackground(f.getFltID());
                  fcv.setSpeed((int)f.getSpeed());
                  coursePanel.update(fi);
               }
            });
         } catch (InterruptedException ex) {
            Logger.getLogger(Sector.class.getName()).log(Level.SEVERE, null, ex);
         } catch (InvocationTargetException ex) {
            Logger.getLogger(Sector.class.getName()).log(Level.SEVERE, null, ex);
         } finally {
            while (manualHeading == -5000.0){
               try {
                  sleep(500);
               } catch (InterruptedException ex) {
                  Logger.getLogger(Sector.class.getName()).log(Level.SEVERE, null, ex);
               }
            }
            try {
               EventQueue.invokeAndWait(new Runnable() {

                  public void run() {
                     f.setSpeed((double)fcv.getSpeed());
                  }
               });
            } catch (InterruptedException ex) {
               ;
            } catch (InvocationTargetException ex) {
               ;
            }
            heading = manualHeading;
            goodBin = (int)heading + 180;
            manualHeading = -5000.0;
            fp = f.doStep(heading, tNext);
            f.updateTime();
            f.addPosition(fp, heading);  // Also updates course
         }
      }  // end manualControl
      else {   // not manually controlled
         Color[] cAngles = new Color[NANGLES];
         for(int i = 0; i < NANGLES; i++) {  // make a new array
            cAngles[i] = hitAngles[NANGLES + i];
         }
         if((int)startAngle == 180) startAngle = -180.0;
         int start = (int)startAngle + 180;
         int j = start, k = start;
         for(int i = start, da = 0; da < NANGLES/2; i++, da++) {
            fp = null;
            j += da;
            if(j >=NANGLES) j = 0;
            k -= da;
            if(k < 0) k = NANGLES-1;
            if(cAngles[j].equals(Color.WHITE))
               goodBin = j;
            else if(cAngles[k].equals(Color.WHITE)) {
               goodBin = k;
            }
            if(goodBin != -5000) {  // Free path
               if((int)(bearToGoal + 180.0) == goodBin){  // go to goal
                  heading = bearToGoal;
                  fp = f.doStep(bearToGoal, tNext);
                  f.updateTime();
                  f.addPosition(fp, bearToGoal);  // Also updates course
               }
               else {  // go to nearest degree
                  heading = goodBin - 180.0;
                  fp = f.doStep(goodBin - 180., tNext);
                  f.updateTime();
                  f.addPosition(fp, goodBin - 180.);  // Also updates course
               }
               break;
            }  // Success - goodBin
         } // scan through angles
      }  // not manually controlled
      if(goodBin != -5000) {
         // Check if it reached the goal
         Point2D.Double pt = fp.getPos();
         // Is it outside the sector?
         if(pt.x > size || pt.x < 0 || pt.y > size || pt.y < 0) {
            results.add(new Double(distance(pt, f.getGoal())));
            flightTotal++;
            double avg = 0.0;
            for(int n = 0; n < results.size(); n++) {
               avg += results.elementAt(n);
            }
            avg = avg/(double)flightTotal;
            NumberFormat nf = new DecimalFormat("0.00");
            final String avgMiles = nf.format(avg);
            // calculate the new average nm from destination
            EventQueue.invokeLater(new Runnable() {
               public void run() {
                  fcv.updateMisses(avgMiles);
               }
            });

            if(f.isIsControlled()) {
               manualControl = -1;
            }
            return 5000.;
         }
         if(f.isNewFlight()) {
            f.setNewFlight(false);
            f.newCourse(heading);  // project the new future course
         }
         final FlightInfo fi = new FlightInfo(hitAngles.clone(), f.getFltID(),
             f.bearingToGoal(), heading);
         EventQueue.invokeLater(new Runnable() {
            public void run() {
               coursePanel.update(fi);
            }
         });
         return heading;
      }  // not failure
      // It failed!
      System.err.println("Failed to find a good course for " + f.getFltID().toString());
      return -5000.;
   }

   /**
    * Look at the interferences with all other flights up to the look-ahead time.
    * The result is in the hitAngles array
    * It is protected because we might want to call it from Flight
    * @param f The flight
    */
   protected Color[] flightInterferences(Flight f) {
      // We will put three instances of the hits array in the same big array
      // One for speed - 10%, speed, speed + 10%
      // zero the hitAngles array with WHITE
      for(int i = 0; i < 3*NANGLES; i++){
         hitAngles[i] = Color.WHITE;
      }
      Color fColor = f.getFltID();
      double speed = f.getSpeed();
      double fSpeed[] = new double[3];
      fSpeed[0] = (.9*speed > Flight.VMIN) ? .9*speed : Flight.VMIN;
      fSpeed[1] = speed;
      fSpeed[2] = (1.1*speed < Flight.VMAX) ? 1.1*speed : Flight.VMAX;
      int fNow = f.getPositions().last().getSecs();
      Point2D.Double p0 = f.getPositions().last().getPos();
      double x0 = p0.x, y0 = p0.y;
      for(int j = 0; j < 3; j++) {    // Loop over the speeds
         Set<Color> keys = fltMap.keySet();  // all flights
         Iterator<Color> itr = keys.iterator();
         while(itr.hasNext()) {
            Color otherFid = itr.next();
            if(otherFid.equals(fColor)) {    // It is this flight
               continue;
            }  // So now it is some other flight for sure
            Flight otherF = fltMap.get(otherFid);
            if(otherF.isNewFlight()) {
               continue;  // not yet started
            }
            otherFid = otherF.getFltID();
            TreeSet<FlightPosition> oCourse = otherF.getCourse();
            Iterator<FlightPosition> oit = oCourse.iterator();

            // Now do the calculation for each DT seconds up to the lookahead time
            for(int t = fNow + DT; t <= fNow + lookAheadTime; t += DT) {
               double r0 = (t - fNow)*fSpeed[j]/3600.0;  // t is in hours!
               int i;
               FlightPosition ofp;
               try {
                  if(oit.hasNext()) {
                     ofp = oit.next();
                     if(ofp.getSecs() < t) { // First one may be earlier
                        if(oit.hasNext()) {
                           ofp = oit.next();
                        }
                        else break;
                     }
                  }
                  else {
                     break;
                  }
                  Point2D.Double opos = ofp.getPos();
                  double x1 = opos.x, y1 = opos.y;
                  double d = distance(opos, p0); // distance between
                  // Check for non-intersection of circles
                  if(d > r0 + r1)  // r1 is the minimum allowed spacing
                     continue;
                  double a = (r0*r0 - r1*r1 + d+d)/(2.0*d); // distance P1 to P2
                  double h = Math.sqrt(r0*r0 - a*a);
                  double x2 = x0 + a*(x1-x0)/d;
                  double y2 = y0 + a*(y1-y0)/d;
                  double x3p = x2 + h*(y1 - y0)/d;
                  double y3p = y2 - h*(x1-x0)/d;
                  double x3m = x2 - h*(y1 - y0)/d;
                  double y3m = y2 + h*(x1-x0)/d;
                  Point2D.Double p3 = new Point2D.Double(x3p, y3p);
                  double d03 = distance(p3, p0); // Distance for p0 to p3
                  double thetapdeg = Math.acos((y3p-y0)/d03)/PI180; // 0-180 deg
                  double thetamdeg = Math.acos((y3m-y0)/d03)/PI180;
                  double psideg = Math.acos((y1 - y0)/d)/PI180; // angle wr North
                  if(x3p < x0) thetapdeg = -thetapdeg;
                  if(x3m < x0) thetamdeg = -thetamdeg;
                  if(x1 < x0) psideg = -psideg;
                  // Get the larger of the thetas
                  double thb, ths;
                  if(thetapdeg >= thetamdeg) {
                     thb = thetapdeg; ths = thetamdeg;
                  }
                  else {
                     ths = thetapdeg; thb = thetamdeg;
                  }
                  if(thb - ths <180.0) { // Can't cross 180
                     double angle = -180;
                     for(i = 0; i < NANGLES; i++, angle += 360.0/(double)NANGLES) {
                        if(angle >= ths && angle <= thb) { // Not across 180
                           hitAngles[j*NANGLES + i] = otherFid;
                        }
                     }
                  }
                  else  { // spans 180
                     double angle = -180;
                     for(i = 0; i < NANGLES; i++, angle += 360.0/(double)NANGLES) {
                        if(angle <= ths || angle >= thb) {
                          hitAngles[j*NANGLES + i] = otherFid; }
                        }
                  }
               } catch (NoSuchElementException e) {
                  System.err.println("Flight " + otherF.getFltID() + " has bad course");
               }
            }  // End of speed loop
         }
      }  // end while it is a different flight
      return hitAngles;
   }

   /**
    * Make a new flight with unique Color, and not to close to others
    * @param time start time
    * @return the Flight
    */
   private Flight newFlight(int time) {
      Flight f = new Flight(time, this);
      // start flights at least TOO_CLOSE from others
      while(checkHit(f, TOO_CLOSE)) {f = new Flight(time, this);}
      // check for unique color and non-WHITE
      Color c = f.getFltID();
      while (c.equals(Color.WHITE) || getFltMap().containsKey(c)){
         c = new Color((float)Math.random(), (float)Math.random(),
         (float)Math.random());
         f.setFltID(c);
      }
      return f;
   }

   /**
    * The distance between twio points
    * @param p1 First point
    * @param p2 Second point
    * @return
    */
   public double distance(Point2D.Double p1, Point2D.Double p2) {
      return Math.sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y));
   }

   /**
    * The square of the distance between p1 and p2
    * @param p1
    * @param p2
    * @return
    */
   public double dsq(Point2D.Double p1, Point2D.Double p2) {
      return (p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y);
   }

   /**
    * Chick if this flights hits any other flight (< d)
    * @param f The flight
    * @param d The distance to check for
    * @return false if no hit, ture if hit
    */
   public boolean checkHit(Flight f, double d) {
      double dsq = d*d;   // Check the squares
      Color fColor = f.getFltID();
      Point2D.Double fpt = f.getPositions().last().getPos();
      Set keys = fltMap.keySet();
      Iterator itr = keys.iterator();
      while(itr.hasNext()) {
         Color otherFID = (Color)itr.next();
         if(otherFID.equals(fColor)) {    // It is this flight
            continue;
         }  // So now it is some other flight for sure
         Flight of = fltMap.get(otherFID);
         Point2D.Double opt = of.getPositions().last().getPos();
         if(dsq(opt, fpt) < dsq)
            return true;
      }

      return false;
   }

   /**
    * @return the size
    */
   public // data
   double getSize() {
      return size;
   }

   /**
    * @param size the size to set
    */
   public void setSize(double size) {
      this.size = size;
   }

   /**
    * @return the nFlights
    */
   public int getNFlights() {
      return nFlights;
   }

   /**
    * @param nFlights the nFlights to set
    */
   public void setNFlights(int nFlights) {
      this.nFlights = nFlights;
   }

   /**
    * @return the fltMap
    */
   public Map<Color, Flight> getFltMap() {
      return fltMap;
   }

   /**
    * @return the lookAheadTime
    */
   public int getLookAheadTime() {
      return lookAheadTime;
   }

   /**
    * @param lookAheadTime the lookAheadTime to set
    */
   public void setLookAheadTime(int lookAheadTime) {
      this.lookAheadTime = lookAheadTime;
   }

   /**
    * @return the controlledFlight
    */
   public Flight getControlledFlight() {
      return controlledFlight;
   }

   /**
    * @param controlledFlight the controlledFlight to set
    */
   public void setControlledFlight(Flight controlledFlight) {
      this.controlledFlight = controlledFlight;
   }

   /**
    * @return the manualControl
    */
   public int isManualControl() {
      return manualControl;
   }

   /**
    * @param manualControl the manualControl to set
    */
   public void setManualControl(int manualControl) {
      this.manualControl = manualControl;
      if(this.manualControl == 0) {
         if(controlledFlight != null) {
            if(!controlledFlight.isNewFlight()) {
               controlledFlight.setIsControlled(false);
               // need to get out of the sleep waiting for user input
               manualHeading = controlledFlight.getOldBearing();
               controlledFlight = null;
            }
         }
      }
      else {
         if(controlledFlight == null) {
            this.manualControl = -1;  // next flight
         }
      }
   }

   private class CourseListener implements MouseListener {

      public void mouseClicked(MouseEvent e) {
         if(manualControl != 1)  // ignore if not controlling
            return;
         int x = e.getX();
         int width = coursePanel.getWidth();
         double dx = (double)width/(double)Sector.NANGLES;
         manualHeading = x/dx - 180.0;
      }

      public void mousePressed(MouseEvent e) {

      }

      public void mouseReleased(MouseEvent e) {

      }

      public void mouseEntered(MouseEvent e) {

      }

      public void mouseExited(MouseEvent e) {

      }
   }
}