#include "UGM/UGM.hpp"

using namespace Ubpa;
using namespace std;

int main() {
    pointf3 q0{ 254.793335, -6.852415, 43.801746 }, q1{ 156.655655, -70.688034, 5.278168 };
    pointf3 q2 = q0;
    q2.z += 1;
    vecf3 pn{ 0,0,1 }, qn{};
    vecf3 pv{ 1,1,0 }, qv = q1 - q0;
    qn = qn.cross(qv, q2 - q0);

    scalef3 s{1, 1, 1};
    quat rot{ pv.normalize(), qv.normalize() };
    transformf mt1{rot};
    pn = mt1 * pn;
    auto a = s * 2;
    float angle = acosf(pn.cos_theta(qn));
    transformf mt2{qv, angle};
    
   // transformf mt3{ pv, qv };
    pointf3 p0{ 1,0,0 }, p1{ 0,1 ,0};
    p0 = mt2 * mt1 * p0;
    p1 = mt2 * mt1 * p1;
    //mt.rotate_with<a_up>(10);
    char buf[1024];
    sprintf(buf, "pointf %d vecf3 %d quatf %d transformf %d \n", sizeof(q0), sizeof(pn), sizeof(rot), sizeof(mt1));
    std::cout << "hello world  " << buf << endl;
    sprintf(buf, "p0(%f %f %f) p0(%f %f %f) \n", p0.x, p0.y , p0.z, p1.x, p1.y, p1.z);
    std::cout << buf << endl;
    return 0;
}