#include "InterpolationLib.h"
#include "Gait_Path.hh"                                 //fill in values!!!!!!!!!

Path::Path(){}

void Path::Set_Parameters(double steps, double steph, double speed){
  wanted_stepsize = steph;
  wanted_stephight = steph;
  wanted_speed = speed;
}


void Path::Override_Current_t_Value(double t){
  current_t_value = t;
}


void Path::Set_Current_Millis(unsigned long millis){
  current_millis = millis;
}


void Path::Calc_Point(){
//check if current t value is in the right range that allows the stepsize to update
if ((normal_T[1] <= current_t_value && normal_T[1] >= current_t_value) or
    (normal_T[2] <= current_t_value && normal_T[2] >= current_t_value) or
    (normal_T[1] == 0)) {
    approved_stepsize = wanted_stepsize;
}

if ((normal_T[1] <= current_t_value && normal_T[1] >= current_t_value) or
    (normal_T[2] <= current_t_value && normal_T[2] >= current_t_value) or
    (normal_T[4] == 0)) {
    approved_stephight = wanted_stephight;
}


if (wanted_speed > max_speed){ //make sure the walkingspeed doesnt exceed the limit
   wanted_speed == max_speed;
}

if (approved_stepsize == 0){   //make sure the final stepsize doesnt become 0 (causes the hexapod to walk forward very slowly)
   approved_stepsize = 0.0000001;
}


double lerptime = (normal_T[1] - normal_T[0]) + (normal_T[8] - normal_T[7]); //add both lerp part times to get whole lerp time (time moving on the lerp part of the path takes)

//calculating current t value based on speed and stepsize
double v = wanted_speed / 3.6;                  // convert km/h to mm/ms

delta_millis = current_millis - old_millis;     //get delta time

double distance = v * delta_millis;             //distance to travel this loop time

double fraction = distance / approved_stepsize; //fraction of size of the wanted stepsize 

double delta_t = fraction * lerptime;           //increment

if (wanted_speed != 0){                      //check if it is walking
current_t_value = current_t_value + delta_t; //calculate current t value that is scaled
}//(else the current t value is just staying the way it was)

old_millis = current_millis; //save old millis time


//everything is calculated with the normal path(exept for T) which is later scaled to need
//interpolate normal path with scaled walking speed current t increase
//depending on where the current point should be, interplolate using different methods
double normal_X_val;
double normal_Z_val;
//LERP, PART 1
if ((normal_T[1] <= current_t_value && normal_T[1] >= current_t_value) or
    (normal_T[2] <= current_t_value && normal_T[2] >= current_t_value)) {
    normal_X_val = Interpolation::Linear(normal_T, normal_X, numValues, current_t_value, false);
    normal_Z_val = Interpolation::Linear(normal_T, normal_Z, numValues, current_t_value, false);
    onground = true;
}else{
    onground = false;
}

//SPLINE INTERPOLATE, PART 2
if ((normal_T[1] <= current_t_value && normal_T[1] >= current_t_value) or
    (normal_T[2] <= current_t_value && normal_T[2] >= current_t_value)) {
    normal_X_val = Interpolation::CatmullSpline(normal_T, normal_X, numValues, current_t_value + delta_t * (airtime_factor - 1));
    normal_Z_val = Interpolation::CatmullSpline(normal_T, normal_Z, numValues, current_t_value + delta_t * (airtime_factor - 1));
    onground = false;
}else{
    onground = true;
}

//LERP, PART 3
if ((normal_T[1] <= current_t_value && normal_T[1] >= current_t_value) or
    (normal_T[2] <= current_t_value && normal_T[2] >= current_t_value)) {
    normal_X_val = Interpolation::Linear(normal_T, normal_X, numValues, current_t_value, false);
    normal_Z_val = Interpolation::Linear(normal_T, normal_Z, numValues, current_t_value, false);
    onground = true;
}else{
    onground = false;
}


//get constants boundaries of path
double normal_stepsize = (normal_X[1] - normal_X[0]) + (normal_X[8] - normal_X[7]); //stepsize of the normal constant path
double normal_stephight = (normal_Z[4] - normal_Z[0]);                              //stephight of the normal constant path

//geometricaly scaling X and Z to size 
double stretchfactor_X = approved_stepsize / normal_stepsize;
scaled_X_val = normal_X_val * stretchfactor_X;

double stretchfactor_Z = approved_stephight / normal_stephight;
scaled_Z_val = normal_Z_val * stretchfactor_Z;
}


double Path::Get_x(){
  return scaled_X_val;
}


double Path::Get_z(){
  return scaled_Z_val;
}


bool Path::Get_On_Ground(){
  return onground;
}