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(est_pose):
    pass

def inch_closer():
    pass

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

    noah = robot.Robot()

    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 drive_until_stopped(drive_time) == drive_time:
                # drive around
                continue
            inch_closer()
            break


if __name__ == "__main__":
    main()