#include "move.h"
#include <math.h>

#include "quat.h"

static const float PI = 3.14159265359f;
static const float TO_DEG = 180.0f/PI;
static const float TO_RAD = PI/180.0f;

Vec3 Motion(Vec3 vec, bool move, float dx, float dy, float range, unsigned long time)
{
  //static float move_x=0, move_y=0;
  
  //static float move_tof_x=0, move_tof_y=0;
  //static float move_flo_x=0, move_flo_y=0;
  
  /*static long timer=0;
  timer++;
  if(timer>1000)
  {
    move_flo_x=move_flo_y=move_tof_x=move_tof_y=0;
    move_x=0; move_y=0;
    timer=0;
  }*/
  
  //if(!move) return {0,0,0};
  
  static float len, high;
  high = range * vec.z;               // cos(Z)
  len = range * sqrtf(1-vec.z*vec.z); // sin(Z)
  
  static float a_x=0, a_y=0;
  a_x=0, a_y=0;
  
  static float v_x, v_y;
  
  static float angle_shift=600;
  
  if(move)
  {
    a_x = (v_x-vec.x)*angle_shift;
    a_y = (v_y-vec.y)*angle_shift;
  }
  
  v_x=vec.x;
  v_y=vec.y;
  
  static Vec3 v;
  
  float x=(dx+a_x), y=(dy-a_y);
  
  static float fx=0, fy=0;
  
  //fx=(fx*3+x)/4;
  //fy=(fy*3+y)/4;
  
  return {x, y, high};
  
  //v = RotateToZ({dx-a_x, dy-a_y, 0});
  
  //v = RotateToZ({(dx+a_x)*(range/1000.0f), (dy-a_y)*(range/1000.0f), 0}, true);
  
  //v = RotateToZ({1000, 0, 0}, true);
  //v = RotateToZ({(dx+a_x)*(range/1000.0f), 0, 0}, true);
  
  //v={dx+a_x, dy-a_y, 0};
  
  //move_x-=v.x;
  //move_y-=v.y;
  
  
  //move_flo_x+=dx;
  //move_flo_y+=dy;
  
  //move_tof_x+=(float)a_x;
  //move_tof_y+=(float)a_y;
  
  return v;
  
}

/*void Motion(Vec3 vec, short dx, short dy, float range, unsigned long time)
{
  static long move_x=0, move_y=0;
  
  move_x+=dx;
  move_y+=dy;
  
  static float len, high;
  high = range * vec.z;               // cos(Z)
  len = range * sqrtf(1-vec.z*vec.z); // sin(Z)
  
  static float a_x, a_y;
  
  a_x = range * vec.x;
  a_y = -range * vec.y;
  
  static float m_x, m_y;
  
  m_x=move_x+a_x;
  m_y=move_y+a_y;
}*/