/*********************
* RB.c
* Robotics Bus
**********************/

/* ----- revision history:

 2006-08-05, Jimmy Sastra: Today I started this revision history.
 - Included actualPos (servo position) in every heartbeat.
 - Blink LED at every heartbeat on PIN_B0
 - Turn LED on/off every 30 TxPM and RxPM on Pin B1 and B4 respectively

*/

#include "RB.h"  // defines and prototypes needed by RBapp.c and RB.c
#include "RBapp.h" // defines the application-specific Bus Objects.
#include "canfunctions.c"   // Our entrypoint to CAN hardware- CANInit, CANSend, etc.
#include "ODaccess.c"      // object dictionary access functions


// Design Rationale:
//
// This code takes care of the Robotics Bus. The Robotics Bus is a modular
// system that facilitates interoperation of many devices in a mobile robot.
//
// Because bus communication is never going to be instantaneous, this code
// does not use interrupts. It is assumed that the application may use
// interrupts to perform its work. This code will not interfere with those
// interrupts.
//
// This bus does its work when URB_Process_Stack() is called from main().
// URB_Process_Stack() should be called at least every millisecond.
//
// Timers
//
// The Robotics Bus system controls Timer0. At least one timer is needed for
// generating a regular heartbeat. The other timers are available for use by
// the application.
//
// CAN Buffers.
//
// The PIC CAN controller has two recieve buffers and three transmit buffers.

// Macro for inquiring the operational state of the node. true=running. false=stopped.
#define URBOperational()      (URBhb.buf[0] == URB_HEARTBEAT_NORMAL)

// Initialize. called by RB_App_ResetApp from main().
// Also may be called by the main loop (RB_ProcessStack).
// Initializes the CAN hardware and RB variables.

void RB_Initialize() {

   // Set up a heartbeat timer.
   // Read config info from EEPROM. If NID=0 then set "factory defaults."
   // Set up heartbeat message in URBhb buffer.
   // Initialize the CAN controller.
   // Set up filters.
   // Set Process Message Identifiers.
   // Initialize the counters.

   setup_timer_0(RTCC_INTERNAL | RTCC_DIV_256);

   RB_setup_CAN();

   //enable_interrupts(GLOBAL); // start the application running

} // end of URBInitialize

// Get default identifiers out of the object dictionary,
// put them into the EEPROM for future use/modification.
// if a PM is not used, set its identifier to zero, so that
// its mask/filter is set harmlessly to zero.
// NOTE: THIS IS ONLY RUN ONCE AFTER WIPING THE ROM !
// routine initialization is handled in RB_initialize()
void RB_PM_SetDefaults(void) {
   int8 entry_num;
   int16 tmpIdent = 0;

   // RX PROCESS MESSAGES

   if (OD_lookup(0x1600, entry_num)) // if the PM exists,
   tmpIdent = RB_RXMAPPING_0_DEFAULT;
   else
      tmpIdent=0;
   RB_RXCONFIG_0 = tmpIdent;

   if (OD_lookup(0x1601, entry_num)) // if the PM exists,
   tmpIdent = RB_RXMAPPING_1_DEFAULT;
   else
      tmpIdent=0;
   RB_RXCONFIG_1 = tmpIdent;

   if (OD_lookup(0x1602, entry_num)) // if the PM exists,
   tmpIdent = RB_RXMAPPING_2_DEFAULT;
   else
      tmpIdent=0;
   RB_RXCONFIG_2 = tmpIdent;

   if (OD_lookup(0x1603, entry_num)) // if the PM exists,
   tmpIdent = RB_RXMAPPING_3_DEFAULT;
   else
      tmpIdent=0;
   RB_RXCONFIG_3 = tmpIdent;

   // TX PROCESS MESSAGES

   if (OD_lookup(0x1A00, entry_num))
      tmpIdent = RB_TXMAPPING_0_DEFAULT;
   else
      tmpIdent=0;
   RB_TXCONFIG_0 = tmpIdent;

   if (OD_lookup(0x1A01, entry_num))
      tmpIdent = RB_TXMAPPING_1_DEFAULT;
   else
      tmpIdent=0;
   RB_TXCONFIG_1 = tmpIdent;

   if (OD_lookup(0x1A02, entry_num))
      tmpIdent = RB_TXMAPPING_2_DEFAULT;
   else
      tmpIdent=0;
   RB_TXCONFIG_2 = tmpIdent;

   if (OD_lookup(0x1A03, entry_num))
      tmpIdent = RB_TXMAPPING_3_DEFAULT;
   else
      tmpIdent=0;
   RB_TXCONFIG_3 = tmpIdent;
    // need to automate this and do it for all four Tx/Rx
}

void RB_Send_PM(byte myBufNum)
{   
   if (URBOperational()) // send TPDO only if the node is operational
   {
      uint8 numMappings, mapCount, type;
        
      numMappings = RB_Get_Tx_Mapping_Length(myBufNum);
      RBtPM[myBufNum].len=0;

      for (mapCount=0; mapCount<numMappings; mapCount++) // number of mappings is the second value in the array. (followed by the mappings themselves)
      {
         type = RB_Get_Tx_Map_Type(myBufNum,mapCount);
         memcpy(&RBtPM[myBufNum].buf[RBtPM[myBufNum].len], RB_Get_Tx_Map_Address(myBufNum,mapCount), URB_Sizeof(type));
         if (type == RB_TYPE_FLOAT) URB_translate_microchip_to_ieee(&RBtPM[myBufNum].buf[RBtPM[myBufNum].len]);
         RBtPM[myBufNum].len += URB_Sizeof(type);
      }
      RBtPMready[myBufNum] = TRUE;
   }

   // Why, you ask, do we wait for main() to send process messages? Doesn't that
   // add needless latency? The problem is, CANSendMessage is called in main()
   // and if it is called in interrupts too, the same function is called twice
   // at the same time, one call interrupting the other, and data gets corrupted.
   // Identifiers, lengths, and data all become corrupted. This causes chaos for
   // configuration GUI programs, among other things, and it's not okay.
   // Therefore, ALL messages are sent from main() and interrupts can only
   // assemble the messages and then set the "ready" flag.

   // Also, for the same reason, it's not OK for interrupts to call any of
   // the same functions called by main(). So interrupts should not call
   // OD_lookup and others. However, URB_Sizeof and translate_microchip_to_ieee
   // are used inside this function which is called from interrupt. So these two
   // functions have been designated #inline.

}  // end of RB_Send_PM()


void URB_Send_SDO_Reply()
{
   while (!(CANSendMessage(URBTxSDO.ID,2,URBTxSDO.buf,URBTxSDO.len,\
   CAN_TX_PRIORITY_0 & CAN_TX_STD_FRAME & CAN_TX_NO_RTR_FRAME)))
   {
      URBErrorCode = URB_ERROR_WAITING_FOR_TXBUF;
   }

   URBErrorCode = URB_ERROR_OK;
}

void RB_Handle_Mapping_Request(int16 index, int8 subindex)
// the mapping has already been shown to exist in the object dictionary,
// although the length is not known. subindex is nonzero, and nonFF.
{
   uint8 myBufNum;
   uint16 tmpIndex;

   myBufNum = index & 0x00FF; // select which PM buff you want to read.
   // if this is not less than 4, there will be trouble.

   switch (index & 0xFF00) {

      case (0x1600):
       if (subindex == 1)
       {
         tmpIndex = RB_Get_Rx_Mapping_Length(myBufNum);
         URBTxSDO.len = 5;
       }
       else
       {
         tmpIndex = RB_Get_Rx_Map_Index(myBufNum, subindex-2);
         URBTxSDO.len = 6;
       }
      URBTxSDO.buf[4] = make8(tmpIndex,0);
      URBTxSDO.buf[5] = make8(tmpIndex,1);
      break;

      case (0x1A00):
       if (subindex == 1)
       {
         tmpIndex = RB_Get_Tx_Mapping_Length(myBufNum);
         URBTxSDO.len = 5;
       }
       else
       {
         tmpIndex = RB_Get_Tx_Map_Index(myBufNum, subindex-2);
         URBTxSDO.len = 6;
       }
      URBTxSDO.buf[4] = make8(tmpIndex,0);
      URBTxSDO.buf[5] = make8(tmpIndex,1);
      break;
   }

   URB_Send_SDO_Reply();
}

/*
 Object Dictionary Lookup

 Perform an index search in the object dictionary, and store the result
  in the value 'num'. Return TRUE if the value was found successfully.

 Uses a basic linear search.
*/

bool OD_lookup(int16 index, int8& num)
{
   int16 temp_index;
   bool index_found = FALSE;

   // search for requested object in object dictionary
   num = 0;   // start at the beginning of dictionary
   // object dictionary is ended with a zero-index-record:
   while ( (objectDictionary[num].index != 0) && (!index_found) )
   {
      temp_index = objectDictionary[num].index;
      #ifdef DEBUGGING
      printf("Looking at index: 0x%LX\r\n",temp_index);
      #endif
      // cycle through all available dictionary indices
      if ( temp_index == index )
         index_found = TRUE;    // found it
      else
         num++;     // go to the next entry in the table
   }
   return index_found;
}

void URB_Handle_SDO_Request(byte *data)
{
   int8 entry_num;
   int8 type, len, cmd, permissions;
   int8 subindex;
   int16 index;

   URBErrorCode = URB_ERROR_SDO_IN_PROCESS;
   cmd = data[0];
   index = make16(data[3],data[2]);
   subindex = data[1];

   #ifdef DEBUGGING
   printf("Packet in: %X to index 0x%LX, 0x%X\r\n",\
      cmd,index,subindex);
   #endif

   // combine two bytes to make the index and perform a lookup...
   // if the index is found, go on
   if ( OD_lookup(index, entry_num) )
   {
      // we found the requested object.
      #ifdef DEBUGGING
      printf("Index found...\r\n");
      #endif
      type = objectDictionary[entry_num].data_type;
      len = URB_Sizeof(type);  // number of bytes in value
      permissions = objectDictionary[entry_num].permissions;

      if (len | (type == RB_TYPE_PM_MAPPING)) // make sure length is valid, known type.
      {
         // begin to prepare the SDO reply
         //   (regardless of whether it is a query response or ack)
         URBTxSDO.ID = RB_DICT_RESP + RB_NODEID;
         URBTxSDO.buf[0] = cmd; // echo command.
         URBTxSDO.buf[1] = subindex; // echo subindex, index ...
         URBTxSDO.buf[2] = make8(index,0);
         URBTxSDO.buf[3] = make8(index,1);

         if (cmd == RB_MSG_DICT_READ)
         {
            URBErrorCode = URB_ERROR_READ_IN_PROCESS;

            if (subindex == 0xFF) // send the "next" link
            {
               index = objectDictionary[entry_num + 1].index;
               URBTxSDO.buf[4] = make8(index,0);
               URBTxSDO.buf[5] = make8(index,1);
               URBTxSDO.len = 6;
               URB_Send_SDO_Reply();
               URBErrorCode = URB_ERROR_OK;
            }
            else if (subindex == 0) // send perms and typs
            {
               URBTxSDO.len = 6;
               URBTxSDO.buf[4] = (permissions & RB_PERM_BITS);
               URBTxSDO.buf[5] = type;
               URB_Send_SDO_Reply();
               URBErrorCode = URB_ERROR_OK;
            }
            else if (type == RB_TYPE_PM_MAPPING)
            {
               URBErrorCode = URB_ERROR_MAPPING_IN_PROCESS;

               // Mapping objects have no label string,
               // nor fixed length. They are handled as
               // a special case.
               RB_Handle_Mapping_Request(index,subindex);
            }
            else if (subindex>=0xF7) // send label string
            {
               int i;
               int s;
               // handle label string read as a special case.
               URBTxSDO.len = 8; // use all four data bytes for chars
               s = 4*(subindex-0xF7);
               // load the string characters into the outgoing message
               for (i=4;i<8;i++)
               {
                  URBTxSDO.buf[i] = objectDictionary[entry_num].name[s];
                  s++;
               }
               URB_Send_SDO_Reply();
               URBErrorCode = URB_ERROR_OK;
            }
            else if ((subindex == 1) && (permissions & RB_PERM_READ_BIT))
            {
               OD_read_data(&URBTxSDO.buf[4],entry_num,type);
               URBTxSDO.len =  4+len;

               URB_Send_SDO_Reply();
               URBErrorCode = URB_ERROR_OK;
            }
            else
            {
               // else read not allowed
               URBErrorCode = URB_ERROR_SDO_PERMISSIONS;
            }
         } // end read command
         else if (cmd == RB_MSG_DICT_WRITE)
         {
            // writes are not allowed for subindices besides "1"
            if ((permissions & RB_PERM_WRITE_BIT) && (subindex == 1))
            {
               // write allowed, so we write the value and then
               //     read it back for verification.

               // now write the data
               OD_write_data(&data[4],entry_num,type);

               // read it back to confirm
               OD_read_data(&URBTxSDO.buf[4],entry_num,type);

               URBTxSDO.len = 4+len;

               URB_Send_SDO_Reply();
               URBErrorCode = URB_ERROR_OK;
            }
            else
            {
               // else, write not allowed
               URBErrorCode = URB_ERROR_SDO_PERMISSIONS;
            }
         } // end write command
         else
         {
            URBErrorCode = URB_ERROR_SDO_UNKNOWN_COMMAND;
         } // end default
      } // end valid data length
   } // end found index
   else
   {
      #ifdef DEBUGGING
      printf("Index Not Found...\r\n");
      #endif
      URBErrorCode = URB_ERROR_SDO_NOT_FOUND; // index not found in object dictionary
   } // end not found index

} // sdoRequest

// check timer, send heartbeat. return 1 if sent, else 0.
bool URBMaintainHeartbeat() {
   // this way, we don't need to use interrupts, which can be used by application.

	if ( get_timer0()  > URB_HEARTBEAT_INTERVAL)  {
      output_toggle(PIN_D6);
      // if timer0 goes past one second,
      // send heartbeat
      URBhb.buf[1] = TXERRCNT;
      URBhb.buf[2] = RXERRCNT;
      URBhb.buf[3] = URBErrorCode;
      memcpy(&URBhb.buf[4], &actualPos,2); // include actual position
      // use lowest priority, this message is not critical, do an unconfirmed send.
      CANSendMessage(URBhb.ID,2,URBhb.buf,URBhb.len, CAN_TX_PRIORITY_0 & CAN_TX_STD_FRAME);
      // prepare for next heartbeat
	  feed_mult=1;
      set_timer0(0); // heartbeat interval is half of the longest period of the timer0.
      return TRUE;
   }
   else
      return FALSE;
}// end heartbeat

bool RB_Process_Outgoing_PM() {
   int i;
   for (i=0; i<4; i++)

      if (RBtPMready[i])
         if (CANSendMessage(RBtPM[i].ID,(i % 2),RBtPM[i].buf,RBtPM[i].len,
            CAN_TX_PRIORITY_0 & CAN_TX_STD_FRAME & CAN_TX_NO_RTR_FRAME))
            // There are three tx buffers. we use buffer # (myBufNum % 2) to
            // "randomly" select buff 1 or 0, to balance the tx load between the two.
            // (Buff 2 is used for heartbeats and dictionary responses.)
            RBtPMready[i] = FALSE;

	return 0;
}//outgoingPM

/*****
* URB Handle Receive Process Data Object
*
* the main program has already found a match between RPDO[index]
* and this incoming message. All left to do is copy the bytes into the
* mapped objects.
*
*******/

void URB_Handle_rPDO(byte *data, byte myBuf)
{
	CAN_MSG myMSG;
    uint8 numMappings, mapCount, bufCount, type, index;
      
    numMappings = RB_Get_Rx_Mapping_Length(myBuf);

    bufCount=0;

    for (mapCount=0; mapCount<numMappings; mapCount++)
    {
       type = RB_Get_Rx_Map_Type(myBuf,mapCount);
       memcpy(RB_Get_Rx_Map_Address(myBuf,mapCount), &data[bufCount], URB_Sizeof(type));
       if (type == RB_TYPE_FLOAT) URB_translate_ieee_to_microchip(&data[bufCount]);
       bufCount += URB_Sizeof(type);
    }
}

void RB_reset_from_eeprom()
{
	RB_NODEID = read_eeprom(RB_EEPROM_NODEID);
   // ID must be within proper range.
   // If NodeID is zero, it means the chip has just been programmed.
   if ((RB_NODEID == 0) || (RB_NODEID > 253))
   {
      write_eeprom( RB_EEPROM_NODEID, RB_NODEID_DEFAULT);
      RB_PM_SetDefaults(); // set identifiers on Process Messages
      RB_App_SetFactoryDefaults();
   }
   RB_NODEID = read_eeprom(RB_EEPROM_NODEID);

   // heartbeat setup
   URBhb.ID = RB_HEARTBEAT + (int16) RB_NODEID;
   URBhb.len = 6;
   URBhb.buf[0] = URB_HEARTBEAT_NORMAL;
}

void RB_setup_CAN()
{
   uint8 i;

   // SJW, BRP, PHSEG1, PHSEG2, PROPSEG
   CANInitialize(1,2,6,6,7,CAN_CONFIG_VALID_STD_MSG & CAN_CONFIG_DBL_BUFFER_ON); //works for 250kbps for 20Mhz
   // 11557 for 500kbps  / 18 Mhz
   // 12557 for 250kbps / 18 Mhz
   // 14557 for 125kbps  / 18 Mhz
   // 15557 for 100kbps  / 18 Mhz
   // 1,10,557 for 50 kbps  / 18 Mhz
   // 1,25,557 for 20  / 18 Mhz

   // Filters will be set up with rollover from Buff0 to Buff1 enabled (the default).
   // Receive Buffer 1, ("B1") the higher priority buffer because it can rollover to B2,
   // will be set up to recieve only NMT messages with Node ID 0,
   // and Dictionary Request messages addressed directly to this node.
   //
   // Receive Buffer 2 ("B2"), however, will catch Process Messages
   // which have one of several Process Message IDs- which are read from EEPROM and put into the
   // broadcast by other nodes. Because this filter can't roll over into B1,
   // it is considered lower priority and PMs may occasionally be lost.

   CANSetOperationMode(CAN_OP_MODE_CONFIG); // must be in config mode to change masks/filters

   // Set Up B1
   CANSetMask(CAN_MASK_B1,CAN_MASK_ELEVEN_BITS , CAN_CONFIG_STD_MSG); // screen on the basis of all CAN-ID bits
   CANSetFilter(CAN_FILTER_B1_F1, RB_DICT_REQ + RB_NODEID, CAN_CONFIG_STD_MSG); // allow SDO messages for own node
   CANSetFilter(CAN_FILTER_B1_F2, RB_NMT, CAN_CONFIG_STD_MSG); // also allow NMT msg with NodeID 0 (master node).

   // Set up B2 - for incoming Rx PMs
   CANSetMask(CAN_MASK_B2, CAN_MASK_ELEVEN_BITS, CAN_CONFIG_STD_MSG); // screen on the basis of all 11 bits

   // Add up to four different identifiers.
   // The number of incoming PMs is limited by the number of filters
   // in the particular CAN controller - in this case four.
   RB_RXCONFIG_0 = RB_NODEID + RB_COMMAND;
   RB_RXCONFIG_1 = RB_NODEID + RB_USART;
   RB_RXCONFIG_2 = RB_NULL;
   RB_RXCONFIG_3 = RB_NULL;

   RBrPM[0].ID = RB_RXCONFIG_0;
   RBrPM[1].ID = RB_RXCONFIG_1;
   RBrPM[2].ID = RB_RXCONFIG_2;
   RBrPM[3].ID = RB_RXCONFIG_3;
   CANSetFilter(CAN_FILTER_B2_F1,RB_RXCONFIG_0, CAN_CONFIG_STD_MSG);
   CANSetFilter(CAN_FILTER_B2_F2,RB_RXCONFIG_1, CAN_CONFIG_STD_MSG);
   CANSetFilter(CAN_FILTER_B2_F3,RB_RXCONFIG_2, CAN_CONFIG_STD_MSG);
   CANSetFilter(CAN_FILTER_B2_F4,RB_RXCONFIG_3, CAN_CONFIG_STD_MSG);

    // set tPM identifiers
	RB_TXCONFIG_0 = RB_NODEID + RB_SENSOR;
	RB_TXCONFIG_1 = 0;
	RB_TXCONFIG_2 = 0;
	RB_TXCONFIG_3 = 0;
	RBtPM[0].ID = RB_TXCONFIG_0;
	RBtPM[1].ID = RB_TXCONFIG_1;
	RBtPM[2].ID = RB_TXCONFIG_2;
	RBtPM[3].ID = RB_TXCONFIG_3;
   CANSetOperationMode(CAN_OP_MODE_NORMAL); // turn the CAN back on after setting filters.

   for (i=0; i<4; i++)
   {
      RBtPMready[i] = FALSE; // no data ready to send
   }

   URBRecvCounter = 0;
   URBRecvOverflowCounter = 0;
   URBErrorCode = URB_ERROR_OK;
}

/*****
* URB Process Stack:
*
* this function should be called at least once every 1-2 msec.
*
* - generate heartbeat message
*
* - respond to NMT requests to Stop/Start
*
* - respond to SDO requests to get/set values
*
* - respond to RPDO by setting the appropriate variables.
*
*******/

byte RB_ProcessStack() {
   byte ret_val = 0; // will return 1 if a message is recieved or sent
   byte  RcvMessageFlags;

   // first, process incoming messages
   if (CANReceiveMessage (&URBStackRcv.ID, URBStackRcv.buf,&URBStackRcv.len,&RcvMessageFlags))
   {
      ret_val = 1;
      URBRecvCounter++;
      if (RcvMessageFlags & (CAN_RX_XTD_FRAME | CAN_RX_RTR_FRAME)) { // don't accept these strange things.
         URBErrorCode = (RcvMessageFlags & 0x60);  // see URB.h for error codes.
      }
      else {
         if (RcvMessageFlags & CAN_RX_OVERFLOW) { // it's not clear what this flag means, but count it.
            URBRecvOverflowCounter++;
         }
         // NOW PROCESS INCOMING MESSAGE!
         URBErrorCode = URB_RX_IN_PROCESS;

         if (URBStackRcv.ID == 0) { // high-priority NMT (start/stop) message
            if ((URBStackRcv.buf[1] == RB_NODEID) || (URBStackRcv.buf[1] == 0)) {
               //if (URBStackRcv.len != 2) URBErrorCode = URB_ERROR_NMT_INVALID_LENGTH; else
               switch (URBStackRcv.buf[0]) {
                  case 1:
                  URBhb.buf[0] = URB_HEARTBEAT_NORMAL;
                  break;  // start the node running
                  case 2:
                  URBhb.buf[0] = URB_HEARTBEAT_STOPPED;
                  break; // stop the node.
                  // preoperational state not implemented
                  case RB_NODE_RESET:
                  RB_App_ResetApp();
                  break; // reset the application.
                  default:
                  URBErrorCode = URB_ERROR_NMT_UNKNOWN_COMMAND;
                  break; // unknown NMT command, not implemented
               } // end switch
            } // end addressed to us
            else URBErrorCode = URB_ERROR_NMT_NOT_ADDRESSED;
         } // end NMT

         else if (URBStackRcv.ID == RB_DICT_REQ + RB_NODEID) // is it an SDO request?
         {
            if (URBStackRcv.len > 3)
               URB_Handle_SDO_Request(URBStackRcv.buf);
            else
               URBErrorCode = URB_ERROR_SDO_MALFORMED;
         }//sdo
         //start rpdo
         else if (URBOperational()) { // if node is operational, handle PMs
            if (URBStackRcv.ID == RBrPM[0].ID)
               URB_Handle_rPDO(URBStackRcv.buf, 0); // copy bytes to rpdo 0

            else  if (URBStackRcv.ID == RBrPM[1].ID)
            	URB_Handle_rPDO(URBStackRcv.buf, 1); // copy bytes to rpdo 1

            else  if (URBStackRcv.ID == RBrPM[2].ID)
               URB_Handle_rPDO(URBStackRcv.buf, 2); // copy bytes to rpdo 2

            else  if (URBStackRcv.ID == RBrPM[3].ID)
               URB_Handle_rPDO(URBStackRcv.buf, 3); // copy bytes to rpdo 3
         } // node operational, handle PMs
      } // end message is valid
   } // end recv message

   // generate the heartbeat message (if it's time) but don't block on it
   ret_val |= URBMaintainHeartbeat();

   // check to see if there are any Process Messages queued to send.
   ret_val |= RB_Process_Outgoing_PM();

   return ret_val;
} // end URBProcessStack