%matplotlib inline
#This cell may take some time to run
# because the matplotlib requirement is quite large...

from ev3dev.auto import *
import time
from __future__ import division
from math import pi
from ipywidgets import widgets, interact

def wait(time_in_ms):
    time.sleep( time_in_ms / 1000)

#Drive demo based on https://github.com/rhempel/ev3dev-lang-python/blob/master/demo/auto-drive.py
#Originally Copyright (c) 2015 Denis Demidov <dennis.demidov@gmail.com>


# In this demo an Explor3r robot with touch sensor attachement drives
# autonomously. It drives forward until an obstacle is bumped (determined by
# the touch sensor), then turns in a random direction and continues. The robot
# slows down when it senses obstacle ahead (with the infrared sensor).
#
# The program may be stopped by pressing any button on the brick.
#
# This demonstrates usage of motors, sound, sensors, buttons, and leds.

from time import sleep
from random import choice, randint

from ev3dev.auto import *

# Connect two large motors on output ports B and C:
motors = [LargeMotor(address) for address in (OUTPUT_B, OUTPUT_C)]

# Every device in ev3dev has `connected` property. Use it to check that the
# device has actually been connected.
assert all([m.connected for m in motors]), \
    "Two large motors should be connected to ports B and C"

def demo_start_motors(duty_cycle_sp=None):
    """
    Start both motors. `run-direct` command will allow to vary motor
    performance on the fly by adjusting `duty_cycle_sp` attribute.
    """
    sp_set=False
    for m in motors:
        m.run_direct() if duty_cycle_sp is None else m.run_direct(duty_cycle_sp=duty_cycle_sp)
        
        if m.duty_cycle_sp!=0: sp_set=True
            
    if not sp_set:
        print('**WARNING**: duty_cycle_sp is zero for both motors')

demo_start_motors()

demo_start_motors(75)

# Stop the motors before exiting.
def demo_stop_motors():
    for m in motors:
        m.stop()

demo_stop_motors()

def demo_turn():
    """
    Turn the robot in random direction.
    At the end of the turn, the motors will be switched off.
    """

    # Turn the robot wheels in opposite directions from 1/4 to 3/4 of a second.
    # Use `random.choice()` to decide which wheel will turn which way.
    power = choice([(1, -1), (-1, 1)])
    t = randint(250, 750)

    for m, p in zip(motors, power):
        m.run_timed(duty_cycle_sp=p*75, time_sp=t)

    # Wait until both motors are stopped:
    while any(m.state for m in motors):
        sleep(0.1)

demo_start_motors()

demo_turn()

def demo_backup():
    """
    Back away from an obstacle.
    """

    # Sound backup alarm.
    Sound.tone([(1000, 500, 500)] * 3).wait()

    # Turn backup lights on:
    for light in (Leds.LEFT, Leds.RIGHT):
        Leds.set_color(light, Leds.RED)

    # Stop both motors and reverse for 1.5 seconds.
    # `run-timed` command will return immediately, so we will have to wait
    # until both motors are stopped before continuing.
    for m in motors:
        m.stop(stop_command='brake')
        m.run_timed(duty_cycle_sp=-50, time_sp=1500)

    # When motor is stopped, its `state` attribute returns empty list.
    # Wait until both motors are stopped:
    while any(m.state for m in motors):
        sleep(0.1)

    # Turn backup lights off:
    for light in (Leds.LEFT, Leds.RIGHT):
        Leds.set_color(light, Leds.GREEN)

demo_start_motors()

demo_backup()

# Connect infrared and touch sensors.
ir = InfraredSensor(); assert ir.connected
ts = TouchSensor();    assert ts.connected

# Put the infrared sensor into proximity mode.
ir.mode = 'IR-PROX'

# Simple get out clause - stop on brick button press
btn = Button()

# Run the robot until a button is pressed.
start()
while not btn.any():

    if ts.value():
        # We bumped an obstacle.
        # Back away, turn and go in other direction.
        backup()
        turn()
        start()

    # Infrared sensor in proximity mode will measure distance to the closest
    # object in front of it.
    distance = ir.value()

    if distance > 60:
        # Path is clear, run at full speed.
        dc = 90
    else:
        # Obstacle ahead, slow down.
        dc = 40

    for m in motors:
        m.duty_cycle_sp = dc

    sleep(0.1)

m = LargeMotor(OUTPUT_B)

def run_m(dc):
    m.duty_cycle_sp=dc
    print('Duty cycle: {}'.format(dc))

interact(run_m, dc=(-100,100,10))

m.run_direct()

m.stop()

import threading

def checkThreads():
    print(', '.join(threading.enumerate()))

#Parameter t is here is just a placeholder in case we want to pass in a specific thread to kill?
#- Timers get cancelled
#- Threads get joined?
# Can we detect type and clear all the ones we started?
#Start of notebook get a list of all threads, then clear back to that list?
def killThreads(n=None,t=None):
    if n is None:
        n=['Repeated Timer','Data logger']
    for t in threading.enumerate():
        if t.getName() in n:
            t.cancel()
            
#We can then kill any threads we know we've created if we have to...
killThreads()

#http://stackoverflow.com/a/13151299/454773down vote
from threading import Timer

class RepeatedTimer(object):
    def __init__(self, interval, function, name='Repeated Timer', *args, **kwargs):
        self._timer     = None
        self.interval   = interval
        self.function   = function
        self.name=name
        self.args       = args
        self.kwargs     = kwargs
        self.is_running = False
        self.start()

    def _run(self):
        self.is_running = False
        self.start()
        self.function(*self.args, **self.kwargs)

    def start(self):
        if not self.is_running:
            self._timer = Timer(self.interval, self._run)
            #name the timer so we can spot it and kill it
            self._timer.setName(self.name)
            self._timer.start()
            self.is_running = True

    def stop(self):
        self._timer.cancel()
        self.is_running = False

R=RemoteControl()

r_output = widgets.Text()

L=Leds
L.all_off()

def test_remote_reading():
    ''' Indicate the last button pressed '''
    txt= 'No button pressed' if not R.any() else 'Buttons pressed: {}'.format(','.join(R.buttons_pressed))
    r_output.value='Remote actions: {}'.format(txt)
    
    if 'red_up' in txt:
        L.set_color(L.RIGHT, L.RED)
    elif 'red_down' in txt:
        L.set_color(L.LEFT, L.RED)
    elif 'blue_up' in txt:
        L.set_color(L.RIGHT, L.GREEN)
    elif 'blue_down' in txt:
        L.set_color(L.LEFT, L.GREEN)


#The output units / value look wrong to me? Decimeters, maybe?
r_output

t_r.stop()

killThreads()

#Set the distance to travel in centimeters
distance_to_travel_cm = 10

#Define a function to convert centimeters to millimeters
def mm_from_cm(cm):
    return 10 * cm

distance_to_travel_mm = mm_from_cm( distance_to_travel_cm )

#Calculate tacho count required to travel that distance
count_for_distance = distance_to_travel_mm * COUNT_PER_MM

#Drive for a specified tacho count

#Set the motor speed
dutycycle=40

#Turn on the motors for the required tacho count
left_motor.run_to_rel_pos(position_sp=count_for_distance, duty_cycle_sp=dutycycle )
right_motor.run_to_rel_pos(position_sp=count_for_distance, duty_cycle_sp=dutycycle )

def travel_N_cm( distance_to_travel_cm , dutycycle= 45):
    distance_to_travel_mm = mm_from_cm( distance_to_travel_cm )
    count_for_distance = distance_to_travel_mm * COUNT_PER_MM
    left_motor.run_to_rel_pos(position_sp=count_for_distance, duty_cycle_sp=dutycycle )
    right_motor.run_to_rel_pos(position_sp=count_for_distance, duty_cycle_sp=dutycycle )

travel_N_cm(5)

travel_N_cm(-5)

##NOT TESTED##
travel_N_cm(5)

while 'running' in left_motor.state or 'running' in right_motor.state :
    time.sleep(0.1)

travel_N_cm(-5)

def actually_travel_N_cm():
    #add some logic to text that we've actually travelled the distance
    pass

#drive one edge
def one_edge(length=20):
    pass

#Turn ninety degrees
def turn(typ='onspot',angle=90):
    pass

def drive_square(side=20):
    done = False
    for i in range(4):
        while not done:
            one_edge(side)
        while not done:
            turn(90)

        

#UNTESTED
#Based on https://github.com/rhempel/ev3dev-lang-python/blob/1c9265db6dd5439884a4764f3a84f8ec6bc85220/tests/motor/ev3dev_port_logger.py
#Another approach is just to run a parallel thread
#This is more akin to the RCX task
#Use this as a model for student's own task?
class myTaskThread(threading.Thread):
    def __init__(self, name='Task thread'):
        threading.Thread.__init__(self)
        self.name = name
        self.done = threading.Event()

    def run(self):
        #to do
        while not self.done.isSet():
            pass
    
    def stop(self, timeout=None):
        self.done.set()
        self.join(timeout)

cs = ColorSensor()
cs.mode

cs.mode='COL-REFLECT'

cs_output = widgets.Text()

colorMap={0:'No color', 1:'Black',2: 'Blue',3: 'Green',
          4: 'Yellow', 5: 'Red', 6: 'White', 7: 'Brown'}

def test_color_sensor():
    if cs.mode=='COL-REFLECT':
        cs_output.value='Colour sensor output: {}'.format(cs.value())
    elif cs.mode=='COL-COLOR':
        cs_output.value='Colour sensor output: {} ({})'.format(cs.value(), colorMap[cs.value()])

#The output units / value look wrong to me? Decimeters, maybe?
cs_output

#Call the timer that polls the sensor
t_cs=RepeatedTimer(0.1,test_color_sensor)

#Stop polling the sensor
t_cs.stop()

cs.mode='COL-COLOR'

#EMERGENCY STOP BUTTON
killThreads()

#Set colour according to switch
ts = TouchSensor()

#Simple timoeut routine from http://stackoverflow.com/a/13293316/454773
timeout = time.time() + 10   # 10 seconds from now

while True:
    #Set Leds depending on whether switch is pressed or not
    L.set_color(L.RIGHT, L.RED if ts.value() else L.GREEN)
    L.set_color(L.LEFT, L.GREEN if ts.value() else L.RED)
    
    #Break out of the loop after a certain amount of time
    if time.time() > timeout:
        break

ir=InfraredSensor()
ir.mode='IR-PROX'

#Set the frequency inversely proportional to the distance
#The frequency should go up as we get closer
#Infra-red range is 0..100, map onto range 20,000..~200
def frequency_by_dist_ir(dist):
    if dist!='' and dist>=0:
        return  20000 / (dist+1)
    return -1        

ir_output = widgets.Text()

def infrared_range():
    try:
        d=ir.value()
    except:
        d=''
    ir_output.value='Infra-red range: {}'.format(d)

def infrared_beeper():
    try:
        d=ir.value()
    except:
        d=''
    f=frequency_by_dist_ir(d)
    if f>0:
        Sound.tone(f,10)

def infrared_response():
    infrared_range()
    infrared_beeper()

ir_output

irt = RepeatedTimer(0.2, infrared_response)

irt.stop()

ir=InfraredSensor()
ir.mode='IR-SEEK'

t_beacon=RepeatedTimer(0.1, ir_beacon_distDir)

t_beacon.stop()

us=UltrasonicSensor()

rt = RepeatedTimer(0.1, ultrasound_range)

#Stop the timer
rt.stop()

#Set the frequency inversely proportional to the distance
#The frequency should go up as we get closer
def frequency_by_dist_us(dist):
    return  100000 / dist

def ultrasound_beeper():
    f=frequency_by_dist_us(us.value())
    if f>0:
        Sound.tone(f,10)

urt = RepeatedTimer(0.2, ultrasound_beeper)

urt.stop()

def demo_forward(tsp=1000,dc=20):
    for m in motors:
        m.run_timed(time_sp=tsp, duty_cycle_sp=dc)

def demo_turn(direction=(-1,1),dc=25):
    for m, p in zip(motors, direction):
        m.run_timed(duty_cycle_sp=p*dc, time_sp=100)

    # Wait until both motors are stopped:
    while any(m.state for m in motors):
        sleep(0.1)

gs=GyroSensor()
gs.mode='GYRO-ANG'

#If we're within ish degrees that's close enough
ish=2

def gyro_turn_till_relative(bearing):
    #What heading does the gyro currently give, in degrees?
    current_heading=gs.value() % 360
    #The target heading is the current heading plus the desired bearing
    target_heading=(360+current_heading+bearing) % 360
    #Keep turning while the current heading is more than ish degrees from the target heading
    #Should really optimise the turn so we turn the shortest angle
    while abs(( gs.value() % 360)-target_heading) >ish:
        if gs.value() % 360 -target_heading <0:
            demo_turn((-1,1))
        else:
            demo_turn((1,-1))

demo_forward()

gyro_turn_till_relative(90)

from threading import Timer
from datetime import datetime
class SensorDataLogger(object):
    def __init__(self, sensor, interval=0.1, maxSamples=20000, name='Data logger'):
        self.sensor     = sensor
        self.interval   = interval #period between samples in seconds
        self.name= name
        self.results=[]
        #self.basetime =time.time()
        self.is_running=False
       
    def getsample(self):
        #now = time.time()
        #who have a FIFO queue to drop early samples and retain more recent ones?
        #Or something more intelligent? eg drop every othersample? (so resample, essentially?)
        if len(self.results)<=maxSamples:
            self.results.append((datetime.now(), self.sensor.value()))
        
    def _run(self):
        self.is_running = False
        self.start()
        self.getsample()

    def start(self):
        if not self.is_running:
            self._timer = Timer(self.interval, self._run)
            self._timer.setName(self.name)
            self._timer.start()
            self.is_running = True

    def stop(self):
        self._timer.cancel()
        self.is_running = False
        killThreads(self.name)
        #return self.results
        
    def log(self):
        return self.results

#Define a colour sensor
cs=ColorSensor()

#Log the colour sensor
dd=SensorDataLogger(cs)

#Start the logger
dd.start()

#Stop the logger
dd.stop()

from pandas import DataFrame

df.to_csv('text.csv')

#Free up memory
dd=None
df=None