#include <Core/Matrices/ROTATION.h>
#include "QT_ARCBALL.h"
#include <iostream>
namespace PhysBAM{

QT_ARCBALL::QT_ARCBALL()
    :sphere_radius(1)
{
    rotation_matrix=TM::Identity_Matrix();
}

void QT_ARCBALL::Begin_Drag(const TV2 &mouse_now)
{
    mouse_down=mouse_now;
    rotation_matrix=TM::Identity_Matrix();
}

void QT_ARCBALL::End_Drag()
{
    rotation_matrix=TM::Identity_Matrix();
}

void QT_ARCBALL::Update(const TV2 &mouse_now)
{
    TV from=Mouse_On_Sphere(mouse_down,center,sphere_radius);
    TV to=Mouse_On_Sphere(mouse_now,center,sphere_radius);
    rotation_matrix=ROTATION<TV>::From_Rotated_Vector(from,to).Rotation_Matrix();
}

auto QT_ARCBALL::Mouse_On_Sphere(const TV2 &mouse,const TV2 &center,T radius) const -> TV
{
    TV2 ball_mouse=(mouse-center)/radius;
    float mag=ball_mouse.Magnitude_Squared();
    if(mag<=1) return TV(ball_mouse[0],ball_mouse[1],sqrt(1-mag));
    ball_mouse=ball_mouse.Normalized();
    return TV(ball_mouse[0],ball_mouse[1],0);
}
}
