# -*- coding: utf-8 -*-
#
#     ||          ____  _ __
#  +------+      / __ )(_) /_______________ _____  ___
#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
#
#  Copyright (C) 2017 Bitcraze AB
#
#  Crazyflie Nano Quadcopter Client
#
#  This program is free software; you can redistribute it and/or
#  modify it under the terms of the GNU General Public License
#  as published by the Free Software Foundation; either version 2
#  of the License, or (at your option) any later version.
#
#  This program 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 Public License for more details.
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software
#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
#  MA  02110-1301, USA.
"""
A script to fly 5 Crazyflies in formation. One stays in the center and the
other four fly around it in a circle. Mainly intended to be used with the
Flow deck.
The starting positions are vital and should be oriented like this

     >

^    +    v

     <

The distance from the center to the perimeter of the circle is around 0.5 m

"""
import math
import time


import cflib.crtp
from basiclog import LoggingExample
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.utils import uri_helper
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm
from cflib.positioning.motion_commander import MotionCommander
from cflib.utils.multiranger import Multiranger
from datetime import datetime
from threading import Timer

# Change uris according to your setup
URI0 = 'radio://0/48/2M/E7E7E7E701'#1
URI1 = 'radio://0/25/2M/E7E7E7E703' #3
URI2 = 'radio://0/50/2M/E7E7E7E702'#2

# d: diameter of circle
# z: altitude
params0 = {'d': 1.0, 'z': 0.3}
params1 = {'d': 1.0, 'z': 0.3}
params2 = {'d': 1.0, 'z': 0.3}
#params3 = {'d': 0.0, 'z': 0.5}
#params4 = {'d': 1.0, 'z': 0.3}

#addresses for each crazyflie
uris = {
    URI0,
    URI1,
    URI2,
   # URI3,
   # URI4,
}

#parameters for each respective crazyflie
params = {
    URI0: [params0],
    URI1: [params1],
    URI2: [params2],
   # URI3: [params3],
   # URI4: [params4],
}
def is_close(range):
    MIN_DISTANCE = 0.3  # m

    if range is None:
        return False
    else:
        return range < MIN_DISTANCE

#method to run the drone and fly
def run_sequence(scf, params):
    with MotionCommander(scf) as motion_commander:
        with Multiranger(scf) as multiranger:
            keep_flying = True

            while keep_flying:
                motion_commander.start_linear_motion(0.2, 0, 0)
                #detects object and will know to turn and continue path
                if is_close(multiranger.front) and is_close(multiranger.right) and is_close(multiranger.left):
                    motion_commander.turn_right(180, 90)
                    motion_commander.start_forward(0.1)

                if is_close(multiranger.front) and multiranger.right < multiranger.left:
                    motion_commander.turn_left(90, 90)

                if is_close(multiranger.front) and multiranger.right > multiranger.left:
                    motion_commander.turn_right(90, 90)

                if is_close(multiranger.front) and is_close(multiranger.right):
                    motion_commander.turn_left(90, 90)
                    motion_commander.start_forward(0.1)

                if is_close(multiranger.front) and is_close(multiranger.left):
                    motion_commander.turn_right(90, 90)
                    motion_commander.start_forward(0.1)

                if is_close(multiranger.back):
                    motion_commander.start_forward(0.2)

                if is_close(multiranger.left):
                    motion_commander.turn_right(45)
                    motion_commander.start_forward(0.1)

                if is_close(multiranger.right):
                    motion_commander.turn_left(45)
                    motion_commander.start_left(0.1)

                if is_close(multiranger.up):
                    motion_commander.land(0.1)
                    keep_flying = False

                time.sleep(0.1)

if __name__ == '__main__':
    cflib.crtp.init_drivers()
    factory = CachedCfFactory(rw_cache='./cache')
    count = 0
    with Swarm(uris, factory=factory) as swarm:
      #  swarm.reset_estimators()
        print("Initiating flight...")
        swarm.parallel(run_sequence, args_dict=params)
        print("End of Program, Goodbye!\n")