#!/usr/bin/python
# -*- coding: UTF-8 -*-
# Copyright (c) 2016, Universal Robots A/S,
# All rights reserved.
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#    * Redistributions of source code must retain the above copyright
#      notice, this list of conditions and the following disclaimer.
#    * Redistributions in binary form must reproduce the above copyright
#      notice, this list of conditions and the following disclaimer in the
#      documentation and/or other materials provided with the distribution.
#    * Neither the name of the Universal Robots A/S nor the names of its
#      contributors may be used to endorse or promote products derived
#      from this software without specific prior written permission.
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY
# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

import argparse
import logging
import sys
import time
import threading
import thread
import signal
import os
import socket
import datetime
from time import sleep, ctime 
sys.path.append('..')
import rtde.rtde as rtde
import rtde.rtde_config as rtde_config
import rtde.csv_writer as csv_writer
import rtde.csv_binary_writer as csv_binary_writer
keep_running = True
timestart = time.time()
def record_ur_data(host, port, csvfilename):
    #parameters
    parser = argparse.ArgumentParser()
    parser.add_argument('--host', default=host, help='name of host to connect to (localhost)')
    parser.add_argument('--port', type=int, default=30004, help='port number (30004)')
    parser.add_argument('--samples', type=int, default=0, help='number of samples to record')
    parser.add_argument('--frequency', type=int, default=125, help='the sampling frequency in Herz')
    parser.add_argument('--config', default='record_configuration.xml', help='data configuration file to use (record_configuration.xml)')
    parser.add_argument('--output', default='robot_data.csv', help='data output file to write to (robot_data.csv)')
    parser.add_argument("--verbose", help="increase output verbosity", action="store_true")
    parser.add_argument("--binary", help="save the data in binary format", action="store_true") 
    args = parser.parse_args()
    args.output = csvfilename
    args.host = host
    args.port = port

    if args.verbose:
        logging.basicConfig(level=logging.INFO)

    conf = rtde_config.ConfigFile(args.config)
    output_names, output_types = conf.get_recipe('out')

    con = rtde.RTDE(args.host, args.port)
    con.connect()

    # get controller version
    con.get_controller_version()

    # setup recipes
    if not con.send_output_setup(output_names, output_types, frequency = args.frequency):
        logging.error('Unable to configure output')
        sys.exit()

    #start data synchronization
    if not con.send_start():
        logging.error('Unable to start synchronization')
        sys.exit()

    writeModes = 'wb' if args.binary else 'w'
    with open(args.output, writeModes) as csvfile:
        writer = None

        if args.binary:
            writer = csv_binary_writer.CSVBinaryWriter(csvfile, output_names, output_types)
        else:
            writer = csv_writer.CSVWriter(csvfile, output_names, output_types)

        writer.writeheader()
        
        i = 1
        global keep_running
         
        while keep_running:
            timestamp = (time.time() - timestart)*1000
            if i%args.frequency == 0:
                if args.samples > 0:
                    sys.stdout.write("\r")
                    sys.stdout.write("{:.2%} done.".format(float(i)/float(args.samples))) 
                    sys.stdout.flush()
                else:
                    sys.stdout.write("\r")
                    sys.stdout.write("{:3d} samples.".format(i)) 
                    sys.stdout.flush()
            if args.samples > 0 and i >= args.samples:
                keep_running = False
            try:
                state = con.receive(args.binary)
                if state is not None:
                    writer.writerow(state, timestamp)
                else:
                    sys.exit()
            except KeyboardInterrupt:
                keep_running = False
            i += 1

    sys.stdout.write("\rComplete!            \n")

    con.send_pause()
    con.disconnect()

def record_ft_sensor(host, port, filename):
    global keep_running
    try:
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        s.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
        s.settimeout(1)
        s.connect((host, port))
        f = open (filename, "w")
        timestart = time.time() #datetime.datetime.now().strftime('%Y%m%d%H%M%S%f')
        try:
            print("Writting in "+ filename + ", Press ctrl + c to stop")
            
            while keep_running:
                timestamp = (time.time() - timestart)*1000#datetime.datetime.now().strftime('%Y%m%d%H%M%S%f')# - timestart
                data = s.recv(1024)
                data = data.replace("(","")
                data = data.replace(")","\n")
                data = str(timestamp) +' , ' + data
                #print(data)
                f.write(data)
        except KeyboardInterrupt:
		    f.close
		    s.close
    except:
        print ('No ft sensor connection')
        
class myThread (threading.Thread):   #继承父类threading.Thread
    def __init__(self, host, port, csvfilename):
        threading.Thread.__init__(self)
        self.HOST = host
        self.PORT = port
        self.CSVFINENAME = csvfilename
    def run(self):                   #把要执行的代码写到run函数里面 线程在创建后会直接运行run函数 
        print "Starting " + self.CSVFINENAME
        record_ur_data(self.HOST,self.PORT, self.CSVFINENAME)
        print "Exiting " + self.CSVFINENAME



def sig_handler(signum, frame):
        global keep_running
        keep_running = False
        print("Ctrl+C")
        os.kill(os.getpid(),signal.SIGKILL)
        sys.exit(0)


def main():
    print ('start:', ctime()) 
    HOST_LEFT = '192.168.1.101'
    HOST_RIGHT = '192.168.1.102'
    UR_PORT = 30004
    FT_PORT = 63351 # The same port as used by the server
    now = time.strftime("%Y-%m-%d-%H_%M_%S",time.localtime(time.time())) 
    Left_UR_CSV="./data/"+now+r"_ur_left.csv"
    Right_UR_CSV="./data/"+now+r"_ur_right.csv"

    Left_FT_CSV="./data/"+now+r"_ft_left.csv"
    Right_FT_CSV="./data/"+now+r"_ft_right.csv"
    
    signal.signal(signal.SIGINT, sig_handler)
    signal.signal(signal.SIGTERM, sig_handler)
    
    # 创建新线程
    thread_left = myThread(HOST_LEFT, UR_PORT, Left_UR_CSV)
    thread_right = myThread(HOST_RIGHT, UR_PORT, Right_UR_CSV)
 
    # 开启线程
    thread_left.start()
    thread_right.start()

    
    thread.start_new_thread(record_ft_sensor, (HOST_LEFT, FT_PORT, Left_FT_CSV)) 
    thread.start_new_thread(record_ft_sensor, (HOST_RIGHT, FT_PORT, Right_FT_CSV)) 
   
    
    global keep_running
    while keep_running:
        try:
            sleep(10)
        except KeyboardInterrupt:
            print "Ctrl+C"
            keep_running = False
    print ('all end:', ctime())    
    print "Exiting Main Thread"
    
    

if __name__ == '__main__':
    main()
    # print(__name__)


