#include <iostream>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/times.h>
#include <string>
#include "Robot_Server.h"
#include <Command_Type.h>
#include "Socket_Request.h"

#define CORBAWAITINFO 1500 // ticks(0.01 sec), wait for info from gl

#define MSG_NUM_ORB_MAX 5 // maximum number of messages from corba clients in queue 
#define MSG_NUM_MAX 10 // maximum number of messages in queue 

int net_timeout = 100;
static Message *ohh_msg_ptr;
static Robot_Socket *ohh_socket_ptr;


Robot_impl::Robot_impl( Message *msg_ptr )
{
  Msg_ptr = msg_ptr;
}

Robot_impl::~Robot_impl( void )
{
  Msg_ptr->~Message();
  system("killall gl_server");
  cerr << "  #:0) " << endl;
}

CORBA::Long
Robot_impl::put_command( const CORBA::Any& any_command )
{
  CORBA::Long retv = -1;
  if( Msg_ptr->size() < MSG_NUM_ORB_MAX ){
    My_Message msg_str;
    Robot_Command com;
    if(any_command >>=  com ) {
      msg_str.message_type = 1; 
      msg_str.command = com.command;
      if( com.command ==  SET_NET_TIMEOUT ) {
	net_timeout = com.lparam;
      }
      for(int i = 0; i < 20; i++)  msg_str.param[i] =  com.param[i]; 
      msg_str.lparam = com.lparam;
      Msg_ptr->send( msg_str );
      retv = Msg_ptr->size();
    }
  } else {
    cerr << "refused to corba client" << endl;
  }
  
  return retv;
}


CORBA::Long
Robot_impl::get_info( CORBA::Any& any_info )
{
  int i;
  My_Message inf;
  //cleaning all old replies
  while( Msg_ptr->receive( inf, 2) != -1 );

  if( Msg_ptr->size() < MSG_NUM_ORB_MAX ){
    My_Message msg_str;
    Robot_Command com;
    if(any_info >>=  com ) {
      msg_str.message_type = 1; 
      msg_str.command = com.command;
      for( i = 0; i < 20; i++)  msg_str.param[i] =  com.param[i]; 
      msg_str.lparam = com.lparam;
      Msg_ptr->send( msg_str );
      // waiting for reply from gl server
      inf.command = 0;
      int tm = times(NULL);
      while( ( (times(NULL) - tm) < CORBAWAITINFO ) &&
	     ( Msg_ptr->receive( inf, 2) == -1) ) {
	usleep(100);
      }
      if( inf.command != 0 ){
	com.command = inf.command;
	com.lparam = inf.lparam;
	for( i = 0; i < 20; i++){
	  com.param[i] = inf.param[i];
	  //cerr << com.param[i] << endl;
	}
	any_info <<= com;
	return ( CORBA::Long ) 0;
      }
    }
  } else {
    cerr << "refused to corba client" << endl;
  }

  return ( CORBA::Long ) -1;
}

void ohhh(int sig){
  char answer = 'n';
  cerr << "Do you really want to exit me ? (y/n) ";
  cin >> answer;
  if ( answer == 'y' ) {
    cerr << "Ohhh, I have been killed, Bye !!!" << endl << endl;
    system("killall gl_server");
    ohh_msg_ptr->~Message();
    ohh_socket_ptr->~Robot_Socket();
    cerr << "  #:0)  " << endl;
    exit(0);
  }
}



int main (int argc, char *argv[])
{
  if( argc != 2) {
    cerr << "Call this programm as \" ./robot_server XXXXX \" ," <<
      endl << "where XXXXX is a free port number( from 1025 till 65536)" << endl;
    return -1;
  }

  int tempint = strtol( argv[1], NULL, 10);
  if( tempint < 1025 || tempint > 65536 ){
    cerr << "Call this programm as \" ./robot_server XXXXX \" ," <<
      endl << "where XXXXX is a free port number( from 1025 till 65536)" << endl;
    return -1;
  }

  signal(SIGINT, ohhh );

  // making ipc queue for gl client
  int msg_key = ftok(".", 'a');
  if( msg_key == -1 ) {
    cerr << "Can't get unique key" << endl;
    exit(1);
  }
  Message message( msg_key );
  ohh_msg_ptr = &message;

  // interface for socket clients
  Robot_Socket robsocket( &message, atoi(argv[1]));
  ohh_socket_ptr = &robsocket;

  // robot_server internet address
  char myhostname[80], strbuf[80];
  gethostname( myhostname, 79 );
  string robserver_addr = "inet:";
  robserver_addr += myhostname;
  robserver_addr +=":";
  robserver_addr += argv[1];

  // activating gl_server
  string gl_server_start = "./gl_server ";
  gl_server_start += gcvt( msg_key, 20, strbuf );
  gl_server_start += " &";
  system( gl_server_start.c_str()); 

  //Initialize the ORB
  int c = 3; 
  char addrstr[80];
  strcpy( addrstr, robserver_addr.c_str() );
  char* v[] = { "robot_server", "-ORBIIOPAddr", addrstr };

  CORBA::ORB_var orb = CORBA::ORB_init( c, v, "mico-local-orb");

  //Obtain a reference to the RootPOA and its Manager
  CORBA::Object_var pobj =orb->resolve_initial_references("RootPOA");
  PortableServer::POA_var poa = PortableServer::POA::_narrow (pobj);
  PortableServer::POAManager_var mgr = poa->the_POAManager();

  // creating robot server
  Robot_impl* robot = new Robot_impl( &message );

  // activating robot
  PortableServer::ObjectId_var oid = poa->activate_object(robot);

  //Activate the POA and start serving requests
  cerr << "Robot is running" << endl;
  mgr->activate ();
  while(1){
    if( orb->work_pending() ) {
      orb->perform_work();
    } else {
      if( message.size() < MSG_NUM_MAX) {
	if( robsocket.check_socket() ) {
	  usleep( net_timeout);
	}
      }
    }
  }

  // shutdown robot server
  poa->destroy (TRUE, TRUE);
  delete robot;

  return 0;
}
