import fisica.*;
import fisica.FWorld;
import fisica.FBody;

float frequency = 5;
float damping = 1;
float puenteY;

FBody[] steps = new FBody[20];
FWorld world;
PImage g_arrowImage;

int boxWidth =  WND_WIDTH /(steps.length) - 2;

void phy_init() 
{
  puenteY = height/3;

  Fisica.init(this);

  world = new FWorld(0,0,WND_WIDTH,WND_HEIGHT);
  world.setEdges();
  //world.remove(world.left);
  //world.remove(world.right);
  world.remove(world.top);

  FCircle bola = new FCircle(40);
  bola.setPosition(width/3, puenteY-10);
  bola.setDensity(0.2);
  bola.setFill(120, 120, 190);
  bola.setNoStroke();
  world.add(bola);

  for (int i=0; i<steps.length; i++) {
    steps[i] = new FBox(boxWidth, 10);
    steps[i].setPosition(map(i, 0, steps.length-1, boxWidth, width-boxWidth), puenteY);
    steps[i].setNoStroke();
    steps[i].setFill(120, 200, 190);
    world.add(steps[i]);
  }

  for (int i=1; i<steps.length; i++) {
    FDistanceJoint junta = new FDistanceJoint(steps[i-1], steps[i]);
    junta.setAnchor1(boxWidth/2, 0);
    junta.setAnchor2(-boxWidth/2, 0);
    junta.setFrequency(frequency);
    junta.setDamping(damping);
    junta.setFill(0);
    junta.calculateLength();
    world.add(junta);
  }

  addWorldWall();

  FCircle left = new FCircle(10);
  left.setStatic(true);
  left.setPosition(0, puenteY);
  left.setDrawable(false);
  world.add(left);

  FCircle right = new FCircle(10);
  right.setStatic(true);
  right.setPosition(width, puenteY);
  right.setDrawable(false);
  world.add(right);

  FDistanceJoint juntaPrincipio = new FDistanceJoint(steps[0], left);
  juntaPrincipio.setAnchor1(-boxWidth/2, 0);
  juntaPrincipio.setAnchor2(0, 0);
  juntaPrincipio.setFrequency(frequency);
  juntaPrincipio.setDamping(damping);
  juntaPrincipio.calculateLength();
  juntaPrincipio.setFill(0);
  world.add(juntaPrincipio);

  FDistanceJoint juntaFinal = new FDistanceJoint(steps[steps.length-1], right);
  juntaFinal.setAnchor1(boxWidth/2, 0);
  juntaFinal.setAnchor2(0, 0);
  juntaFinal.setFrequency(frequency);
  juntaFinal.setDamping(damping);
  juntaFinal.calculateLength();
  juntaFinal.setFill(0);
  world.add(juntaFinal);

  /* 遏｢蜊ｰ陦ｨ遉ｺ逕ｨ縺ｫ蛻ｩ逕ｨ縺吶ｋ繝�け繧ｹ繝√Ε縺ｮ繝ｭ繝ｼ繝�*/
  g_arrowImage = loadImage("arrow.png");
}

void addWorldWall()
{
  FCircle left = new FCircle(10);
  left.setStatic(true);
  left.setPosition(0, puenteY);
  left.setDrawable(false);
  world.add(left);

  FCircle right = new FCircle(10);
  right.setStatic(true);
  right.setPosition(width, puenteY);
  right.setDrawable(false);
  world.add(right);
}

void phy_draggable( boolean isDraggable )
{
  world.setGrabbable( isDraggable  );
}

/* 迚ｩ逅�ｒ譖ｴ譁ｰ縺励�謠冗判縺吶ｋ */
void phy_updateAndRender( boolean isShowBodiesInfo )
{
  /* world縺ｮ譖ｴ譁ｰ */
  /* step(dt,ite)繧剃ｽｿ縺�ｈ繧翫ｂstep()繧定､�焚蝗樔ｽｿ縺｣縺溘⊇縺�′縲∵ｻ代ｉ縺九↑縺ｮ縺ｧ縺薙ｌ繧貞茜逕ｨ縺吶ｋ */
  //int iteratoNum = 2;
  //world.step( 2.0f/frameRate, iteratoNum );
  for( int i=0;i<5;++i)
  {
    world.step();
  }
  world.draw();
  
  if( isShowBodiesInfo )
  {
    /* TODO diviede otherfunc */
    ArrayList bodies = world.getBodies();
    for( Object obj: bodies )
    {
      FBody body = (FBody)obj;
      float vx = body.getVelocityX();
      float vy = body.getVelocityY();
      float x  = body.getX();
      float y  = body.getY(); 
      float rot = atan2( vy, vx );
      float bodyScale = min( sqrt( vy*vy+ vy*vy )*0.01f-0.2f, 0.5f );
      float bodyAlpha = bodyScale;
      if( bodyScale < 0.0 )
      { continue; }
      /* show arrow at body */
      pushMatrix();
      translate( x, y );
      rotate( rot + PI/2.0f );
      scale( bodyScale );
      translate( -g_arrowImage.width/2, -g_arrowImage.height );
      image( g_arrowImage, 0, 0 );
      popMatrix();
    }
   }
}

/* Add circle body */
void AddCircle( float x, float y, float radius )
{  
  FCircle bola = new FCircle(radius);
  bola.setPosition(x, y);
  bola.setDensity(0.2);
  bola.setFill(120, 120, 190);
  bola.setNoStroke();
  world.add(bola);
}

/* Add box body */
void AddBox( float x, float y, float width, float height, float rot )
{
  x += width/2;
  y += height/2;

  FBox newBox = new FBox(width,height);
  newBox.setPosition(x, y);
  newBox.setRotation( rot );
  newBox.setDensity(0.2);
  newBox.setFill(180, 120, 190);
  newBox.setNoStroke();
  world.add(newBox);
}

/**/
void AddTriangle( float x, float y, float width, float height, float rot )
{  
  FPoly newPoly = new FPoly();
  
  float w3 = width/3;
  float h3 = height/3;
  
  x += w3;
  y += h3*2;
  
  if( width < 0 )
  { print("unko"); }
  print(""+width+" "+height  );
  
  newPoly.vertex(-w3, -h3*2 );
  newPoly.vertex(-w3, +h3 );
  newPoly.vertex(+w3*2, +h3 );
  
  newPoly.setPosition(x, y);
  newPoly.setRotation( rot );
  newPoly.setDensity(0.2);
  newPoly.setFill(120, 180, 120);
  newPoly.setNoStroke();
  
  world.add( newPoly );
}

/* 謖�ｮ壹＠縺滉ｽ咲ｽｮ縺ｮ繧ｪ繝悶ず繧ｧ繧ｯ繝医�"繧ｹ繧ｿ繝�ぅ繝�け"竍�繝�う繝翫Α繝�け"縺ｮ蛻�ｊ譖ｿ縺医ｒ陦後≧ */
void phy_switchStaticAt( int aMouseX, int aMouseY )
{
  FBody body = world.getBody( aMouseX, aMouseY );
  if( body != null )
  {
    if( !body.isStatic() )
    {
      body.setStaticBody( true );
      body.setStatic( true );
    }
    else
    {
      float x = body.getX();
        float y = body.getY();
      if( body instanceof FBox )
      {
        FBox box = (FBox)body;
        float width  = box.getWidth();
        float height = box.getHeight();
        float rot    = box.getRotation();
        world.remove( body );
        AddBox( x-width/2, y-height/2, width, height, rot );
      }
      else if( body instanceof FCircle )
      {
        FCircle circle = (FCircle)body;
        float radius = circle.getSize();
        world.remove( body );
        AddCircle( x, y, radius );
      }
      else if( body instanceof FPoly )
      {
        //FPoly poly = (FPoly)body;
        //poly.getvertex();
        //float radius = circle.getSize();
        world.remove( body );
        //AddCircle( x, y, radius );
      }
    }
  }
}

/**/
void setBodyColor_( FBody body )
{  
  if( body.isStatic() )
  { body.setFillColor( 0xFF7878BE ); }
  else
  { body.setFillColor( 0xFF39395F ); }
}

void phy_rotobj( int x, int y, int mouseDelta )
{
 FBody body = world.getBody( x,  y );
 if( body != null && body.isStatic() )
 {
    float rot = body.getRotation();
    rot += mouseDelta * PI/36.0f;
    body.setRotation( rot );
 }
}

/* 謖�ｮ壹＠縺溽ｮ�園縺ｫ繧ｪ繝悶ず繧ｧ繧ｯ繝医′縺ゅｋ縺具ｼ�*/
boolean phy_ishitobj(int x, int y )
{
  return ( world.getBody(x, y) != null );
}

/* clear all bodies in world */
void phy_clearAllObj()
{
  world.clear();
  world.setEdges(0,0,WND_WIDTH,WND_HEIGHT);
  world.remove(world.top);
}