#!/usr/bin/env python
"""
Script used only to calibrate the camera
"""
import rospy
import numpy as np
import cv2 as cv

import include.calibration_lib as calib
import include.yaml_data_handling as yaml_handle

DEBUG = True

# Get Camera ID from ROS Param Server
CAMERA_ID = rospy.get_param("robocheckers/camera_id")


def main():
    cap = cv.VideoCapture(CAMERA_ID)

    mtx, dist = yaml_handle.load_camera_params()

    corners = None

    while corners is None:
        ret, frame = cap.read()

        # Return 0 if camera not found
        if not ret:
            print "Error: Camera not found!"
            rospy.sleep(5)
            cap.release()
            return False

        dst, _ = calib.calibrate_frame(frame, mtx, dist)

        cv.imshow('Frame', dst)
        cv.waitKey(1)

        corners = calib.get_corners(dst)

    cv.destroyAllWindows()

    four_corners, dx, dy = calib.get_chessboard_data(corners)

    center = calib.get_center(four_corners)

    angle = calib.get_angle(four_corners)

    center = np.array(center)
    center = tuple(center.astype(int))

    # Find the apexes of the keaboard and rotate them
    apexes = calib.adjust_corners(four_corners, center, angle, dx, dy)

    # Rewrite old data with new
    yaml_handle.save_chessboard_params(center, angle, apexes)
    center, angle, apexes = yaml_handle.load_chessboard_params()

    dst = calib.rotate_frame(dst, center, angle)
    dst = calib.adjust_frame(dst, apexes)

    # Calculating chessboard lenghts in pixels
    apexes = sorted(apexes, reverse=True, key=lambda x: x[0])
    chessboard_x = abs((apexes[0][0] + apexes[1][0])/2 - (apexes[2][0] +  apexes[3][0])/2)
    apexes = sorted(apexes, reverse=True, key=lambda x: x[1])
    chessboard_y = abs((apexes[0][1] + apexes[1][1])/2 - (apexes[2][1] +  apexes[3][1])/2)
    # Calculating average chessboard len in pixels
    chessboard_px = (chessboard_x + chessboard_y)/2

    print chessboard_x, chessboard_y

    # Save dimensions data
    yaml_handle.save_dimensions_data(dx, dy, chessboard_px)
    yaml_handle.load_dimensions_data()

    if DEBUG:
        cv.imshow('Center', dst)
        cv.waitKey(1)
        print "Press any key to exit."
        cv.waitKey(-1)

    # When function terminated, release the capture and values
    cap.release()
    cv.destroyAllWindows()

if __name__ == '__main__':
    main()
