// -*- C++ -*-
/*!
 * @file  BBcont.cpp * @brief BBcont component * $Date$ 
 *
 * $Id$ 
 */

#include <stdio.h>

#define _USE_MATH_DEFINES 
#include <math.h>
#include "BBcont.h"

// Module specification
// <rtc-template block="module_spec">
static const char* bbcont_spec[] =
  {
    "implementation_id", "BBcont",
    "type_name",         "BBcont",
    "description",       "BBcont component",
    "version",           "0.0",
    "vendor",            "RDE",
    "category",          "Generic",
    "activity_type",     "SPORADIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    ""
  };
// </rtc-template>

BBcont::BBcont(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_acgyroIn  ("acgyro" , m_acgyro),
    m_acaccelIn ("acaccel", m_acaccel),
    m_acjointIn ("acjoint", m_acjoint),
    m_cmjointOut("cmjoint", m_cmjoint),
    m_cmspeedOut("cmspeed", m_cmspeed),
    m_cmaccelOut("cmaccel", m_cmaccel),
    m_cmforceOut("cmforce", m_cmforce)

    // </rtc-template>
{
	ec=0;
}

BBcont::~BBcont()
{
}


RTC::ReturnCode_t BBcont::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("acgyro" , m_acgyroIn);
  addInPort("acaccel", m_acaccelIn);
  addInPort("acjoint", m_acjointIn);

  // Set OutPort buffer
  addOutPort("cmjoint", m_cmjointOut);
  addOutPort("cmspeed", m_cmspeedOut);
  addOutPort("cmaccel", m_cmaccelOut);
  addOutPort("cmforce", m_cmforceOut);

  // Set service provider to Ports

  // Set service consumers to Ports

  // Set CORBA Service Ports

  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable

  // </rtc-template>

  m_acgyro.data.length(3);
  m_acaccel.data.length(3);
  m_acjoint.data.length(8);
  m_cmjoint.data.length(4);
  m_cmspeed.data.length(4);
  m_cmaccel.data.length(4);
  m_cmforce.data.length(8);

  return RTC::RTC_OK;
}



RTC::ReturnCode_t BBcont::onFinalize()
{
	puts("onFinalize()");
	return RTC::RTC_OK;
}

RTC::ReturnCode_t BBcont::onStartup(RTC::UniqueId ec_id)
{
	puts("onStartup");
	return RTC::RTC_OK;
}

RTC::ReturnCode_t BBcont::onShutdown(RTC::UniqueId ec_id)
{
	puts("onShutdown");
	return RTC::RTC_OK;
}

RTC::ReturnCode_t BBcont::onActivated(RTC::UniqueId ec_id)
{
	puts("onActivated");
    for(int i=0;i<4;i++)
  	  m_cmjoint.data[i]=m_cmspeed.data[i]=m_cmaccel.data[i]=m_cmforce.data[i]=0;
	ec=0;
	agx=agy=cgx=cgy=cgax=cgay=0;
	opx=opy=0;
	tx=ty=0;
	state=1;
	return RTC::RTC_OK;
}

RTC::ReturnCode_t BBcont::onDeactivated(RTC::UniqueId ec_id)
{
	puts("onDeactivated");
	state=0;
	return RTC::RTC_OK;
}


RTC::ReturnCode_t BBcont::onExecute(RTC::UniqueId ec_id)
{
	int i;
	//puts("onExecute");
	ec++;
	if(ec%100==0) { putchar('.'); fflush(stdout); }

	// 関節角度InPortの値をアップデート
	if(state==0) return RTC::RTC_OK;

	i=0;
	while(1)
	{
		if(m_acgyroIn.isNew())
		{ m_acgyroIn.read(); printf("G%06d ",m_acgyro.tm.nsec/1000); }
		if(m_acaccelIn.isNew())
		{ m_acaccelIn.read(); printf("A%06d ",m_acaccel.tm.nsec/1000); }
		if(m_acjointIn.isNew())
		{
			m_acjointIn.read(); printf("A%06d ",m_acjoint.tm.nsec/1000); 
			now=m_acjoint.tm.sec+m_acjoint.tm.nsec*1e-9;
			printf("% 7.3lf ",now);
			if((state==1)&&(m_acjoint.tm.sec==0)&&(m_acjoint.tm.nsec==0)) 
			{ printf("control sync start"); state=2; }
			if(state==2) break;  // continue control
		} else {
			if(state==2)
			{
				puts("no data in regular control state");
				return RTC::RTC_OK;
			}
		}
		i++;
		if(i>10) 
		{
			puts("Controller starting synchronization failed");
			return RTC::RTC_OK;
		}
	}
	/*printf("J% 3d.%06d ",m_acjoint.tm.sec,m_acjoint.tm.nsec/1000);
	for(i=0;i<m_acjoint.data.length();i++)
	{	
		printf("%lf ",m_acjoint.data[i]);
	}*/

	CalculateInclination();
	CalculatePosition();
	DoControl();


	// dangerous trick for roller pushing slider
	for(i=0;i<4;i++)
	{
		// clip position of the slide
		m_cmjoint.data[i]=m_acjoint.data[i+4];
		const double D=0.005;
		if(m_cmjoint.data[i]>D) 
			m_cmjoint.data[i]=D;
		if(m_cmjoint.data[i]<-D) 
			m_cmjoint.data[i]=-D;
		m_cmspeed.data[i]=0;  // force velocity=0
		m_cmaccel.data[i]=0;  // force acceleration=0
	}
	// /trick

	m_cmforce.data[0]= tx;  // torque for roller
	m_cmforce.data[1]=-tx;
	m_cmforce.data[2]=-ty;
	m_cmforce.data[3]= ty;

	const double PF=100;  // pressure force for slide
	m_cmforce.data[4]=-PF;
	m_cmforce.data[5]=-PF;
	m_cmforce.data[6]=-PF;
	m_cmforce.data[7]=-PF;

	m_cmjoint.tm.sec =m_cmforce.tm.sec =m_acjoint.tm.sec;
	m_cmjoint.tm.nsec=m_cmforce.tm.nsec=m_acjoint.tm.nsec;
	m_cmjointOut.write();
	m_cmspeedOut.write();
	m_cmaccelOut.write();
	m_cmforceOut.write();

	printf("\n");
	return RTC::RTC_OK;
}

inline double Rad2Deg(double rad)
{ return rad/M_PI*180; }

void BBcont::CalculateInclination(void)
{
	wx=m_acgyro.data[0];
	wy=m_acgyro.data[2];
	//double ax=atan2(m_acaccel.data[0],m_acaccel.data[2]);
	//double ay=atan2(m_acaccel.data[1],m_acaccel.data[2]);
	double ax=atan2( m_acaccel.data[1],9.8);
	double ay=atan2( m_acaccel.data[0],9.8);

	agx+=wx*DT;  agy+= wy*DT;
	const double R=0.001;
	cgx=cgx*(1-R)+(ax-agx)*R;  cgax=cgx+agx;
	cgy=cgy*(1-R)+(ay-agy)*R;  cgay=cgy+agy;

	printf("(A% 7.3lf G% 7.3lf C% 7.3lf) ",Rad2Deg(ax),Rad2Deg(agx),Rad2Deg(cgax));
	printf("(A% 7.3lf G% 7.3lf C% 7.3lf) ",Rad2Deg(ay),Rad2Deg(agy),Rad2Deg(cgay));

	thx=cgax;  thy=cgay;

}

void BBcont::CalculatePosition(void)
{
	// roller radius=0.02
	const double rad2m=0.02;
	px=(( m_acjoint.data[0])+(-m_acjoint.data[1]))/2*rad2m; // aprroximately:-)
	py=((-m_acjoint.data[2])+( m_acjoint.data[3]))/2*rad2m; // aprroximately:-)
	pvx=(px-opx)/DT; pvy=(py-opy)/DT;
	opx=px; opy=py;
	printf("P(% 7.2lf,% 7.2lf) V(% 7.2lf,% 7.2lf) ",px*1000,py*1000,pvx*1000,pvy*1000);
}


inline void Limit(double &v,double l)
{ if(v>l) v=l; if(v<-l) v=-l; }

void BBcont::DoControl(void)
{
	const double K1=30, K2=3, K3=-10, K4=-3;
	double xr=0.1,yr=-0.1,thxr=0,thyr=0;
	/*if(now<2.0) {
		thxr=0.03; thyr=-0.03;
	} else {
		thxr=-0.04; thyr=0.04;
	}*/
	if(now>25) { xr=-0.1; yr=0.1; }

	tx=K1*(thxr-thx)+K2*(0-wx)+K3*(xr-px)+K4*(0-pvx);
	ty=K1*(thyr-thy)+K2*(0-wy)+K3*(yr-py)+K4*(0-pvy);
	Limit(tx,1.0);	
	Limit(ty,1.0);
	printf("(TX % 7.4lf,  TY % 7.4lf) ",tx,ty);
}

/*
RTC::ReturnCode_t BBcont::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t BBcont::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t BBcont::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t BBcont::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t BBcont::onRateChanged(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/


extern "C"
{
 
  void BBcontInit(RTC::Manager* manager)
  {
    coil::Properties profile(bbcont_spec);
    manager->registerFactory(profile,
                             RTC::Create<BBcont>,
                             RTC::Delete<BBcont>);
  }
  
};