import argparse 


import time
import math
import numpy as np
import curses



from helpfunctions import *
from ui import UI
from oscsend import OSCSend
from oscreceive import OSCReceive
from ur import UR


#Looptimings
frequency_loop=500






 
joint_min=[toRad(-360),toRad(-360),toRad(-360),toRad(-360),toRad(-360),toRad(0)]
joint_max=[toRad(360),toRad(360),toRad(360),toRad(360),toRad(360),toRad(360)]


ui = UI(joint_min,joint_max)

oscsend = OSCSend()
oscreceive = None
ur = UR(joint_min,joint_max)


def main(stdscr):

    ui.begin(stdscr)

    
    loop_dt = 1 / frequency_loop
    
    
    
    
    loopcounter = 0
    loopdurations = np.zeros(100)
    loopdurations_pos=0
    
    
    keyDown=False
    
    try:
        while ui.getKey() != ord('q'):
            looptime = time.time()
            
            start = time.time()
            
            ur.receiveJoints(looptime)
            #ur.receiveStatus(looptime)

            #send data to all osc receivers
            oscsend.send(looptime,ur.getReceivedQ(),ur.getReceivedTCPPose())

            if ur is not None:
                ur.send(looptime)

            #Display
            ui.update(ur.getReceivedQ(),looptime)
                
            #Looptiming
            end = time.time()
            loopduration = end - start
            loopdurations[loopdurations_pos]=loopduration
            loopdurations_pos+=1
            loopdurations_pos%=np.size(loopdurations)

            ui.setDebugtext(0,"loopcounter="+str(loopcounter)+"\tControlEnabled="+str(ur.isControlEnabled()))
            ui.setDebugtext(1,"loop usage="+str(round(np.mean(loopdurations)/loop_dt,3)))
            if oscreceive is not None:
                ui.setDebugtext(2,"osc receive frequency="+str(round(oscreceive.getCurrentReceiveFrequency(),0)))
            #ui.setDebugtext(3,"invkin"+str(ur.getForwardKinematics()))
            tcp=ur.getReceivedTCPPose()
            tcp_deg=[tcp[0],tcp[1],tcp[2],toDeg(tcp[3]),toDeg(tcp[4]),toDeg(tcp[5])]
            #ui.setDebugtext(3,"osc test:"+str(oscreceive.getDebug())+"\t tcp_deg="+str(tcp_deg))
            ui.setDebugtext(3,"osc test:"+str(oscreceive.getDebug()))
            
            if ui.getKey() == ord('e') and keyDown==False:
                keyDown=True
                ur.enableControl()
            if ui.getKey() == ord('d') and keyDown==False:
                keyDown=True
                ur.disableControl()
            if ui.getKey()==-1:
                keyDown=False
            
            
            

            if loopduration < loop_dt:
                time.sleep(loop_dt - loopduration)
            loopcounter += 1

    except (KeyboardInterrupt):        
        print("\nKeyboardInterrupt")
    except Exception as e:
        pass
    curses.endwin()
        

if __name__ == "__main__": 
    parser = argparse.ArgumentParser() 
    parser.add_argument("--listenip",
        default="127.0.0.1", help="The ip to listen on")
    parser.add_argument("--listenport",
        type=int, default=5005, help="The port to listen on")

    parser.add_argument("--oscsendip",
        default="127.0.0.1", help="The ip to connect to")
    parser.add_argument("--oscsendport",
        type=int, default=5005, help="The port to connect to")
    

    parser.add_argument("--robotip",
        default="127.0.0.1", help="The ip to connect to")
    
    parser.add_argument("--test", action='store_true', default=False, help="Disable network and robot communication")
    
    
    '''
    parser.add_argument(
        "-fs",
        "--frequencysend",
        dest="frequencysend",
        help="the frequency at which the robot control data is send",
        type=float,
        default=500.0,
        metavar="<frequency>")

    parser.add_argument(
        "-fr",
        "--frequencyreceive",
        dest="frequencyreceive",
        help="the frequency at which the robot actualq data is received",
        type=float,
        default=500.0,
        metavar="<frequency>")
    '''
    
    args = parser.parse_args()


    
    if args.test is not True:
        ur.connect(args.robotip)

        oscreceive = OSCReceive(args.listenip, args.listenport,oscsend,ur)    

        oscsend.setupReceivers([args.oscsendip],args.oscsendport)

        

    curses.wrapper(main)

    if oscreceive is not None:
        oscreceive.disconnect()
    if ur is not None:
        ur.disconnect()