/******************************************************/
/*		ACQUISITION DE DONNES TELEMETRE ALASCA	      */
/******************************************************/


#include "sensor/AlascaComponent.h"
#include "sensor/AlascaSocket.h"
#include <qsocket.h>
#include <qapplication.h>
#include "kernel/ComponentFactory.h"
#include <iostream.h>
#include <string.h>


// Construct the factory
ComponentFactory<AlascaComponent>* factory = new ComponentFactory<AlascaComponent>("AlascaComponent"); 



//Constructeur
AlascaComponent::AlascaComponent(const char * name) : ComponentBase(name)
{   
  A_socket = new AlascaSocket(this);
  //QApplication::connect(A_socket, SIGNAL(configuration()), this, SLOT(Config()) ); 
}


//Destructeur
AlascaComponent::~AlascaComponent()
{
  delete A_socket;
}




void AlascaComponent::stopActivity()
{
  A_socket->closeSocket();
}



void AlascaComponent::startActivity()
{
  A_socket->connectToServer("172.17.130.205", 12000);	// demarrage du telemetre et/ou envoi des donnees sur ethernet
}




void AlascaComponent::configureComponent(XmlComponentConfig config)
{
  param.localCopy(config.qDomElement()); 
}






/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
long AlascaComponent::findMagicWord(const char * message, const int length)
{
  unsigned long magicWord=0x00000000;
  unsigned long i=0;

  if (length < 4)
    return -1; 
  
  magicWord =  ( (*(message+i+3)) & 0x000000FF ) +								
    ( ((*(message+i+2))<<8) & 0x0000FF00 ) +
    ( ((*(message+i+1))<<16) & 0x00FF0000 ) +
    ( ((*(message+i))<<24) & 0xFF000000 );
  
  while ( magicWord !=0xAFFEC0C0)
  {
    i++;
    magicWord =  ( (*(message+i+3)) & 0x000000FF ) +								
      ( ((*(message+i+2))<<8) & 0x0000FF00 ) +
      ( ((*(message+i+1))<<16) & 0x00FF0000 ) +
      ( ((*(message+i))<<24) & 0xFF000000 );
    
    if (i==length)
    { 
      return -1;
    }
    else continue ;
  }
    
  return i;
}




///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  // decoupe le paquet en messages et remplit la liste de 'Message'
  // remplit le message header 
  // remplit le scan header
  // détermine si le message est complet (isFull) 
  // renvoie le nombre de pending bytes


unsigned long AlascaComponent:: splitPacket(const char * packet, const int length)
{ 
























  unsigned long ptr = 0; 
  long index; 
  Message msg; // the current message of the list, often the last 

  if ( !msgList.empty() )
  {
    if (msgList.back().isIncomplete == 1)
      msg = msgList.back();
  }

  
  // Seulement si c'est un nouveau mesage qui n'est pas encore dans la liste
  // ou si le dernier message de la liste est complet
  if (msg.isIncomplete == -1)
  {
    // on cherche le magicword
    index = findMagicWord(packet + ptr,length - ptr);
    if ( index != -1) 
    {
      // on vérifie que l'on a le message header (16 octets )
      if (index + 16 <= length)
      {
        fillMessageHeader(msg, packet, index );
        // test si le message est complet "MH + SH + body"
        if (index + 32 + msg.hMsg.size <= length)
        {
          // message complet
          msg.isIncomplete = 0; 
          memcpy(msg.body, packet + index, index + 32 + msg.hMsg.size); 
          msg.bodyPtr = index + 32 + msg.hMsg.size; 
        }
        else
        {
          // message incomplet MH + une partie de 'SH+body'
          msg.isIncomplete = 1; 
          memcpy(msg.body, packet + index, length - index); 
          msg.bodyPtr = length - index; 
        }
      }
      else 
      {
        // message incomplet - MW sans le MH
        memcpy(msg.body, packet + index, length - index); 
        msg.isIncomplete = 1; 
        msg.bodyPtr = length - index;
      }
    }
    else 
    {
      // pas de magic word
      memcpy(msg.body, packet + ptr, length - ptr ); 
      msg.isIncomplete = 1; 
      msg.bodyPtr = length - index;
    }
  }
  else 
  {
    // on travaille sur le dernier message de la liste qui n'est pas complet
    // msg.isIncomplete == 1
    if (msg.hMsg.magicWord != 0)
    {
      // on a deja trouvé le MW
      
    }
    else 
    {
      char * packetCopy = new char [length + msg.bodyPtr]; 
      memcpy(packetCopy, msg.body, msg.bodyPtr); 
      memcpy(packetCopy + msg.bodyPtr, packet, length); 
      index = findMagicWord(packetCopy ,sizeof(packetCopy));

      if (index == -1)
      {
        // pas de magic word
        memcpy(msg.body, packetCopy, sizeof(packetCopy) ); 
        msg.isIncomplete = 1; 
        msg.bodyPtr = sizeof(packetCopy);
      }

      if (index != 0)
        qDebug("Error at line %d - file %s",__LINE__, __FILE__ ); 
      else 
      {
        // tester si on a le MH
        if (index + 16 <= sizeof(packetCopy))
        {
          fillMessageHeader(msg, packetCopy, index );
          // test si le message est complet "MH + SH + body"
          if (index + 32 + msg.hMsg.size <= sizeof(packetCopy))
          {
            // message complet
            msg.isIncomplete = 0; 
            memcpy(msg.body, packetCopy + index, index + 32 + msg.hMsg.size); 
            msg.bodyPtr = index + 32 + msg.hMsg.size; 
          }
          else
          {
            // message incomplet MH + une partie de 'SH+body'
            msg.isIncomplete = 1; 
            memcpy(msg.body, packetCopy + index, sizeof(packetCopy) - index); 
            msg.bodyPtr = sizeof(packetCopy) - index; 
          }
        }
        else 
        {
          // message incomplet - MW sans le MH
          memcpy(msg.body, packet + index, length - index); 
          msg.isIncomplete = 1; 
          msg.bodyPtr = length - index;
        }
        
      }

  }



//
//
//  if ( !msgList.empty() )
//  {
//    msg = msgList.back();
//    if ( (msg.isIncomplete == 1) || (msg.isIncomplete == 0) )
//    {
//      // je vais en (1)
//    }
//    else if (msg.isIncomplete == 2)
//    {
//      // je vais en (2)
//    }
//    else if (msg.isIncomplete == 3)
//    {
//      // je vais en (3)
//    }
//    else if (msg.isIncomplete == 4)
//    {
//      // je vais en (4)
//    }
//    else 
//    {
//      qWarning("Error in the message list ! A message has been found with an incorrect status"); 
//      return 0; 
//    }
//  }
//  else 
//  {
//    msgList.push_back(msg); 
//  }
  
  // tester dans la liste qu'il n'y a pas un message incomplet
  // si oui => le compléter 
  // sinon on continue 
  /*
  // (1)
  index = findMagicWord(packet,length );
  if ( index == -1) 
  { 
    return length;
  }
    

  // (2)
  // test if the message header is complete (16 bytes)
  if (index + 16 <= length)
  {
  }
  else 
  {
    return length; 
  }


  // (3)
  // test if the scan header is complete (16 bytes)
  if (index + 32 <= length)
  {
    fillScanHeader(msg, packet, index )
  }
  else 
    return length; 


  // (4)
  // test if the message body is complete ( index + 32 + msg.hScan.numPoints * 12 <= length)
  if (index + 32 + msg.hScan.numPoints * 12 <= length)
  {
    msg.isIncomplete = 0; 
    memcpy(msg.body, packet + index + 32, msg.hScan.numPoints * 12); 
  }
  else 
  {
    msg.isIncomplete = 4; 
    // on copie ce qu'il reste dans le paquet
    memcpy(msg.body, packet + index + 32, length - index + 32); 
  }








    msgList.push_back(msg); 

          
/*


          // tabmessages [0]=i;
  // nbmessages=1;
  ptr=i+4;
  while ( ptr<length)
  {
    i= findMagicWord(packet+ptr,length-ptr);
    if(i==-1)
    { 
      break;
    }
    
    tabmessages[ nbmessages ]=i+ptr;
    ptr = ptr + i + 4;
    nbmessages++;
  }
  
    
      /// a voir 
return nbmessages;*/
return 0;
}
}


void AlascaComponent::fillScanHeader(Message &msg, const char * packet, const int magicWordIndex )
{
  msg.hScan.version = *(packet+magicWordIndex+16); 
  msg.hScan.scannerType = *(packet+magicWordIndex+17); 
  msg.hScan.ecuId = *(packet+magicWordIndex+18); 
  // byte 19 is a padding byte
  
  msg.hScan.timeStamp = ((*(packet+magicWordIndex+23))&0x000000FF)+								
    ((*(packet+magicWordIndex+22)<<8)&0x0000FF00)+
    ((*(packet+magicWordIndex+21)<<16)&0x00FF0000)+
    ((*(packet+magicWordIndex+20)<<24)&0xFF000000);
  
  msg.hScan.startAngle = ((*(packet+magicWordIndex+25))&0x00FF)+								
    ((*(packet+magicWordIndex+24)<<8)&0xFF00);
  
  msg.hScan.endAngle = ((*(packet+magicWordIndex+27))&0x00FF)+								
    ((*(packet+magicWordIndex+26)<<8)&0xFF00); 
  
  msg.hScan.scanCounter = ((*(packet+magicWordIndex+29))&0x00FF)+
    ((*(packet+magicWordIndex+28)<<8)&0xFF00); 
  
  msg.hScan.numPoints = ((*(packet+magicWordIndex+31))&0x00FF)+
    ((*(packet+magicWordIndex+30)<<8)&0xFF00);
}



void AlascaComponent::fillMessageHeader(Message &msg, const char * packet, const int magicWordIndex )
{
  msg.hMsg.magicWord = ((*(packet+magicWordIndex+3))&0x000000FF) +								
    ((*(packet+magicWordIndex+2)<<8)&0x0000FF00) +
    ((*(packet+magicWordIndex+1)<<16)&0x00FF0000)+
    ((*(packet+magicWordIndex)<<24)&0xFF000000);
  
  msg.hMsg.size  =  ((*(packet+magicWordIndex+7))&0x000000FF)+								
    ((*(packet+magicWordIndex+6)<<8)&0x0000FF00)+
    ((*(packet+magicWordIndex+5)<<16)&0x00FF0000)+
    ((*(packet+magicWordIndex+4)<<24)&0xFF000000);
  
  msg.hMsg.dataType = ((*(packet+magicWordIndex+11))&0x000000FF)+								
    ((*(packet+magicWordIndex+10)<<8)&0x0000FF00)+
    ((*(packet+magicWordIndex+9)<<16)&0x00FF0000)+
    ((*(packet+magicWordIndex+8)<<24)&0xFF000000);
  
  msg.hMsg.timestamp =((*(packet+magicWordIndex+15))&0x000000FF)+								
    ((*(packet+magicWordIndex+14)<<8)&0x0000FF00)+
    ((*(packet+magicWordIndex+13)<<16)&0x00FF0000)+
    ((*(packet+magicWordIndex+12)<<24)&0xFF000000);
}


int AlascaComponent::splitMessage(const char * message, const int length)
{ 

  /*
  unsigned long i;
  unsigned long compteur;
    


  // verification que le message est complet 
  // msgheader.size = taille de Message Body (16 octets pour le message header)
  if (msgheader.size + 16 != length)
  {
    qDebug(" msgheader.size (%d) != length (%d) ", msgheader.size + 16, length);
    return 0; 
  }

  
  // 15 = FILE_TYPE_COMPRESSED_SCAN
  if  ((msgheader.dataType )!= 0x0F) { 
    printf ("-");   // affiche - si ce n'est pas un message SCAN
    return -1;
  } 	
  else 
  {        
    printf("+"); // affiche + quand il y a un nouveau message SCAN


    compteur=32;  
    
    nbOfPtsInScanList =  sc.nbPoint;
    //printf("nbOfPtsInScanList = %d\n",nbOfPtsInScanList); 
    
    for (i=0;i<	sc.nbPoint;i++)
    {    
      scanList[i].layerNumber = ((*(message+compteur))&(0xFF));
      scanList[i].x=       ((*(message+compteur+5))&(0x00FF))+
        ((*(message+compteur+4)<<8)&(0xFF00));
      
      
      scanList[i].y=        ((*(message+compteur+7))&(0x00FF))+
        ( (*(message+compteur+6)<<8)&(0xFF00));
      
      
      scanList[i].z=        ((*(message+compteur+9))&(0x00FF))+
        ((*(message+compteur+8)<<8)&(0xFF00));
      
      
      
      scanList[i].width=    ((*(message+compteur+11))&(0x00FF))+
        ((*(message+compteur+10)<<8)&(0xFF00));
      
            
      compteur+=12;
      
    }  

    
    return sc.nbPoint;
  } //fin de else
*/

  return 0; 

}




//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void AlascaComponent::customEvent(QCustomEvent * e)
{   
  unsigned long j,ptr,nbpoints,taille,i,size;
  int nbmessages  ;

  int type = e->type(); 
  
  FRAME * frame= (FRAME*)e->data(); 
  /*  
  // le paquet complet 
  char *messg = new char [frame->size];	
  
  memcpy(messg, frame->msg, frame->size);
  delete [] frame->msg;
  
  // resultat de splitPacket 
  char *messgtemp=new char[frame->size];
  j=0;
  size=frame->size;
  
  
  nbmessages=splitPacket(messg,size);
  if (nbmessages==0) 
  { 
    printf ("0");
    delete [] messgtemp;
    delete [] messg;
    return;
  }
  
  for (i=0;i<(nbmessages-1);i++)
  {         
    ptr=0;
    while (j<tabmessages[i+1])
    {
      memcpy(messgtemp+ptr,messg+j,1);
      ptr++;
      j++;
    }
    nbpoints= splitMessage(messgtemp,tabmessages[i+1]-tabmessages[i]);
  } 
  
  
  // tester si les octets restants sont completes pour constituer un message
  // sinon on leur memorise pour que la fois prochaine on leur complete
  memcpy(restOfBytes,messg+tabmessages[nbmessages-1],size-tabmessages[nbmessages-1]);
  if ((size-tabmessages[nbmessages-1])>=8)
  {  
    taille  =  (restOfBytes[4])&0x000000FF+								
      (restOfBytes[5]<<8)&0x0000FF00+
      (restOfBytes[6]<<16)&0x00FF0000+
      (restOfBytes[7]<<24)&0xFF000000;
    
    if ( taille <= (size-tabmessages[nbmessages-1]))                                                     
      nbpoints= splitMessage(restOfBytes,size-tabmessages[nbmessages-1]);
  } 
  
  delete [] messgtemp;
  delete [] messg;

  */
}




void AlascaComponent::Config()
{
  double 	CalOffsetX;		//Postponement in x-direction.
  double 	CalOffsetY;		//Postponement in y-direction.
  double 	CalOffsetZ;		//Postponement in z-direction.
  double 	HAngleOffset;	//Rotation horizontale du télémètre.
  double 	VAngleOffset;	//Rotation verticale du télémètre.
  double 	StartAngle;		//Angle de début.
  double 	EndAngle;		//Angle de fin.
  double 	ScanFrequency;	//Configure la fréquence de scan du télémètre.
  //bool	UpsideDown;		//Specifie si le télémètre est monté à l'envers. 
  
  
  cout<<"Configuration du telemetre"<<endl;
  cout<<"Offset x:"<<endl;	cin>>CalOffsetX;
  cout<<"Offset y:"<<endl;	cin>>CalOffsetY;
  cout<<"Offset z:"<<endl;;	cin>>CalOffsetZ;
  cout<<"Rotation vericale:"<<endl;	cin>>HAngleOffset;
  cout<<"Rotation horizontale:"<<endl;	cin>>VAngleOffset;
  cout<<"Angle de debut:"<<endl;	cin>>StartAngle;
  cout<<"Angle de fin:"<<endl;	cin>>EndAngle;
  cout<<"Frequence:"<<endl;	cin>>ScanFrequency;
  cout<<"Telemetre monte a l'envers? Oui=1, Non=0 :"<<endl;	
  cout<<"Configuration enregistree"<<endl;
  
  //Il faut ensuite envoyer les données au télémètre pour réaliser la configuration
  //A_socket->sendToServer(...); 
  // a voir si utile de renvoyer de nouveau la configuration au telemetre
  
  
  cout<<"Telemetre configure.\nRedemarrer l'ECU pour la prise en compte de la configuration\n";
  
}