#!python
# encoding: utf-8
#
# @Author: José Sánchez-Gallego
# @Date: Dec 1, 2017
# @Filename: jaeger
# @License: BSD 3-Clause
# @Copyright: José Sánchez-Gallego

import asyncio
import logging
from functools import update_wrapper

import click
import numpy

from jaeger import log
from jaeger.commands.bootloader import load_firmware
from jaeger.fps import FPS


def cli_coro(f):
    """Decorator function that allows defining coroutines with click."""

    f = asyncio.coroutine(f)

    def wrapper(*args, **kwargs):
        loop = asyncio.get_event_loop()
        return loop.run_until_complete(f(*args, **kwargs))

    return update_wrapper(wrapper, f)


@click.group(invoke_without_command=True)
@click.option('-p', '--profile', type=str, help='the bus interface profile')
@click.option('-l', '--layout', type=str, help='the FPS layout')
@click.option('-a', '--actor', is_flag=True, help='start as an actor')
@click.option('-v', '--verbose', is_flag=True, help='debug mode')
@click.pass_context
@cli_coro
def jaeger(ctx, layout=None, profile=None, actor=False, verbose=False):
    """CLI for the SDSS-V focal plane system."""

    if verbose:
        log.set_level(logging.DEBUG)

    ctx.obj = {}
    ctx.obj['can_profile'] = profile
    ctx.obj['layout'] = layout

    # If we call jaeger without a subcommand and with the actor flag,
    # start the actor.
    if actor and ctx.invoked_subcommand is None:
        fps = FPS(**ctx.obj)
        yield from fps.initialise()
        fps.start_actor()
    elif ctx.invoked_subcommand is None:
        raise click.UsageError('not enough arguments')


@jaeger.command(name='upgrade-firmware')
@click.argument('firmware-file', nargs=1, type=click.Path(exists=True))
@click.option('-f', '--force', is_flag=True, help='forces skipping of invalid positioners')
@click.option('-s', '--positioners', type=str, help='comma-separated positioners to upgrade')
@click.pass_context
@cli_coro
def upgrade_firmware(ctx, firmware_file, force=False, positioners=None):
    """Upgrades the firmaware."""

    if positioners is not None:
        positioners = [int(positioner.strip()) for positioner in positioners.split(',')]

    fps = FPS(**ctx.obj)
    yield from fps.initialise()

    yield from load_firmware(fps, firmware_file, positioners=positioners, force=force)


@jaeger.command()
@click.argument('positioner_id', metavar='POSITIONER', type=int)
@click.argument('alpha', metavar='ALPHA', type=float)
@click.argument('beta', metavar='BETA', type=float)
@click.option('--speed', type=(float, float), default=(1000, 1000),
              help='the speed for the alpha and beta motors.',
              show_default=True)
@click.pass_context
@cli_coro
def goto(ctx, positioner_id, alpha, beta, speed=None):
    """Moves a robot to a given position."""

    if alpha < 0 or alpha >= 360:
        raise click.UsageError('alpha must be in the range [0, 360)')

    if beta < 0 or beta >= 360:
        raise click.UsageError('beta must be in the range [0, 360)')

    if speed[0] < 0 or speed[0] >= 3000 or speed[1] < 0 or speed[1] >= 3000:
        raise click.UsageError('speed must be in the range [0, 3000)')

    fps = FPS(**ctx.obj)
    yield from fps.initialise()

    positioner = fps.positioners[positioner_id]
    result = yield from positioner.initialise()
    if not result:
        log.error('positioner is not connected or failed to initialise.')
        return

    result = yield from positioner.goto(alpha=alpha, beta=beta,
                                        alpha_speed=speed[0],
                                        beta_speed=speed[1])

    if result is False:
        return


@jaeger.command(name='set-positions')
@click.argument('positioner_id', metavar='POSITIONER', type=int)
@click.argument('alpha', metavar='ALPHA', type=float)
@click.argument('beta', metavar='BETA', type=float)
@click.pass_context
@cli_coro
def set_positions(ctx, positioner_id, alpha, beta):
    """Sets the position of the alpha and beta arms."""

    if alpha < 0 or alpha >= 360:
        raise click.UsageError('alpha must be in the range [0, 360)')

    if beta < 0 or beta >= 360:
        raise click.UsageError('beta must be in the range [0, 360)')

    fps = FPS(**ctx.obj)
    yield from fps.initialise()

    positioner = fps.positioners[positioner_id]

    result = yield from positioner._set_position(alpha, beta)

    if not result:
        log.error('failed to set positions.')
        return

    log.info(f'positioner {positioner_id} set to {alpha, beta}.')


@jaeger.command()
@click.argument('positioner_id', metavar='POSITIONER', type=int)
@click.option('-n', '--moves', type=int,
              help='number of moves to perform. Otherwise runs forever.')
@click.option('--alpha', type=(int, int), default=(0, 360),
              help='range of alpha positions.', show_default=True)
@click.option('--beta', type=(int, int), default=(0, 180),
              help='range of beta positions.', show_default=True)
@click.option('--speed', type=(int, int), default=(500, 1500),
              help='range of speed.', show_default=True)
@click.pass_context
@cli_coro
def demo(ctx, positioner_id, alpha=None, beta=None, speed=None, moves=None):
    """Moves a robot to random positions."""

    if (alpha[0] >= alpha[1]) or (alpha[0] < 0 or alpha[1] > 360):
        raise click.UsageError('alpha must be in the range [0, 360)')

    if (beta[0] >= beta[1]) or (beta[0] < 0 or beta[1] > 360):
        raise click.UsageError('beta must be in the range [0, 360)')

    if (speed[0] >= speed[1]) or (speed[0] < 0 or speed[1] >= 3000):
        raise click.UsageError('speed must be in the range [0, 3000)')

    fps = FPS(**ctx.obj)
    yield from fps.initialise()

    positioner = fps.positioners[positioner_id]
    result = yield from positioner.initialise()
    if not result:
        log.error('positioner is not connected or failed to initialise.')
        return

    done_moves = 0
    while True:

        alpha_move = numpy.random.randint(low=alpha[0], high=alpha[1])
        beta_move = numpy.random.randint(low=beta[0], high=beta[1])
        alpha_speed = numpy.random.randint(low=speed[0], high=speed[1])
        beta_speed = numpy.random.randint(low=speed[0], high=speed[1])

        log.warning(f'running step {done_moves+1}')

        result = yield from positioner.goto(alpha=alpha_move, beta=beta_move,
                                            alpha_speed=alpha_speed,
                                            beta_speed=beta_speed)

        if result is False:
            return

        done_moves += 1

        if moves is not None and done_moves == moves:
            return


if __name__ == '__main__':
    asyncio.run(jaeger(obj={}))
