#include <global.hpp>
#include <matr.hpp>
#include <2D_vector.hpp>



std::pair<float,float> bidimensional_vector::diff(std::pair<float,float> a, std::pair<float,float> b){
  std::pair<float,float> res(a.first-b.first,a.second-b.second);
  return res;
}

float bidimensional_vector::angle(std::pair<float,float> a, std::pair<float,float> b){
  std::pair<float,float> a_norm=normalize(a);
  std::pair<float,float> b_norm=normalize(b);

  float dot_p=dot_product(a_norm, b_norm);
  float angle=acos(dot_p);

  if(cross_product(a,b)<0){
    angle=-angle;
  }

  return angle;
}


float bidimensional_vector::dot_product(std::pair<float,float> a, std::pair<float,float> b){

  return (a.first * b.first) + (a.second * b.second);

}

float bidimensional_vector::cross_product(std::pair<float,float> a, std::pair<float,float> b){
  return a.first * b.second - a.second * b.first;

}


float bidimensional_vector::magn(std::pair<float,float> a){

  return sqrt(a.first * a.first + a.second * a.second);

}



std::pair<float,float> bidimensional_vector::normalize(std::pair<float,float> a){
  static const float zero_thr=0.001;
  float mag=magn(a);
  std::pair<float,float> res=a;    
  
  if(!similar_to(mag, 0.0f, zero_thr)){
    res.first/=mag;
    res.second/=mag;
  }

  return res;
  
}


std::pair<float, float> bidimensional_vector::rotate(std::pair<float, float> v, float angle){
  matrix<float> the_vec(2,1);
  matrix<float> transformation(2,2);
  std::pair<float, float> res(0,0);
  try{
    the_vec(0,0)=v.first;
    the_vec(1,0)=v.second;
    transformation(0,0)=cos(angle);
    transformation(0,1)=sin(angle);
    transformation(1,0)=-sin(angle);
    transformation(1,1)=cos(angle);

    matrix<float> mat_res=  transformation * the_vec;
    res.first=mat_res(0,0);
    res.second=mat_res(1,0);
  }catch (matrix_error ex){
    cerr << "exception in " <<  __LINE__ << " "
	 << __FILE__        <<  ex.what() << endl;
  }

  return res;

}

std::pair<float, float> bidimensional_vector::translate(std::pair<float, float> v, 
                                                        float dx, float dy){
  std::pair<float, float> res(v.first + dx , v.second + dy);
  return res;
}


std::pair<float, float> bidimensional_vector::scale(std::pair<float, float> v, 
                                                    float scale_factor){
  std::pair<float, float> res(v.first * scale_factor , v.second * scale_factor);
  return res;
}


std::pair<float, float> bidimensional_vector::project_par(std::pair<float,float> v, std::pair<float,float> n){
  n=normalize(n);
  std::pair<float,float> res=scale(n,dot_product(v,n));
  return res;
}

std::pair<float, float> bidimensional_vector::project_perp(std::pair<float,float> v, std::pair<float,float> n){
  std::pair<float,float> par=project_par(v,n);
  std::pair<float,float> res=diff(v,par);

  return res;
}


void bidimensional_vector::print(std::ostream& str, std::pair<float, float> v){
  str << v.first << " " << v.second;
}
