#!/usr/bin/env python
from __future__ import print_function

import pyexotica as exo
from numpy import array
from pyexotica.publish_trajectory import sig_int_handler, publish_trajectory
from time import sleep
import signal

import exotica_scipy_solver

exo.Setup.init_ros()
# problem = exo.Setup.load_problem(
#     '{exotica_examples}/resources/configs/example_trajectory_constrained.xml')
problem = exo.Setup.load_problem(
    '{exotica_examples}/resources/configs/example_constrained_trajectory_optimization.xml')
solver = exotica_scipy_solver.SciPyTimeIndexedSolver(problem=problem, method='trust-constr', debug=True)
# solver = exotica_scipy_solver.SciPyTimeIndexedSolver(problem=problem, method='SLSQP', debug=True)

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

# for t in range(0, problem.T):
#     if float(t) * problem.tau < 0.8:
#         problem.set_rho_eq('Position', 0.0, t)
#     else:
#         problem.set_rho_eq('Position', 1, t)

t = 0.0
q = array([0.0] * 7)
print('Solving...')
signal.signal(signal.SIGINT, sig_int_handler)
solver.max_iterations = 100
solution = solver.solve()
print('Publishing...')
publish_trajectory(solution, problem.T * problem.tau, problem)
