import time
import numpy as np
import cv2

import robot

LANDMARKS = [7,11,10,6]

LANDMARK_POSITIONS = {
    7: [0,0],
    11: [300,0],
    10: [0,400],
    6: [300,400]
}

POWER = 70

TURN_T = 7.9 # 1 degree
DRIVE_T = 22 # 1 centimeter

RIGHT_WHEEL_OFFSET = 4

CLOCKWISE_OFFSET = 0.96

FOCAL_LENGTH = 1691

CAMERA_MATRIX = np.array(
    [[FOCAL_LENGTH,0,512],[0,FOCAL_LENGTH,360],[0,0,1]],
    dtype=np.float32
)
DIST_COEF = np.array([0,0,0,0,0], dtype=np.float32)

def look_around():
    pass

def turn_towards_landmark(est_pose, landmark):
    pass

def time_to_landmark(est_pose, landmark):
    """Kør indenfor 1 meter"""
    pass

def drive_until_stopped(noah):
    noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
    start = time.time()
    while True:
        forward_dist = noah.read_front_sensor()
        if forward_dist < 1000:
            noah.stop()
            break

    end = time.time()
    return end - start

def drunk_drive(noah):
    pass

def inch_closer(noah):
    noah.go_diff(POWER, POWER+RIGHT_WHEEL_OFFSET, 1, 1)
    while True:
        forward_dist = noah.read_front_sensor()
        if forward_dist < 300:
            noah.stop()
            break

def main():
    landmark_order = LANDMARKS + [
        LANDMARKS[0]
    ]

    noah = robot.Robot() # Noah er vores robots navn

    for landmark in landmark_order:
        while True:
            est_pose = look_around()
            est_pose = turn_towards_landmark(est_pose, landmark)
            drive_time = time_to_landmark(est_pose, landmark)
            if not abs(drive_until_stopped(noah) - drive_time) < 0.5:
                drunk_drive(noah)
                continue
            inch_closer(noah)
            break


if __name__ == "__main__":
    main()