#!/usr/bin/env python3
# Suggestion: 	implement odometry as class that is not using the ev3dev.ev3 package
# 				establish value exchange with main driving class via getters and setters

import time
from planet import Direction
import math
import decimal

'''
Documentation:
good and simple explanation on how odometry works:
https://www.youtube.com/watch?v=qsdiIZncgqo

Summary:
axis a (measure distance where wheels have most friction)
calibration factor c = diameter/ticks_per_turn
distance d = ticks wheel * c (calculate for each wheel individually)
vector_length = (d_l + d_r)/2
vector_angle ~= (d_l - d_r)/a      (only applicable for small distances)

Functionality that should be implemented in this class:
- tracking of relative distances
- track the roboters orientation
- maybe have seperate functions for vector length and orientation (only if it is beneficial)
- closest coordinate to relative distance estimation/calculation
'''


def angle2dir(angle):
    if angle % (2 * math.pi) >= 0 and angle % (2 * math.pi) < math.pi * 0.25 or angle % (2 * math.pi) >= 7 / 4 * math.pi and angle % (2 * math.pi) < 2 * math.pi:
        return Direction.NORTH
    elif angle % (2 * math.pi) >= math.pi * 0.25 and angle % (2 * math.pi) < math.pi * 0.5 or angle % (2 * math.pi) >= math.pi * 0.5 and angle % (2 * math.pi) < math.pi * 0.75:
        return Direction.EAST
    elif angle % (2 * math.pi) >= math.pi * 0.75 and angle % (2 * math.pi) < math.pi or angle % (2 * math.pi) >= math.pi and angle % (2 * math.pi) < 5 / 4 * math.pi:
        return Direction.SOUTH
    else:
        return Direction.WEST


def posround(pos):
    return decimal.Decimal(pos/50).quantize(0, rounding=decimal.ROUND_HALF_UP)


class Odometry:
    def __init__(self):  # planet
        self._wheel_axis = 11
        self._distance_per_tick = 0.0488
        self._pi = math.pi
        self._posxy = [0, 0]
        self._distance = [0, 0]
        self._wheel_center = 0
        self._v_length = 0
        self._v_angle = math.radians(Direction.NORTH)

    def reset(self):
        self._distance = [0, 0]
        self._v_length = 0
        self._posxy = [0, 0]

    def angle_set(self, newangle):
        self._v_angle = math.radians(newangle)

    def angle_get(self):
        return angle2dir(self._v_angle)

    def coordinates_get(self):
        return ((posround(self._posxy[0]), posround(self._posxy[1])), angle2dir(self._v_angle))

    def pos_update(self, ticks_wheel):
        self._distance[0] = ticks_wheel[0] * self._distance_per_tick
        self._distance[1] = ticks_wheel[1] * self._distance_per_tick
        self._v_length = self._v_length + (self._distance[0] + self._distance[1])/2
        self._v_angle = self._v_angle + (self._distance[0] - self._distance[1])/self._wheel_axis
        self._wheel_center = (self._distance[0] + self._distance[1])/2
        self.coordinates_update()

    def coordinates_update(self):  # worked as function outside the class
        self._posxy[0] = self._posxy[0] + self._wheel_center * math.sin(self._v_angle)
        self._posxy[1] = self._posxy[1] + self._wheel_center * math.cos(self._v_angle)

    def dir2ticks(self, destdir):  # return amount of ticks to turn to a given direction (current direction should be _v_angle)
        return (destdir - (math.degrees(self._v_angle) % 360)) * 2