import os, sys


sys.path.append(os.path.dirname(os.path.realpath(f'{__file__}/..')))
import pathlib
import numpy as np
from time import sleep

from src.GymEnvs import GiadogEnv, TerrainScene, QuadrupedRobot, GiadogEasyEnv

from src.__env__ import CONTROLLER_LATENCY_STEPS, GRAVITY_VECTOR, \
    HISTORIAL_DATA, NON_PRIVILIGED_DATA, NON_PRIVILIGED_METHODS, \
    PRIVILIGED_DATA, PRIVILIGED_METHODS, ACTION_DATA, SELECTED_GAIT, \
    SIM_SECONDS_PER_STEP, UPDATE_METHODS, STATE_SIZE, PRIVILEGED_STATE_SIZE,\
        STATE_FEATURES, PRIVILEGED_STATE_FEATURES, JOINTS_IDS, ACTION_SPACE_SIZE,\
        TOES_IDS, H_OFF, V_OFF, SHANK_L, THIGH_L


from src.GymEnvs import pyBulletPainter as pbp

from src.GymEnvs.TestingFunctions import test_height_scan

from src.simulation import hills, steps, stairs

from pybullet_utils import bullet_client
import pybullet

import matplotlib.pyplot as plt

quadruped_urdf = str(pathlib.Path(__file__).parent.parent.resolve()) +\
            '/mini_ros/urdf/spot.urdf'


terrain_file = 'terrains/steps.txt'

client = bullet_client.BulletClient(pybullet.GUI)

robot = QuadrupedRobot(
                quadruped_urdf,
                client,
                UPDATE_METHODS,
                STATE_FEATURES,
                PRIVILEGED_STATE_FEATURES
                )       
scene = TerrainScene(
                client, 
                GRAVITY_VECTOR, 
                SIM_SECONDS_PER_STEP,
                CONTROLLER_LATENCY_STEPS
                ) 

client.resetSimulation()
scene.episode_restart()
#steps_terray = steps(100, 100, 0.4, 0.3, 4)
hills_terrain = hills(100, 100, 0.001, 0.1, 0.01, 452)
scene.load_terrain_from_array(hills_terrain)

scene.place_terrain_in_simulation(robot, first_execution = True)
robot.add_to_scene(scene, first_execution = True)

x = np.random.uniform(-4, 4)
y = np.random.uniform(-4, 4)
robot.reset(np.array([3, -3]))



X_vel = [np.zeros(3)]
X_vel_real = [np.zeros(3)]
while scene.elapsed_time < 58:
    

    # Update the state of te robot
    state = robot.state()
    
    scene.global_step()
    robot.turn_dir = np.array([0])
    robot.apply_action(np.zeros(ACTION_SPACE_SIZE))
    
    robot.elapsed_time = scene.elapsed_time

    estimated_base_velocity = robot.base_linear_vel_estimation
    X_vel.append(estimated_base_velocity)
    X_vel_real.append(robot.base_linear_vel* 0.01 + X_vel_real[-1]*0.99)
    
while scene.elapsed_time < 62:
    

    # Update the state of te robot
    state = robot.state()
    
    scene.global_step()

    #robot.apply_action(np.zeros(ACTION_SPACE_SIZE))
    
    robot.elapsed_time = scene.elapsed_time

    estimated_base_velocity = robot.base_linear_vel_estimation
    
    X_vel.append(estimated_base_velocity)
    X_vel_real.append(robot.base_linear_vel* 0.01 + X_vel_real[-1]*0.99)

X_vel = np.array(X_vel)
X_vel_real = np.array(X_vel_real)
print(sum([x * SIM_SECONDS_PER_STEP * CONTROLLER_LATENCY_STEPS for x in X_vel]))
print(sum([x * SIM_SECONDS_PER_STEP * CONTROLLER_LATENCY_STEPS for x in X_vel_real]))
plt.plot(X_vel_real[:, 0])
plt.plot(X_vel[:, 0])

plt.show()
plt.plot(X_vel_real[:, 1])
plt.plot(X_vel[:, 1])

plt.show()


plt.plot(X_vel_real[:, 2])
plt.plot(X_vel[:, 2])
plt.show()





    
    


    
import os, sys


sys.path.append(os.path.dirname(os.path.realpath(f'{__file__}/..')))
import pathlib
import numpy as np
from time import sleep

from src.GymEnvs import GiadogEnv, TerrainScene, QuadrupedRobot, GiadogEasyEnv

from src.__env__ import CONTROLLER_LATENCY_STEPS, GRAVITY_VECTOR, \
    HISTORIAL_DATA, NON_PRIVILIGED_DATA, NON_PRIVILIGED_METHODS, \
    PRIVILIGED_DATA, PRIVILIGED_METHODS, ACTION_DATA, SELECTED_GAIT, \
    SIM_SECONDS_PER_STEP, UPDATE_METHODS, STATE_SIZE, PRIVILEGED_STATE_SIZE,\
        STATE_FEATURES, PRIVILEGED_STATE_FEATURES, JOINTS_IDS, ACTION_SPACE_SIZE,\
        TOES_IDS, H_OFF, V_OFF, SHANK_L, THIGH_L


from src.GymEnvs import pyBulletPainter as pbp

from src.GymEnvs.TestingFunctions import test_height_scan

from src.simulation import hills, steps, stairs

from pybullet_utils import bullet_client
import pybullet

import matplotlib.pyplot as plt

quadruped_urdf = str(pathlib.Path(__file__).parent.parent.resolve()) +\
            '/mini_ros/urdf/spot.urdf'


terrain_file = 'terrains/steps.txt'

client = bullet_client.BulletClient(pybullet.GUI)

robot = QuadrupedRobot(
                quadruped_urdf,
                client,
                UPDATE_METHODS,
                STATE_FEATURES,
                PRIVILEGED_STATE_FEATURES
                )       
scene = TerrainScene(
                client, 
                GRAVITY_VECTOR, 
                SIM_SECONDS_PER_STEP,
                CONTROLLER_LATENCY_STEPS
                ) 

client.resetSimulation()
scene.episode_restart()
#steps_terray = steps(100, 100, 0.4, 0.3, 4)
hills_terrain = hills(100, 100, 0.001, 0.1, 0.01, 452)
scene.load_terrain_from_array(hills_terrain)

scene.place_terrain_in_simulation(robot, first_execution = True)
robot.add_to_scene(scene, first_execution = True)

x = np.random.uniform(-4, 4)
y = np.random.uniform(-4, 4)
robot.reset(np.array([3, -3]))



X_vel = [np.zeros(3)]
X_vel_real = [np.zeros(3)]
while scene.elapsed_time < 58:
    

    # Update the state of te robot
    state = robot.state()
    
    scene.global_step()
    robot.turn_dir = np.array([0])
    robot.apply_action(np.zeros(ACTION_SPACE_SIZE))
    
    robot.elapsed_time = scene.elapsed_time

    estimated_base_velocity = robot.base_linear_vel_estimation
    X_vel.append(estimated_base_velocity)
    X_vel_real.append(robot.base_linear_vel* 0.01 + X_vel_real[-1]*0.99)
    
while scene.elapsed_time < 62:
    

    # Update the state of te robot
    state = robot.state()
    
    scene.global_step()

    #robot.apply_action(np.zeros(ACTION_SPACE_SIZE))
    
    robot.elapsed_time = scene.elapsed_time

    estimated_base_velocity = robot.base_linear_vel_estimation
    
    X_vel.append(estimated_base_velocity)
    X_vel_real.append(robot.base_linear_vel* 0.01 + X_vel_real[-1]*0.99)

X_vel = np.array(X_vel)
X_vel_real = np.array(X_vel_real)
print(sum([x * SIM_SECONDS_PER_STEP * CONTROLLER_LATENCY_STEPS for x in X_vel]))
print(sum([x * SIM_SECONDS_PER_STEP * CONTROLLER_LATENCY_STEPS for x in X_vel_real]))
plt.plot(X_vel_real[:, 0])
plt.plot(X_vel[:, 0])

plt.show()
plt.plot(X_vel_real[:, 1])
plt.plot(X_vel[:, 1])

plt.show()


plt.plot(X_vel_real[:, 2])
plt.plot(X_vel[:, 2])
plt.show()





    
    


    
#steps_terray = steps(100, 100, 0.4, 0.3, 4) # Update the state of te robot # Update the state of te robot #robot.apply_action(np.zeros(ACTION_SPACE_SIZE))