/*
 * main.cpp
 *
 *  Created on: Nov 14, 2010
 *      Author: dnad
 */
#include <exception>
#include <iostream>

#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/array.hpp>
#include <LABUSTTypes.h>
#include <iostream>
#include <string.h>
#include <VehicleSimulator.h>
#include <STVSentence.h>
#include <TAUSentence.h>
#include <CoreDataSet.h>

using namespace LABUST::TYPES;

std::string outgoing;

LABUST::SIMULATION::VehicleSimulator sim;
boost::asio::io_service io;
/**
 VERY IMPORTANT !!!
  USE RESOLVING AND SIMILAR TO DEFINE THE ENDPOINT ADDRESS
  SELECTING USING boost::asio::ip::udp::v4() CHRASES ON WINDOWS MACHINES APPARENTLY
*/
boost::asio::ip::udp::endpoint remote(boost::asio::ip::address::from_string("127.0.0.1"), 33453);
/** HOWEVER, IN THIS CASE IT WORKS ?! */
boost::asio::ip::udp::socket socket_(io,boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 33452));
char sentence[500];
double last_time = 0;

void handle_receive(const boost::system::error_code& error, std::size_t transferred);
void handle_send(const boost::system::error_code& ,std::size_t transferred);

void start_receive()
{
  using namespace boost::asio;
  socket_.async_receive(buffer(sentence,sizeof(sentence)),boost::bind(&handle_receive, placeholders::error, placeholders::bytes_transferred));
}

void handle_receive(const boost::system::error_code& error, std::size_t transferred)
{
  using namespace boost::asio;
  if (!error || error == boost::asio::error::message_size)
  {
    /**
     * Get time
     */
	  /*
    struct timeval tv;
    struct timezone tz;
    gettimeofday(&tv, &tz);
    double val = tv.tv_sec + tv.tv_usec/1000000.;
    std::cout<<"time value:"<<val<<std::endl;
	*/

    using namespace LABUST::COMMUNICATION;
    using namespace LABUST::VECTORS;
    using namespace LABUST::VEHICLES::VEHICLESTATE;

    TAUSentence sen;
    sen.populate(std::string(&sentence[0],transferred));
    TAU tau;
    sen.toTAU(tau);

    //std::cout<<"Tau:"<<tau.X<<std::endl;

    /*
     * Simulate vehicle
     */
    //sim.model.setTs(last_time?val-last_time:0.1);
    
    using namespace LABUST::VEHICLES::COREDATASET; 
    tauMap tauM;
    tauM[X]=tau.X;
    tauM[Y]=tau.Y;
    tauM[Z]=tau.Z;
    tauM[K]=tau.K;
    tauM[M]=tau.M;
    tauM[N]=tau.N;

    sim.setTAU(tauM);
    //last_time = val;

    LABUST::VEHICLES::COREDATASET::stateMap statesMap;
    LABUST::TYPES::stateMap states;
    sim.getState(statesMap);
using namespace LABUST::VEHICLES;
    states[VEHICLESTATE::x] = statesMap[COREDATASET::x];
states[VEHICLESTATE::y] = statesMap[COREDATASET::y];
states[VEHICLESTATE::z] = statesMap[COREDATASET::z];
states[VEHICLESTATE::roll] = statesMap[COREDATASET::roll];
states[VEHICLESTATE::pitch] = statesMap[COREDATASET::pitch];
states[VEHICLESTATE::yaw] = statesMap[COREDATASET::yaw];
states[VEHICLESTATE::u] = statesMap[COREDATASET::u];
states[VEHICLESTATE::v] = statesMap[COREDATASET::v];
states[VEHICLESTATE::w] = statesMap[COREDATASET::w];
states[VEHICLESTATE::p] = statesMap[COREDATASET::p];
states[VEHICLESTATE::q] = statesMap[COREDATASET::q];
states[VEHICLESTATE::r] = statesMap[COREDATASET::r];
states[VEHICLESTATE::X_e] = statesMap[COREDATASET::X_e];
states[VEHICLESTATE::Y_e] = statesMap[COREDATASET::Y_e];
states[VEHICLESTATE::Z_e] = statesMap[COREDATASET::Z_e];
states[VEHICLESTATE::N_e] = statesMap[COREDATASET::N_e];
states[VEHICLESTATE::xUSV] = statesMap[COREDATASET::xUSV];
states[VEHICLESTATE::yUSV] = statesMap[COREDATASET::yUSV];
states[VEHICLESTATE::zUSV] = statesMap[COREDATASET::zUSV];
states[VEHICLESTATE::dV] = statesMap[COREDATASET::dV];
states[VEHICLESTATE::dH] = statesMap[COREDATASET::dH];
states[VEHICLESTATE::gamma] = statesMap[COREDATASET::gamma];
states[VEHICLESTATE::range] = statesMap[COREDATASET::range];
states[VEHICLESTATE::bearing] = statesMap[COREDATASET::bearing];
    
STVSentence state_sen;
    state_sen.fromStateMap(states);

    outgoing = state_sen.toString();

	socket_.async_send_to(boost::asio::buffer(outgoing), remote,
        boost::bind(&handle_send, boost::asio::placeholders::error,
        boost::asio::placeholders::bytes_transferred));

    start_receive();
  }
}

void handle_send(const boost::system::error_code& e,
    std::size_t transferred)
{
	//std::cout<<"Sent stuff."<<e.message()<<std::endl;
}


int main()
{
  try
  {
    sim.setupXML("configure.xml","simulator");
    sim.configure();
	std::cout<<sim.model<<std::endl;
    start_receive();
    io.run();
  }
  catch (std::exception& e)
  {
    std::cerr << e.what() << std::endl;
  }

  return 0;
}
