package jp.co.yaskawa.rtc.integratedMotion;
// -*- Java -*-
/*!
 * @file  IntegratedMotionImpl.java
 * @brief ${rtcParam.description}
 * @date  $Date$
 *
 * $Id$
 */

import java.io.BufferedWriter;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileWriter;
import java.io.IOException;
import java.util.Arrays;
import java.util.Properties;
import java.util.InvalidPropertiesFormatException;

import jp.co.yaskawa.rtc.integratedMotion.controller.AlarmSeqHolder;
import jp.co.yaskawa.rtc.integratedMotion.controller.CarPosWithElbowHolder;
import jp.co.yaskawa.rtc.integratedMotion.controller.FrameType;
import jp.co.yaskawa.rtc.integratedMotion.controller.RedundantAxesMask;
import jp.co.yaskawa.rtc.integratedMotion.controller.RedundantLimit;
import jp.co.yaskawa.rtc.integratedMotion.controller.ReturnID;
import jp.co.yaskawa.rtc.integratedMotion.controller.RightLeft;
import jp.co.yaskawa.rtc.integratedMotion.controller.UnitType;
import jp.co.yaskawa.rtc.integratedMotion.controller.LimitValue;
import jp.co.yaskawa.rtc.integratedMotion.module.UnitOperation;

import jp.co.yaskawa.rtc.integratedMotion.armService.ManipulatorCommonInterface_Common;
import jp.co.yaskawa.rtc.integratedMotion.module.IntegratedMotionInterpolator;
import jp.co.yaskawa.rtc.integratedMotion.RTC.RETURN_ID;
import jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RedundancyConfig;

import jp.co.yaskawa.rtc.integratedMotion.RTC.TimedDoubleSeq;
import jp.co.yaskawa.rtc.integratedMotion.RTC.Time;
import jp.co.yaskawa.rtc.integratedMotion.RTC.TimedJointPos;
import jp.go.aist.rtm.RTC.DataFlowComponentBase;
import jp.go.aist.rtm.RTC.Manager;
import jp.go.aist.rtm.RTC.port.ConnectorDataListenerT;
import jp.go.aist.rtm.RTC.port.ConnectorDataListenerType;
import jp.go.aist.rtm.RTC.port.InPort;
import jp.go.aist.rtm.RTC.port.OutPort;
import jp.go.aist.rtm.RTC.util.DataRef;
import jp.go.aist.rtm.RTC.port.CorbaConsumer;
import jp.go.aist.rtm.RTC.port.CorbaPort;
import jp.go.aist.rtm.RTC.port.ConnectorBase.ConnectorInfo;

import org.omg.CORBA.IntHolder;
import org.omg.PortableServer.POAPackage.ObjectNotActive;
import org.omg.PortableServer.POAPackage.ServantAlreadyActive;
import org.omg.PortableServer.POAPackage.WrongPolicy;
import jp.go.aist.rtm.RTC.util.StringHolder;
import RTC.ReturnCode_t;

/*!
 * @class IntegratedMotionImpl
 * @brief ${rtcParam.description}
 *
 */
public class IntegratedMotionImpl extends DataFlowComponentBase implements UnitOperation{

    private RedundantAxesMask redundantAxesMask_ = null; //!< 冗長軸の指定情報
    private RedundantLimit redundantLimit_ = null; //!< 冗長軸のリミット値情報
    private boolean doSendRetry = false; //!< 再送信フラグ 
    Properties prop = new Properties();
    int segmentPeriod = 0; //!< セグメント周期[ms]
    double[] cartesianFeedback_;
    double[] kinematicsResult_;
    double[] redundantFeedback_;
    double[] armR_jointFeedback_;
    double[] armL_jointFeedback_;
    double[] lumbar_jointFeedback_;
    double[] vehicle_Feedback_;
    
    boolean isConnectArmR_;
    boolean isConnectArmL_;
    boolean isConnectLumbar_;
    boolean isConnectVehicle_;
    
    boolean isServoOn_;
    
    File logCarPos;
    BufferedWriter outCarPos;
	
  /*!
   * @brief constructor
   * @param manager Maneger Object
   */
	public IntegratedMotionImpl(Manager manager) {  
        super(manager);
        // <rtc-template block="initializer">
        m_arm_cartesian_fb_in_val = new TimedDoubleSeq();
        m_arm_cartesian_fb_in = new DataRef<TimedDoubleSeq>(m_arm_cartesian_fb_in_val);
        m_arm_cartesian_fb_inIn = new InPort<TimedDoubleSeq>("arm_cartesian_fb_in", m_arm_cartesian_fb_in);
        m_iKinema_result_in_val = new TimedDoubleSeq();
        m_iKinema_result_in = new DataRef<TimedDoubleSeq>(m_iKinema_result_in_val);
        m_iKinema_result_inIn = new InPort<TimedDoubleSeq>("iKinema_result_in", m_iKinema_result_in);
        m_redundant_fb_in_val = new TimedDoubleSeq();
        m_redundant_fb_in = new DataRef<TimedDoubleSeq>(m_redundant_fb_in_val);
        m_redundant_fb_inIn = new InPort<TimedDoubleSeq>("redundant_fb_in", m_redundant_fb_in);
        m_joint_fb_in_val = new TimedDoubleSeq();
        m_joint_fb_in = new DataRef<TimedDoubleSeq>(m_joint_fb_in_val);
        m_joint_fb_inIn = new InPort<TimedDoubleSeq>("joint_fb_in", m_joint_fb_in);
        m_status_in_val = new TimedDoubleSeq();
        m_status_in = new DataRef<TimedDoubleSeq>(m_status_in_val);
        m_status_inIn = new InPort<TimedDoubleSeq>("status_in", m_status_in);
        m_armR_joint_cmd_out_val = new TimedJointPos();
        m_armR_joint_cmd_out = new DataRef<TimedJointPos>(m_armR_joint_cmd_out_val);
        m_armR_joint_cmd_outOut = new OutPort<TimedJointPos>("armR_joint_cmd_out", m_armR_joint_cmd_out);
        m_armL_joint_cmd_out_val = new TimedJointPos();
        m_armL_joint_cmd_out = new DataRef<TimedJointPos>(m_armL_joint_cmd_out_val);
        m_armL_joint_cmd_outOut = new OutPort<TimedJointPos>("armL_joint_cmd_out", m_armL_joint_cmd_out);
        m_lumbar_cmd_out_val = new TimedJointPos();
        m_lumbar_cmd_out = new DataRef<TimedJointPos>(m_lumbar_cmd_out_val);
        m_lumbar_cmd_outOut = new OutPort<TimedJointPos>("lumbar_cmd_out", m_lumbar_cmd_out);
        m_vehicle_cmd_out_val = new TimedDoubleSeq();
        m_vehicle_cmd_out = new DataRef<TimedDoubleSeq>(m_vehicle_cmd_out_val);
        m_vehicle_cmd_outOut = new OutPort<TimedDoubleSeq>("vehicle_cmd_out", m_vehicle_cmd_out);
        m_arm_cartesian_cmd_out_val = new TimedDoubleSeq();
        m_arm_cartesian_cmd_out = new DataRef<TimedDoubleSeq>(m_arm_cartesian_cmd_out_val);
        m_arm_cartesian_cmd_outOut = new OutPort<TimedDoubleSeq>("arm_cartesian_cmd_out", m_arm_cartesian_cmd_out);
        m_redundant_cmd_out_val = new TimedDoubleSeq();
        m_redundant_cmd_out = new DataRef<TimedDoubleSeq>(m_redundant_cmd_out_val);
        m_redundant_cmd_outOut = new OutPort<TimedDoubleSeq>("redundant_cmd_out", m_redundant_cmd_out);
        m_controllerPort = new CorbaPort("controller");
        m_toRedundancyPort = new CorbaPort("toRedundancy");
        m_armL_servicePort = new CorbaPort("armL_service");
        m_armR_servicePort = new CorbaPort("armR_service");
        // </rtc-template>
        redundantAxesMask_ = new RedundantAxesMask();
        redundantAxesMask_.allEnable();
        
        redundantLimit_ = new RedundantLimit();
        redundantLimit_.rightElbow = new LimitValue();
        redundantLimit_.leftElbow = new LimitValue();
        redundantLimit_.lumbarRx = new LimitValue();
        redundantLimit_.lumbarRy = new LimitValue();
        redundantLimit_.lumbarRz = new LimitValue();
        redundantLimit_.vehicleX = new LimitValue();
        redundantLimit_.vehicleY = new LimitValue();
        redundantLimit_.vehicleTheta = new LimitValue();
        redundantLimit_.rightElbow.upper = 90.0;
        redundantLimit_.rightElbow.lower = -90.0;
        redundantLimit_.leftElbow.upper = 90.0;
        redundantLimit_.leftElbow.lower = -90.0;
        redundantLimit_.lumbarRx.upper = 90.0;
        redundantLimit_.lumbarRx.lower = 0.0;
        redundantLimit_.lumbarRy.upper = 90.0;
        redundantLimit_.lumbarRy.lower = 0.0;
        redundantLimit_.lumbarRz.upper = 90.0;
        redundantLimit_.lumbarRz.lower = 0.0;
        redundantLimit_.vehicleX.upper = 0.0;
        redundantLimit_.vehicleX.lower = 0.0;
        redundantLimit_.vehicleY.upper = 0.0;
        redundantLimit_.vehicleY.lower = 0.0;
        redundantLimit_.vehicleTheta.upper = 90.0;
        redundantLimit_.vehicleTheta.lower = -90.0;

        isConnectArmR_ = false;
        isConnectArmL_ = false;
        isConnectLumbar_ = false;
        isConnectVehicle_ = false;
        
        isServoOn_ = false;

        m_armR_joint_cmd_out.v.pos = new double[8];
        m_armL_joint_cmd_out.v.pos = new double[8];
        m_lumbar_cmd_out.v.pos = new double[3];
        m_vehicle_cmd_out.v.data = new double[3];
        m_arm_cartesian_cmd_out.v.data = new double[24];
        m_redundant_cmd_out.v.data = new double[8];
        
        m_armR_joint_cmd_out.v.tm = new Time(0,0);
        m_armL_joint_cmd_out.v.tm = new Time(0,0);
        m_lumbar_cmd_out.v.tm = new Time(0,0);
        m_vehicle_cmd_out.v.tm = new Time(0,0);
        m_arm_cartesian_cmd_out.v.tm = new Time(0,0);
        m_redundant_cmd_out.v.tm = new Time(0,0);
/*        
        m_iKinema_result_inIn.addConnectorDataListener(
        		ConnectorDataListenerType.ON_BUFFER_WRITE,
        		new DataListener("ON_BUFFER_WRITE")
        		);
*/
    }

    /*!
     *
     * The initialize action (on CREATED->ALIVE transition)
     * formaer rtc_init_entry() 
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
    @Override
    protected ReturnCode_t onInitialize() {
        // Registration: InPort/OutPort/Service
        // <rtc-template block="registration">
        // Set InPort buffers
        addInPort("arm_cartesian_fb_in", m_arm_cartesian_fb_inIn);
        addInPort("iKinema_result_in", m_iKinema_result_inIn);
        addInPort("redundant_fb_in", m_redundant_fb_inIn);
        addInPort("joint_fb_in", m_joint_fb_inIn);
        addInPort("status_in", m_status_inIn);
        
        // Set OutPort buffer
        addOutPort("armR_joint_cmd_out", m_armR_joint_cmd_outOut);
        addOutPort("armL_joint_cmd_out", m_armL_joint_cmd_outOut);
        addOutPort("lumbar_cmd_out", m_lumbar_cmd_outOut);
        addOutPort("vehicle_cmd_out", m_vehicle_cmd_outOut);
        addOutPort("arm_cartesian_cmd_out", m_arm_cartesian_cmd_outOut);
        addOutPort("redundant_cmd_out", m_redundant_cmd_outOut);
        
        // Set service provider to Ports
        try {
        	m_controllerPort.registerProvider("toClient", "IntegratedMotionInterface", m_toClient);
        } catch (ServantAlreadyActive e) {
            e.printStackTrace();
        } catch (WrongPolicy e) {
            e.printStackTrace();
        } catch (ObjectNotActive e) {
            e.printStackTrace();
        }
        
        // Set service consumers to Ports
        m_toRedundancyPort.registerConsumer("toRedundancy", "RedundancyConfig", m_redundancyConfigBase);
        m_armL_servicePort.registerConsumer("armService", "ManipulatorCommonInterface_Common", m_armLBase);
        m_armR_servicePort.registerConsumer("armService", "ManipulatorCommonInterface_Common", m_armRBase);
        
        // Set CORBA Service Ports
        addPort(m_controllerPort);
        addPort(m_toRedundancyPort);
        addPort(m_armL_servicePort);
        addPort(m_armR_servicePort);
        // </rtc-template>
        bindParameter("configFile", m_configFile, "default.conf");
        return super.onInitialize();
    }

    /***
     *
     * The finalize action (on ALIVE->END transition)
     * formaer rtc_exiting_entry()
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    protected ReturnCode_t onFinalize() {
//        return super.onFinalize();
//    }

    /***
     *
     * The startup action when ExecutionContext startup
     * former rtc_starting_entry()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    protected ReturnCode_t onStartup(int ec_id) {
//        return super.onStartup(ec_id);
//    }

    /***
     *
     * The shutdown action when ExecutionContext stop
     * former rtc_stopping_entry()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    protected ReturnCode_t onShutdown(int ec_id) {
//        return super.onShutdown(ec_id);
//    }

    /***
     *
     * The activated action (Active state entry action)
     * former rtc_active_entry()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
    @Override
    protected ReturnCode_t onActivated(int ec_id) {
    	System.out.println("onActivated()");
    	
    	try {
    		prop.loadFromXML(new FileInputStream(m_configFile.value));
    	} catch(InvalidPropertiesFormatException e) {
    		System.err.println(e);
    		return ReturnCode_t.RTC_ERROR;
    	} catch(IOException e) {
    		System.err.println(e);
    		return ReturnCode_t.RTC_ERROR;
    	}

    	try {
    		double d = Double.parseDouble(prop.getProperty("segmentPeriod"));
    		segmentPeriod = (int)Math.floor(d * 1000); // セグメント周期．1ミリ秒以下は切り捨て
//    		nJoint = Integer.parseInt(prop.getProperty("nJoint")); // 関節数
//    		nPosture = Integer.parseInt(prop.getProperty("nPosture")); // 位置姿勢データ数
    		
    		// 関節オフセット取得
//    		String keyBase ="offset.";
//    		String key;
//    		offset = new double[nJoint];
//    		for (int i = 0; i < nJoint; i++) {
//    			key = keyBase + i;
//    			offset[i] = Double.parseDouble(prop.getProperty(key));
//    		}
    		
    	} catch(NumberFormatException e) {
    		System.err.println(e);
    		return ReturnCode_t.RTC_ERROR;
    	}    	
    	
    	imInterpolator.setSegmentPeriod((double)segmentPeriod/1000);
   
        if (m_armR == null) {
    		m_armR = m_armRBase._ptr();
    	}    	
    	if (m_redundancyConfig == null) {
    		m_redundancyConfig = m_redundancyConfigBase._ptr();
    	}
        
    	//! TODO
    	isConnectArmR_ = true;

    	//	
    	boolean[] isUnits = new boolean[4]; 
    		
    	isUnits[0] = isConnectArmR_;
    	isUnits[1] = isConnectArmL_;
    	isUnits[2] = isConnectLumbar_;
    	isUnits[3] = isConnectVehicle_;
    	
    	imInterpolator.setConnectUnitsInfo(isUnits);
    	//
    	
    	cartesianFeedback_ = new double[24]; //!< x,y,z,3x3Matrix * 2 (Right & Left)
    	for (int i=0; i<24; i++) {
    		cartesianFeedback_[i] = 0.0;
    	}
    	
    	redundantFeedback_ = new double[2]; //!< right elbow, left elbow
    	for (int i=0; i<2; i++) {
    		redundantFeedback_[i] = 0.0;
    	}

    	kinematicsResult_ = new double[20]; //!< vehicle(x,y,theta),lumbar1,2,3,right arm,left arm
    	for (int i=0; i<20; i++) {
    		kinematicsResult_[i] = 0.0;
    	}

        armR_jointFeedback_ = new double[8]; //!< J1-J7, Gripper
    	for (int i=0; i<armR_jointFeedback_.length; i++) {
    		armR_jointFeedback_[i] = 0.0;
    	}

    	armL_jointFeedback_ = new double[8]; //!< J1-J7, Gripper
    	for (int i=0; i<armL_jointFeedback_.length; i++) {
    		armL_jointFeedback_[i] = 0.0;
    	}
    	
    	lumbar_jointFeedback_ = new double[3]; //!< lumber1,lumber2,lumber3
    	for (int i=0; i<lumbar_jointFeedback_.length; i++) {
    		lumbar_jointFeedback_[i] = 0.0;
    	}

    	vehicle_Feedback_ = new double[3]; //!< x,y,theta
    	for (int i=0; i<vehicle_Feedback_.length; i++) {
    		vehicle_Feedback_[i] = 0.0;
    	}
    	    	
//    	setCallback();
    	
    	imInterpolator.getQ().clear();
    	imInterpolator.getQ().setReadProtect(false);
    	imInterpolator.getQ().setWriteProtect(false);
    	imInterpolator.setPower(true);
    	
    	
		try {
	        logCarPos = new File("logCarPos.dat");    					
			outCarPos = new BufferedWriter(new FileWriter(logCarPos));
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

        return super.onActivated(ec_id);
    }

    /***
     *
     * The deactivated action (Active state exit action)
     * former rtc_active_exit()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
    @Override
    protected ReturnCode_t onDeactivated(int ec_id) {
    	System.out.println("onDeactivated()");
    	
    	try {
			outCarPos.close();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    	
        return super.onDeactivated(ec_id);
    }

    /***
     *
     * The execution action that is invoked periodically
     * former rtc_active_do()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
    @Override
    protected ReturnCode_t onExecute(int ec_id) {
    	//    	if (m_iKinema_result_inIn.isNew()) {
    	//		m_iKinema_result_inIn.read();
    	//	}    	
	
    	m_joint_fb_inIn.read();
    	if(m_joint_fb_in.v.data != null) {
    		for(int i=0; i<7; i++) {
    			armR_jointFeedback_[i] = m_joint_fb_in.v.data[i+6];
    			}
    		}

    	m_arm_cartesian_fb_inIn.read();
    	if(m_arm_cartesian_fb_in.v.data != null) {
    		for(int i=0; i<12; i++) {
    			cartesianFeedback_[i] = m_arm_cartesian_fb_in.v.data[i];
    			}
    		}
	    
    	m_redundant_fb_inIn.read();
    	if(m_redundant_fb_in.v.data != null) {
    		for(int i=0; i<2; i++) {
    			redundantFeedback_[i] = m_redundant_fb_in.v.data[i+6];
    			}
    		}
    	
//    	if (m_iKinema_result_inIn.isNew()) {
//    		System.out.println("k");
//    		sendSegmentData(kinematicsResult_);    		
//    		}    	

    	if (m_iKinema_result_inIn.isNew()) {
    		m_iKinema_result_inIn.read();
    		synchronized (kinematicsResult_) {
    			int i;
    			for (i=0; i<m_iKinema_result_in.v.data.length; i++) {
    				kinematicsResult_[i] = m_iKinema_result_in.v.data[i];
    			}
    			if (i<kinematicsResult_.length) {
    				for (; i<kinematicsResult_.length; i++) {
    					kinematicsResult_[i] = 0.0;
    				}
    			}
    							
    		}
    		sendSegmentData(kinematicsResult_);    		
    		
    	}
    	    	
    	//	sendSegmentData(kinematicsResult_);
    	imInterpolator.doSegment();
	    	
        return super.onExecute(ec_id);
    }

    /***
     *
     * The aborting action when main logic error occurred.
     * former rtc_aborting_entry()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//  @Override
//  public ReturnCode_t onAborting(int ec_id) {
//      return super.onAborting(ec_id);
//  }

    /***
     *
     * The error action in ERROR state
     * former rtc_error_do()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    public ReturnCode_t onError(int ec_id) {
//        return super.onError(ec_id);
//    }

    /***
     *
     * The reset action that is invoked resetting
     * This is same but different the former rtc_init_entry()
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    protected ReturnCode_t onReset(int ec_id) {
//        return super.onReset(ec_id);
//    }

    /***
     *
     * The state update action that is invoked after onExecute() action
     * no corresponding operation exists in OpenRTm-aist-0.2.0
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    protected ReturnCode_t onStateUpdate(int ec_id) {
//        return super.onStateUpdate(ec_id);
//    }

    /***
     *
     * The action that is invoked when execution context's rate is changed
     * no corresponding operation exists in OpenRTm-aist-0.2.0
     *
     * @param ec_id target ExecutionContext Id
     *
     * @return RTC::ReturnCode_t
     * 
     * 
     */
//    @Override
//    protected ReturnCode_t onRateChanged(int ec_id) {
//        return super.onRateChanged(ec_id);
//    }
//
	// Configuration variable declaration
	// <rtc-template block="config_declare">
    /*!
     * 
     * - Name:  configFile
     * - DefaultValue: default.conf
     */
    protected StringHolder m_configFile = new StringHolder();
	// </rtc-template>

    // DataInPort declaration
    // <rtc-template block="inport_declare">
    protected TimedDoubleSeq m_arm_cartesian_fb_in_val;
    protected DataRef<TimedDoubleSeq> m_arm_cartesian_fb_in;
    /*!
     * control point current position (right arm, left arm)
     */
    protected InPort<TimedDoubleSeq> m_arm_cartesian_fb_inIn;

    protected TimedDoubleSeq m_iKinema_result_in_val;
    protected DataRef<TimedDoubleSeq> m_iKinema_result_in;
    /*!
     * inverse kinematics result.
     * (vehicle, lumbar, right arm, left arm position)
     */
    protected InPort<TimedDoubleSeq> m_iKinema_result_inIn;

    protected TimedDoubleSeq m_redundant_fb_in_val;
    protected DataRef<TimedDoubleSeq> m_redundant_fb_in;
    /*!
     * redundant axes(elbow) current postion (theta)
     */
    protected InPort<TimedDoubleSeq> m_redundant_fb_inIn;

    protected TimedDoubleSeq m_joint_fb_in_val;
    protected DataRef<TimedDoubleSeq> m_joint_fb_in;
    /*!
     * axes current position (theta)
     */
    protected InPort<TimedDoubleSeq> m_joint_fb_inIn;

    protected TimedDoubleSeq m_status_in_val;
    protected DataRef<TimedDoubleSeq> m_status_in;
    /*!
     * redundancy module status
     */
    protected InPort<TimedDoubleSeq> m_status_inIn;

    
    // </rtc-template>

    // DataOutPort declaration
    // <rtc-template block="outport_declare">
    protected TimedJointPos m_armR_joint_cmd_out_val;
    protected DataRef<TimedJointPos> m_armR_joint_cmd_out;
    /*!
     * right arm command position
     */
    protected OutPort<TimedJointPos> m_armR_joint_cmd_outOut;

    protected TimedJointPos m_armL_joint_cmd_out_val;
    protected DataRef<TimedJointPos> m_armL_joint_cmd_out;
    /*!
     * left arm command position
     */
    protected OutPort<TimedJointPos> m_armL_joint_cmd_outOut;

    protected TimedJointPos m_lumbar_cmd_out_val;
    protected DataRef<TimedJointPos> m_lumbar_cmd_out;
    /*!
     * lumbar command position
     */
    protected OutPort<TimedJointPos> m_lumbar_cmd_outOut;

    protected TimedDoubleSeq m_vehicle_cmd_out_val;
    protected DataRef<TimedDoubleSeq> m_vehicle_cmd_out;
    /*!
     * vehicle command position
     */
    protected OutPort<TimedDoubleSeq> m_vehicle_cmd_outOut;

    protected TimedDoubleSeq m_arm_cartesian_cmd_out_val;
    protected DataRef<TimedDoubleSeq> m_arm_cartesian_cmd_out;
    /*!
     */
    protected OutPort<TimedDoubleSeq> m_arm_cartesian_cmd_outOut;

    protected TimedDoubleSeq m_redundant_cmd_out_val;
    protected DataRef<TimedDoubleSeq> m_redundant_cmd_out;
    /*!
     * redundant axes command position.
     * (vehicle, lumbar, right elbow, left elbow)
     */
    protected OutPort<TimedDoubleSeq> m_redundant_cmd_outOut;

    
    // </rtc-template>

    // CORBA Port declaration
    // <rtc-template block="corbaport_declare">
    /*!
     */
    protected CorbaPort m_controllerPort;
    /*!
     */
    protected CorbaPort m_toRedundancyPort;
    /*!
     */
    protected CorbaPort m_armL_servicePort;
    /*!
     */
    protected CorbaPort m_armR_servicePort;
    
    // </rtc-template>
    protected IntegratedMotionInterpolator imInterpolator = new IntegratedMotionInterpolator(this);

    // Service declaration
    // <rtc-template block="service_declare">
    /*!
     */
    protected IntegratedMotionInterfaceSVC_impl m_toClient = new IntegratedMotionInterfaceSVC_impl(imInterpolator, this);
    
    // </rtc-template>

    // Consumer declaration
    // <rtc-template block="consumer_declare">
    protected CorbaConsumer<RedundancyConfig> m_redundancyConfigBase = new CorbaConsumer<RedundancyConfig>(RedundancyConfig.class);
    /*!
     */
    protected RedundancyConfig m_redundancyConfig;
    protected CorbaConsumer<ManipulatorCommonInterface_Common> m_armLBase = new CorbaConsumer<ManipulatorCommonInterface_Common>(ManipulatorCommonInterface_Common.class);
    /*!
     */
    protected ManipulatorCommonInterface_Common m_armL;
    protected CorbaConsumer<ManipulatorCommonInterface_Common> m_armRBase = new CorbaConsumer<ManipulatorCommonInterface_Common>(ManipulatorCommonInterface_Common.class);
    /*!
     */
    protected ManipulatorCommonInterface_Common m_armR;

    public ReturnID abort() {
		System.out.println("abort()");
		ReturnID ret = new ReturnID();

		boolean result = false;
		result = imInterpolator.abort();

		if(result)
			ret.id = 0;
		else
			ret.id = -1;
			
		return ret;
		
    }

	public ReturnID clearAlarms() {
		System.out.println("clearAlarms()");
		ReturnID ret = new ReturnID();

		imInterpolator.clearAlarm();
		
		RETURN_ID rtn = new RETURN_ID();
		rtn = m_armR.clearAlarms();
		
		ret.id = rtn.id;
		ret.comment = rtn.comment;
		
		return ret;
	}

	public ReturnID getActiveAlarm(AlarmSeqHolder alarms) {
		// TODO Auto-generated method stub
		return null;
	}

	public ReturnID getFeedbackPosCartesian(RightLeft type,
			FrameType frameType, CarPosWithElbowHolder pos) {
		
		ReturnID ret = new ReturnID(0,"");

		int index = 0;		
		synchronized (cartesianFeedback_) {
			if (type == RightLeft.LEFT) {
				index = 12;
			}			
			pos.value.carPos[0][3] = cartesianFeedback_[index];   //!< x
			pos.value.carPos[1][3] = cartesianFeedback_[++index]; //!< y
			pos.value.carPos[2][3] = cartesianFeedback_[++index]; //!< z
			
			for (int i=0; i<3; i++) {
				for (int j=0; j<3; j++) {
					index++;
					pos.value.carPos[i][j] = cartesianFeedback_[index]; //!< HgMat[i][j]					
				}
			}
			
		}

		synchronized (redundantFeedback_) {
			if (type == RightLeft.LEFT) {
				pos.value.elbow = redundantFeedback_[1];
			} else {
				pos.value.elbow = redundantFeedback_[0];
			}
					
		}
				
		return ret;
		
	}

	public ReturnID getFeedbackPosJoint(UnitType type, double[] pos) {

		ReturnID ret = new ReturnID(0,"");

		if (type == UnitType.RIGHT_ARM) {
			
			for(int i=0; i<7; i++)
			{
				pos[i] = armR_jointFeedback_[i];
			}
		}
		else if (type == UnitType.LEFT_ARM) {
			pos = armL_jointFeedback_;
		}
		else if (type == UnitType.LUMBER) {
			pos = lumbar_jointFeedback_;
		}
		else if (type == UnitType.VEHICLE) {
			pos = vehicle_Feedback_;
		}
		else {
			ret.id = -1;
			ret.comment = "unit type error";			
		}
		
		return ret;
		}

	public void getRedundantPos(double[] redPos) {
		synchronized (redundantFeedback_) {
			System.out.println(redundantFeedback_[0]);
			System.out.println(redundantFeedback_[1]);
			redPos[0] = redundantFeedback_[0]; 
			redPos[1] = redundantFeedback_[1];			
		}		
	}

	public ReturnID getState(IntHolder statusId,
			org.omg.CORBA.StringHolder message) {
		// TODO Auto-generated method stub
		return null;
	}

	public ReturnID getVersion(org.omg.CORBA.StringHolder version) {
		System.out.println("getVersion()");
		version.value = "1.0.0";

		ReturnID ret = new ReturnID();
		ret.id = 0;
		ret.comment = "";
		
		return ret;
	}

	public boolean isMoving() {
		// TODO Auto-generated method stub
		return false;
	}

	public boolean isServoOn() {
		return isServoOn_;
	}

	public ReturnID pause() {
		System.out.println("pause()");
		ReturnID ret = new ReturnID();

		boolean result = false;
		result = imInterpolator.pause();

		if(result)
			ret.id = 0;
		else
			ret.id = -1;
			
		return ret;
	}

	public ReturnID resetOriginalFrame() {
		// TODO Auto-generated method stub
		return null;
	}

	public ReturnID resume() {
		System.out.println("resume()");
		ReturnID ret = new ReturnID();

		boolean result = false;
		result = imInterpolator.resume();

		if(result)
			ret.id = 0;
		else
			ret.id = -1;
			
		return ret;
	}

	public ReturnID selectRedundantAxes(RedundantAxesMask redundancy) {
		
		System.out.println("selectRedundantAxes()");
		ReturnID ret = new ReturnID();
		
		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID rtn = 
			new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID();

		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RedundantAxesMask mask = 
			new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RedundantAxesMask();
		
		mask.elbow = redundancy.elbow;
		mask.lumbarRx = redundancy.lumbarRx;
		mask.lumbarRy = redundancy.lumbarRy;
		mask.lumbarRz = redundancy.lumbarRz;
		mask.vehicleX = redundancy.vehicleX;
		mask.vehicleY = redundancy.vehicleY;
		mask.vehicleTheta = redundancy.vehicleTheta;

		rtn = m_redundancyConfig.selectRedundancy(mask);
		System.out.println(mask.elbow);
		System.out.println(mask.lumbarRx);
		System.out.println(mask.lumbarRy);
		System.out.println(mask.lumbarRz);
		System.out.println(mask.vehicleX);
		System.out.println(mask.vehicleY);
		System.out.println(mask.vehicleTheta);
		
		if(rtn.id != 0) {
			ret.id = rtn.id;
			ret.comment = rtn.comment;
			return ret;
		}
		
		ret.id = rtn.id;
		ret.comment = rtn.comment;
        
		return ret;
	}

	public ReturnID selectRedundantLimit(RedundantLimit limit) {
		
		System.out.println("selectRedundantLimit()");
		ReturnID ret = new ReturnID();
		
		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID rtn = 
			new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID();

		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RedundantLimit lim =
			new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RedundantLimit();
		
		lim.rightElbow = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.leftElbow = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.lumbarRx = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.lumbarRy = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.lumbarRz = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.vehicleX = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.vehicleY = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
		lim.vehicleTheta = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.LimitValue();
        lim.rightElbow.upper = limit.rightElbow.upper;
        lim.rightElbow.lower = limit.rightElbow.lower;
        lim.leftElbow.upper = limit.leftElbow.upper;
        lim.leftElbow.lower = limit.leftElbow.lower;
        lim.lumbarRx.upper = limit.lumbarRx.upper;
        lim.lumbarRx.lower = limit.lumbarRx.lower;
        lim.lumbarRy.upper = limit.lumbarRy.upper;
        lim.lumbarRy.lower = limit.lumbarRy.lower;
        lim.lumbarRz.upper = limit.lumbarRz.upper;
        lim.lumbarRz.lower = limit.lumbarRz.lower;
        lim.vehicleX.upper = limit.vehicleX.upper;
        lim.vehicleX.lower = limit.vehicleX.lower;
        lim.vehicleY.upper = limit.vehicleY.upper;
        lim.vehicleY.lower = limit.vehicleY.lower;
        lim.vehicleTheta.upper = limit.vehicleTheta.upper;
        lim.vehicleTheta.lower = limit.vehicleTheta.lower;

		rtn = m_redundancyConfig.setRedundantAxesLimit(lim);
		
		ret.id = rtn.id;
		ret.comment = rtn.comment;
        
		return ret;
	}
	
	public ReturnID servoOff() {
		System.out.println("servoOff()");
		ReturnID ret = new ReturnID();

		RETURN_ID rtn = new RETURN_ID(0,"");

		rtn = m_armR.servoOFF();

		if(rtn.id == 0) {
			imInterpolator.setServo(false);
			imInterpolator.setAmp(false);			
			imInterpolator.getQ().clear();
			isServoOn_ = false;
		}		
		
		ret.id = rtn.id;
		ret.comment = rtn.comment;

		return ret;
	}

	public ReturnID servoOn() {
		System.out.println("servoOn()");
		ReturnID ret = new ReturnID();

		imInterpolator.getQ().clear();
		
		RETURN_ID rtn = new RETURN_ID(0,"");

		rtn = m_armR.servoON();
		
		if(rtn.id == 0) {
			imInterpolator.setAmp(true);
			imInterpolator.setServo(true);
			imInterpolator.getQ().clear();
			isServoOn_ = true;
		}
		
		ret.id = rtn.id;
		ret.comment = rtn.comment;
		
		return ret;
	}

	/**
	 * アームの手先目標位置・姿勢（回転行列）を冗長性利用モジュールに出力する
	 * @param cartesianPos 左右アームの目標位置・姿勢
	 */	
	public void setCartesianCommand(double[] cartesianPos) {
		m_arm_cartesian_cmd_out_val.data = Arrays.copyOfRange(cartesianPos, 0, 24);
		m_arm_cartesian_cmd_out_val.tm = new Time(0,0);
		
		if (false) {
			for (double dd: m_arm_cartesian_cmd_out_val.data) {
				try {
					outCarPos.write(String.valueOf(dd) + ",");
				} catch (IOException e) {
					e.printStackTrace();
				}
			}
			
			try {
				outCarPos.newLine();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			
		}
		m_arm_cartesian_cmd_outOut.write();
	}

    /**
     * 制御点のオフセット量を指定する
     * @param [in] type 右or左アーム
     * @param [in] offset オフセット量（同次変換行列 3x4）
     */		
	public ReturnID setControlPointOffset(RightLeft type, double[][] offset) {

		ReturnID ret = new ReturnID();
		
		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RightLeft rl;
		
		if(type == RightLeft.RIGHT) {
			rl = jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RightLeft.RIGHT;
		} else {
			rl = jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RightLeft.LEFT;
		}
		
		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID rid = 
			new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID();
		
		rid = m_redundancyConfig.setControlPointOffset(rl, offset);

		ret.id = rid.id;
		ret.comment = rid.comment;
		
		return ret;
		
	}

	public ReturnID setJogTimeout(int time) {
		// TODO Auto-generated method stub
		return null;
	}

	public void setJointCommand(double[] jointPos) {

		m_armR_joint_cmd_out_val.pos = Arrays.copyOfRange(jointPos, 0, 7);
		m_armR_joint_cmd_out_val.tm = new Time(0,0);		

		if (false) {
			System.out.print("joint ref out");
			for (double dd: m_armR_joint_cmd_out_val.pos) {
				System.out.print(" " + dd);
			}
			System.out.println();
		}
		m_armR_joint_cmd_outOut.write();
	
	}

	/**
	 * 冗長軸の目標位置を冗長性利用モジュールに出力する
	 * @param redundantPos 冗長軸の目標位置
	 */	
	public void setRedundantCommand(double[] redundantPos) {
		//! vehicle x,y,theta,lumbar1,2,3,right elbow,left elbow
		m_redundant_cmd_out_val.data = Arrays.copyOfRange(redundantPos, 0, 8);
		if (false) {
			System.out.print("redundant_cmd out");
			for (double dd: m_redundant_cmd_out_val.data) {
				System.out.print(" " + dd);
			}
			System.out.println();
		}
		m_redundant_cmd_outOut.write();
		
	}

	public ReturnID stop() {
		// TODO Auto-generated method stub
		return null;
	}
  
	public ReturnID selectMode(short mode, RightLeft type,
			boolean isHoldOtherArm) {
		
		System.out.println("selectMode()");
		ReturnID ret = new ReturnID();
		
		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.ReturnID rtn;

		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RightLeft typearg;

		if(type == RightLeft.RIGHT){
			typearg = jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RightLeft.RIGHT;
		}
		else{
			typearg = jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RightLeft.LEFT;
		}
		
		rtn = m_redundancyConfig.selectMode(mode, typearg, isHoldOtherArm);
//		System.out.println(rtn.id);
		
//		jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RedundantAxesMask redundancy = new jp.co.yaskawa.rtc.integratedMotion.toRedundancy.RedundantAxesMask();
//		redundancy.elbow = true;
		
//		rtn = m_redundancyConfig.selectRedundancy(redundancy);
//		System.out.println(rtn.id);
				
		ret.id = rtn.id;
		ret.comment = rtn.comment;
		
		return ret;
		
	}
		
    // </rtc-template>
	boolean sendSegmentData(double[] data) {
		
		// データポートへ出力
		double [] armRjointPos = new double[8];
		double [] armLjointPos = new double[8];

		for (int i=0; i<7; i++) {
			armRjointPos[i] = data[6+i];
		}
//		for (int i=0; i<7; i++) {
//			armLjointPos[i] = data[13+i];
//		}

		if (true) {
			for (double dd: m_armR_joint_cmd_out_val.pos) {
				try {
					outCarPos.write(String.valueOf(dd) + ",");
				} catch (IOException e) {
					e.printStackTrace();
				}
			}
			
			try {
				outCarPos.newLine();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			
		}
		
		
		m_armR_joint_cmd_out_val.pos = armRjointPos;
		m_armR_joint_cmd_outOut.write();
		
		return true;
	}

    class DataListener extends ConnectorDataListenerT<TimedDoubleSeq>{
        public DataListener(final String name){
            super(TimedDoubleSeq.class);
            m_name = name;
        }
		@Override
		public void operator(ConnectorInfo info, TimedDoubleSeq inData) {

			synchronized (kinematicsResult_) {
				int i;
				for (i=0; i<inData.data.length; i++) {
					kinematicsResult_[i] = inData.data[i];
				}
				if (i<kinematicsResult_.length) {
					for (; i<kinematicsResult_.length; i++) {
						kinematicsResult_[i] = 0.0;
					}
				}
								
			}
						
		}
        public String m_name;
		
    }

}
