package jp.co.yaskawa.rtc.lumbarConnector;


import RTC.TimedDoubleSeq;
import jp.go.aist.rtm.RTC.DataFlowComponentBase;
import jp.go.aist.rtm.RTC.Manager;
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.CorbaPort;
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 LumbarConnectorImpl
 * @brief ${rtcParam.description}
 *
 */
public class LumbarConnectorImpl extends DataFlowComponentBase {

  /*!
   * @brief constructor
   * @param manager Maneger Object
   */
	public LumbarConnectorImpl(Manager manager) {  
        super(manager);
        // <rtc-template block="initializer">
        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_joint_cmd_in_val = new TimedDoubleSeq();
        m_joint_cmd_in = new DataRef<TimedDoubleSeq>(m_joint_cmd_in_val);
        m_joint_cmd_inIn = new InPort<TimedDoubleSeq>("joint_cmd_in", m_joint_cmd_in);
        m_joint_fb_out_val = new TimedDoubleSeq();
        m_joint_fb_out = new DataRef<TimedDoubleSeq>(m_joint_fb_out_val);
        m_joint_fb_outOut = new OutPort<TimedDoubleSeq>("joint_fb_out", m_joint_fb_out);
        m_joint_cmd_out_val = new TimedDoubleSeq();
        m_joint_cmd_out = new DataRef<TimedDoubleSeq>(m_joint_cmd_out_val);
        m_joint_cmd_outOut = new OutPort<TimedDoubleSeq>("joint_cmd_out", m_joint_cmd_out);
        m_lumbar_servicePort = new CorbaPort("lumbar_service");
        m_lumbar_common_servicePort = new CorbaPort("lumbar_common_service");
        // </rtc-template>

    }

    /*!
     *
     * 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("joint_fb_in", m_joint_fb_inIn);
        addInPort("joint_cmd_in", m_joint_cmd_inIn);
        
        // Set OutPort buffer
        addOutPort("joint_fb_out", m_joint_fb_outOut);
        addOutPort("joint_cmd_out", m_joint_cmd_outOut);
        
        // Set service provider to Ports
        try {
        	m_lumbar_servicePort.registerProvider("lumbarService", "LumbarUnit", m_lumbarService);
        	m_lumbar_common_servicePort.registerProvider("lumbarCommonService", "ExtAxes", m_lumbarCommonService);
        } catch (ServantAlreadyActive e) {
            e.printStackTrace();
        } catch (WrongPolicy e) {
            e.printStackTrace();
        } catch (ObjectNotActive e) {
            e.printStackTrace();
        }
        
        // Set service consumers to Ports
        
        // Set CORBA Service Ports
        addPort(m_lumbar_servicePort);
        addPort(m_lumbar_common_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) {
        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) {
        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) {
        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_joint_fb_in_val;
    protected DataRef<TimedDoubleSeq> m_joint_fb_in;
    /*!
     * lumbar joint feedback position (degree)
     */
    protected InPort<TimedDoubleSeq> m_joint_fb_inIn;

    protected TimedDoubleSeq m_joint_cmd_in_val;
    protected DataRef<TimedJointPos> m_joint_cmd_in;
    /*!
     * lumbar joint command position (degree)
     */
    protected InPort<TimedDoubleSeq> m_joint_cmd_inIn;

    
    // </rtc-template>

    // DataOutPort declaration
    // <rtc-template block="outport_declare">
    protected TimedDoubleSeq m_joint_fb_out_val;
    protected DataRef<TimedDoubleSeq> m_joint_fb_out;
    /*!
     * lumbar joint feedback position
     */
    protected OutPort<TimedDoubleSeq> m_joint_fb_outOut;

    protected TimedDoubleSeq m_joint_cmd_out_val;
    protected DataRef<TimedDoubleSeq> m_joint_cmd_out;
    /*!
     * lumbar joint command position
     */
    protected OutPort<TimedDoubleSeq> m_joint_cmd_outOut;

    
    // </rtc-template>

    // CORBA Port declaration
    // <rtc-template block="corbaport_declare">
    /*!
     */
    protected CorbaPort m_lumbar_servicePort;
    /*!
     */
    protected CorbaPort m_lumbar_common_servicePort;
    
    // </rtc-template>

    // Service declaration
    // <rtc-template block="service_declare">
    /*!
     */
    protected LumbarUnitSVC_impl m_lumbarService = new LumbarUnitSVC_impl();
    /*!
     */
    protected ExtAxesSVC_impl m_lumbarCommonService = new ExtAxesSVC_impl();
    
    // </rtc-template>

    // Consumer declaration
    // <rtc-template block="consumer_declare">
    
    // </rtc-template>


}
