//## begin module%1.4%.codegen_version preserve=yes
//   Read the documentation to learn more about C++ code generator
//   versioning.
//## end module%1.4%.codegen_version

//## begin module%3D86D3BE02DA.cm preserve=no
//	  %X% %Q% %Z% %W%
//## end module%3D86D3BE02DA.cm

//## begin module%3D86D3BE02DA.cp preserve=no
//## end module%3D86D3BE02DA.cp

//## Module: Communicator%3D86D3BE02DA; Pseudo Package body
//## Source file: C:\Program Files\Rational\Rose\C++\source\Communicator.cpp

//## begin module%3D86D3BE02DA.additionalIncludes preserve=no
//## end module%3D86D3BE02DA.additionalIncludes

//## begin module%3D86D3BE02DA.includes preserve=yes
#include "Commander.h"
//## end module%3D86D3BE02DA.includes

// Communicator
#include "Communicator.h"
//## begin module%3D86D3BE02DA.additionalDeclarations preserve=yes
#include "math.h"
//## end module%3D86D3BE02DA.additionalDeclarations


// Class Communicator 

//## begin Communicator::DONE%3D9ABF3E02B2.attr preserve=no  public: static int {UC} 0
const int Communicator::DONE = 0;
//## end Communicator::DONE%3D9ABF3E02B2.attr

//## begin Communicator::NOT_ONE_RESPONSE%3D9ABF600244.attr preserve=no  public: static int {UC} 1
const int Communicator::NOT_ONE_RESPONSE = 1;
//## end Communicator::NOT_ONE_RESPONSE%3D9ABF600244.attr

//## begin Communicator::ROBOT_LEFT%3D9ABF610014.attr preserve=no  public: static int {UC} 2
const int Communicator::ROBOT_LEFT = 2;
//## end Communicator::ROBOT_LEFT%3D9ABF610014.attr

Communicator::Communicator()
  //## begin Communicator::Communicator%3D86D3BE02DA_const.hasinit preserve=no
      : haveClusterPoint(false),
        waitingPeriod(450)
  //## end Communicator::Communicator%3D86D3BE02DA_const.hasinit
  //## begin Communicator::Communicator%3D86D3BE02DA_const.initialization preserve=yes
  //## end Communicator::Communicator%3D86D3BE02DA_const.initialization
{
  //## begin Communicator::Communicator%3D86D3BE02DA_const.body preserve=yes
  //## end Communicator::Communicator%3D86D3BE02DA_const.body
}

void Communicator::initialize(void)
{
  radio = Radio::getRadio();
  drive = Drive::getDrive();
  sir = SIR::getSIR();
  //actually drive and sir should be set by the 'initialize' method of 'Behavior'.
  //we do it here just to go sure
  compass = Compass::getCompass();

}


//## Other Operations (implementation)
int Communicator::activate ()
{
  //## begin Communicator::activate%3D9291FF02C6.body preserve=yes
  LCDPrintf("in comm.\n");
  drive->stop(); //stop drive commands from other behaviors
  emptyMessageQueue(); //the other robot might start sending immediately as it might finish its justification process earlier


  if (justify())
  {
    if (exchangeData())
    {
      return DONE;
    }
    else
      return NOT_ONE_RESPONSE;
  }
  else
    return ROBOT_LEFT;


  //## end Communicator::activate%3D9291FF02C6.body
}

bool Communicator::getHaveClusterPoint ()
{
  //## begin Communicator::getHaveClusterPoint%3D9D37BE03AC.body preserve=yes
  return haveClusterPoint;
  //## end Communicator::getHaveClusterPoint%3D9D37BE03AC.body
}

bool Communicator::setClusterPoint (double x, double y)
{
  //## begin Communicator::setClusterPoint%3D9EC02E0262.body preserve=yes
  if (!getHaveClusterPoint())
  {
    drive->setPosition(-x, -y, 0.0); //phi doesn't matter
    return true;
  }
  else
    return false;
  //## end Communicator::setClusterPoint%3D9EC02E0262.body
}

bool Communicator::justify ()
{
  //## begin Communicator::justify%3D9967D5010E.body preserve=yes
  return true;
  //## end Communicator::justify%3D9967D5010E.body
}

bool Communicator::exchangeData ()
{
  //## begin Communicator::exchangeData%3D99680B00D2.body preserve=yes
  LCDPrintf("exchangeData\n");
  //first send our data: position and our cluster point density/age
  double x, y, phi;
  drive->getPosition(x, y, phi);

  int heading = compass->getHeading(); //get a few readings as compass data is often not reliable
  myCommander->doSleep(50);
  int heading1 = compass->getHeading();
  myCommander->doSleep(50);
  int heading2 = compass->getHeading();
  myCommander->doSleep(50);
  int heading3 = compass->getHeading();

  LCDClear();
  LCDPrintf("compass 1 %d\n", heading);
  LCDPrintf("compass 2 %d\n", heading1);
  LCDPrintf("compass 3 %d\n", heading2);
  LCDPrintf("compass 4 %d\n", heading3);
//  KEYWait(KEY3);

  if ((heading==0) || (heading1==0) || (heading2==0) || (heading3==0)) //compass is probably not working
  {
    heading = -1;
  }
  else if ((fabs(heading-heading1)>4) || (fabs(heading-heading2)>4) || (fabs(heading-heading3)>4)
           ||
           (fabs(heading1-heading2)>4) || (fabs(heading1-heading3)>4)
           ||
           (fabs(heading2-heading3)>4)) //compass values are not steady
  {
    heading = -1;
  }
  else
  {
    heading = int((heading + heading1 + heading2 + heading3)/4.0);
  }
    
//  LCDClear();
//  while (true)
//  {
//    heading = compass->getHeading();
//    LCDSetPos(0,0);
//    LCDPrintf("my heading %d\n", heading);
//    OSWait(100);
//  }
//  KEYWait(KEY3);

  if (heading != -1) //sent data only if we have reliable compass readings
  {
    int messageToSendLength = 20; //for 5 ints
    BYTE messageToSend[messageToSendLength];

    int intsToSend[5];
    intsToSend[0] = int(x*1000.0); //we convert the double to int, accuracy of 1 mm
    intsToSend[1] = int(y*1000.0); //we convert the double to int, accuracy of 1 mm
    intsToSend[2] = int(phi); //angle has an accuracy of 1 degree
    intsToSend[3] = int(myClusterPointDensity*1000.0); //we convert the double to int, accuracy of 1/1000
    intsToSend[4] = heading;

    intArray2BYTEArray(5, intsToSend, messageToSend);

    bool active[MAXEYE];
    radio->getActiveRobots(active);
    LCDPrintf("my id %d\n", radio->getMyID());
    for (int id=1; id<MAXEYE; id++) //starts with 1, so should we go to id<=MAXEYE???
    {
      if (active[id])
        LCDPrintf("t");
      else
        LCDPrintf("f");
    }
    int time = timer.getCount();
    while (timer.getCount() - time < waitingPeriod)
    {
    }
    for (int id=1; id<MAXEYE; id++) //starts with 1, so should we go to id<=MAXEYE???
    {
      if (active[id] && (id!=radio->getMyID()))
      {
        LCDPrintf("sending to id %d\n", id);
        radio->send(id, messageToSendLength, messageToSend);
      }
    }
    LCDPrintf("sent\n");
  }
  else
  {
    bool active[MAXEYE];
    radio->getActiveRobots(active);
    LCDPrintf("my id %d\n", radio->getMyID());
    for (int id=1; id<MAXEYE; id++) //starts with 1, so should we go to id<=MAXEYE???
    {
      if (active[id])
        LCDPrintf("t");
      else
        LCDPrintf("f");
    }
    //waiting for answer
    int time = timer.getCount();
    while (timer.getCount() - time < waitingPeriod)
    {
    }
    LCDPrintf("compass failed\n");
  }

  LCDPrintf("wait done %d\n", radio->checkForMessages());
  if (radio->checkForMessages() == 1) //process the reply //the queue was emptied when entering the behavior, so if there is only a single reply we know that must be the robot we want to communicate with
  {
    LCDPrintf("one answer\n");
    BYTE receivedFromID;
    int receivedMessageLength;
    BYTE receivedMessage[MSGMAXLEN];
    radio->receive(&receivedFromID, &receivedMessageLength, receivedMessage); //get message

    int receivedInts[5];
    BYTEArray2IntArray(5, receivedMessage, receivedInts);
    double otherRobotX = double(receivedInts[0])/1000.0;
    double otherRobotY = double(receivedInts[1])/1000.0;
    double otherRobotPhi = double(receivedInts[2]);
    double otherRobotClusterPointDensity = double(receivedInts[3])/1000.0;
    int otherRobotHeading = receivedInts[4];
    
    LCDPrintf("o. x %f\n", otherRobotX);
    LCDPrintf("o. y %f\n", otherRobotY);
    LCDPrintf("o. phi %f\n", otherRobotPhi);
    LCDPrintf("o. d %f\n", otherRobotClusterPointDensity);
    LCDPrintf("o. h %d\n", otherRobotHeading);
//    KEYWait(KEY3);

    if ((otherRobotClusterPointDensity>myClusterPointDensity) || ((otherRobotClusterPointDensity==myClusterPointDensity) && (receivedFromID < radio->getMyID())))
    {
      myClusterPointDensity = otherRobotClusterPointDensity;

      double distanceToOtherRobotX = 0.12; //in local coordinates. We know it's about 0.12 in x direction as the robots are facing each other
      double distanceToOtherRobotY = 0.0; //in local coordinates. We know it's about 0.0 in y direction as the robots are facing each other
      double myNewX = 0;
      double myNewY = 0;
      double myNewPhi = 0;
      myNewPhi  =  otherRobotPhi + double(otherRobotHeading-heading);
      LCDPrintf("oPhi %f\n", otherRobotPhi);
      LCDPrintf("mH %d\n", heading);
      LCDPrintf("oH %d\n", otherRobotHeading);
      LCDPrintf("n phi %f\n", myNewPhi);
//      KEYWait(KEY3);
      drive->local2global(otherRobotX, otherRobotY, otherRobotPhi, distanceToOtherRobotX, distanceToOtherRobotY, myNewX, myNewY); //know we calculate at what position we are regarding the other robot's coordinate system
      drive->setPosition(myNewX, myNewY, myNewPhi);
      LCDPrintf("n x %f\n", myNewX);
      LCDPrintf("n y %f\n", myNewY);
      LCDPrintf("oPhi %f\n", otherRobotPhi);
      LCDPrintf("mH %d\n", heading);
      LCDPrintf("oH %d\n", otherRobotHeading);
      LCDPrintf("n phi %f\n", myNewPhi);
//      KEYWait(KEY3);
    }
    else
      LCDPrintf("I have higher prio.\n");

    return true;
  }
  LCDPrintf("m: %d\n", radio->checkForMessages());
  return false;
  //## end Communicator::exchangeData%3D99680B00D2.body
}

// Additional Declarations
  //## begin Communicator%3D86D3BE02DA.declarations preserve=yes
  void Communicator::int2BYTEArray(int valueToConvert, BYTE* arrayToContainValue)
  {
    BYTE* pointerToInt = (BYTE*)&valueToConvert;
    for (int i=0; i<4; i++)
    {
      arrayToContainValue[i] = pointerToInt[i]; //we make true copies, then our array retains the correct values even if the source (here: valueToConvert) ceases to exist
    }
  }

  void Communicator::intArray2BYTEArray(int numberOfValues, int* values, BYTE* arrayToContainValues)
  {
    BYTE arrayToContainOneValue[4];
    for (int i=0; i<numberOfValues; i++)
    {
      int2BYTEArray(values[i], arrayToContainOneValue);
      for (int j=0; j<4; j++)
      {
        arrayToContainValues[i*4+j] = arrayToContainOneValue[j]; //we make true copies, then our array retains the correct values even if the source (here: values) ceases to exist
      }
    }
  }

  void Communicator::BYTEArray2Int(BYTE* arrayContainingValue, int& value)
  {
    int* tempInt = (int*)arrayContainingValue;
    value = *tempInt; //we make true copies, then our int retains the correct value even if the source (here: arrayContainingValue) ceases to exist
  }

  void Communicator::BYTEArray2IntArray(int numberOfValues, BYTE* arrayContainingValues, int values[])
  {
    BYTE arrayToContainOneValue[4];
    for (int i=0; i<numberOfValues; i++)
    {
      for (int j=0; j<4; j++)
      {
        arrayToContainOneValue[j] = arrayContainingValues[i*4+j];
      }
      int tempInt;
      BYTEArray2Int(arrayToContainOneValue, tempInt);
      values[i] = tempInt; //we make true copies, then our ints retain the correct values even if the source (here: arrayContainingValues) ceases to exist
    }

  }
  void Communicator::emptyMessageQueue(void)
  {
    BYTE receivedFromID;
    int receivedMessageLength;
    BYTE receivedMessage[MSGMAXLEN];
    LCDPrintf("m? %d\n", radio->checkForMessages());

    while (radio->checkForMessages() > 0)
      radio->receive(&receivedFromID, &receivedMessageLength, receivedMessage); //get a message
    LCDPrintf("empty %d\n", radio->checkForMessages());
  }

  //## end Communicator%3D86D3BE02DA.declarations

//## begin module%3D86D3BE02DA.epilog preserve=yes
//## end module%3D86D3BE02DA.epilog