#include <cmath>
#include "coord.h"

Coordinate::Coordinate(){
  // default is the Cartesian coordinate system
  O.setvtr(0.0, 0.0, 0.0);
  x_axis.setvtr(1.0, 0.0, 0.0);
  y_axis.setvtr(0.0, 1.0, 0.0);
  z_axis.setvtr(0.0, 0.0, 1.0);
}

void Coordinate::setO(const vtr &vv){
  O = vv;
}

void Coordinate::setO(const fp_t &x, const fp_t &y, const fp_t &z){
  O.setvtr(x, y, z);
}

void Coordinate::setx_axis(const fp_t &x, const fp_t &y, const fp_t &z){
  x_axis.setvtr(x, y, z);
}

void Coordinate::sety_axis(const fp_t &x, const fp_t &y, const fp_t &z){
  y_axis.setvtr(x, y, z);
}

void Coordinate::setz_axis(const fp_t &x, const fp_t &y, const fp_t &z){
  z_axis.setvtr(x, y, z);
}

void Coordinate::setx_axis(const vtr &xaxis){
  x_axis = xaxis;
  x_axis.unitvtr();
}

void Coordinate::sety_axis(const vtr &yaxis){
  y_axis = yaxis;
  y_axis.unitvtr();
}

void Coordinate::setz_axis(const vtr &zaxis){
  z_axis = zaxis;
  z_axis.unitvtr();
}

vtr Coordinate::Transform(const vtr &vv) const{
  vtr trans, dd;
  
  dd = vv - O;
  trans.setvtr(dotP(dd, x_axis), dotP(dd, y_axis), dotP(dd, z_axis));

  return trans;
}

vtr Coordinate::invTransform(const vtr &vv) const{
  vtr invtrans;

  invtrans = x_axis * vv.getx() + y_axis * vv.gety() + z_axis * vv.getz();
  invtrans = invtrans + O;

  return invtrans;
}

tensor Coordinate::Transform(const tensor &ct) const{
  tensor trans, J, JT;

  J.settensor(x_axis, y_axis, z_axis);
  JT = J.transpose();
  J = JT.inverse();
  trans = ct * J;
  trans = JT * trans;

  return trans;
}

tensor Coordinate::invTransform(const tensor &ct) const{
  tensor invtrans, J, JT, JTinv;

  J.settensor(x_axis, y_axis, z_axis);
  JT = J.transpose();
  JTinv = JT.inverse();

  invtrans = ct * JT;
  invtrans = JTinv * invtrans;

  return invtrans;
}

void Coordinate::print(){
  O.print();
  x_axis.print();
  y_axis.print();
  z_axis.print();
}

vtr Coordinate::SphereToXYZ(fp_t r, fp_t Theta, fp_t Phi){
  vtr vv;
  fp_t x, y, z;
  
  x = r * sin(Theta) * cos(Phi);
  y = r * sin(Theta) * sin(Phi);
  z = r * cos(Theta);
  vv.setvtr(x, y, z);
  vv = vv + O;

  return vv;
}