#!/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

'''
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
'''


class Odometry:

    def __init__(self, planet):
      self._wheel_axis = 11
      self._distance_per_tick= 0.0488
      self._pi = 3.141

        self.distance = 0


      #worked fine outside the class as function in nano, maybe we must fix it a bit :)
    def degree(self, delta_rotation_l, delta_rotation_r,star_Direc): #delta_rotation_l/r should work with getmovement method
        self.alpha = 0 #should be start_Direc
        self.alpha=star_Direc
        while True:
         self.alpha = ((self.delta_rotation_l * self._distance_per_tick - self.delta_rotation_r * self._distance_per_tick)/self._wheel_axis) + self.alpha
         self.distance=(self.delta_rotation_r * self._distance_per_tick + self.delta_rotation_l * self._distance_per_tick)/2 + self.distance
         if self.alpha % (2 * self._pi) >=0 and self.alpha % (2 * pi) <pi*0.25 or self.alpa % (2 * pi ) >= 7/4*pi and self.alpha %(2 * pi) <2*pi:
             return(Direction.NORTH)

         elif self.alpha % (2 * self._pi) >= self._pi*0.25 and self.alpha % ( 2 * pi ) < self._pi*0.5 or self.alpha % (2 * self._pi ) >= self._pi * 0.5 and self.alpha % (2 * self._pi ) < self._pi *0.75:
             return(Direction.EAST)

         elif self.alpha % (2 * self._pi)>=self._pi * 0.75 and self.alpha % (2* self._pi)< self._pi or self.alpha % (2 * self_pi) >=self._pi and self.alpha < self.alpha * 5/4 pi :
             return(Direction.SOUTH)
         else:
             return(Direction.WEST)

    def coordinates(self,delta_rota_l,delta_rota_r,Y_koord,X_koord): # worked as function outside the class
        self.y= self.Y_koord
        self.x= self.X_koord
        self.wheel_center = (delta_rota_r + delta_rota_l) /2
        self.alpha = ((self.delta_rotation_l * self._distance_per_tick - self.delta_rotation_r * self._distance_per_tick)/self._wheel_axis) + self.alpha
        self.y = self.y + wheel_center * math.cos(alpha)
        self.x = self.x + wheel_center * math.sin(alpha)
        return(self.x,self.y)