/*
 * Jeffrey Ladino - jnl22@ccs.neu.edu
 *
 * File: bb.cpp
 *
 * Change Log
 ************
 ** August 18, 1999 - Jeff Ladino
 * Created.
 *
 */

#include "bb.h"
#include "transforms.h"
#include <assert.h>
#include <math.h>
#include <stdio.h>

void bb::setup(Real xc, Real yc, Real xv, Real yv, Real r){
  position[Xpt] = xc;
  position[Ypt] = yc;
  position[Zpt] = 1.0;

  velocity[Xpt] = xv;
  velocity[Ypt] = yv;
  velocity[Zpt] = 1.0;

  radius = r;

  led[0].setanchor(xc-(.65*r), yc-(.5*r) );
  led[1].setanchor(xc+(.2*r), yc-(.5*r) );
  led[0].setsize(r);
  led[1].setsize(r);

  ball.setradius(radius);
  ball.setcenter(position[Xpt], position[Ypt]);

  // Rotational characteristics
  rotation = 0;
  angular_velocity = FULL_CIRCLE/360;

  if(VERBOSITY > 1){
    printf("bb::setup()\n");
    print();
    printf("bb::setup()\n");
  }
}

// default constructor
bb::bb(){}

bb::bb(Real xc, Real yc, Real xv, Real yv, Real r, unsigned num){
  setup(xc, yc, xv, yv, r);
  assert(0 <= num);
  assert(num <= 99);
  led[0].setdigit(num/10);
  led[1].setdigit(num%10);

  if(VERBOSITY > 1){
    printf("bb::bb()\n");
    print();
    printf("bb::bb()\n");
  }
}

//
// setters
//
void bb::init(Real xc, Real yc, Real xv, Real yv, Real r, Real av,
	      unsigned num, unsigned long bc, unsigned long lc){

  setup(xc, yc, xv, yv, r);
  setangularvelocity(av);
  setballcolor(bc);
  setledcolor(lc);
  assert(num <= 99);
  led[0].setdigit(num/10);
  led[1].setdigit(num%10);
}


void bb::setballcolor(unsigned long color){
  assert(0 <= color < 256);
  ballcolor = color;
}

void bb::setledcolor(unsigned long color){
  assert(0 <= color < 256);
  ledcolor = color;
}

void bb::setangularvelocity(Real av){
  angular_velocity = av;
}

void bb::setvelocity(vector3d& v){
  velocity[Xpt] = v[Xpt];
  velocity[Ypt] = v[Ypt];
  velocity[Zpt] = v[Zpt];
}

void bb::setcenter(vector3d& v){
  position[Xpt] = v[Xpt];
  position[Ypt] = v[Ypt];
  position[Zpt] = v[Zpt];
}

// 
// getters
//
vector3d& bb::getcenter(){
  vector3d* v = new vector3d();
  (*v)[Xpt] = position[Xpt];
  (*v)[Ypt] = position[Ypt];
  (*v)[Zpt] = position[Zpt];
  return *v;
}

vector3d& bb::getvelocity(){
  vector3d* v = new vector3d();
  (*v)[Xpt] = velocity[Xpt];
  (*v)[Ypt] = velocity[Ypt];
  (*v)[Zpt] = velocity[Zpt];
  return *v;
}

Real bb::getradius(){ return radius; }

void bb::draw(SimpleDraw& sd, matrix3d m){
  if(VERBOSITY > 1){
    printf("BEGIN bb::draw()\n");
    print();
    printf("END bb::draw()\n");
  }

  matrix3d A = Rotate_about(rotation, position[Xpt], position[Ypt]);
  matrix3d B = m*A;
  ball.draw(sd, m);
  led[0].draw(sd, B);
  led[1].draw(sd, B);
}

//
// draw
//
void bb::draw_color(SimpleDraw& sd, matrix3d m){
  matrix3d A = Rotate_about(rotation, position[Xpt], position[Ypt]);
  matrix3d B = m*A;

  sd.SetForeColor(ballcolor);
  ball.draw(sd, m);
  sd.SetForeColor(ledcolor);
  led[0].draw(sd, B);
  led[1].draw(sd, B);
  
  // slow down the ball for displaying
  for(long slow=0;slow<10000;slow++);
}

void bb::movebb(Real xc, Real yc){
  //printf("bb::movebb(%e, %e)\n", xc, yc);

  position[Xpt] = xc;
  position[Ypt] = yc;

  led[0].setanchor(xc-(.65*radius), yc-(.5*radius) );
  led[1].setanchor(xc+(.2*radius), yc-(.5*radius) );

  ball.setcenter(position[Xpt], position[Ypt]);

  // Rotational movement adjusted around a FULL_CIRCLE
  rotation = rotation + angular_velocity;
  if(rotation > FULL_CIRCLE)
    rotation -= FULL_CIRCLE;
  if(rotation < FULL_CIRCLE)
    rotation += FULL_CIRCLE;

  if(VERBOSITY > 1){
    printf("bb::movebb()\n");
    print();
    printf("bb::movebb()\n");
  }
}

//
// move
//
void bb::move(Real xmin, Real ymin, Real xmax, Real ymax){
  vector3d temp = position + velocity;

  // Major debugging output!!!
  /*
  printf("\npos(%e, %e)\nvel(%e, %e\n temp(%e, %e)\n",
	 position[Xpt], position[Ypt], velocity[Xpt],
	 velocity[Ypt], temp[Xpt], temp[Ypt] );
  */

  if(ymin+radius > temp[Ypt]){
    velocity[Ypt] = -velocity[Ypt];
    temp[Ypt] = ymin+radius;
    //temp[Ypt] = position[Ypt]+velocity[Ypt];
    angular_velocity = -angular_velocity;
    /*
    printf("\nif(ymin+radius > temp[Ypt])\n");
    printf("pos(%e, %e)\nvel(%e, %e\n temp(%e, %e)\n",
	 position[Xpt], position[Ypt], velocity[Xpt],
	 velocity[Ypt], temp[Xpt], temp[Ypt] );
    */
  }
  if(ymax-radius < temp[Ypt]){
    velocity[Ypt] = -velocity[Ypt];
    temp[Ypt] = ymax-radius;
    //temp[Ypt] = position[Ypt]+velocity[Ypt];
    angular_velocity = -angular_velocity;
    /*
    printf("\nif(ymax-radius < temp[Ypt])\n");
    printf("\npos(%e, %e)\nvel(%e, %e\n temp(%e, %e)\n",
	 position[Xpt], position[Ypt], velocity[Xpt],
	 velocity[Ypt], temp[Xpt], temp[Ypt] );
    */
  }
  if(xmin+radius > temp[Xpt]){
    velocity[Xpt] = -velocity[Xpt];
    temp[Xpt] = xmin+radius;
    //temp[Xpt] = position[Xpt]+velocity[Xpt];
    angular_velocity = -angular_velocity;
    /*
    printf("\nif(xmin+radius > temp[Xpt])\n");
    printf("\npos(%e, %e)\nvel(%e, %e\n temp(%e, %e)\n",
	 position[Xpt], position[Ypt], velocity[Xpt],
	 velocity[Ypt], temp[Xpt], temp[Ypt] );
    */
  }
  if(xmax-radius < temp[Xpt]){
    velocity[Xpt] = -velocity[Xpt];
    temp[Xpt] = xmax-radius;
    //temp[Xpt] = position[Xpt]+velocity[Xpt];
    angular_velocity = -angular_velocity;
    /*
    printf("\nif(xmax-radius < temp[Xpt])\n");
    printf("\npos(%e, %e)\nvel(%e, %e\n temp(%e, %e)\n",
	 position[Xpt], position[Ypt], velocity[Xpt],
	 velocity[Ypt], temp[Xpt], temp[Ypt] );
    */

  }
  movebb(temp[Xpt], temp[Ypt]);
}

//
// collide_with
//
// two balls collide.  this function has side
// effects on both of the balls that collide!
void bb::collide_with(bb& b, Real radius){
  vector3d u1, u2;
  vector3d p1, p2;
  Real length_u1, length_u2;

  u1 = b.getcenter() - getcenter();
  // unitize u1
  length_u1 = sqrt(u1.mag_sq());
  u1[Xpt] = u1[Xpt]/length_u1;
  u1[Ypt] = u1[Ypt]/length_u1;
  u1[Zpt] = 1.0;
  // project velocity onto u1
  p1 = u1 * (getvelocity()*u1);

  u2 = getcenter() - b.getcenter();
  // unitize u2
  length_u2 = sqrt(u2.mag_sq());
  u2[Xpt] = u2[Xpt]/length_u2;
  u2[Ypt] = u2[Ypt]/length_u2;
  u2[Zpt] = 1.0;
  // project b.getvelocity() onto u2
  p2 = u2 * (b.getvelocity()*u2);

  setvelocity(getvelocity() - p1 + p2);
  b.setvelocity(b.getvelocity() - p2 + p1);
}

// OLD VERSION - MOSTLY JUNK
/*
void bb::collide_with(bb& b, Real radius){
  vector3d temp;
  temp[Xpt] = velocity[Xpt];
  temp[Ypt] = velocity[Ypt];
  temp[Zpt] = velocity[Zpt];

  //printf("bb::collide_with() BEGIN\n");
  //velocity.print();
  //b.getvelocity().print();

  // swap the balls' velocities
  setvelocity(b.getvelocity());
  b.setvelocity(temp);

  // move the centers of the balls to prevent sticking
  vector3d difference = getcenter() - b.getcenter();
  Real distance = sqrt(difference.mag_sq());
  difference[Xpt] = difference[Xpt]/distance;
  difference[Ypt] = difference[Ypt]/distance;
  setcenter(difference*(radius+2) );

  //Real collide = 2*radius;
  //Real ratio = collide/distance;
  //distance = distance/ratio + 3.0;
  // parametric line equation
  //setcenter(getcenter()*ratio+b.getcenter()*(1.0-ratio));

  //printf("bb::collide_with() END\n");
  //velocity.print();
  //b.getvelocity().print();
}
*/

//
// print
//
void bb::print(){
  printf("\nBB print begin\n");
  printf("position = (%e,%e)\n", position[Xpt], position[Ypt]);
  printf("radius = %e\n",radius);
  printf("velocity = (%e,%e)\n", velocity[Xpt], velocity[Ypt]);
  printf("current rotation = %e : rotational velocity = %e\n",
	 rotation, angular_velocity);
  printf("ballcolor = %i : ledcolor = %i", ballcolor, ledcolor);
  printf("Characters are ...\n");
  led[0].print();
  led[1].print();
  printf("ball is ...\n");
  ball.print();
  printf("\nBB print end\n");
}