#include <YMcmd.h>
#include <sstream>

//using namespace std;


  const char* ElseItem::toXML(char* tab = "") {
    std::stringstream 	oss;
    oss.str("");
    oss << tab << "<Else>\n";
    oss << tab << "</Else>\n";
    return oss.str().c_str();
  }


  
  
  const char* DownLinkItem::toXML(char* tab = ""){
    stringstream oss;
    oss.str("");
    return oss.str().c_str();
  }


  void OrbitalInfoItem::fill(TArrayC* data){
  	//pos = float[3];
  	//vel = float[3]
  	long tempData = 0;
  	time = (((data->At(1) << 24) & 0xFF000000) + ((data->At(0) << 16) & 0x00FF0000) + ((data->At(3) << 8) & 0x0000FF00) + (data->At(2) & 0x000000FF))/128.0;
  	
  	short index = 0;
  	for (int i = 0; i < 3; i++){
  		index = 4*i;
  		tempData = ((data->At(index + 4) << 24) & 0xFF000000) + ((data->At(index + 5) << 16) & 0x00FF0000) + ((data->At(index + 6) << 8) & 0x0000FF00) + (data->At(index + 7) & 0x000000FF);
  		if (data->At(index + 4) >> 8) {
				pos[i] = (~tempData * -1.0)/262144.0;
			} else {
				pos[i] = tempData /262144.0;
		}
		
		tempData = ((data->At(index + 16) << 24) & 0xFF000000) + ((data->At(index + 17) << 16) & 0x00FF0000) + ((data->At(index + 18) << 8) & 0x0000FF00) + (data->At(index + 19) & 0x000000FF);
  		if (data->At(index + 16) >> 8) {
				vel[i] = (~tempData * -1.0)/134217728.0;
			} else {
				vel[i] = tempData /134217728.0;
		}
  	}
  }

  const char* OrbitalInfoItem::toXML(char* tab = ""){
    stringstream oss;
    oss.str("");
    oss << tab << "<param name = 'time'>" <<  time << "</param>\n";
    oss << tab << "<param name = 'posX'>" <<  pos[0] << "</param>\n";
    oss << tab << "<param name = 'posY'>" <<  pos[1] << "</param>\n";
    oss << tab << "<param name = 'posZ'>" <<  pos[2] << "</param>\n";
    oss << tab << "<param name = 'velX'>" <<  vel[0] << "</param>\n";
    oss << tab << "<param name = 'velY'>" <<  vel[1] << "</param>\n";
    oss << tab << "<param name = 'velZ'>" <<  vel[2] << "</param>\n";
    return oss.str().c_str();
  }


  void InclinationInfoItem::fill(TArrayC* data){
  	short extIndex = 0;
  	short innIndex = 0;
  	long tempData = 0;
  	for (int i = 0; i < 6; i++){
  		extIndex = 20*i;
  		time[i] = (((data->At(extIndex) << 24) & 0xFF000000) +
  			((data->At(extIndex + 1) << 16) & 0x00FF0000) + ((data->At(extIndex + 2) << 8) & 0x0000FF00) +
  			(data->At(extIndex + 3) & 0x000000FF))/128.0;
  		
  		for (int j = 0; j < 4; j++){
			innIndex = extIndex + 4*j;
			tempData = ((data->At(innIndex + 4) << 24) & 0xFF000000) + ((data->At(innIndex + 5) << 16) & 0x00FF0000) + ((data->At(innIndex + 6) << 8) & 0x0000FF00) + (data->At(innIndex + 7) & 0x000000FF);
			if (data->At(innIndex + 4) >> 8) {
				quat[i][j] = (~tempData * -1.0)/1073741824.0;
			} else {
				quat[i][j] = tempData / 1073741824.0;
			}
  		}
  	}
  }

  const char* InclinationInfoItem::toXML(char* tab = ""){
    stringstream oss;
    oss.str("");
    for (int i = 0; i < 6; i++){
    	oss << tab << "<QUATERNION>\n";
  	oss << tab << "\t <param name = 'time'>" <<  time[i]    << "</param>\n";
  	oss << tab << "\t <param name = 'L0'>"   <<  quat[i][0] << "</param>\n";
    	oss << tab << "\t <param name = 'L1'>"   <<  quat[i][1] << "</param>\n";
    	oss << tab << "\t <param name = 'L2'>"   <<  quat[i][2] << "</param>\n";
    	oss << tab << "\t <param name = 'L3'>"   <<  quat[i][3] << "</param>\n";
    	oss << tab << "</QUATERNION>\n";
  }
    return oss.str().c_str();
  }

  void TimeSyncItem::fill(TArrayC* data){
    newTime = (((unsigned int)data->At(0)<<24)&0xFF000000) + (((unsigned int)data->At(1)<<16)&0x00FF0000) + (((unsigned int)data->At(2)<<8)&0x0000FF00) + (((unsigned int)data->At(3))&0x000000FF);
  }

  const char* TimeSyncItem::toXML(char* tab = ""){
    stringstream oss;
    oss.str("");
    oss << tab << "<param name = 'newTime'>" <<  newTime << "</param>\n";
    return oss.str().c_str();
  }


  void SelectOpModeItem::fill(TArrayC* data){
    /*for (int i = 0; i<data->GetSize(); i++){
      printf("array[%i]: %x", i, data->At(i));
      }*/
    mode        = data->At(1)&0xFF;
    paramNumber = (((unsigned int)data->At(2)<<8)&0x00000100) + (((unsigned int)data->At(3))&0x000000FF);
  }

  const char* SelectOpModeItem::toXML(char* tab = ""){
    stringstream oss;
    oss.str("");
    oss << tab << "<param name = 'mode'>"        << hex << mode << dec << "</param>\n";
    oss << tab << "<param name = 'paramNumber'>" << paramNumber << "</param>\n";
    return oss.str().c_str();
  }




  
  void BiteItem::fill(TArrayC* data){
    fillerWord =  (((unsigned int)data->At(0)<<8)&0x0000FF00) + (((unsigned int)data->At(1))&0x000000FF);
  }

  const char* BiteItem::toXML(char* tab = ""){
    stringstream oss;
    oss.str("");
    oss << tab << "<param name = 'fillerWord'>" <<  fillerWord << "</param>\n";
    return oss.str().c_str();
  }


 const char* McmdScan::extract(TArrayC *data, int id, char* tabs){
   switch(id){
   case 0xE0:
     mcmdItem = new TimeSyncItem();
     break;
   case 0xE1:
     mcmdItem = new OrbitalInfoItem();
     break;
   case 0xE2:
     mcmdItem = new InclinationInfoItem();
     break;
   case 0x33:
     mcmdItem = new SelectOpModeItem();
     break;
   default:
     mcmdItem = new ElseItem();
     break;
   }
   mcmdItem->fill(data);
   return mcmdItem->toXML(tabs);
 }