from rtde_receive import RTDEReceiveInterface as RTDEReceive
import time
import argparse
import sys
import curses
import math


def toRad(d):
    return d/360*2*math.pi

def toDeg(r):
    return r*360/2/math.pi


def mapFromTo(x,a,b,c,d):
   y=(x-a)/(b-a)*(d-c)+c
   return y


def parse_args(args):
    """Parse command line parameters

    Args:
      args ([str]): command line parameters as list of strings

    Returns:
      :obj:`argparse.Namespace`: command line parameters namespace
    """
    parser = argparse.ArgumentParser(
        description="Record data example")
    parser.add_argument(
        "-ip",
        "--robot_ip",
        dest="ip",
        help="IP address of the UR robot",
        type=str,
        default='localhost',
        metavar="<IP address of the UR robot>")
    parser.add_argument(
        "-o",
        "--output",
        dest="output",
        help="data output (.csv) file to write to (default is \"robot_data.csv\"",
        type=str,
        default="robot_data.csv",
        metavar="<data output file>")
    parser.add_argument(
        "-f",
        "--frequency",
        dest="frequency",
        help="the frequency at which the data is recorded (default is 500Hz)",
        type=float,
        default=500.0,
        metavar="<frequency>")

    return parser.parse_args(args)

joint_min=[toRad(-90),toRad(-90-40),toRad(90-90),toRad(-180-80),toRad(-90-90),toRad(0-75)]
joint_max=[toRad(90),toRad(-90+40),toRad(90+40),toRad(-180+110),toRad(-90+90),toRad(0+75)]



def main(stdscr):
    args=sys.argv[1:]
    """Main entry point allowing external calls

    Args:
      args ([str]): command line parameter list
    """

    # Clear and refresh the screen for a blank canvas
    stdscr.clear()
    stdscr.refresh()

    curses.cbreak()
    stdscr.keypad(1)
    stdscr.nodelay(1)

    
    # Start colors in curses
    curses.start_color()
    curses.init_pair(1, curses.COLOR_CYAN, curses.COLOR_BLACK)
    curses.init_pair(2, curses.COLOR_RED, curses.COLOR_BLACK)
    curses.init_pair(3, curses.COLOR_BLACK, curses.COLOR_WHITE)

    
    height, width = stdscr.getmaxyx()
    

    args = parse_args(args)
    dt = 1 / args.frequency
    rtde_r = RTDEReceive(args.ip, args.frequency)
    
    
    key = ''
    i = 0
    try:
        while key != ord('q'):
            key = stdscr.getch()
            stdscr.clear()
            start = time.time()
            
            q=rtde_r.getActualQ()
            #print(q)
            #sys.stdout.write("\r")
            #sys.stdout.write("{:3d} samples.".format(i))
            #sys.stdout.flush()

            stdscr.addstr(0, 0, "test "+str(i), curses.color_pair(1))


            for j in range(6):
                posx=mapFromTo(q[j],joint_min[j],joint_max[j],0,width) #scale to screen width
                for px in range(width):
                    diff=abs(posx-px)
                    poschar=''
                    if diff<3:
                        poschar='.'
                    if diff<2:
                        poschar='i'
                    if diff<0.5:
                        poschar='|'
                        
                    if (len(poschar)>0):
                        stdscr.addstr(2+j, px, poschar, curses.color_pair(1))
                valuetext=str(round(q[j],2))
                if q[j]>=0: #has no negative sign
                    valuetext=" "+valuetext
                stdscr.addstr(2+j, 0, valuetext, curses.color_pair(1))

                
            # Refresh the screen
            stdscr.refresh()


            end = time.time()
            duration = end - start

            if duration < dt:
                time.sleep(dt - duration)
            i += 1

    except KeyboardInterrupt:
        
        print("\nData recording stopped.")
        curses.endwin()
        exit()


if __name__ == "__main__":
    curses.wrapper(main)