#include <iostream>
#include <iomanip>

using namespace std;

#if _MSC_VER >= 1400
#define sprintf sprintf_s
#endif

#include "util/util.h"
#include "param/vector3.h"
#include "param/complex.h"
#include "param/matrix.h"

typedef double accuracy;
typedef Matrix<accuracy> matrix;
typedef Vector3<accuracy> vector3;

#define pow2(x) ((x)*(x))

void calc(const matrix &A){
  cout << setprecision(10);
  
  cout << "A:" << A << endl;
  
  vector3 u_01, u_02, u_03;
  {
    u_01[0] = 1; u_01[1] = 0; u_01[2] = 0;
    u_02[0] = 0; u_02[1] = 1; u_02[2] = 0;
    u_03[0] = 0; u_03[1] = 0; u_03[2] = 1;
  }
  
  cout << "u_01:" << u_01 << endl;
  cout << "u_02:" << u_02 << endl;
  cout << "u_03:" << u_03 << endl;
  
  /*
   *       + u_01^{T} + 
   * U_0 = | u_02^{T} | (misc.pdf)
   *       + u_03^{T} +
   */
  matrix U_0(3, 3);
  for(int i = 0; i < U_0.columns(); i++){
    U_0(0, i) = u_01[i] / u_01.abs();
    U_0(1, i) = u_02[i] / u_02.abs();
    U_0(2, i) = u_03[i] / u_03.abs();
  }
  
  cout << "U_0:" << U_0 << endl;
  
  matrix U(U_0.inverse() * A);
  cout << "U = U_0^{-1} * A:" << U << endl;
  
  accuracy K_1(sqrt(pow2(U(0, 0)) + pow2(U(1, 0)) + pow2(U(2, 0)))),
           K_2(sqrt(pow2(U(0, 1)) + pow2(U(1, 1)) + pow2(U(2, 1)))),
           K_3(sqrt(pow2(U(0, 2)) + pow2(U(1, 2)) + pow2(U(2, 2))));
           
  cout << "K_1:" << K_1 << endl;
  cout << "K_2:" << K_2 << endl;
  cout << "K_3:" << K_3 << endl;
  
  matrix U_eigen(U.copy());
  for(int i = 0; i < U_eigen.columns(); i++){
    U_eigen(i, 0) /= K_1;
    U_eigen(i, 1) /= K_2;
    U_eigen(i, 2) /= K_3;
  }
  
  cout << "U_eigen:" << U_eigen << endl;
  for(int i = 0; i < U_eigen.rows(); i++){
    cout << "theta" << (i + 1) <<  ":" << rad2deg(acos(U_eigen(i, i))) << endl;
  }
  
  /*
   * misc.pdf "センサのアライメント補正"
   */
  cout << "{{U_eigen}^{T}}^{-1}" << U_eigen.transpose().inverse() << endl;
}

int main(){
  
  matrix A(3, 3);
  
  // 0号機
  {
    cout << endl << endl 
         << "No.0" 
         << endl;
    
    A(0, 0) = 42375.761259;
    A(0, 1) = 378.159608;
    A(0, 2) = -502.761842;
    
    A(1, 0) = -399.684889;
    A(1, 1) = 43343.199974;
    A(1, 2) = 209.526950;
    
    A(2, 0) = 64.229949;
    A(2, 1) = 537.270228;
    A(2, 2) = 42685.509933;
    
    calc(A);
  }
  
  
  // 1号機
  {
    cout << endl << endl 
         << "No.1" 
         << endl;
    
    A(0, 0) = 1121.211092;
    A(0, 1) = 452.951402;
    A(0, 2) = -66092.521602;
    
    A(1, 0) = -65875.240887;
    A(1, 1) = 14.776116;
    A(1, 2) = 1341.967777;
    
    A(2, 0) = -928.798493;
    A(2, 1) = 66103.040312;
    A(2, 2) = -955.976289;
    
    calc(A);
  }
}