#!/usr/bin/env python3

import ev3dev.ev3 as ev3
from wheel import Wheel
from sensor import Sensor
from odometry import Odometry
from planet import Direction
from time import sleep


def capat100(value):
    if(value > 100):
        return 100
    elif(value < -100):
        return -100
    else:
        return value


class Move:
    def __init__(self):
        try:
            self._wheel_l = Wheel('outB')
            self._wheel_r = Wheel('outC')
            self._sensor = Sensor()
            self._calibrate()
        except OSError:
            print("ERROR: Cannot find Motor/Sensor")
        self.defaultspeed = 20
        self._odometry = Odometry()

    '''
    determine maximum and minimum brightness of lines/white space
    depending on environment lighting
    '''
    def _calibrate(self):
        ev3.Sound.beep()
        while(not self._sensor.bumperwaspressed):
            self._sensor.checkbumper()
        self._sensor.reset()
        sleep(1)
        self._sensor.calibrateBrightness()
        sleep(5)
        '''
        ev3.Sound.beep()
        while(not self._sensor.bumperwaspressed):
            self._sensor.checkbumper()
        self._sensor.reset()
        sleep(1)
        self._sensor.calibrateRGB(self._sensor.red)
        sleep(2)
        ev3.Sound.beep()
        while(not self._sensor.bumperwaspressed):
            self._sensor.checkbumper()
        self._sensor.reset()
        sleep(1)
        self._sensor.calibrateRGB(self._sensor.blue)
        sleep(5)
        '''
    '''
    Function to correct errors should the robot wander astray
    '''

    def _aligntoedge(self):
        self._wheel_l.speed_set(20)
        self._wheel_r.speed_set(-20)
        self._wheel_l.run()
        self._wheel_r.run()
        self._sensor.refresh()
        while(self._sensor.getbrightness() > self._sensor.edgebrightness):
            self._sensor.refresh()
        self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
        self._odometry.angle_set(self._odometry.angle_get())
        self._wheel_l.stop()
        self._wheel_r.stop()
        self._wheel_l.turnbyamount(20, 40)
        self._wheel_r.turnbyamount(-20, 40)
        sleep(1)

    def setcurcir(self, curdir):
        self.odometry.angle_set(curdir[1])

    '''
    uses odometry and the planet object to map edges that are connected to
    current node.

    Use either relative motor pos or timed driving to center on node.
    Next turn around and detect edges through drops in _sensor.getbrightness()
    '''
    def getstationedges(self):
        found_edges = []
        self._odometry.reset()
        while(True):
            self._aligntoedge()
            self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
            if(self._odometry.angle_get() in found_edges):
                break
            else:
                found_edges.append(self._odometry.angle_get())
                self._odometry.angle_set(self._odometry.angle_get())
                self._wheel_l.turnbyamount(20, 40)
                self._wheel_r.turnbyamount(-20, 40)
                sleep(2)
        return found_edges

    def setcurdir(self, newdir):
        self._odometry.angle_set(newdir[1])

    def getstats(self):
        stats = list(self._odometry.coordinates_get())
        if(self._sensor.bumperwaspressed):
            stats.append(-1)
        else:
            stats.append(1)
        return stats

    def turnto(self, direction):
        turnval = self._odometry.dir2ticks(direction)
        self._wheel_l.turnbyamount(turnval, 120)
        self._wheel_r.turnbyamount(-turnval, 120)
        sleep(5)
        self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))

    def reset(self):
        self._odometry.reset()
        self._sensor.reset()

    def resetwheels(self):
        self._wheel_l._motor.position = 0
        self._wheel_r._motor.position = 0

    def traversetonextstation(self, isknownstation):
        self._wheel_l.speed_set(self._sensor.edgebrightness//2)
        self._wheel_r.speed_set(self._sensor.edgebrightness//2)
        self._wheel_l.run()
        self._wheel_r.run()
        while (not self._sensor.bumperwaspressed):
            self._sensor.refresh()
            self._sensor.checkbumper()
            self._sensor.getcolor()
            if(self._sensor.lastcolor is not None):
                break
            try:
                self._wheel_l.speed_set(capat100(self._sensor.edgebrightness/2 - self._sensor.getbrightness()/2)+self.defaultspeed)
                self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self._sensor.edgebrightness/2)+self.defaultspeed)
            except OSError:
                print("ERROR: Invalid motor speed value")
            if(not isknownstation):
                self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
        self._wheel_l.stop()
        self._wheel_r.stop()
        if(self._sensor.bumperwaspressed is True):
            ev3.Sound.beep()
            sleep(0.2)
            ev3.Sound.beep()
            sleep(0.2)
            ev3.Sound.beep()
            sleep(0.2)
            ev3.Sound.beep()
            self._wheel_l.turnbyamount(-200, 120)
            self._wheel_r.turnbyamount(-200, 120)
            sleep(1)
            self._wheel_l.turnbyamount(320, 120)
            self._wheel_r.turnbyamount(-320, 120)
            sleep(2)
            self._aligntoedge()
            self._wheel_l.run()
            self._wheel_r.run()
            while(True):
                self._sensor.refresh()
                self._sensor.getcolor()
                if(self._sensor.lastcolor is not None):
                    break
                try:
                    self._wheel_l.speed_set(capat100(self._sensor.edgebrightness/2 - self._sensor.getbrightness()/2)+self.defaultspeed)
                    self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self._sensor.edgebrightness/2)+self.defaultspeed)
                except OSError:
                    print("ERROR: Invalid motor speed value")
                if(not isknownstation):
                    self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
        self._wheel_l.turnbyamount(200, 90)
        self._wheel_r.turnbyamount(200, 90)
        sleep(2)