# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# sot-motion-planner is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
# General Lesser Public License for more details.  You should have
# received a copy of the GNU Lesser General Public License along with
# sot-motion-planner. If not, see <http://www.gnu.org/licenses/>.

from dynamic_graph import plug
from dynamic_graph.sot.motion_planner import Localizer


l = Localizer("localizer")
l.add_landmark_observation("obs")
l.add_landmark_observation("obs2")

# Feature space is geometrical space without the orientation.
# Here we would have some kind of "global" detector
# returning (x,y) world coordinate of some objects.
JfeatureReferencePosition = ((1, 0, 0), (0, 1, 0))

# Robot is sensor, so jacobian is identity.
JsensorPosition = ((1, 0, 0), (0, 1, 0), (0, 0, 1))


# First observation.
l.obs_JfeatureReferencePosition.value = JfeatureReferencePosition
l.obs_JsensorPosition.value = JsensorPosition
c1_M_c = np.matrix(
    [[ 0.,  0.,  1., 0.],
     [-1.,  0.,  0., 0.],
     [ 0., -1.,  0., 0.],
     [ 0.,  0.,  0., 1.]])


g_M_c = matrixToTuple(g_M_c1 * c1_M_c)

plug(robot.dynamic.gaze, w_M_c.positionIN)
plug(robot.dynamic.Jgaze, w_M_c.jacobianIN)
w_M_c.setTransformation(g_M_c)


# Localization
l = Localizer('localizer')

sensors = [w_M_c]

f = 1.
(px, py) = (1., 1.)
(u0, v0) = (0., 0.)

C = np.matrix(
    [[ f * px, 0.,     u0 ],
     [ 0.,     f * py, v0 ],
     [ 0.,     0.,     1. ]],
    dtype = np.dtype(np.float)
   )

def S(q, sensorId):
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# sot-motion-planner is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
# General Lesser Public License for more details.  You should have
# received a copy of the GNU Lesser General Public License along with
# sot-motion-planner. If not, see <http://www.gnu.org/licenses/>.

from dynamic_graph import plug
from dynamic_graph.sot.motion_planner import Localizer


l = Localizer('localizer')
l.add_landmark_observation('obs')

# Feature space is geometrical space without the orientation.
# Here we would have some kind of "global" detector
# returning (x,y) world coordinate of some objects.
l.obs_JfeatureReferencePosition.value = (
    (1, 0, 0),
    (0, 1, 0)
    )

# Robot is sensor, so jacobian is identity.
l.obs_JsensorPosition.value = (
    (1, 0, 0),
    (0, 1, 0),
    (0, 0, 1),
#
#        .
#       / \
#        | x+
#  y+ <--X
#
# (0, 0) = O = robot position
# (r, 0) = sensor position
#
# Q = (x, y, theta)
# S = (x, y)
# IM = D
#


l = Localizer('localizer')

r = 1.            # robot radius
error = (1, 0, 0) # dx, dy, dtheta

expectedRobot = (0, 0, 0) # x, y, theta
realRobot = tuple(map(lambda (p, e): p + e, zip(expectedRobot, error))) # x, y, theta

# Sensor positions around the robot:
#
#     1
#   3   2
#     .
#
sensorPos = (0., pi / 2.)