#include <cartesian/vector.h>
#include <cartesian/point.h>
#include <cartesian/units.h>
#include <cartesian/io.h>

#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/io.hpp>

#include <boost/assign.hpp>

#include <iostream>
#include <iomanip>
#include <cstddef>

using namespace std;
using namespace boost;
using namespace boost::units;
using namespace cartesian;

typedef quantity<si::length,   point3d  > Position;
typedef quantity<si::length,   vector3d > Displacement;
typedef quantity<si::velocity, vector3d > Velocity;

typedef quantity<si::time> Time;

int main() {
  static const si::length m = si::meter;
  static const si::time   s = si::second;

  Position p1 = point3d(1.0, 2.0, 3.0) * m;
  Position p2 = point3d(4.0, 5.0, 6.0) * m;
  Position p3 = point3d(3.0, 9.0, 4.0) * m;

  Displacement disp1(p2 - p1);
  cout << disp1 << endl;
  cout << abs(disp1) << endl;
  cout << abs_squared(disp1) << endl;

  p1 += disp1;
  cout << p1 << endl;

  Time dt(Time(0.1 * s));
  Velocity v((p3 - p2) / dt);
  cout << v << endl;

  cartesian::vector<4, int> v_move = assign::list_of(3)(-2)(17)(33);
  cout << "v_move=" << v_move << endl;

  struct ECEF { };
  point<4, int, ECEF> ecef_pt1 = assign::list_of(3)(2)(-10)(-13);
  point<4, int, ECEF> ecef_pt2 = ecef_pt1 + v_move;
  cout << ecef_pt1 << ' ' << ecef_pt2 << ' ' << (ecef_pt2 - ecef_pt1) << endl;

  struct Local { };
  point<4, int, Local> local_pt1 = assign::list_of(19)(-18)(17)(-16);
  point<4, double, Local> local_pt2 = local_pt1 + v_move;
  cout << local_pt1 << ' ' << local_pt2
       << ' ' << (local_pt2 - local_pt1) << endl;

  // cout << (local_pt1 - ecef_pt1) << endl; // won't compile

  // Compiles, but is senseless.
  cout << (local_pt1.from_origin() - ecef_pt1.from_origin()) << endl;

  return EXIT_SUCCESS;
} // main