import processing.opengl.*;
//import processing.serial.*;

//Serial port;
PFont font;
int [] serialOut = new int [2];
int [] serialIn = new int [2];
boolean handshake=true;
boolean startup=true;

// 796 deKalb
//float wLatitude=40.690645; //north in decimal, not degrees,minutes,seconds (NMEA)
//float wLongitude=-73.941636; //west. west is negative. NMEA uses E or W both positive
float wLatitude;//=4069.0645; 
float wLongitude;//=7394.1636;
//char wLatDir='N';
//char wLongDir='W';

// 796 deKalb
float myLatitude=40.690645; //north
float myLongitude=-73.941636; //west

int  distance,angle;
float diam;
float initDist;

LatLon ll;

float zoom=1.01;
boolean zoomOn=false;

float [] wayPoint = new float[2];
float [] myPoint= new float[2];


void setup(){
  //size(1000,500,OPENGL); 
  size(1000,500,P3D); 
  diam=height*.75;
  //noCursor();
  cursor(CROSS);
  font = createFont("FFScala", 14);
  ll = new LatLon();
  ll.requestLatLon();
  // smooth();
  //println(Serial.list());
  //port = new Serial(this, Serial.list()[0], 9600);
  //port.write(255);
  //decimalMinutes(33830.0000,'N');
}

void draw(){
  background(150);
  //zoom
  drawGrid();

  //drawBackground();
  if (handshake == true){
    wLatitude = ll.getLat();
    wLongitude = ll.getLon();
    //wLatitude=40.690645; 
    //wLongitude=73.941636;
    myPoint= drawMypoint(myLatitude,myLongitude);
    //float [] wayPoint = drawWaypoint(decimalMinutes(wLatitude,wLatDir),decimalMinutes(wLongitude,wLongDir));
    wayPoint = drawWaypoint(wLatitude,wLongitude);

    //calculate initial position
    if(startup){
      initDist=dist(myPoint[0],myPoint[1],wayPoint[0],wayPoint[1]);
      startup=false;
    }
    //camera(eyeX, eyeY, eyeZ, centerX, centerY, centerZ, upX, upY, upZ);
    float zoomMax=initDist/2;
    if(zoomOn==true){
      if(zoom<zoomMax){
        zoom=zoom*1.1;
      } 
    }
    else{
      if(zoom>1.1){
        zoom=zoom*.9;
      }
    }
    camera(width/2.0*(1-zoom/zoomMax)+wayPoint[0]*zoom/zoomMax,height/2.0*(1-zoom/zoomMax)+wayPoint[1]*zoom/zoomMax, 
    (height/2.0*(1-zoom/zoomMax)+initDist*2.0*zoom/zoomMax) / tan(PI*60.0 / 360.0), width/2.0*(1-zoom/zoomMax)+wayPoint[0]*zoom/zoomMax,height/2.0*(1-zoom/zoomMax)+wayPoint[1]*zoom/zoomMax, 0, 0, 1, 0);


    //draw init boundary
    noFill();
    if(hoverLine()) {
      stroke(255,0,0);
    }
    else if(hoverCircle()) {
      stroke(255,200,0);
    }
    else stroke(140);
    if (zoomOn==false )ellipse(wayPoint[0],wayPoint[1],initDist*2,initDist*2);

    //line
    stroke(200);
    line(myPoint[0],myPoint[1],wayPoint[0],wayPoint[1]);
    float a = atan2(wayPoint[1]-myPoint[1], wayPoint[0]-myPoint[0])/TWO_PI; //float from 0-1
    angle=int((a)*254+128);//translate angle to 0-255

    float d1=dist(myPoint[0],myPoint[1],wayPoint[0],wayPoint[1])/initDist;// float from 0-1
    distance=int(d1*128);//translate distance to 0-128

      //simlulate 360 rotation with 180 servo rotation
    if(angle<128){
      distance=distance+128; //128 is weight in zero position / facing down
      angle=255-angle*2;
    }
    else{
      distance=128-distance;
      angle=255-(angle-128)*2;
    }

    serialOut[0]=(constrain(distance,0,254));//constrain output to 254, using 255 as header
    serialOut[1]=(constrain(angle,0,254));

    /*
    port.write(255); //header
     port.write(serialOut[0]);
     port.write(serialOut[1]);
     */

    //display text
    fill(220);
    textFont(font, 9); 
    pushMatrix();
    translate( myPoint[0], myPoint[1]);
    text("d : "+serialOut[0]+"",20,0);//dist
    rotate(atan2(wayPoint[1]-myPoint[1], wayPoint[0]-myPoint[0])+PI);
    text("a : "+serialOut[1]+"", 60,0);//angle
    popMatrix();
  }
}

boolean hoverCircle(){
  boolean hover;
  if (dist(mouseX,mouseY,wayPoint[0],wayPoint[1])<=initDist-15)  hover=true;
  else   hover=false;
  return hover; 
}

boolean hoverLine(){
  boolean hover;
  if (dist(mouseX,mouseY,wayPoint[0],wayPoint[1])<=initDist+15 && 
    dist(mouseX,mouseY,wayPoint[0],wayPoint[1])>=initDist-15){
    hover=true;
  }
  else   hover=false;
  return hover; 
}

float  [] drawMypoint(float latitude,float longitude){
  float x=longToScreen(longitude);
  float y=latToScreen(latitude);
  pushMatrix();
  translate(x,y);
  noStroke();
  fill(110,0,0);
  ellipse(0,0,10,10); 
  popMatrix();

  textFont(font, 9); 

  textAlign(LEFT,CENTER);
  pushMatrix();
  //translate(x,y);
  rotate(PI/2);
  text("  x:"+nf(x,3,2)+" long:"+longitude+"",0,-x);
  popMatrix();
  text("  y:"+nf(y,3,2)+" lat:"+latitude+"",0,y);
  float [] myPoint={
    x,y                                        };
  return myPoint;
}

float [] drawWaypoint(float latitude ,float longitude){
  float x=longToScreen(longitude);
  float y=latToScreen(latitude);

  pushMatrix();
  translate(x,y);
  noStroke();
  fill(100);
  ellipse(0,0,5,5); 
  popMatrix();

  textFont(font, 9); 

  textAlign(LEFT,CENTER);
  pushMatrix();
  //translate(x,y);
  rotate(PI/2);
  text("  x:"+nf(x,3,2)+" long:"+longitude+"",0,-x);
  popMatrix();
  text("  y:"+nf(y,3,2)+" lat:"+latitude+"",0,y);
  float[] wayPoint={
    x,y                                        };
  return wayPoint;
}


//translate long and lat to x and y points as if on a world map
float longToScreen(float longitude){
  float x=0;
  x=width/2+longitude/180*width/2;
  return x;
}

float latToScreen(float latitude){
  float y=0;
  y=height/2-latitude/90*height/2;
  return y;
}

//on screen position control
void keyPressed(){
  if(keyCode==UP)  myLatitude=myLatitude+.1;
  if(keyCode==DOWN)  myLatitude=myLatitude-.1;
  if(keyCode==LEFT)  myLongitude=myLongitude-.1;
  if(keyCode==RIGHT)  myLongitude=myLongitude+.1;
  if(key=='-')  zoom--;
  if(key=='=')  zoom++;
}

void mouseDragged(){
  if(hoverLine()==true){
    initDist=dist(mouseX,mouseY,wayPoint[0],wayPoint[1]);
  }
  if(hoverLine()==false && hoverLine()==false){
    myLatitude=myLatitude-(mouseY-pmouseY)/3f;
    myLongitude=myLongitude+(mouseX-pmouseX)/3f;

  }
}

void mousePressed(){
  if (hoverCircle()){
    zoomOn=true;
  }
  else if(zoomOn==true){

    zoomOn=false;
  }
}

/*
void serialEvent(Serial port) {
 if (handshake == false) {
 handshake = true;
 }
 //int inByte=port.read();
 }
 */

void drawBackground(){
  pushMatrix();
  translate(width/2,height/2);
  rotate(PI/2);
  stroke(160);
  fill(160);
  ellipse(0,0,diam,diam);
  popMatrix();
  //draw lines
  stroke(255);
  line(width/2,height/2,mouseX,mouseY); //angle
  //north
  line(width/2,height/4,width/2,height/2-diam/2); 
  //position
  fill(100,0,0);
  noStroke();
  ellipse(mouseX,mouseY,3,3);
}

void drawGrid(){
  stroke(140);
  fill(170);
  textFont(font, 8); 
  for (int i=-180;i<=180;i=i+15){
    float x=width/2+i*width/360;
    line(x,0,x,height);
    text(i,x+2,height/2+5);
  } 
  for (int i=-90;i<=90;i=i+15){
    float y=height/2-i*height/180;
    line(0,y,width,y);
    text(i,width/2+2,y+5);
  } 
}



float decimalMinutes(float l, char d){ 
  //GPGLL
  //Example (signal not acquired): $GPGLL,0000.0000,N,00000.0000,E,235947.000,V*2D
  //Example (signal acquired): $GPGLL,4250.5589,S,14718.5084,E,092204.999,A*2D
  //latitude ddmm.mmmm
  //longitude dddmm.mmmm
  float decimal;  //ddmm.mmmm
  float ll=l/100;//dd.mmmmmmm
  // println(ll);
  float dd=floor(ll);//dd
  // println(dd);
  float mm=ll%1;// .mmmmmm
  // println(mm);
  mm=mm*10/6; // .mmmmmm in decimal
  decimal=dd+mm;
  if(d=='W'||d=='S')decimal=decimal*-1;
  //println(decimal);
  return decimal;
}
//LatLon class
class LatLon {
  float latitude = 0;
  float longitude = 0;

  //get lat
  float getLat() {
    return latitude;
  }

  //get lon
  float getLon() {
    return longitude;
  }

  //make the XML request
  void requestLatLon() {
    //get all xml source and put into a single string
    String url = "http://timhibbard.com/webservices/engraphgps/engraph.asmx/GetTimsLocation";
    String[] lines = loadStrings(url);
    String xml = join(lines, " ");
    
    //search for latitude
    String lookforlat ="<Lat>";
    String endlat = "</Lat>";
    latitude = float(TextBetween(xml,lookforlat,endlat));

    //search for longitude
    String lookforlon ="<Lon>";
    String endlon = "</Lon>";
    longitude = float(TextBetween(xml,lookforlon,endlon));
  }

  //a function that returns a substring
  String TextBetween(String s, String before, String after) {
    String found = "";
    int start = s.indexOf(before); //index of beginning tag
    if (start==-1) return "";//if we don't find anything send back a blank string
    start+=before.length();//move to end of beginning tag
    int end = s.indexOf(after,start);//find end of end tag
    if(end==-1) return ""; //if we don't find anything send back a blank string
    return s.substring(start,end);
  }
}