from pinouts import NodeMCU as pins
from hcsr04 import HCSR04
import machine
import time


class Table:
    def __init__(self):
        # Distance sensor
        self.hcsr04 = HCSR04(trigger_pin=pins.D2, echo_pin=pins.D3)

        # Movement sensor
        self.movement_sensor = machine.Pin(pins.D4, mode=machine.Pin.IN)

        # Relay to move up and down
        self.relay_up = machine.Pin(pins.D5, mode=machine.Pin.OUT)
        self.relay_down = machine.Pin(pins.D6, mode=machine.Pin.OUT)

        # Reset both relays on set up
        self.triggerRelay(0)


    def currentHeight(self):
        """
        Get current height of the table in CM.

        Might return False if measuring error.
        """

        distances = []

        # Measure a bunch of times...
        for x in range(0, 20):
            distances.append(self.hcsr04.distance_cm())

        # Take the longest distance, I've not seen any that is too far
        distance = max(distances)

        if distance > 0:
            return distance

        return False


    def getMovement(self):
        """
        Get movement from sensor.
        """
        return self.movement_sensor.value()


    def triggerRelay(self, direction):
        """
        Direction should be an integer:
         -  1 to move up
         -  0 to stop movement
         - -1 to move down
        """

        if direction == 1:
            self.relay_down.value(0)
            self.relay_up.value(1)
        elif direction == -1:
            self.relay_down.value(1)
            self.relay_up.value(0)
        else:
            self.relay_up.value(1)
            self.relay_down.value(1)


    def move(self, distance):
        """
        Move distance centimeters:
         - Positive distances is up
         - Negative distances is down
         - Zero distances is stop
        """

        # Calculate target height (Assuming we're measuring the distance
        # to the ceiling)
        targetHeight = self.currentHeight() - distance

        # Default to not move in any direction
        direction = 0

        while True:
            # Calculate how far to move
            currentHeightDiff = (self.currentHeight() - targetHeight) * -1

            # Calculate which direction to move in (Assuming that we're
            # measuring the diff height to the ceiling)
            if currentHeightDiff > 1:
                direction = -1
            elif currentHeightDiff < -1:
                direction = 1
            else:
                # Reset the relays
                self.triggerRelay(0)

                # Break the loop
                break

            # Move up or down
            self.triggerRelay(direction)

            # Sleep for a bit
            time.sleep(0.1)

        print("my work is done")