#!/usr/bin/env python
from __future__ import print_function
import pyexotica as exo
from pyexotica.publish_trajectory import *
import signal
from time import sleep


def publish_pose(q, problem, t=0):
    '''Custom pose publishing to force update the task map debug marker publishing'''
    problem.update(q, t)
    problem.get_scene().get_kinematic_tree().publish_frames()


def publish_trajectory(traj, T, problem):
    '''Custom trajectory publishing to force update the task map debug marker publishing'''
    if len(traj) == 0:
        print("Trajectory has zero elements")
        raise
    signal.signal(signal.SIGINT, sig_int_handler)
    print('Playing back trajectory ' + str(T) + 's')
    dt = float(T) / float(len(traj))
    t = 0
    while True:
        try:
            publish_pose(traj[t], problem, t)
            sleep(dt)
            t = (t + 1) % len(traj)
        except KeyboardInterrupt:
            return False


if __name__ == "__main__":
    exo.Setup.init_ros()
    solver = exo.Setup.load_solver(
        '{exotica_examples}/resources/configs/example_smooth_collision_distance.xml')
    problem = solver.get_problem()

    for t in range(0, problem.T - 1):
        problem.set_rho('Pose', 0.0, t)

    solution = solver.solve()
    print("Solved in", solver.get_planning_time())

    publish_trajectory(solution, problem.T * problem.tau, problem)
