#!/usr/bin/env python3

import ev3dev.ev3 as ev3
from wheel import Wheel
from sensor import Sensor
import time


class Move:
    def __init__(self, planet):
        try:
            self._wheel_l = Wheel('outB')
            self._wheel_r = Wheel('outC')
            self._sensor = Sensor()
            self._bumper = ev3.TouchSensor()
        except:
            print("ERROR: Cannot find Motor/Sensor")

    '''
    determine maximum and minimum brightness of lines/white space
    depending on environment lighting
    '''
    def _calibrate(self):
        pass

    '''
    Function to correct errors should the robot wander astray
    '''
    # Probably redundant because aligning can be done while scanning other paths

    def _aligntoedge(self):
        '''
        isonedge = False
        alternatingvalue = 2  # value used to switch turning directions using mod 2 hence theres only two possible values when you keep increasing the value
        if(self._sensor.getbrightness() < 24):
            while (not (self._sensor.getbrightness() == 24)):
                self._wheel_l.speed_set(5)
                self._wheel_r.speed_set(-5)
        elif(self._sensor.getbrightness() > 24):
            while (not isonedge):
                if alternatingvalue % 2 == 0:
                    isonedge = self._findedgeonrightside(alternatingvalue)
                    self._wheel_l.stop()
                    self._wheel_r.stop()
                if alternatingvalue % 2 == 1:
                    isonedge = self._findedgeonleftside(alternatingvalue)
                    self._wheel_l.stop()
                    self._wheel_r.stop()
                alternatingvalue = alternatingvalue + 1
        '''
        pass

    '''
    def _findedgeonleftside(self, alternatingvalue):
        i = 0
        while i < alternatingvalue:
            self._wheel_r.speed_set(5)
            self._wheel_l.speed_set(-5)
            if (self._sensor.getbrightness() == 24):
                return True
            i = i + 1
        return False

    def _findedgeonrightside(self, alternatingvalue):
        i = 0
        while i < alternatingvalue:
            self._wheel_l.speed_set(5)
            self._wheel_r.speed_set(-5)
            if (self._sensor.getbrightness() == 24):
                time.sleep(0.05)
                while(not (self._sensor.getbrightness() == 24)):
                    self._wheel_l.speed_set(5)
                    self._wheel_r.speed_set(-5)
                return True
            i = i + 1
        return False
    '''

    '''
    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):
        self._wheel_l.turnbyamount(500, 250)
        self._wheel_r.turnbyamount(-500, 250)

    def traversetonextstation(self, isknownstation):
        self._wheel_l.speed_set(24)
        self._wheel_r.speed_set(24)
        self._wheel_l.run()
        self._wheel_r.run()
<<<<<<< HEAD
	while (not (self._bumper.value() or self._sensor.isRed() or self._sensor.isBlue())):
            self._wheel_l.speed_set(48 - self._sensor.getbrightness())
            self._wheel_r.speed_set(self._sensor.getbrightness())
            if(isknownstation == False):
                pass #run odometry stuff here

        if (not self._bumper.value()):
            time.sleep(1)
            # self._aligntoedge()

        self._wheel_l.stop()
        self._wheel_r.stop()