#!/usr/bin/env python
# coding: utf-8

import requests as rq
import sys
import time
import binascii
import numpy as np
rq.packages.urllib3.connection.HTTPConnection.default_socket_options = [(6,3,1)]

# global position
#path = "/golem/database/www/plot_app/plot_app/"
# path = "/tmp/" #Prozatim pro Vojtu 1222
# position = int(np.loadtxt(f'rad_position')) # -1 mm


address=31 # motor otoceni adresa 31, motor vertikalni pozice adresa 32
def post(index,subindex,value):
        rq.post(f'http://192.168.2.{address}/od/{index}/{subindex}',data=f'{value}',
                    headers={'Content-Type': 'application/x-www-form-urlencoded'})
        time.sleep(0.001)


def get(index,subindex,show):
        output = rq.get(f'http://192.168.2.{address}/od/{index}/{subindex}').text
        if(show==True):
            print(f'object: {index}:{subindex}-------------')
            print('hexadec: '+ str(output))
            print('decimal:' + str(int(output[1:-1],16)))
            print('binary: '+ str(bin(int(output[1:-1], 16))[2:]))
        time.sleep(0.001)
        return output

def inic():
    post("2030","00",'"00000001"') # motor vert. pozice ma 000000C8, motor otoceni ma 00000001
    time.sleep(0.01)
    post("2031","00",'"000003E8"')
    time.sleep(0.01)
    post("6075","00",'"000001F4"')
    time.sleep(0.01)
    post("3202","00",'"00000008"')
    time.sleep(0.01)

def shut_down():
    inic()
    post("6040","00",'"0000"')                  # turn everything off
    post("60FE","01",'"00000001"')              # brake on
    post("6040","00",'"0080"') # remove error


def slow_rotation(t : float, direction : str):
    """
    Runs the motor with the slowest possible velocity for the specified time in seconds.

    Args:
        t (float): Duration for which the motor should run, in seconds.
        direction (str): Direction of motor rotation. 
            - 'cw': clockwise (when viewed from the top)
            - 'acw': anticlockwise (when viewed from the top)
    """
    # inic()  # už jen jednou při startu ??? (ask Petr) #TODO TRY to uncomment it, maybe run it only once at the beginning
    time.sleep(0.1)

    post("60FE", "01", '"00000000"')  # brake off
    time.sleep(0.1)
    post("6060", "00", '"03"')        # velocity mode
    time.sleep(0.1)
    post("6042", "00", '"0001"')    # ← odstranit, to brzdí dynamiku # TODO decide - this recommendation was from AI 
    post("606D", "00", '"0001"')
    time.sleep(0.1)
    post("606E", "00", '"0010"')
    time.sleep(0.1)
    post("60FF", "00", '"00000001"')  # teď už platí opravdu tato rychlost v = "00000001" (nejpomalejší možná) # TODO make a variable for the speed
    time.sleep(0.1)

    if direction == 'acw':
        post("607E", "00", '"40"')
    elif direction == 'cw':
        post("607E", "00", '"00"')
    else:
        raise ValueError("Invalid direction. Set direction to 'acw' or 'cw'.")

    time.sleep(0.1)

    post("6040", "00", '"0006"')
    time.sleep(0.1)
    get("6041", "00", False)
    post("6040", "00", '"0007"')
    time.sleep(0.1)
    post("6040", "00", '"000F"')
    time.sleep(0.5)
    time.sleep(t)

    post("6040", "00", '"0000"')      # stop
    time.sleep(0.01)
    post("60FE", "01", '"00000001"')  # brake on
    time.sleep(0.01)
    post("6040", "00", '"0080"')      # clear error


if __name__ == "__main__":
    print("Running radial_control.py")
    # If this script is run directly, the following code will be executed:
    
    # Example usage:
    # slow_rotation(0.5, 'cw') # This will run the motor with the slowest possible velocity for 0.5 seconds in the clockwise direction.
    # slow_rotation(0.5, 'acw') # This will run the motor with the slowest possible velocity for 0.5 seconds in the anticlockwise direction.
    
    # usage
    slow_rotation(0.2, 'cw') # This will run the motor with the slowest possible velocity for 0.5 seconds in the clockwise direction.