# -*- 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 datetime import datetime
from threading import Timer

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

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


uris = {
    URI0,
    URI1,
  #  URI2,
   # URI3,
   # URI4,
}

params = {
    URI0: [params0],
   # URI1: [params1],
   # URI2: [params2],
   # URI3: [params3],
   # URI4: [params4],
}


def poshold(cf, t, z):
    steps = t * 10

    for r in range(steps):
        cf.commander.send_hover_setpoint(0, 0, 0, z)
        time.sleep(0.1)


def run_sequence(scf, params):
    cf = scf.cf

    # Number of setpoints sent per second
    fs = 4
    fsi = 1.0 / fs

    # Compensation for unknown error :-(
    comp = 1.3

    # Base altitude in meters
    base = 0.15

    d = params['d']
    z = params['z']

    poshold(cf, 2, base)
    print("JUST BEFORE THE LOGGING")
    ramp = fs * 2
    for r in range(ramp):
        cf.commander.send_hover_setpoint(0, 0, 0, base + r * (z - base) / ramp)
        time.sleep(fsi)

    poshold(cf, 2, z)

    for _ in range(2):
        # The time for one revolution
        circle_time = 8

        steps = circle_time * fs
        for _ in range(steps):
            cf.commander.send_hover_setpoint(d * comp * math.pi / circle_time,
                                             0, 360.0 / circle_time, z)
            time.sleep(fsi)

    poshold(cf, 2, z)

    for r in range(ramp):
        cf.commander.send_hover_setpoint(0, 0, 0,
                                         base + (ramp - r) * (z - base) / ramp)
        time.sleep(fsi)

    poshold(cf, 1, base)

    cf.commander.send_stop_setpoint()


def GetBattery(scf):
##    log_conf = LogConfig(name='Battery Level', period_in_ms=1000)
##    log_conf.add_variable('pm.batteryLevel', 'float')
##    scf.cf.log.add_config(log_conf)
##    #log_conf.data_received_cb.add_callback(lambda timestamp, data, logconf:
##    log_conf.start()
##    uri = scf.cf.link_uri
##    now = datetime.now()
##    dt_string = now.strftime("%m/%d/%Y %H:%M:%S")
##
##    print(f'[{dt_string}][{uri}]: ', end='')
##    #for name, value in data.items():
##    print("Radio: " + uri +"\n")
##    #print(log_conf.items())
##    #time.sleep(1)
    #print("For this URI : ", list(uris.keys())[0])
##    global done
##    if done == 0:
##        print("URI0")
##        le = LoggingExample(URI0)
##        done = done + 1
##    elif done == 1:
##        print("URI1")
##        le2 = LoggingExample(URI1)
    le2 = LoggingExample(URI1)


if __name__ == '__main__':
    cflib.crtp.init_drivers()
    #initialize logger for battery
    #lg_stab = LogConfig(name='Battery Level', period_in_ms=1000)
    #lg_stab.add_variable('pm.batteryLevel', 'float')
    factory = CachedCfFactory(rw_cache='./cache')
    with Swarm(uris, factory=factory) as swarm:
      #  swarm.reset_estimators()
        #le = LoggingExample(URI0)
        print("INSIDE SWARM")
        print("STARTING BATTERY")
        swarm.parallel_safe(GetBattery)
        print("GOT BATTERY")
        #le = LoggingExample(URI0)
        print("SUCCESS")
        #swarm.parallel(run_sequence, args_dict=params)
        print("ENDING SWARM")