/*****************************************************************************
 *
 *  Copyright (C) 2003 Cédric Brégardis <cedric.bregardis@free.fr>
 *
 *  This file is part of BRIQUOLO
 *
 *  BRIQUOLO is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  BRIQUOLO is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with BRIQUOLO; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 *****************************************************************************/
//#include <iostream>
#include <MOGL_Temps.h>
#include "ElementCollision.h"

#define DIST_COL 0.001
#define MAX_LOOP 50
#define DIST_MAX 3000
#define TEMPS_FIN_COLLISON 0.05

ElementCollision * ElementCollision::_DernierElementCollision=NULL;
Balle * ElementCollision::_DerniereBalle=NULL;
float ElementCollision::_DernierTempsCollision = 0;


ElementCollision::ElementCollision(): ElementGraphique()
{
}

ElementCollision::ElementCollision(const ElementCollision & p_ElementCollision)
  : ElementGraphique(p_ElementCollision)
{
}

ElementCollision::~ElementCollision()
{
}

void ElementCollision::SetDerniereCollision(ElementCollision * p_Element, Balle * p_Balle)
{
  _DernierElementCollision=p_Element;
  _DerniereBalle=p_Balle;
  _DernierTempsCollision = MOGL_Temps::GetTemps();
}

void ElementCollision::Deplacer(double p_Temps)
{
}

bool ElementCollision::Toucher(Balle * p_Balle, double p_Temps)
{
  return false;
}

Generateur * ElementCollision::CreerGenerateurParticules()
{
  return NULL;
}

Struct_Collision ElementCollision::_CircleCollisionWithRotSearch(float c1ix, float c1iy, float r1, float v1x, float v1y,
                                                                 float brick_center_x, float brick_center_y, float brick_rot_i,
                                                                 float teta0, float ra, float r2, float v2x, float v2y, float v2r,
                                                                 float tmin, float dtmin,
                                                                 float tmax, float dtmax)
{
  Struct_Collision col;
  _Niveau = _Niveau + 1;

  if (_Niveau < MAX_LOOP)
  {
    float t = (tmin + tmax) / 2;

    float teta = brick_rot_i + teta0 + t * v2r;

    float axt = ra * cos(teta) + brick_center_x;
    float ayt = ra * sin(teta) + brick_center_y;

    float bxt = c1ix + (v1x - v2x)*t;
    float byt = c1iy + (v1y - v2y)*t;

    // On calule la distance
    float dist = sqrt((bxt-axt)*(bxt-axt) + (byt-ayt)*(byt-ayt));

    if (fabs(dist - (r1+r2)) < DIST_COL)
    {
      // On a la collision
      col.TempsCollision = t;
      col.PositionX = c1ix + v1x * t;
      col.PositionY = c1iy + v1y * t;
      col.VitesseX = -v1x;
      col.VitesseY = -v1y;

      return col;
    }
    else
    {
      // On a pas encore la collision
      if ((dist > dtmin) && (dist < (r1+r2)))
      {
        tmin = t;
        dtmin = dist;
      }
      else
      {
        tmax = t;
        dtmax = dist;
      }
      return _CircleCollisionWithRotSearch(c1ix, c1iy, r1, v1x, v1y, brick_center_x, brick_center_y, brick_rot_i,
                                           teta0, ra, r2, v2x, v2y, v2r, tmin, dtmin, tmax, dtmax);

    }
  }
  else
  {
    col.TempsCollision = -1;
    return col;
  }
}

Struct_Collision ElementCollision::_CircleCollisionWithRot(float c1ix, float c1iy, float r1, float v1x, float v1y,
                                                           float brick_center_x, float brick_center_y, float brick_rot_i,
                                                           float c2ix, float c2iy, float r2, float v2x, float v2y, float v2r,
                                                           float tmax)
{
  float teta0 = atan2(c2iy, c2ix);
  float ra = sqrt(c2ix*c2ix + c2iy*c2iy);

  // On regarde tmin
  float teta = brick_rot_i + teta0;

  float axt = ra * cos(teta) + brick_center_x;
  float ayt = ra * sin(teta) + brick_center_y;
    
  float bxt = c1ix;
  float byt = c1iy;

  float dist_min = sqrt((bxt-axt)*(bxt-axt) + (byt-ayt)*(byt-ayt));
  
  // On regarde tmax
  teta = brick_rot_i + teta0 + tmax * v2r;

  axt = ra * cos(teta) + brick_center_x;
  ayt = ra * sin(teta) + brick_center_y;
    
  bxt = c1ix + (v1x - v2x)*tmax;
  byt = c1iy + (v1y - v2y)*tmax;

  float dist_max = sqrt((bxt-axt)*(bxt-axt) + (byt-ayt)*(byt-ayt));

  Struct_Collision col;
  col.TempsCollision = -1;

  if (dist_min < (r1+r2) && dist_max > (r1+r2))
  {
    col =_CircleCollisionWithRotSearch(c1ix, c1iy, r1, v1x, v1y, brick_center_x, brick_center_y, brick_rot_i,
                                       teta0, ra, r2, v2x, v2y, v2r, 0, dist_min, tmax, dist_max);
  }
  else if (dist_min > (r1+r2) && dist_max < (r1+r2))
  {
    col =_CircleCollisionWithRotSearch(c1ix, c1iy, r1, v1x, v1y, brick_center_x, brick_center_y, brick_rot_i,
                                       teta0, ra, r2, v2x, v2y, v2r, tmax, dist_max, 0, dist_min);
  }

  if (col.TempsCollision == -1)
  {
    col.TempsCollision = tmax;
  }
  else
  {
    float new_vx, new_vy;

    float x = c1ix + col.TempsCollision*v1x;
    float y = c1iy + col.TempsCollision*v1y;

    teta = brick_rot_i + teta0 + col.TempsCollision * v2r;
    float bx = ra * cos(teta) + brick_center_x;
    float by = ra * sin(teta) + brick_center_y;

    //_Rebond(x-c2ix, y-c2iy, v1x, v1y, new_vx, new_vy);
    _Rebond(x-bx, y-by, v1x, v1y, new_vx, new_vy);
    
    col.PositionX = x;
    col.PositionY = y;
    col.VitesseX = new_vx;
    col.VitesseY = new_vy;
  }

  return col;

}

Struct_Collision ElementCollision::_CircleCollision(float c1ix, float c1iy, float r1, float v1x, float v1y,
                                                    float brick_center_x, float brick_center_y, float brick_rot_i,
                                                    float c2ix, float c2iy, float r2, float v2x, float v2y, float v2r,
                                                    float tmax)
{
  if (v2r == 0 && brick_rot_i == 0)
  {
    return _CircleCollisionWithoutRot(c1ix, c1iy, r1, v1x, v1y, brick_center_x, brick_center_y, c2ix, c2iy, r2, v2x, v2y, tmax);
  }
  else
  {
    return _CircleCollisionWithRot(c1ix, c1iy, r1, v1x, v1y, brick_center_x, brick_center_y, brick_rot_i, c2ix, c2iy, r2, v2x, v2y, v2r, tmax);
  }
}

Struct_Collision ElementCollision::_CircleCollisionWithoutRot(float c1ix, float c1iy, float r1, float v1x, float v1y,
                                                              float brick_center_x, float brick_center_y,
                                                              float c2ix, float c2iy, float r2, float v2x, float v2y, float tmax)
{
  Struct_Collision col;

  c2ix += brick_center_x;
  c2iy += brick_center_y;

  float t, t2;
  float R = r1+r2;
  float Vx = v1x - v2x;
  float Vy = v1y - v2y;
  float a = ((R*R - c2ix*c2ix + 2*c1ix*c2ix - c1ix*c1ix)*Vy*Vy + ((2*c2ix-2*c1ix)*c2iy - 2*c1iy*c2ix + 2*c1ix*c1iy)*Vx*Vy + 
             (R*R - c2iy*c2iy + 2*c1iy*c2iy - c1iy*c1iy)*Vx*Vx);
  if (a >= 0)
  {
    t = ((sqrt(a)
          + (c2iy-c1iy)*Vy + (c2ix-c1ix)*Vx)/
         (Vy*Vy + Vx*Vx));

    t2 = -((sqrt(a)
            + (c1iy-c2iy)*Vy + (c1ix-c2ix)*Vx)/
           (Vy*Vy + Vx*Vx));

    if (t<0 || t2>0 && t2<t)
    {
      t=t2;
    }


    if (t < tmax && t > 0)
    {
      float new_vx, new_vy;
      float x = c1ix + t*v1x;
      float y = c1iy + t*v1y;
      _Rebond(x-c2ix, y-c2iy, v1x, v1y, new_vx, new_vy);
    
      col.TempsCollision = t;
      col.PositionX = x;
      col.PositionY = y;
      col.VitesseX = new_vx;
      col.VitesseY = new_vy;

      return col;
    }
    else
    {
      col.TempsCollision = tmax;
      return col;
    }
  }
  else
  {
    col.TempsCollision = tmax;
    return col;
  }
}

Struct_Collision ElementCollision::_SegmentCollision(float c1ix, float c1iy, float r1, float v1x, float v1y,
                                                     float brick_center_x, float brick_center_y, float brick_rot_i,
                                                     float teta0, float ra, float gama0, float rb,
                                                     float vtx, float vty, float vr,
                                                     float tmin, float dtmin, float detmin,
                                                     float tmax, float dtmax, float detmax)
{
  Struct_Collision col;
  _Niveau = _Niveau + 1;

  if (_Niveau < MAX_LOOP)
  {
    float t = (tmin + tmax) / 2;

    float teta = brick_rot_i + teta0 + t * vr;

    float axt = ra * cos(teta) + brick_center_x;
    float ayt = ra * sin(teta) + brick_center_y;

    float gama = brick_rot_i + gama0 + t * vr;

    float bxt = rb * cos(gama) + brick_center_x;
    float byt = rb * sin(gama) + brick_center_y;

    float a = byt - ayt;
    float b = axt - bxt;
    float c = ayt*bxt - axt*byt;

    float xi = c1ix + v1x*t - vtx*t;
    float yi = c1iy + v1y*t - vty*t;

    // On calcul le vecteur normal à la droite
    float n = sqrt(a*a + b*b);
    float nx = a/n;
    float ny = b/n;

    float xit, yit;

    // On effectue la translation correspondant au rayon
    if (detmin > 0)
    {
      xit = xi - nx * r1;
      yit = yi - ny * r1;
    }
    else
    {
      xit = xi + nx * r1;
      yit = yi + ny * r1;
    }
    float dist2 = fabs((a*xit+b*yit+c)*(a*xit+b*yit+c) / (a*a + b*b)) ;

    if (dist2 < DIST_COL)
    {
      float k1 = 0.5;
      if (fabs(bxt - axt) > 0.0001)
      {
        k1 = (xit - axt) / (bxt - axt);
      }
      float k2 = 0.5;
      if (fabs(byt - ayt) > 0.0001)
      {
        k2 = (yit - ayt) / (byt - ayt);
      }

      if (k1>=0 && k1<=1 && k2>=0 && k2<=1)
      {
        float new_vx, new_vy;
        _Rebond(a, b, v1x, v1y, new_vx, new_vy);

        col.TempsCollision = t;
        col.PositionX = xi;
        col.PositionY = yi;
        col.VitesseX = new_vx;
        col.VitesseY = new_vy;
        return col;
      }
      else
      {
        col.TempsCollision = -1;
        return col;
      }
    }
    else
    {
      float detc = _Determinant(xit - axt, yit - ayt, bxt - axt, byt - ayt);
      if (detc*detmin > 0)
      {
        return _SegmentCollision(c1ix, c1iy, r1, v1x, v1y,
                                 brick_center_x, brick_center_y, brick_rot_i,
                                 teta0, ra, gama0, rb,
                                 vtx, vty, vr,
                                 t, dist2, detc,
                                 tmax, dtmax, detmax);
      }
      else
      {
        return _SegmentCollision(c1ix, c1iy, r1, v1x, v1y,
                                 brick_center_x, brick_center_y, brick_rot_i,
                                 teta0, ra, gama0, rb,
                                 vtx, vty, vr,
                                 tmin, dtmin, detmin,
                                 t, dist2, detc);
      }
    }
  }
  else
  {
    col.TempsCollision = -1;
    return col;
  }
}


Struct_Collision ElementCollision::_SegmentCollision(float c1ix, float c1iy, float r1, float v1x, float v1y,
                                                     float brick_center_x, float brick_center_y, float brick_rot_i,
                                                     float ax, float ay, float bx, float by, float vtx, float vty, float vr,
                                                     float tmin, float tmax)
{
  Struct_Collision col;
  col.TempsCollision = tmax;


  _Niveau = 0;
  float teta0 = atan2(ay, ax);
  float ra = sqrt(ax*ax + ay*ay);
  float gama0 = atan2(by, bx);
  float rb = sqrt(bx*bx + by*by);



  // ai
  float teta = brick_rot_i + teta0 + tmin * vr;
  float axi = ra * cos(teta) + brick_center_x;
  float ayi = ra * sin(teta) + brick_center_y;

  // bi
  float gama = brick_rot_i + gama0 + tmin * vr;
  float bxi = rb * cos(gama) + brick_center_x;
  float byi = rb * sin(gama) + brick_center_y;

  // af
  teta = brick_rot_i + teta0 + tmax * vr;
  float axf = ra * cos(teta) + brick_center_x;
  float ayf = ra * sin(teta) + brick_center_y;

  // bf
  gama = brick_rot_i + gama0 + tmax * vr;
  float bxf = rb * cos(gama) + brick_center_x;
  float byf = rb * sin(gama) + brick_center_y;

  // **** Premier coté
  // ** Debut
  float a = byi - ayi;
  float b = axi - bxi;
  float c = ayi*bxi - axi*byi;

  // On calcul le vecteur normal à la droite
  float n = sqrt(a*a + b*b);
  float nx = a/n;
  float ny = b/n;

  // ci
  float cxi = c1ix + tmin*(v1x - vtx) - nx * r1;
  float cyi = c1iy + tmin*(v1y - vty) - ny * r1;

  // ** Fin
  a = byf - ayf;
  b = axf - bxf;
  c = ayf*bxf - axf*byf;

  // On calcul le vecteur normal à la droite
  n = sqrt(a*a + b*b);
  nx = a/n;
  ny = b/n;


  // cf
  float cxf = c1ix + tmax*(v1x - vtx) - nx * r1;
  float cyf = c1iy + tmax*(v1y - vty) - ny * r1;

  float deti = _Determinant(cxi - axi, cyi - ayi, bxi - axi, byi - ayi);
  float detf = _Determinant(cxf - axf, cyf - ayf, bxf - axf, byf - ayf);

  if (deti*detf < 0)
  {
    col = _SegmentCollision(c1ix, c1iy, r1, v1x, v1y,
                            brick_center_x, brick_center_y, brick_rot_i,
                            teta0, ra, gama0, rb,
                            vtx, vty, vr,
                            tmin, DIST_MAX, deti,
                            tmax, DIST_MAX, detf);
    if (col.TempsCollision == -1)
    {
      col.TempsCollision = tmax;
    }
    return col;
    
  }

  // **** Deuxième coté
  // ** Debut
  a = byi - ayi;
  b = axi - bxi;
  c = ayi*bxi - axi*byi;

  // On calcul le vecteur normal à la droite
  n = sqrt(a*a + b*b);
  nx = a/n;
  ny = b/n;

  // ci
  cxi = c1ix + tmin*(v1x - vtx) + nx * r1;
  cyi = c1iy + tmin*(v1y - vty) + ny * r1;

  // ** Fin
  a = byf - ayf;
  b = axf - bxf;
  c = ayf*bxf - axf*byf;

  // On calcul le vecteur normal à la droite
  n = sqrt(a*a + b*b);
  nx = a/n;
  ny = b/n;


  // cf
  cxf = c1ix + tmax*(v1x - vtx) + nx * r1;
  cyf = c1iy + tmax*(v1y - vty) + ny * r1;

  deti = _Determinant(cxi - axi, cyi - ayi, bxi - axi, byi - ayi);
  detf = _Determinant(cxf - axf, cyf - ayf, bxf - axf, byf - ayf);

  if (deti*detf < 0)
  {
    Struct_Collision col2 = _SegmentCollision(c1ix, c1iy, r1, v1x, v1y,
                                              brick_center_x, brick_center_y, brick_rot_i,
                                              teta0, ra, gama0, rb,
                                              vtx, vty, vr,
                                              tmin, DIST_MAX, deti,
                                              tmax, DIST_MAX, detf);
    if (col2.TempsCollision == -1)
    {
      col2.TempsCollision = tmax;
    }

    if (col2.TempsCollision < col.TempsCollision)
    {
      col = col2;
    }
  }
  return col;
}

float ElementCollision::_Determinant(float d1x, float d1y, float d2x, float d2y)
{
  return d1x*d2y - d1y*d2x;
}

void ElementCollision::_Rebond(float nx, float ny, float vix, float viy, float & vfx, float & vfy)
{
  float vit=sqrt(vix*vix + viy*viy);
  float beta=atan2(viy, vix);
  float gama=atan2(ny , nx);

  vfx=-vit*cos(2*gama-beta);
  vfy=-vit*sin(2*gama-beta);

}

string ElementCollision::GetLigneOption() const
{
  return _LigneOption;
}

void ElementCollision::SetLigneOption(const string & p_Ligne)
{
  _LigneOption = p_Ligne;
  _AnalyserLigne();
}

void ElementCollision::_AnalyserLigne()
{
  unsigned int posi, posf = 0;
  _Params.clear();
  while (posf < _LigneOption.size())
  {
    posi = posf;
    while(posf < _LigneOption.size() && _LigneOption[posf] != '=' && 
          _LigneOption[posf] != ' ' && _LigneOption[posf] != '\t' && _LigneOption[posf] != '\n')
    {
      posf++;
    }
  
    if ((posf < _LigneOption.size()) && (_LigneOption[posf] == '=') && (posf > posi))
    {
      string param = _LigneOption.substr(posi, posf - posi);
      posi = ++posf;
      while(posf < _LigneOption.size() && _LigneOption[posf] != ' ' && 
            _LigneOption[posf] != '\t' && _LigneOption[posf] != '\n')
      {
        posf++;
      }
      if (posf > posi)
      {
        string value = _LigneOption.substr(posi, posf - posi);
        _Params[param] = value;
      }
    }
    posf++;
  }
}

Point ElementCollision::GetVisualCenter()
{
  return Point(_X, _Y);
}

float ElementCollision::_Modulo(float a, float b)
{
  float f = fmodf(a, b);
  if (f < 0)
  {
    f = 360 + f;
  }
  return f;
}

bool ElementCollision::IsDerniereCollisionOk(Balle * p_Balle)
{
/*
  if (p_Balle == _DerniereBalle && _DernierElementCollision == this)
  {
    if (_DernierTempsCollision + TEMPS_FIN_COLLISON < MOGL_Temps::GetTemps())
    {
      _DernierTempsCollision = NULL;
      return true;
    }
    else
    {
      return false;
    }
  }
  return true;
*/
  if (p_Balle == _DerniereBalle && _DernierElementCollision == this)
  {
    return false;
  }
  else
  {
    return true;
  }
}