#include <global.hpp>
#include <2D_vector.hpp>
#include <similar_to.hpp>
#include <newton_integ.hpp>

newton_integ::newton_integ()
  :_dt(1e-1),
   _thrs_pos(1e-6),
   _m(1)
{

}
newton_integ::newton_integ(newton_integ &other)
  :_dt(other._dt),
   _thrs_pos(other._thrs_pos),
   _m(other._m),
   _accel(other._accel),
   _velo(other._velo),
   _pos(other._pos)

{

}

newton_integ::~newton_integ(){

}


void newton_integ::pos(std::vector<float> nw){
  _pos=nw;
  _accel.empty();
  _accel.resize(_pos.size(),0.0f);
  _velo.empty();
  _velo.resize(_pos.size(),0.0f);

}

std::vector<float> newton_integ::pos(){
  return _pos;
}

void  newton_integ::dt(float nw){
  _dt=nw;
}
float newton_integ::dt(){
  return _dt;
}


void  newton_integ::m(float nw){
  _m=nw;
}

float newton_integ::m(){
  return _m;
}


void  newton_integ::thrs_pos(float nw){
  _thrs_pos=nw;
}

float newton_integ::thrs_pos(){
  return _thrs_pos;
}

bool newton_integ::update(){
  _savedpos=_pos;

  std::vector<float> f=force();
  
  for(unsigned int i=0; i < f.size(); i++){
    _accel[i]=f[i]/m();
  }
  
  for(unsigned int i=0; i < f.size(); i++){
    _velo[i]=_accel[i] * _dt;
  }

  for(unsigned int i=0; i < f.size(); i++){
    _pos[i]+=_velo[i] * _dt;
    //std::cerr << _velo[i] * _dt << std::endl;
  }

  bool can_continue=false;
  
  //std::cerr << "saved " << _savedpos[0] << " " << _savedpos[1] << std::endl;

  //std::cerr << _pos[0] << " " << _pos[1] << std::endl;

  for(unsigned int i=0;i<_pos.size();i++){
    if( ! similar_to<float>(_pos[i],_savedpos[i],_thrs_pos) ) {
      can_continue=true;
    }
  }

  return can_continue;
}



bidimensional_vinculated_particle::bidimensional_vinculated_particle()
:_k_spring(1)
{
  
}

bidimensional_vinculated_particle::bidimensional_vinculated_particle(bidimensional_vinculated_particle& other)
:newton_integ::newton_integ(other),
  _particles(other._particles),
  _center(other._center),
  _k_spring(other._k_spring)
{
  
}

bidimensional_vinculated_particle::~bidimensional_vinculated_particle(){

}

void bidimensional_vinculated_particle::add_particle(std::pair<float, float> particle){
  _particles.push_back(particle);
}

void bidimensional_vinculated_particle::add_center(std::pair<float, float> center){
  _center=center;
}


void bidimensional_vinculated_particle::k_spring(float nw){
  _k_spring=nw;
}

float bidimensional_vinculated_particle::k_spring(){
  return _k_spring;  
}


std::vector<float> bidimensional_vinculated_particle::force(){
  std::vector< std::pair<float,float> > diff_f;
  std::pair<float,float> pos2(_pos[0],_pos[1]);

  for(unsigned int i=0;i<_particles.size();i++){
    std::pair<float,float> ppos=_particles[i];
    std::pair<float,float> ppos_diff=bidimensional_vector::diff(ppos,_center);
    std::pair<float,float> ppos_scal=bidimensional_vector::normalize(ppos_diff);
    ppos_scal=bidimensional_vector::translate(ppos_scal,_center.first, _center.second);

    std::pair<float,float> tmp=bidimensional_vector::diff(pos2,ppos_scal);
    diff_f.push_back(tmp);
  }

  for(unsigned int i=0;i<diff_f.size();i++){
    diff_f[i].first  *=_k_spring;
    diff_f[i].second *=_k_spring;
  }

  // for(unsigned int i=0;i<diff_f.size();i++){
  //   diff_f[i].first  += 1E-11 / diff_f[i].first;
  //   diff_f[i].second += 1E-11 / diff_f[i].second;
  // }


  std::pair<float,float> sum;

  for(unsigned int i=0;i<diff_f.size();i++){
    sum.first  += diff_f[i].first;
    sum.second += diff_f[i].second;

    

  }


  std::pair<float,float> rad=bidimensional_vector::diff(pos2, _center);
  std::pair<float,float> f_tang=bidimensional_vector::project_perp(sum,rad);


  vector<float> res;
  res.push_back(f_tang.first);
  res.push_back(f_tang.second);
  return res;
}
