# Copyright 2015 The Chromium OS Authors. All rights reserved.
# Use of this source code is governed by a BSD-style license that can be
# found in the LICENSE file.

""" Fingertip retrieval training script

This script it run manually by the operator to train the robot how to pick up
a single fingertip from the nest.  You can configure which tip it is by editing
the three constants BASENAME, FINGER, and ROW which are defined by the physical
properties of the fingertip in question.

Usage:
  1. Configure the constants to describe the fingertip in question
  2. Run the script "python train_fingertip.py"
  3. Follow the instructions presented on screen.

This will store a pickled fingertip object in nest_locations that will
automatically be picked up by the Touchbot class.
"""

import copy
import sys
import time

from touchbot import Touchbot
from touchbot.fingertip import Fingertip
from touchbot.position import Position


# Configure which fingertip you would like to train the position for here
BASENAME = 'round_8mm'   # The name of this fingertip
FINGER = 1               # Which finger it goes on (1, 2, or None for both)
ROW = 1                  # Which row of the nest the fingertip sits in


# Constants defined by the size/shape/location of the nest
ROW_ONE_SLIDE_OFFSET = -30
ROW_TWO_SLIDE_OFFSET = 60


def main():
  bot = Touchbot()
  bot.PushSpeed(Touchbot.SPEED_MEDIUM)

  name = ('%d%s' % (FINGER, BASENAME)) if FINGER else BASENAME

  # Build a new fingertip object
  fingertip = Fingertip(name, FINGER)

  # Prompt the user to align the robot's fingers over the tip and record the
  # location as the "above" position
  print 'Please align finger(s) above the tip "%s"' % name
  bot._MoveBothFingersAbsolute(0, 0)
  fingertip.above_pos = bot._CalibratePosition()

  # Extend the fingers into the probe and record their positions
  pos1 = pos2 = 0
  if FINGER is None:
    pos1 = bot._ExtendFingerUntilContact(1)
    pos2 = bot._ExtendFingerUntilContact(2)
  elif finger_number == 1:
    pos1 = bot._ExtendFingerUntilContact(1)
  else:
    pos2 = bot._ExtendFingerUntilContact(2)

  bot._MoveBothFingersAbsolute(0, 0)
  if FINGER in Touchbot.FINGER_NUMBERS:
    fingertip.extension1, fingertip.extension2 = pos1, pos2
  else:
    fingertip.extension1, fingertip.extension2 = pos1, pos1

  # Based on which row the tip is in, calculate the position that will slide
  # the tip out of the nest.
  fingertip.slide_pos = copy.deepcopy(fingertip.above_pos)
  if ROW == 1:
    fingertip.slide_pos.x += ROW_ONE_SLIDE_OFFSET
  elif ROW == 2:
    fingertip.slide_pos.x += ROW_TWO_SLIDE_OFFSET
  else:
    print 'ERROR: Unable to determine slide location.  Invalid ROW (%d)' % ROW
    sys.exit(1)

  # Now we have the robot try putting it in and taking it out repeatedly, to
  # allow the user to tune the fingertip height.  The tips should slide in/out
  # without any noise.  Keep tweaking this value until they move silently.
  orig_extension1 = fingertip.extension1
  orig_extension2 = fingertip.extension2
  while True:
    # Try applying a new offset
    extra_descent = int(raw_input('Type a guess for the extra descent needed: '))
    time.sleep(2)
    fingertip.extension1 = orig_extension1 + extra_descent
    fingertip.extension2 = orig_extension2 + extra_descent

    # Trying to get/drop the finger to see how well it works
    bot._GetFingertip(fingertip)
    time.sleep(1)
    bot._DropFingertip(fingertip)

    yorn = raw_input('Accept this path? (y/n)')
    if yorn == 'y':
      break

  # Finally, save the results
  fingertip.SaveToPickledFile('touchbot/nest_locations/%s.p' % name)

  bot.PopSpeed()


if __name__ == "__main__":
  main()