#include "blob.h"
#include <GL/gl.h>

Blob::Blob()
{
}

Blob::Blob(const float xPos,
           const float yPos,
           const float aRadius)
: middle(xPos, yPos, 1.0f)
, radius(aRadius)
{
  // point masses
  for(int i = 0; i < NUM_POINTMASSES; i++)
  {
    float t = (float) i / (float) NUM_POINTMASSES * M_PI * 2.0f;
    LP_Pointmass2 tPointmass(xPos + cos(t) * radius, yPos + sin(t) * radius, 1.0f);
    pointmasses.Add(tPointmass);
    pointmassesRandomAccess.Add(&(pointmasses.GetLast()));
  }

  for(int i = 0; i < pointmassesRandomAccess.Count(); i++)
  {
    int i1 = random() % pointmassesRandomAccess.Count();
    int i2 = random() % pointmassesRandomAccess.Count();
    LP_Pointmass2* t = pointmassesRandomAccess[i1];
    pointmassesRandomAccess[i1] = pointmassesRandomAccess[i2];
    pointmassesRandomAccess[i2] = t;
  }

  // sticks
  for(int i = 1; i < NUM_POINTMASSES; i++)
  {
    sticks.Add(LP_Stick2(pointmasses[i - 1], pointmasses[i]));
    sticksRandomAccess.Add(&(sticks.GetLast()));
  }
  sticks.Add(LP_Stick2(pointmasses[0], pointmasses[NUM_POINTMASSES - 1]));
  sticksRandomAccess.Add(&(sticks.GetLast()));

  for(int i = 0; i < sticksRandomAccess.Count(); i++)
  {
    int i1 = random() % sticksRandomAccess.Count();
    int i2 = random() % sticksRandomAccess.Count();
    LP_Stick2* t = sticksRandomAccess[i1];
    sticksRandomAccess[i1] = sticksRandomAccess[i2];
    sticksRandomAccess[i2] = t;
  }

  // joints
  float shortConst = 1.07;
  float longConst = 1.45f;

  for(int i = 0; i < NUM_POINTMASSES; i++)
  {
    int t;

    t = PrivClampIndex(i - 5, NUM_POINTMASSES);
    joints.Add(LP_Joint2(pointmasses[i], pointmasses[t], shortConst, longConst));
    jointsRandomAccess.Add(&(joints.GetLast()));
    t = PrivClampIndex(i + 5, NUM_POINTMASSES);
    joints.Add(LP_Joint2(pointmasses[i], pointmasses[t], shortConst, longConst));
    jointsRandomAccess.Add(&(joints.GetLast()));

    t = PrivClampIndex(i + NUM_POINTMASSES / 2 - 6, NUM_POINTMASSES);
    joints.Add(LP_Joint2(pointmasses[i], pointmasses[t], shortConst, longConst));
    jointsRandomAccess.Add(&(joints.GetLast()));
    t = PrivClampIndex(i + NUM_POINTMASSES / 2 + 6, NUM_POINTMASSES);
    joints.Add(LP_Joint2(pointmasses[i], pointmasses[t], shortConst, longConst));
    jointsRandomAccess.Add(&(joints.GetLast()));

    joints.Add(LP_Joint2(pointmasses[i], middle, shortConst, longConst));
    jointsRandomAccess.Add(&(joints.GetLast()));
  }

  for(int i = 0; i < jointsRandomAccess.Count(); i++)
  {
    int i1 = random() % jointsRandomAccess.Count();
    int i2 = random() % jointsRandomAccess.Count();
    LP_Joint2* t = jointsRandomAccess[i1];
    jointsRandomAccess[i1] = jointsRandomAccess[i2];
    jointsRandomAccess[i2] = t;
  }

  mouthStyle = MOUTH_CLOSED;
  eyeStyle = EYES_OPEN;
}

int
Blob::PrivClampIndex(int index, const int highBound) const
{
  while(index < 0)
    index = highBound + index;
  while(index >= highBound)
    index = index - highBound;

  return index;
}

void
Blob::PrivComputeMiddle(LP_Vector2& aMiddle)
{
  aMiddle.Set(0.0f, 0.0f);

  for(int i = 0; i < pointmasses.Count(); i++)
    aMiddle += pointmasses[i].currPos;
  aMiddle.Scale(1.0f / (float) NUM_POINTMASSES);
}

void
Blob::Move(const float dt)
{
  PrivComputeMiddle(middle.currPos);
  for(int i = 0; i < pointmasses.Count(); i++)
    pointmasses[i].Move(dt);
  middle.Move(dt);
}

float
Blob::PrivComputeVolume()
{
  float volume = 0.0f, t;

  for(int i = 0; i < sticks.Count(); i++)
  {
    LP_Stick2& stick = sticks[i];

    t  = stick.a->currPos.x * (stick.b->currPos.y - middle.currPos.y);
    t += stick.b->currPos.x * (middle.currPos.y - stick.a->currPos.y);
    t += middle.currPos.x * (stick.a->currPos.y - stick.b->currPos.y);

    volume += fabs(t);
  }

  return volume * 0.5f;
}

void
Blob::PrivPumpBlob()
{
  float targetVolume, actualVolume, scaleFactor;
  LP_Vector2 tMiddle;

  targetVolume = radius * radius * M_PI * 1.5f;
  actualVolume = PrivComputeVolume();

  if(actualVolume < targetVolume * 0.0001f)
    actualVolume = targetVolume * 0.0001f;

  scaleFactor = targetVolume / actualVolume;
  scaleFactor -= 1.0f;
  scaleFactor *= 0.1f;

  PrivComputeMiddle(tMiddle);

  for(int i = 0; i < pointmasses.Count(); i++)
  {
    LP_Vector2 t(pointmasses[i].currPos);
    t -= tMiddle;
    t.Scale(scaleFactor);
    pointmasses[i].currPos += t;
  }
}

void
Blob::Satisfy(Env& env)
{
  for(int i = 0; i < 3; i++)
  {
    PrivPumpBlob();

    for(int j = 0; j < jointsRandomAccess.Count(); j++)
      jointsRandomAccess[j]->Satisfy();

    for(int j = 0; j < sticksRandomAccess.Count(); j++)
      sticksRandomAccess[j]->Satisfy();

    for(int j = 0; j < pointmassesRandomAccess.Count(); j++)
    {
      LP_Pointmass2* p = pointmassesRandomAccess[j];
      if(env.Collide(p->currPos))
        p->SetFriction(0.99f);
      else
        p->SetFriction(0.01f);
    }
  }
}

bool
Blob::PrivSimpleDistanceTest(Blob& a, Blob& b)
{
//  float dist = LP_Vector2::Dist(a.middle.currPos, b.middle.currPos);
//  if(dist > 1.5f * (a.GetRadius() + b.GetRadius()))
//    return false;
  float dist = LP_Vector2::DistSquared(a.middle.currPos, b.middle.currPos);
  float k = 1.5f * (a.GetRadius() + b.GetRadius());
  if(dist > k * k)
    return false;

  return true;
}

void
Blob::PrivFindSeparationPlane(Blob& a, Blob& b, LP_Vector2& aMiddle, LP_Vector2& resultA, LP_Vector2& resultB)
{
  float tx, ty, px, py;
  float k, kA, kB;
  LP_Vector2 bMiddle;

  /* M_PI constant removed, kA and kB are only valid for k computation */
  /* FIXME, get rid of sqrt by using one step taylor or something like that */
  kA = sqrt(a.PrivComputeVolume());
  kB = sqrt(b.PrivComputeVolume());
  k = kA / (kA + kB);

  b.PrivComputeMiddle(bMiddle);

  tx = bMiddle.x - aMiddle.x;
  ty = bMiddle.y - aMiddle.y;

  px = - ty;
  py = tx;

  /* reuse tx, ty */
  tx = tx * k + aMiddle.x;
  ty = ty * k + aMiddle.y;

  /* reuse px, py */
  px = px * 0.5f;
  py = py * 0.5f;

  resultA.x = tx + px;
  resultA.y = ty + py;

  resultB.x = tx - px;
  resultB.y = ty - py;
}

bool
Blob::PrivLineIntersect(LP_Vector2& lineA, LP_Vector2& lineB, LP_Vector2& segA, LP_Vector2& segB, LP_Vector2& result)
{
  float s, k;

  /* the lines are paralle if k is zero */
  k = ((lineB.x-lineA.x) * (segB.y-segA.y) - (lineB.y-lineA.y) * (segB.x-segA.x));
  if(k == 0.0f)
    return false;

  s = ((lineA.y-segA.y) * (lineB.x-lineA.x) - (lineA.x-segA.x) * (lineB.y-lineA.y)) / k;
  if(s < 0.0f || s > 1.0f)
    return false;

  result = segB;
  result -= segA;
  result.Scale(s);
  result += segA;

  return true;
}

void
Blob::CollisionTest(Blob& another)
{
  if(!Blob::PrivSimpleDistanceTest(*this, another))
    return;

  LP_Vector2 tMiddle;
  PrivComputeMiddle(tMiddle);

  LP_Vector2 spA, spB;
  Blob::PrivFindSeparationPlane(*this, another, tMiddle, spA, spB);

  for(int i = 0; i < pointmasses.Count(); i++)
  {
    LP_Vector2 r;
    LP_Pointmass2& p = pointmasses[i];
    if(Blob::PrivLineIntersect(spA, spB, tMiddle, p.currPos, r))
    {
      p.currPos = r;
      p.SetFriction(0.5f);
    }
  }
}

void
Blob::SetForce(LP_Vector2& aForce)
{
  for(int i = 0; i < pointmasses.Count(); i++)
    pointmasses[i].SetForce(aForce);
  middle.SetForce(aForce);
}

void
Blob::AddForce(LP_Vector2& aForce)
{
  for(int i = 0; i < pointmasses.Count(); i++)
    pointmasses[i].AddForce(aForce);
  middle.AddForce(aForce);
}

void
Blob::PrivDrawArc(float aRadius, float startDeg, float numDegs, bool filled)
{
  if(filled)
    glBegin(GL_POLYGON);
  else
    glBegin(GL_LINE_STRIP);

  float degInc = aRadius; // / 10.0f;
  float x, y;
  for(float d = startDeg; d < startDeg + numDegs; d += degInc)
  {
    x = cos(d) * aRadius;
    y = sin(d) * aRadius;
    glVertex2f(x, y);
  }

  glEnd();
}

void
Blob::PrivDrawMouth()
{
  glLineWidth(2.0f);

  glPushMatrix();
  glTranslatef(0.0f, -0.5f * radius, 0.0f);
  if(mouthStyle == MOUTH_CLOSED)
    PrivDrawArc(radius * 0.2f, 0.0f, M_PI, false);
  if(mouthStyle == MOUTH_OPEN)
    PrivDrawArc(radius * 0.2f, 0.0f, M_PI, true);
  glPopMatrix();
}

void
Blob::PrivDrawEyes()
{
  glLineWidth(1.0f);

  glPushMatrix();
  glTranslatef(-0.15f * radius, -0.7f * radius, 0.0f);
  PrivDrawArc(radius * 0.05f, 0.0f, 2.0f * M_PI, false);
  glPopMatrix();

  glPushMatrix();
  glTranslatef(0.15f * radius, -0.7f * radius, 0.0f);
  PrivDrawArc(radius * 0.05f, 0.0f, 2.0f * M_PI, false);
  glPopMatrix();
}

void
Blob::PrivDrawFace()
{
  LP_Vector2 orientation(pointmasses[0].currPos);
  orientation += pointmasses[1].currPos;
  orientation -= pointmasses[2].currPos;
  orientation -= pointmasses[3].currPos;

  glLoadIdentity();
  glBegin(GL_LINE_STRIP);
  glVertex2f(middle.currPos.x, middle.currPos.y);
  glVertex2f(middle.currPos.x + orientation.x, middle.currPos.y + orientation.y);
  glEnd();
  glLoadIdentity();
  glTranslatef(middle.currPos.x + orientation.x, middle.currPos.y + orientation.y, 0.0f);
  PrivDrawArc(radius * 0.02f, 0.0f, 2.0f * M_PI, true);

  LP_Vector2 up(0.0f, 1.0f);
  orientation.Normalize(1.0f);
  float angle = acos(LP_Vector2::DotProd(orientation, up));
  if(orientation.x > 0.0f)
    angle = -angle;

  glLoadIdentity();
  glTranslatef(middle.currPos.x, middle.currPos.y, 0.0f);
  float degs = 360.0f * angle / (2.0f * M_PI);
  glRotatef(degs, 0.0f, 0.0f, 1.0f);

  PrivDrawMouth();
  PrivDrawEyes();
}

void
Blob::PrivDrawBody()
{
  glLoadIdentity();
  glColor3f(1.0f, 1.0f, 1.0f);
  glBegin(GL_POLYGON);
  for(int i = 0; i < pointmasses.Count(); i++)
  {
    LP_Pointmass2& p = pointmasses[i];
    glVertex2f(p.currPos.x, p.currPos.y);
  }
  glEnd();

  glLoadIdentity();
  glLineWidth(2.0f);
  glColor3f(0.0f, 0.0f, 0.0f);
  glBegin(GL_LINE_LOOP);
  for(int i = 0; i < pointmasses.Count(); i++)
  {
    LP_Pointmass2& p = pointmasses[i];
    glVertex2f(p.currPos.x, p.currPos.y);
  }
  glEnd();
}

void
Blob::Draw()
{
  PrivDrawBody();
  //PrivDrawFace();
}












