/* built with Studio Sketchpad: 
 *   https://sketchpad.cc
 * 
 * observe the evolution of this sketch: 
 *   https://studio.sketchpad.cc/sp/pad/view/ro.8UI4lNtO0Ss/rev.3368
 * 
 * authors: 
 *   
 *   Scott Mahr

 * license (unless otherwise specified): 
 *   creative commons attribution-share alike 3.0 license.
 *   https://creativecommons.org/licenses/by-sa/3.0/ 
 */ 



// 1 - render something that looks like a strafe bot
// 2 - make it move around with key presses
// 3 - have it draw and remember the path that it took
// 4 - output the path as motor velocities
// 

int mem = 200;
int[][][] pos = new int[4][mem][2]; 
int posIdx = 0;

float m;float a;float b;float c;
float sv;float cv;float[][] cnrs;
int[] cur = {200,200,0}; 
int[] max = {400,400,360}; 

int step = 2;
    
void setup()
{
  size(400, 400);
  noStroke();
  background(0);
  
  frameRate(15);
}

void draw()
{ 
  smooth(); 
  background(0);
  noStroke();
  fill(226);  
  
  pushMatrix();
  translate(cur[0], cur[1]); 
  rotate(radians(cur[2]));
  drawBot();
  popMatrix();
  drawPath();

  if(keyPressed || mousePressed){
    addPoint();
    if(keyPressed){
      if(key == 'w'){cur[1] -= step;}
      if(key == 'a'){cur[0] -= step;}
      if(key == 'd'){cur[0] += step;}
      if(key == 's'){cur[1] += step;}
    }
    if(mousePressed){
        cur[2] += (mouseX-200)/15;  
    }
    for(int i=0;i<2;i++){
      if(cur[i]<0){cur[i]==0;}
      if(cur[i]>max[i]){cur[i]==max[i];} 
    }
    if(cur[2] < 0){cur[2] += max[2];}
    if(cur[2] > max[2]){cur[2] -= max[2];}
  }
}


void addPoint()
{
    sv = sin(radians(cur[2]))*30;
    cv = cos(radians(cur[2]))*30;
    cnrs = {{sv,-cv},{-sv,cv},{-cv,-sv},{cv,sv}};
    for(int i=0;i<4;i++){
        for(int j=0;j<2;j++){
            pos[i][posIdx][j] = cur[j]+cnrs[i][j];
        }
    }
    posIdx +=1; 
    if(posIdx >= mem){posIdx = 0;}   
}

void drawPath()
{
  for(int i = 1; i < mem; i = i+1){
      float[] vel = getSpeed(i-1);
      //float[] vel = [0,0,0,0];
      for(int j=0;j<4;j++){
       if(vel[j]<0){stroke(255,0,0);}
       else if(vel[j]>0){stroke(0,0,255);}
       else(stroke(0,255,0);}
       point(pos[j][i][0], pos[j][i][1]);
    }
  }
}

void drawBot()
{
  quad(0, 0-30, 0+30, 0, 0, 0+30, 0-30, 0);
  rect(0-5, 0-33, 10, 6);
  
  rect(0-5, 0+30, 10, 3);
  rect(0-33, 0-5, 3 ,10);
  rect(0+30, 0-5, 3, 10); 
}

//this is for calculation
float[] getSpeed(int idx)
{
  m = 1.0*(pos[1][idx][1]-pos[0][idx][1])/(pos[1][idx][0]-pos[0][idx][0]); 
  a = -m;
  b = 1;
  c = -pos[1][idx][1]+m*pos[1][idx][0];
  a2 = 1/m;
  c2 = -pos[3][idx][1]-1/m*pos[3][idx][0];
  float[] vel = new float[4];
  for(int j=0;j<4;j++){   
    if(j<2){
      vel[j] = (a*pos[j][idx+1][0]+b*pos[j][idx+1][1]+c)/sqrt(sq(a)+sq(b));
      
    }
    else{
      vel[j]=(a2*pos[j][idx+1][0]+b*pos[j][idx+1][1]+c2)/sqrt(sq(a2)+sq(b));
      vel[j] *= m/abs(m);
    }
  }
  
  if(pos[0][idx][0]>pos[1][idx][0]){vel[0]*=-1;}
  else{vel[1]*=-1;}
  
  if(pos[2][idx][1]<pos[3][idx][1]){vel[2]*=-1;}
  else{vel[3]*=-1;}
  
  return vel; 
}

void keyReleased() 
{
  if(key == 'p') {
    for(int i = 1; i < mem; i = i+1){
      float[] vel = getSpeed(i-1);
      print("line"+i);
      for(int j=0;j<4;j++){
        print(","+round(vel[j]*100)/100);
      }
      print("\n");
    }
    println("done");
  }
}