//Written by Michael Chang
//for MAXIS

//USAGE: Hit PLAY on the left, then click and drag to create bezier lines on the graph.
//hit GENERATE to generate samples from the graph. Copy and paste this into your particle sim.
import java.awt.Toolkit; 
import java.awt.datatransfer.Clipboard; 
import java.awt.datatransfer.Transferable; 
import java.awt.datatransfer.DataFlavor; 
import java.awt.datatransfer.StringSelection; 

public class particle_graph extends BApplet 
{ 
 
Transferable clipData;
Clipboard clipboard; 
controlPoint nodes[];
int curIndex=0;
position mousePos;



BFont uiFont;
BFont graphFont;
BImage illegal;

boolean overControl;
boolean moveControl;
boolean overAnchor;

boolean invalid=false;
boolean showSampled=false;

controlPoint selectedControl;
controlPoint manipulatedControl;

button clearButton;
button flattenButton;
button sharpenButton;
button softenButton;
button generateButton;

position clickPos;
position releasePos;

HScrollbar hs1;
HScrollbar hs2;

float maxVal=100;
float minVal=-100;

void setup()
{
  size(800,630);
  cursor(CROSS);
  //smooth();
  nodes=new controlPoint[1000];
  ellipseMode(CENTER_DIAMETER);

  uiFont=loadFont("Homespun_TT_BRK.vlw");
  graphFont=loadFont("Verdana.vlw");

  clearButton=new button(new position(20,height-26),48,25,"clear");
  flattenButton=new button(new position(100,height-26),68,25,"flatten");
  sharpenButton=new button(new position(172,height-26),72,25,"sharper");
  softenButton=new button(new position(248,height-26),56,25,"wider");
  generateButton=new button(new position(width-104,height-26),83,25,"generate");
  
  hs1 = new HScrollbar(380, height-22, 300, 10, 2,600);  
  hs2 = new HScrollbar(380, height-8, 300, 10, 2,480);  
//  clipboard=Toolkit.getDefaultToolkit().getSystemClipboard();
//  clipData=clipboard.getContents(null);
}
void loop()
{
  background(89);

  mousePos=new position(mouseX,mouseY);

  if(curIndex>0)
  {
    fill(79);
    noStroke();
    rect(0,0,nodes[curIndex-1].x(),height-30);
  }
  noStroke();
  fill(219,225,253,40);
  rect(0,height-30,width,height);
  drawGrid();
  drawScales();
  drawGraph();
  stroke(130);
  line(0,300,width,300);  

  
  updateNodes();
  


  stroke(98,98,105);
  if(mousePressed&&clickPos!=null)
  {
    vector opposite=new vector(clickPos,mousePos);
    opposite.m*=-1;
    if(moveControl)
    {
      selectedControl.p=new position(mousePos);
      manipulatedControl=selectedControl;
      int nodeIndex=-1;
      for(int i=0;i<nodes.length;i++)
      if(selectedControl==nodes[i])
      {
        nodeIndex=i;
      }

      if(nodeIndex>1&&findOverlap(nodes[nodeIndex-1].x(),nodes[nodeIndex-1].anchor.x,nodes[nodeIndex].anchorOp.x,nodes[nodeIndex].x()))
      {

        invalid=true;

      }
      else
      if(nodeIndex!=curIndex-1&&nodeIndex>1&&findOverlap(nodes[nodeIndex].x(),nodes[nodeIndex].anchor.x,nodes[nodeIndex+1].anchorOp.x,nodes[nodeIndex+1].x()))
      {
        invalid=true;
      }
      else
      {
        invalid=false;
      }
      if(nodeIndex<=1&&findOverlap(nodes[nodeIndex].x(),nodes[nodeIndex].anchor.x,nodes[nodeIndex+1].anchorOp.x,nodes[nodeIndex+1].x()))
      {
        invalid=true;
      }
    }
    else
    {
      if(isectRectPointCorner(new position(20,0),width,height-30,mousePos)&&clickPos!=null)
      {
        fill(105);
        noStroke();
        ellipse(clickPos.x,clickPos.y,15,15);
        fill(0);
        ellipse(clickPos.x,clickPos.y,5,5);
        stroke(185,195,240);
        line(clickPos.x,clickPos.y,mouseX,mouseY);

        if(curIndex>0)
        {
          if(isectRectPointCorner(new position(0,0),nodes[curIndex-1].x(),height-30,clickPos))
          stroke(220,20,20);
          if(findOverlap(nodes[curIndex-1].x(),nodes[curIndex-1].ep().x,opposite.ep().x,clickPos.x))
          stroke(220,20,20);

          line(clickPos.x,clickPos.y,opposite.ep().x,opposite.ep().y);
          if(curIndex>0)
          bezier(nodes[curIndex-1].x(),nodes[curIndex-1].y(),nodes[curIndex-1].ep().x,nodes[curIndex-1].ep().y,opposite.ep().x,opposite.ep().y,clickPos.x,clickPos.y);
        }
        else
        line(clickPos.x,clickPos.y,opposite.ep().x,opposite.ep().y);
      }
    }

    stroke(120);
  }
  line(mouseX,0,mouseX,height);
  line(0,mouseY,width,mouseY);
  clearButton.draw();
  flattenButton.draw();
  sharpenButton.draw();
  softenButton.draw();
  generateButton.draw();
  if(!moveControl)
  {
    clearButton.update();
    flattenButton.update();
    sharpenButton.update();
    softenButton.update();
    generateButton.update();
    if(clearButton.pressed)
    {
      curIndex=0;
      nodes=new controlPoint[1000];
    }
    if(flattenButton.pressed)
    {
      if(nodes!=null)
      {
        for(int i=0;i<nodes.length;i++)
        {
          if(nodes[i]!=null)
          nodes[i].a=0;
        }
      }
    }
    if(sharpenButton.pressed)
    {
      if(nodes!=null)
      {
        for(int i=0;i<nodes.length;i++)
        {
          if(nodes[i]!=null)
          {
            nodes[i].m*=.9;
          }
        }
      }
    }
    if(softenButton.pressed)
    {
      if(nodes!=null)
      {
        for(int i=0;i<curIndex;i++)
        {
          if(nodes[i]!=null)
          {
            controlPoint test=new controlPoint(nodes[i]);
            test.m*=1.09;

            if(i<=1)
            {
              if(!findOverlap(test.x(),test.anchor.x,nodes[i+1].anchorOp.x,nodes[i+1].x()))
              nodes[i].m*=1.1;
            }
            else
            if(i==curIndex-1)
            {
              if(!findOverlap(nodes[i-1].x(),nodes[i-1].anchor.x,test.anchorOp.x,test.x()))
              nodes[i].m*=1.1;
            }
            else
            {
              if(!findOverlap(nodes[i-1].x(),nodes[i-1].anchor.x,test.anchorOp.x,test.x()))
              if(!findOverlap(test.x(),test.anchor.x,nodes[i+1].anchorOp.x,nodes[i+1].x()))
              nodes[i].m*=1.1;
            }
          }
        }
      }
    }

  }
  fill(204);
  textFont(uiFont,18);
  text("samples",328,height-18);  
  textFont(uiFont,18);
  text("scale",346,height-4);    
  hs1.update();
  hs1.draw();
  hs2.update();
  hs2.draw();
  maxVal=hs2.getPos();
  minVal=-1*maxVal;
  if(showSampled)
  {
    visualize();     
  }
}

position dataList[];
int dataIndex=0;
float outputData[];
int outputIndex=0;
void visualize()
{
  for(int i=0;i<dataList.length;i++)
  {
    if(dataList[i]!=null)
    {
      fill(150,240,70);
      stroke(180);
      ellipse(dataList[i].x,dataList[i].y,4,4);
      textFont(graphFont,15);
      
      text(outputData[i],dataList[i].x,dataList[i].y);
    }
  }
}
void generateTangents()
{
//  println("---------------------");
  dataList=new position[1000];  
  dataIndex=0;
  outputData=new float[1000];
  outputIndex=0;

  float dataRate=hs1.swidth-hs1.getPos();
  

  if(dataRate<10)
    dataRate=10;  
  
  //int xTracker=20;
  

  controlPoint currentNode=nodes[1];
  int blarg=0;
  for(float xs=20;xs<width;xs+=dataRate)
  {
    for(int i=1;i<curIndex-1;i++)
    {
      if(xs>nodes[i].x()&&xs<nodes[i+1].x())
      {

/*
        print("between: ");
        print(i);
        print(",");
        println(i+1);
*/        

        float sampleRate=.01;
        float px;
        float py;
        position curPoint;
        position nextPoint;
        for(float t=0;t<1;t+=sampleRate)
        {
          px=bezierPoint(nodes[i].x(),nodes[i].anchor.x,nodes[i+1].anchorOp.x,nodes[i+1].x(),t);
          py=bezierPoint(nodes[i].y(),nodes[i].anchor.y,nodes[i+1].anchorOp.y,nodes[i+1].y(),t);
          curPoint=new position(px,py);
          px=bezierPoint(nodes[i].x(),nodes[i].anchor.x,nodes[i+1].anchorOp.x,nodes[i+1].x(),t+sampleRate);
          py=bezierPoint(nodes[i].y(),nodes[i].anchor.y,nodes[i+1].anchorOp.y,nodes[i+1].y(),t+sampleRate); 
          nextPoint=new position(px,py);          
          if(xs>=curPoint.x&&xs<nextPoint.x)
          {
            float h=dist(curPoint,nextPoint);
            float xtotal=nextPoint.x-curPoint.x;
            float xdelta=xs-curPoint.x;
            float ratio=xdelta/xtotal;
            float length=h*ratio;
            float angle=getHeading(curPoint,nextPoint);
            
            position dataPoint=new position(curPoint);
            dataPoint=dataPoint.displace(angle,length);
            
            dataList[dataIndex]=new position(dataPoint);
            dataIndex++;
            
            if(dataPoint.y<=300)
            {
              float val=(300-dataPoint.y)/300*maxVal;
              outputData[outputIndex]=val;
//              println(val);              
            }
            else
            if(dataPoint.y>300)
            {
              float val=(300-dataPoint.y)/300*maxVal;
              outputData[outputIndex]=val;
//              println(val);
            }
            outputIndex++;            
/*            
            print("between t: ");
            print(t);
            print(", ");
            println(t+sampleRate);
            print("intersection estimated at: ");
            print(dataPoint.x);
            print(", ");
            println(dataPoint.y);
*/           
          } 
          else
          {

          }
        } 
      }//
    }
  }  
  float finalList[]=new float[outputIndex];
  for(int i=0;i<finalList.length;i++)
  {
    if(maxVal>10)
      finalList[i]=int(outputData[i]*100)/float(100);
    else
      finalList[i]=int(outputData[i]*1000)/float(1000);    
  }

//  String outToFile=join(finalList," ");
//  String list[]=splitStrings(finalList);
  //println(outToFile);
//  StringSelection data=new StringSelection(outToFile);
//  clipboard=Toolkit.getDefaultToolkit().getSystemClipboard();
//  clipboard.setContents(data,data);
//  saveStrings("output.txt",finalList);
}//

boolean createNew=false;
void mousePressed()
{

  if(clickPos==null)
  {
    clickPos=new position(mouseX,mouseY);
  }
  for(int i=0;i<nodes.length;i++)
  {
    if(nodes[i]!=null)
    if(nodes[i].over)
    moveControl=true;
  }
  if(moveControl)
  {
    //    if(selectedControl!=null)
    //      selectedControl.clickPos=mousePos;
  }
  else
  {
    if(isectRectPointCorner(new position(20,0),width,height-30,mousePos))
    clickPos=new position(mouseX,mouseY);
    else
    clickPos=null;
    createNew=true;
  }
}
void mouseReleased()
{
  if(generateButton.pressed)
  {
    if(nodes!=null&&curIndex!=0)
    {
      generateTangents(); 
      showSampled=true;  
    }
  }
  else
  {
    showSampled=false;  
  }
  if(moveControl)
  {

    moveControl=false;
    if(invalid)
    {
      if(manipulatedControl!=null)
      manipulatedControl.p=new position(manipulatedControl.clickPos);
      invalid=false;
    }
    else
    {
      //if(selectedControl!=null)
      manipulatedControl.clickPos=new position(mouseX,mouseY);
    }
    manipulatedControl=null;
  }
  if(createNew)
  {
    if(clickPos!=null)
    {
      releasePos=new position(mouseX,mouseY);
      if(curIndex==0)
      {
        nodes[curIndex]=new controlPoint(clickPos,releasePos);
        curIndex++;
        nodes[curIndex]=new controlPoint(clickPos,releasePos);
        curIndex++;
      }
      else
      {
        vector opposite=new vector(clickPos,mousePos);
        opposite.m*=-1;
        if(!isectRectPointCorner(new position(0,0),nodes[curIndex-1].x(),height-30,clickPos))
        {
          if(!findOverlap(nodes[curIndex-1].x(),nodes[curIndex-1].anchor.x,opposite.ep().x,clickPos.x))
          {
            nodes[curIndex]=new controlPoint(clickPos,releasePos);
            selectedControl=nodes[curIndex];
            curIndex++;
          }
        }
      }
    }
    createNew=false;
  }
}


void updateNodes()
{
  for(int i=0;i<nodes.length;i++)
  {
    if(nodes[i]!=null)
    nodes[i].update();
  }
}
void drawGrid()
{
  float interval=hs1.swidth-hs1.getPos();
  if(interval<10)
    interval=10;
  stroke(95,95,102);
  for(int x=20;x<width;x+=interval)
  {
    line(x,0,x,height-30);
  }
  for(int y=0;y<height-30;y+=60)
  {
    line(20,y,width,y);
  }
}

void drawScales()
{
  fill(255);
  textFont(uiFont,15);
/*  for(float i=minVal;i<=maxVal;i+=10)
  {
    text(i,7,-2+(height-30)-(i*((height-30)/maxY)));
  }*/

  for(int i=0;i<10;i++)
  {
    //ellipse(7,15+i*60,20,20);
    text(maxVal-(maxVal/5*i),7,12+i*60);
  }

//  text(maxVal,7,10);
}

void drawGraph()
{
  if(showSampled)
    stroke(130);
  else
    stroke(255);
  beginShape(LINE_STRIP);
  for(int i=1;i<nodes.length;i++)
  {
    if(nodes[i]!=null)
    {
      if(invalid)
        stroke(220,20,20);
      if(i>1)
      {
        bezierVertex(nodes[i-1].x(),nodes[i-1].y());
        bezierVertex(nodes[i-1].anchor.x,nodes[i-1].anchor.y);
        bezierVertex(nodes[i].anchorOp.x,nodes[i].anchorOp.y);
        bezierVertex(nodes[i].x(),nodes[i].y());
      }
    }
  }
  endShape();
  overControl=false;
  for(int i=1;i<nodes.length;i++)
  {
    if(nodes[i]!=null)
    {
      vector opposite=new vector(nodes[i].p,nodes[i].a,nodes[i].m*-1);
      //      if(overControl==false&&dist(mousePos,nodes[i].p)<10)
      if(selectedControl==nodes[i])
      {
        noStroke();
        fill(255);
        if(!showSampled)        
          ellipse(nodes[i].x(),nodes[i].y(),13,13);
        noFill();
        stroke(90,240,90,100);
        if(!showSampled)
          ellipse(nodes[i].x(),nodes[i].y(),25,25);
        overControl=true;
        if(mousePressed)
        {
        }
      }
      else
      {
        if(nodes[i].over)
        {
          noStroke();
          fill(180);
          if(!showSampled)          
            ellipse(nodes[i].x(),nodes[i].y(),12,12);
        }
      }

      stroke(0);
      fill(90,240,90);
      if(!showSampled)      
        ellipse(nodes[i].x(),nodes[i].y(),5,5);
      stroke(20,20,240);
      if(!showSampled)      
        line(nodes[i].anchor.x,nodes[i].anchor.y,nodes[i].anchorOp.x,nodes[i].anchorOp.y);
      noFill();
      stroke(40);
      if(!showSampled)      
      {
        ellipse(nodes[i].anchor.x,nodes[i].anchor.y,5,5);
        ellipse(nodes[i].anchorOp.x,nodes[i].anchorOp.y,5,5);
      }
/*
      float increment=.1;
      for(float t=increment;t<1;t+=increment)
      {
        if(i>1)
        {
          float curX=bezierPoint(nodes[i-1].x(),nodes[i-1].ep().x,opposite.ep().x,nodes[i].x(),t);
          float curY=bezierPoint(nodes[i-1].y(),nodes[i-1].ep().y,opposite.ep().y,nodes[i].y(),t);
          fill(150,120,70);
          stroke(180);
          ellipse(curX,curY,4,4);
        }
      }
*/      

    }
  }
}

void drawScene()
{

}

class button
{
  position p;
  float buttonWidth;
  float buttonHeight;
  String buttonText;
  boolean over;
  boolean pressed;

  button(position pos,float w,float h,String t)
  {
    p=new position(pos);
    buttonWidth=w;
    buttonHeight=h;
    buttonText=t;
  }
  void update()
  {
    if(isectRectPointCorner(p,buttonWidth,buttonHeight,mousePos))
    {

      over=true;
      if(mousePressed)
      {
        pressed=true;
      }
      else
      pressed=false;
    }
    else
    {

      over=false;
    }
  }
  void draw()
  {
    if(pressed)
    {
      push();
      translate(1,1);
      stroke(40);
      fill(220,230,240);
      rect(p.x-2,p.y-2,buttonWidth-2,buttonHeight-2);
      fill(0);
      noStroke();
      textFont(uiFont,25);
      text(buttonText,p.x+2,p.y+16);
      pop();
    }
    else
    if(over)
    {
      stroke(40);
      fill(255);
      rect(p.x-2,p.y-2,buttonWidth-2,buttonHeight-2);
      fill(0);
      noStroke();
      textFont(uiFont,25);
      text(buttonText,p.x+2,p.y+16);
    }
    else
    {
      stroke(40);
      fill(55);
      rect(p.x-2,p.y-2,buttonWidth-2,buttonHeight-2);
      fill(210);
      noStroke();
      textFont(uiFont,25);
      text(buttonText,p.x+2,p.y+16);
    }
    /*
    stroke(40);

    if(pressed)
    fill(220,230,240);
    else
    if(over)
    fill(255);
    else
    fill(55);
    rect(p.x-2,p.y-2,buttonWidth-2,buttonHeight-2);

    if(over)
    fill(0);
    else
    fill(210);
    noStroke();
    textFont(uiFont,25);
    text(buttonText,p.x+2,p.y+16);
    */
  }
}

class controlPoint extends vector
{
  position clickPos;
  boolean over;
  position anchor;
  position anchorOp;
controlPoint(){}
  controlPoint(float tx,float ty,float angle,float magnitude)
  {
    p=new position(tx,ty);
    a=angle;
    m=magnitude;
    anchor=ep();
    vector opposite=new vector(p,a,m*-1);
    anchorOp=opposite.ep();
    clickPos=new position(p);
  }
  controlPoint(position start,position end)
  {
    p=new position(start);
    a=getHeading(start,end);
    m=dist(start,end);
    anchor=ep();
    vector opposite=new vector(p,a,m*-1);
    anchorOp=opposite.ep();
    clickPos=new position(p);
  }
  controlPoint(position pos,float angle,float magnitude)
  {
    p=new position(pos);
    a=angle;
    m=magnitude;
    anchor=ep();
    vector opposite=new vector(p,a,m*-1);
    anchorOp=opposite.ep();
    clickPos=new position(p);
  }
  controlPoint(controlPoint v)
  {
    p=new position(v.p);
    a=v.a;
    m=v.m;
    anchor=ep();
    vector opposite=new vector(p,a,m*-1);
    anchorOp=opposite.ep();
    clickPos=new position(p);
  }

  void update()
  {
    if(m<.01)
    m=.01;
    anchor=ep();
    vector opposite=new vector(p,a,m*-1);
    anchorOp=opposite.ep();
    if(p.x<20)
    p.x=20;
    if(p.x>width)
    p.x=width;
    if(p.y<0)
    p.y=0;
    if(p.y>height-30)
    p.y=height-30;
    if(dist(mousePos,p)<10)
    {
      over=true;
      if(mousePressed)
      {
        selectedControl=this;
      }
    }
    else
    over=false;
  }
}






class HScrollbar
{
  int swidth, sheight;    // width and height of bar
  int xpos, ypos;         // x and y position of bar
  float spos, newspos;    // x position of slider
  int sposMin, sposMax;   // max and min values of slider
  int loose;              // how loose/heavy
  boolean over;           // is the mouse over the slider?
  boolean locked;
  float ratio;

  HScrollbar (int xp, int yp, int sw, int sh, int l,int sp) {
    swidth = sw;
    sheight = sh;
    int widthtoheight = sw - sh;
    ratio = (float)sw / (float)widthtoheight;
    xpos = xp;
    ypos = yp-sheight/2;
    spos = sp;//xpos + swidth/1.2 - sheight/2;
    newspos = spos;
    sposMin = xpos;
    sposMax = xpos + swidth - sheight;
    loose = l;
  }

  void update() {
    if(over()) {

      over = true;
    } else {

      over = false;
    }
    if(mousePressed && over) {
      locked = true;
    }
    if(!mousePressed) {
      locked = false;
    }
    if(locked) {
      newspos = constrain(mouseX-sheight/2, sposMin, sposMax);
    }
    if(abs(newspos - spos) > 1) {
      spos = spos + (newspos-spos)/loose;
    }
  }

  int constrain(int val, int minv, int maxv) {
    return min(max(val, minv), maxv);
  }

  boolean over() {
    if(mouseX > xpos && mouseX < xpos+swidth &&
    mouseY > ypos && mouseY < ypos+sheight) {
      
      return true;
    } else {
      return false;
    }
  }

  void draw() {

    fill(55);
    rect(xpos, ypos, swidth, sheight);
    if(over || locked) {
      fill(255);
    } else {
      fill(215);
    }
    rect(spos, ypos, sheight, sheight);

  }

  float getPos() {
    // convert spos to be values between
    // 0 and the total width of the scrollbar
    return ((spos-xpos) * ratio);
  }
}












boolean isectRectPointCorner(position rp,float rw,float rh,position p)
{
  if(p.x>rp.x)
  if(p.x<rp.x+(rw))
  if(p.y>rp.y)
  if(p.y<rp.y+(rh))
  return true;
  return false;
}
boolean isectRectPointCenter(position rp,float rw,float rh,position p)
{
  if(p.x>rp.x-(rw/2))
  if(p.x<rp.x+(rw/2))
  if(p.y>rp.y-(rh/2))
  if(p.y<rp.y+(rh/2))
  return true;
  return false;
}
boolean findOverlap(float a,float b,float c,float d)
{
  float increment=.005;
  for(float t=increment;t<=1;t+=increment)
  {
    float prev=bezierPoint(a,b,c,d,t-increment);
    float cur=bezierPoint(a,b,c,d,t);
    if(prev>cur)
    return true;
  }
  return false;
}
//-----------------------------------------VECTOR/COORDINATE LIBRARY
//Written by Michael Chang
//last revision 11/30/03
static float conversionArc=180.0/PI;                              //180/PI used for many calculations
class position                                           //position class that stores 2 values x and y
{
  float x,y;
position(){}
  position(float tx,float ty)
  {
    x=(tx);
    y=(ty);
  }
  position(position p)
  {
    if(p!=null)
    {
      x=p.x;
      y=p.y;
    }
  }
  position displace(float angle,float magnitude)         //returns the position displaced by an angle and distance
  {
    float ra=radians(angle);
    /*
    code optimization:
    ra is used for calculating radian conversion only once instead
    of once per line.
    */

    position newP=new position(x,y);
    newP.x+=(cos(ra)*magnitude);
    newP.y-=(sin(ra)*magnitude);
    return newP;
  }
  void set(float tx,float ty)                            //sets a new coordinate
  {
    x=(tx);
    y=(ty);
  }
  void set(position p)
  {
    x=p.x;
    y=p.y;
  }
}
class vector                                            //vector class that stores a position, angle, and magnitude
{
  float a,m;
  position p;

vector(){}
  vector(float tx,float ty,float angle,float magnitude)
  {
    p=new position(tx,ty);
    a=angle;
    m=magnitude;
  }
  vector(position start,position end)
  {
    p=new position(start);
    a=getHeading(start,end);
    m=dist(start,end);
  }
  vector(position pos,float angle,float magnitude)
  {
    p=new position(pos);
    a=angle;
    m=magnitude;
  }
  vector(vector v)
  {
    p=new position(v.p);
    a=v.a;
    m=v.m;
  }
  position endPoint()                                    //returns the endpoint of the vector as type position
  {
    return displace(p,a,m);
  }
  position ep()
  {
    return endPoint();
  }
  vector add(vector v)                                   //returns a vector addition
  {
    position newEnd=new position(endPoint().displace(v.a,v.m));
    float newAngle=getHeading(p,newEnd);
    vector newVector=new vector(p,newAngle,dist(p.x,p.y,newEnd.x,newEnd.y));
    return newVector;
  }
  vector add(float angle,float magnitude)                //returns a vector addition
  {
    vector v=new vector(p,angle,magnitude);
    position newEnd=new position(endPoint().displace(v.a,v.m));
    float newAngle=getHeading(p,newEnd);
    vector newVector=new vector(p,newAngle,dist(p.x,p.y,newEnd.x,newEnd.y));
    return newVector;
  }
  vector sub(vector v)                                   //returns a vector subration (unimplemented)
  {
    return v;
  }
  void set(vector v)
  {
    p.set(v.p);
    a=v.a;
    m=v.m;
  }
  void set(position np,float na,float nm)
  {
    p.set(np);
    a=na;
    m=nm;
  }
  void setStart(position np)                             //sets a new starting point for the vector
  {
    p=np;
  }
  void setEnd(position np)                               //sets a new ending point for the vector
  {
    a=getHeading(p,np);
    m=dist(p,np);
  }
  void setEnd(float px,float py)                         //sets a new ending point for the vector
  {
    position np=new position(px,py);
    a=getHeading(p,np);
    m=dist(p.x,p.y,np.x,np.y);
  }
  float x()
  {
    return p.x;
  }
  float y()
  {
    return p.y;
  }
  int isUnder(position s)
  {
    if(a==90||a==270)
    return 0;
    position e=new position(endPoint());
    float slope=(p.y-e.y)/(p.x-e.x);
    float yIntercept=p.y-(slope*p.x);
    float testY=(slope*s.x)+yIntercept;
    //    if(slope>0)
    //    {
      if(s.y>testY)
      return 1;
      else
      return -1;
    /*    }
    else
    {
      if(s.y>testY)
      return -1;
      else
      return 1;
    }*/
  }
}

position displace(position p,float angle,float magnitude) //displaces a position by an angle and a magnitude, then returns it
{
  position newP=new position(p);
  float ra=radians(angle);
  newP.x+=(cos(ra)*magnitude);
  newP.y-=(sin(ra)*magnitude);

  return newP;
}
float getHeading(position p1,position p2)                //gets the absolute heading between one position and another relative to the first
{
  if(p1.x==p2.x&&p1.y==p2.y)
  return 0;
  float xd=p2.x-p1.x;
  float yd=p2.y-p1.y;
  float angle=float(Math.atan(yd/xd));
  //  angle=degrees(angle);
  angle=angle*conversionArc;
  if(xd>0&&yd<0)
  {
    angle=-1*angle;
  }
  else
  if(xd<0&&yd<0)
  angle=180-angle;
  else
  if(xd<0&&yd>0)
  angle=180-angle;
  else
  if(xd<0&&yd==0)
  angle=180;
  else
  if(xd==0&&yd<0)
  angle=90;
  else
  angle=360-angle;
  return angle;
}
float dist(position p1,position p2)
{
  return dist(p1.x,p1.y,p2.x,p2.y);
}
float normalizeHeading(float ang)                        //normalizes a heading between 0 and 360
{
  while(ang > 360)ang -= 360;
  while(ang < 0)ang += 360;
  return ang;
}
void drawVector(vector v)                                //draws a vector in white
{
  colorMode(RGB,255,255,255);
  stroke(255);
  noFill();
  ellipseMode(CENTER_DIAMETER);
  ellipse(v.p.x,v.p.y,5,5);
  position e=v.endPoint();
  line(v.p.x,v.p.y,e.x,e.y);
}
void drawVector(vector v,float s)                           //draws a vector with a longer line for representation in white
{
  colorMode(RGB,255,255,255);
  stroke(255);
  noFill();
  ellipseMode(CENTER_DIAMETER);
  ellipse(v.p.x,v.p.y,5,5);
  vector temp=new vector(v);
  temp.m*=s;
  position e=temp.endPoint();
  line(temp.p.x,temp.p.y,e.x,e.y);
}
void drawVector(vector v,int r,int g,int b,float s)                           //draws a vector with a longer line for representation in white
{
  colorMode(RGB,255,255,255);
  stroke(r,g,b);
  noFill();
  ellipseMode(CENTER_DIAMETER);
  ellipse(v.p.x,v.p.y,5,5);
  vector temp=new vector(v);
  temp.m*=s;
  position e=temp.endPoint();
  line(temp.p.x,temp.p.y,e.x,e.y);
}
void drawVector(vector v,int r,int g,int b)              //draws a vector in RGB
{
  colorMode(RGB,255,255,255);
  stroke(r,g,b);
  noFill();
  ellipseMode(CENTER_DIAMETER);
  ellipse(v.p.x,v.p.y,5,5);
  position e=v.endPoint();
  line(v.p.x,v.p.y,e.x,e.y);
}



}