// -*- C++ -*-
/*!
 * @file  CeilingNavigation.cpp
 * @brief CeilingNavigation
 * @date $Date$
 *
 * $Id$
 */

#include "CeilingNavigation.h"
#include "cv.h"
#include "highgui.h"

//#define _USE_SHOW_MAP	1
//#define _OFFLINE_IMAGE	1

#ifdef _USE_SHOW_MAP
#include "Common/sharedmem.h"
robot_status *rstat = NULL;
int	count = 130;
int delaycount=0;
#endif

// Module specification
// <rtc-template block="module_spec">
static const char* ceilingnavigation_spec[] =
  {
    "implementation_id", "CeilingNavigation",
    "type_name",         "CeilingNavigation",
    "description",       "CeilingNavigation",
    "version",           "1.0.0",
    "vendor",            "NAIST and FSI",
    "category",          "Navigation",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
#ifdef _OFFLINE_IMAGE
    "conf.default.NavigationMap", "./ceiling-map-2009-01-19_b.pgm",
    "conf.default.OfflineImage", "./offline_image",
    "conf.default.X_POS", "367",
    "conf.default.Y_POS", "51",
    "conf.default.THETA", "-2.0",
#else
    "conf.default.NavigationMap", "./CeilingImage2_Small.BMP",
    "conf.default.OfflineImage", "",
    "conf.default.X_POS", "367",
    "conf.default.Y_POS", "51",
    "conf.default.THETA", "-2.0",
#endif
    "conf.default.BlockResolution", "5",
    "conf.default.BlockSize", "64",
    "conf.default.BlockCoefficient", "0.3",
    "conf.default.SearchScope", "8",
    "conf.default.BlackWhiteValue", "240",
    "conf.default.Center_X", "160",
    "conf.default.Center_Y", "127",
	"conf.default.RealMapHeight", "100.0",
	"conf.default.RealMapWidth", "100.0",
	"conf.default.VirtualMapHeight", "100.0",
	"conf.default.VirtualMapWidth", "100.0",
    ""
  };
// </rtc-template>

CeilingNavigation::CeilingNavigation(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_CameraDataIn("CameraData", m_CameraData),
    m_CeilingOdometryOut("CeilingOdometry", m_CeilingOdometry),
	m_CeilingOdometrydOut("CeilingOdometryd", m_CeilingOdometryd),
    // </rtc-template>
	dummy(0)
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  registerInPort("CameraData", m_CameraDataIn);
  
  // Set OutPort buffer
  registerOutPort("CeilingOdometry", m_CeilingOdometryOut);
  registerOutPort("CeilingOdometryd", m_CeilingOdometrydOut);
  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>

m_CeilingOdometryd.data.length(3);   // X, Y, Theta  

}

CeilingNavigation::~CeilingNavigation()
{
}



RTC::ReturnCode_t CeilingNavigation::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
#ifdef _OFFLINE_IMAGE
  bindParameter("NavigationMap", m_NavigationMap, "./ceiling-map-2009-01-19_b.pgm");
  bindParameter("OfflineImage", m_OfflineImage, "./offline_image");
  bindParameter("X_POS", x_pos, "367");
  bindParameter("Y_POS", y_pos, "51");
  bindParameter("THETA", theta, "-2.0");
#else
  bindParameter("NavigationMap", m_NavigationMap, "./ceiling-map-2009-01-19_b.pgm");
  bindParameter("OfflineImage", m_OfflineImage, "");
  bindParameter("X_POS", x_pos, "367");
  bindParameter("Y_POS", y_pos, "51");
  bindParameter("THETA", theta, "-2.0");
#endif
  bindParameter("BlockResolution", m_BlockResolution, "5");
  bindParameter("BlockSize", m_BlockSize, "64");
  bindParameter("BlockCoefficient", m_BlockCoefficient, "0.3");
  bindParameter("SearchScope", m_SearchScope, "8");
  bindParameter("BlackWhiteValue", m_BlackWhiteValue, "240");
  bindParameter("Center_X", m_Center_X, "160");
  bindParameter("Center_Y", m_Center_Y, "127");
  bindParameter("RealMapHeight",m_RealMapHeight,"100.0");
  bindParameter("RealMapWidth",m_RealMapWidth,"100.0");
  bindParameter("VirtualMapHeight",m_VirtualMapHeight,"100.0");
  bindParameter("VirtualMapWidth",m_VirtualMapWidth,"100.0");


  // </rtc-template>
#ifdef _USE_SHOW_MAP
	rstat = (robot_status *)InitSharedMem(SHM_KEY, sizeof(robot_status));	// 共有メモリ関連・・・(移植時は削除)
#endif


cvNamedWindow("HENKANGO", CV_WINDOW_AUTOSIZE);
	return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t CeilingNavigation::onFinalize()
{
  return RTC::RTC_OK;
}
*/


RTC::ReturnCode_t CeilingNavigation::onStartup(RTC::UniqueId ec_id)
{
	// ナビゲーション用マップ画像読み込み
	m_CeilingMap.ReadNavigationMap(m_NavigationMap);

	// 初期位置特定
#if 0	// コンフィグに移動
#ifdef _OFFLINE_IMAGE
	m_CeilingMap.SetLocation(376,  51, -2.00);
#else
	m_CeilingMap.SetLocation(x_pos,  y_pos, theta);
#endif
#endif
	return RTC::RTC_OK;
}


RTC::ReturnCode_t CeilingNavigation::onShutdown(RTC::UniqueId ec_id)
{
	// 動的確保メモリ開放

	return RTC::RTC_OK;
}

RTC::ReturnCode_t CeilingNavigation::onActivated(RTC::UniqueId ec_id)
{
	ImageData cameraImage;

	// カメラコンポーネントから送信されてきている画像情報を読み込む
	if(m_OfflineImage.length() == 0){
		m_CameraDataIn.read();
		// バイナリ配列を適切な状態に変更する
		cameraImage.SetData(m_CameraData);
	}
#ifdef _OFFLINE_IMAGE
	else{
		char			fname[256];

		// オフラインデータを読み込んで、m_CameraDataに格納する
nnn		memset(fname, 0, sizeof(fname));
		sprintf( fname, "%s/cap_%05d.png", m_OfflineImage.c_str(), count );
		cameraImage.LoadImage( fname, CV_LOAD_IMAGE_GRAYSCALE);
	}
#endif
	m_BlockMat.CreateBlockMap(m_Center_X, m_Center_Y,
        cameraImage.GetWidth(), cameraImage.GetHeight(),
	m_BlockSize, m_BlockCoefficient, m_BlockResolution, m_BlackWhiteValue);

	return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t CeilingNavigation::onDeactivated(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

RTC::ReturnCode_t CeilingNavigation::onExecute(RTC::UniqueId ec_id)
{
	ImageData cameraImage;
	// カメラコンポーネントから送信されてきている画像情報を読み込む
	if(m_OfflineImage.length() == 0){
		m_CameraDataIn.read();
		// 形式変換
		// バイナリ配列を適切な状態に変更する
		cameraImage.SetData(m_CameraData);
	}
#ifdef _OFFLINE_IMAGE
	else{
		char			fname[256];

		// オフラインデータを読み込んで、m_CameraDataに格納する
		memset(fname, 0, sizeof(fname));
		if(count > 1061) count = 130;
		sprintf( fname, "%s/cap_%05d.png", m_OfflineImage.c_str(), count++ );
		cameraImage.LoadImage( fname, CV_LOAD_IMAGE_GRAYSCALE);
	}
#endif
	m_CeilingMap.SetLocation(x_pos,  y_pos, theta);

//cvShowImage("HENKANGO",cameraImage);
	m_BlockMat.MapTracking(m_CeilingMap, cameraImage);
	
	// 結果情報(ｘ,ｙ,ω)出力
	#if 1
	theta = m_CeilingMap.GetTheta();
	x_pos = m_CeilingMap.GetPosX();
	y_pos = m_CeilingMap.GetPosY();
	#endif

	//下からここにもってきた

	//printf("Location = %ld, %ld, %f\n", x_pos, y_pos, theta);


	  std::cout <<"[output Data]: "<< m_CeilingOdometryd.data[0]<< ","
		    << m_CeilingOdometryd.data[1]<< ","
		    <<m_CeilingOdometryd.data[2] << std::endl;


  if(delaycount==0){
	m_CeilingOdometry.theta = theta;
	m_CeilingOdometry.x = x_pos;
	m_CeilingOdometry.y = y_pos;
	m_CeilingOdometryd.data[0]=m_RealMapWidth/m_VirtualMapWidth*(double)x_pos+1.7;//1.7
	//m_CeilingOdometryd.data[0]=m_RealMapWidth/(m_VirtualMapWidth-20)*(double)x_pos+0.8;//1.7
	m_CeilingOdometryd.data[1]=m_RealMapHeight-m_RealMapHeight/m_VirtualMapHeight*(double)y_pos-2.3;
	//m_CeilingOdometryd.data[2]=theta-1.57;
	m_CeilingOdometryd.data[2]=theta-1.5;


	  std::cout <<"[output Data]: "<< m_CeilingOdometryd.data[0]<< ","
		    << m_CeilingOdometryd.data[1]<< ","
		    <<m_CeilingOdometryd.data[2] << std::endl;

	m_CeilingOdometryOut.write();
	m_CeilingOdometrydOut.write();
        ///////ここで処理を遅らせている//////
        delaycount=100;
  }
  delaycount--;


	return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t CeilingNavigation::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t CeilingNavigation::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t CeilingNavigation::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t CeilingNavigation::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t CeilingNavigation::onRateChanged(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

extern "C"
{
 
  void CeilingNavigationInit(RTC::Manager* manager)
  {
    RTC::Properties profile(ceilingnavigation_spec);
    manager->registerFactory(profile,
                             RTC::Create<CeilingNavigation>,
                             RTC::Delete<CeilingNavigation>);
  }
  
};
