/*
Copyright 2003/2004, Freie Universitaet Berlin. All rights reserved.

These sources were developed at the Freie Universität Berlin, Computer
Systems and Telematics group.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

- Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.

- Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
 
- Neither the name of Freie Universitaet Berlin (FUB) nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.

This software is provided by FUB and the contributors on an "as is"
basis, without any representations or warranties of any kind, express
or implied including, but not limited to, representations or
warranties of non-infringement, merchantability or fitness for a
particular purpose. In no event shall FUB or contributors be liable
for any direct, indirect, incidental, special, exemplary, or
consequential damages (including, but not limited to, procurement of
substitute goods or services; loss of use, data, or profits; or
business interruption) however caused and on any theory of liability,
whether in contract, strict liability, or tort (including negligence
or otherwise) arising in any way out of the use of this software, even
if advised of the possibility of such damage.

This implementation was developed by the CST group at the FUB.
Contributors: Bjoern Lichtblau

For documentation and questions please use the web site
http://scatterweb.mi.fu-berlin.de and the mailinglist
scatterweb@lists.spline.inf.fu-berlin.de (subscription via the Website).
Berlin, 2003/2004/2005
*/

#include "ScatterWeb.System.h"

#define POTI_PORT     (P2OUT)
#define POTI_INC      (0x10)
#define POTI_UD       (0x20)
#define POTI_CS       (0x40)
#define STARTBYTE0    (0xFF)
#define STARTBYTE1    (0x01)
#define STARTBYTE2    (0x7F)
#define DUMMYBYTE     (0x00)
#define   PACKET_TO      	(0)
#define   PACKET_FROM  	 	(2)
#define   PACKET_TYPE  		(4)
#define   PACKET_NUM    	(5)
#define   PACKET_LENGTH  	(6)
#define   PACKET_DATA    	(8)
#define   RXSTATE_ERROR     0          ///< Receied packet is wrong, see receivedFlags. 
#define   RXSTATE_START1    1          ///< Waiting for first start byte 0x01. 
#define   RXSTATE_START2    2          ///< Waiting for second start byte 0x7F. 
#define   RXSTATE_BYTE      3          ///< Waiting for first half of a byte. 
#define   RXSTATE_BYTE2     4          ///< Waiting for second half of a byte.
#define   RXSTATE_CSUM1     5
#define   RXSTATE_CSUM2     6
#define   RXSTATE_CSUM3     7
#define   RXSTATE_CSUM4     8
#define   RXSTATE_FULL      9          ///< Complete packet received. 
#define RXERROR_TOO_LONG       (0x02)  ///< Correct length received, but to long for rxBufer.
#define RXERROR_CHECKSUM       (0x10)  /// 
#define MAX_RXNUM_TABLE     (5)        ///< Length of ::rxNumTable. 
#define RTX_IDLE        (0x10)  ///< Idle state. Set to ::RTX_WAITING when ::radio_tx_heartbeat detects data in buffer. All control is then done by the tx interrupt.
#define RTX_WAITING     (0x11)  ///< Waiting for free channel. Set to ::RTX_BACKOFF when free channel is recognized.
#define RTX_BACKOFF     (0x12)  ///< Backoff Time is counted down. Set to ::RTX_WAITING if channel is busy again and set to ::RTX_TURNON if backoff time elapsed.
#define RTX_TURNON      (0x13)  ///< Puts a constant 1 for some bytes on the receiver for packet detection, then goes to ::RTX_PREAMBLE
#define RTX_PREAMBLE    (0x14)  ///< Send some preamble bytes (0xAA) and then goes to RTX_SENDING1.
#define RTX_FF          (0x15)  ///< Send 0xFF.
#define RTX_01          (0x16)  ///< Send 0x01.
#define RTX_7F          (0x17)  ///< Send 0x7F.
#define RTX_SENDING1    (0x18)  ///< Toggles between ::RTX_SENDING1 and ::RTX_SENDING2 until all data is sent.
#define RTX_SENDING2    (0x19)  ///< Toggles between ::RTX_SENDING1 and ::RTX_SENDING2 until all data is sent.
#define RTX_POSTAMBLE   (0x1A)  ///< Send some postamble bytes.
#define RTX_ACKWAITING  (0x1B)  ///< We are waiting for an ACK.
#define RTX_SENT        (0x1C)  ///< Final state after sending one packet. ::tx_heartbeat takes control again.
#define RTX_NOTSENT     (0x1D)  ///< Final state after packet has been sent MAX_RETRIES without ack.
#define RTX_A_TURNON    (0x20)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_PREAMBLE  (0x21)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_FF        (0x22)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_01        (0x23)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_7F        (0x24)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_SENDING1  (0x25)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_SENDING2  (0x26)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_A_POSTAMBLE (0x27)  ///< Likes corresponding state, for sending from the ::ackBuffer.
#define RTX_S_SENDING1  (0x28)
#define RTX_S_SENDING2  (0x29)
#define UINT(x)  (*((UINT16*)& x))
#define RADIOTX_RB_SIZE		    (RADIO_TXBUF_SIZE)
#define RADIOTX_RB_LEN(b)		  ( (b).tail >= (b).head ? (b).tail - (b).head : (b).tail + RADIOTX_RB_SIZE - (b).head )
#define RADIOTX_RB_FREE(b)    ( RADIOTX_RB_SIZE - (RADIOTX_RB_LEN(rtxrb) - 1))
#define RADIOTX_RB_EMPTY(b)	  ( (b).head == (b).tail )
#define RADIOTX_RB_FULL(b)		( RADIOTX_RB_LEN(b) == (RADIOTX_RB_SIZE-1) )
#define RADIOTX_RB_INIT(b)		{ (b).head = 0; (b).tail = 0;							}
#define RADIOTX_RB_GET(b, x)	{ x=(b).buffer[(b).head]; if(++((b).head) >= RADIOTX_RB_SIZE) (b).head = 0;	}
#define RADIOTX_RB_I(b, i)    ( (b).buffer[( ((b).head+i) >= RADIOTX_RB_SIZE ? ((b).head+i) - RADIOTX_RB_SIZE : ((b).head+i) )] )
#define RADIOTX_RB_INT(b, i)   UINT(RADIOTX_RB_I(b, i))
#define RADIOTX_RB_PUT(b, x)	{ (b).buffer[(b).tail]=x; if(++((b).tail) >= RADIOTX_RB_SIZE) (b).tail = 0; 	}
#define INIT_FCS  0xffff   /* Initial FCS value */
#define MAN_ENCODE(unenc)     ((((unenc & 0xAA) | ((~unenc & 0xAA)>>1))<<8) | ((unenc & 0x55) | ((~unenc & 0x55)<<1)))
#define MAN_DECODE(odd, even) ((odd & 0xAA)|(even & 0x55))
#define RX_STANDARD_LENGTH     (50)   ///< How many bytes are received when the length is bad.
#define BACKOFF_LENGTH        (50)  ///< Backoff delay will be randomally chosen from [1, BACKOFF_DELAY+1]
#define TURNON_LENGTH         (2)   ///< How much bytes is the constant 1 kept when turning on?
#define PREAMBLE_LENGTH       (5)   ///< Length of preamble (in fact its +1 long).
#define POSTAMBLE_LENGTH      (3)   ///< Length of postamble.
#define ACKWAITING_LENGTH     (30)  ///< Number of bytes to wait for an acknowledgement.
#define MAX_TX_RETRIES        (15)  ///< Maximum count of retries.
volatile UINT8 receivedFlags;          ///< Flags for received packet.
volatile UINT8 rxState;                ///< State of receiving state machine.
volatile UINT16 rxBufPos;              ///< Next position in the buffer.
volatile UINT16 rxDataLength;          ///< Length of the packet, set after the sixth byte.
volatile UINT8 rxBuf[RADIO_RXBUF_SIZE+PACKET_DATA+2]; ///< Receiving buffer.
volatile UINT8 cdCounter;              ///< If >0 radio someone is sending.
volatile UINT8 detect;
volatile packet_t rxPacket;            ///< Static struct for received Packets, user functions should use this!
volatile time_t rxTime;
volatile UINT16 rx_c_csum;
volatile UINT8 rx_r_csum[2];
volatile UINT8 txState;                 ///< State of transmitting state machine.
volatile UINT16 txCounter;                ///< General counter used in different ways depending on ::txState.
volatile UINT16 txLength;                 ///< Length of packet to transmit.
volatile UINT16 txPos;                    ///< Current pos in packet to transmit.
volatile UINT16 txEncodeBuf;              ///< Encoding buffer while transmitting.
volatile UINT8 txRetries;               ///< Number of retries done.
volatile UINT8 txNum;                   ///< Current sequence number used in next packet.
volatile UINT8 ackBuffer[8];            ///< Holds the acknowledge packet.
volatile UINT8 data1, data2;
packet_t sentPacket;
UINT8 rxNumTablePos;                    ///< Next pos to enter a new value into ::rxNumTable.

volatile struct {
  time_t synchTime;
  UINT16 checksum;
} synch;

struct {                        
  UINT16 id;
  UINT8 num;
} rxNumTable[MAX_RXNUM_TABLE]; ///< This table holds previously received packet numbers with corresponding id.

volatile struct { 
	UINT8 buffer[RADIOTX_RB_SIZE];
	UINT16 head, tail;
} rtxrb; ///< Ringbuffer

UINT16 const ccitt_crc16_table[256] = {
 	0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
 	0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
 	0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
 	0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
 	0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
 	0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
 	0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
 	0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
 	0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
 	0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
 	0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
 	0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
 	0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
 	0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
 	0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
 	0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
 	0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
 	0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
 	0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
 	0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
 	0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
 	0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
 	0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
 	0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
 	0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
 	0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
 	0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
 	0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
 	0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
 	0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
 	0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
 	0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
};
 
UINT16 ccitt_fcs(UINT16 fcs, UINT8 c) {
 	return (((fcs) >> 8) ^ ccitt_crc16_table[((fcs) ^ (c)) & 0xff]);
}
 
UINT16 crc_calc(UINT16 fcs, UINT8 const *buf, UINT16 len) {
	while (len--) fcs = ccitt_fcs(fcs, *buf++);
	return fcs;
}

void setFunkRx() {
  #if defined(ESS430)
  P3SEL = 0xE0; 
  P2OUT |= 0xC0; 
  #else
  P3SEL = 0xE0; 
  P5OUT |= 0xC0;     
  #endif
}

void setFunkOff() {
  #if defined(ESS430)
  P2OUT &= 0x3F;
  #else
  P5OUT &= 0x3F;
  #endif
}

void sendFunk(UINT8 data) {
  while (!(IFG1 & UTXIFG0));        // loop until USART0 TX buffer ready
  TXBUF0 = data;
}

void rxClear() {
  runModule &= ~MF_RADIO_RX;
  rxBufPos = 0;
  rxDataLength = 0;
  rxpValue = 0;
  receivedFlags = 0;
  rxState = RXSTATE_START1; 
  rx_c_csum = INIT_FCS;
  rx_r_csum[0] = 0x00;
  rx_r_csum[1] = 0x00;
}

void printRxPacket() {
  if(rxState==RXSTATE_FULL)  Comm_print("+ ");
  if(rxState==RXSTATE_ERROR) Comm_print("- ");
  if(receivedFlags & RXERROR_CHECKSUM)     Comm_print("CS [csr: %x csc: %x]", UINT(rx_r_csum[0]), rx_c_csum);
  Comm_print("(%i)", detect);
  Comm_print("%b[%i -> %i] %i,%i\r\n", rxBuf[PACKET_TYPE], UINT(rxBuf[PACKET_FROM]), UINT(rxBuf[PACKET_TO]), 
                                                        UINT(rxBuf[PACKET_LENGTH]), rxBuf[PACKET_NUM]);
  if(rxBuf[PACKET_TYPE] == PING_PACKET && rxState==RXSTATE_ERROR){
    UINT16 i; 
    Comm_print("WRONG BYTES: ");
    for(i=0; i<rxPacket.data_length; i++){
      if( rxPacket.data[i] != (UINT8)i ){
        Comm_print("%i(%i) ", i, rxPacket.data[i]);
      }
    }
    Comm_print("\r\n");
  }
}

void txClear() {
  	RADIOTX_RB_INIT(rtxrb); // init/clear buffer
  	txState = RTX_IDLE;
  	txCounter = 0;
  	// init ack buffer statics: type and length
    ackBuffer[PACKET_TYPE] = ACK_PACKET;
    UINT(ackBuffer[PACKET_LENGTH]) = 0x0000;
}

interrupt (UART0RX_VECTOR) radio_rx_isr() {
  UINT8 rec;
  if(U0RCTL & (FE + PE + OE + BRK + RXERR)){ // OE occurs when interrupts disabled for longer time like with sendIR
    U0RCTL &= ~ (FE + PE + OE + BRK + RXERR); // Users guide recommends always resetting this 
  }
  rec = RXBUF0;
  switch(rxState){
    case RXSTATE_START1:  // Waiting for the first startbyte 0x01.
      if(rec==STARTBYTE1) rxState=RXSTATE_START2;  
      return;
    case RXSTATE_START2: // Waiting for the second startbyte 0x7F.
      if(rec==STARTBYTE2){
        rxState = RXSTATE_BYTE;
        detect = cdCounter;
        cdCounter = 20; // It takes 8 bytes to receive the length, doubled = 16
        { extern volatile time_t currentTime; rxTime = currentTime; }
      }
      else rxState = RXSTATE_START1;
      return;
    case RXSTATE_BYTE: // Receive a data byte.
      data1 = rec;
      rxState = RXSTATE_BYTE2;
      return;
    case RXSTATE_BYTE2: // Check the negated byte.
      // Note: this case gets very long if its the last byte!!
      rxState = RXSTATE_BYTE; // Should be the next state if no error occured  
      rxBuf[rxBufPos] = MAN_DECODE(data1, rec);
      rx_c_csum = ccitt_fcs(rx_c_csum, rxBuf[rxBufPos]);
      rxBufPos++;  
      // if pos==PACKET_DATA whole header is received => read out the length
      if(rxBufPos==PACKET_DATA){
          rxDataLength = UINT(rxBuf[PACKET_LENGTH]);
          if(rxDataLength > RADIO_RXBUF_SIZE){ 
            receivedFlags |= RXERROR_TOO_LONG;
            rxState = RXSTATE_ERROR;
            WAKEUPLPM1(radio_rx_isr, MF_RADIO_RX);
            return;
          }
          // Update cdCounter to length of Packet.
          cdCounter = (rxDataLength << 1) + 2 + 10; // one cdCounter tick is 1 byte                    
      }
      // check if no more bytes to receive
      if( rxBufPos - (PACKET_DATA) == rxDataLength ){
          // Special case. If its an ACK and ids and numbers match
          // release our transmitting state machine from RTX_ACKWAITING. ACKs have no Checksum.
          if( (rxBuf[PACKET_TYPE]==ACK_PACKET)){
            if( (txState==RTX_ACKWAITING) &&
                ( RADIOTX_RB_INT(rtxrb, PACKET_TO) == UINT(rxBuf[PACKET_FROM]) ) && 
                ( RADIOTX_RB_I(rtxrb, PACKET_NUM)  == rxBuf[PACKET_NUM]) ){
                  txState = RTX_SENT;
                  WAKEUPLPM1(radio_rx_isr, MF_RADIO_TX);
            }
            rxState = RXSTATE_FULL;
            WAKEUPLPM1(radio_rx_isr, MF_RADIO_RX);
          }  
          else rxState = RXSTATE_CSUM1;         
      } 
      return;
    case RXSTATE_CSUM1:
     data1 = rec;
      rxState = RXSTATE_CSUM2;
      return;
    case RXSTATE_CSUM2:
      rx_r_csum[0] = MAN_DECODE(data1, rec);
      rxState = RXSTATE_CSUM3;
      return;
    case RXSTATE_CSUM3:
      data1 = rec;
      rxState = RXSTATE_CSUM4;
      return;
    case RXSTATE_CSUM4:
      rx_r_csum[1] = MAN_DECODE(data1, rec);  
        if( UINT(rx_r_csum[0]) != rx_c_csum ) {
                receivedFlags |= RXERROR_CHECKSUM;
                rxState = RXSTATE_ERROR;
                WAKEUPLPM1(radio_rx_isr, MF_RADIO_RX);
                break;
        }
      // Checksum ok, If packet is not a broadcast then ack this packet!      
      if(Configuration.id == UINT(rxBuf[PACKET_TO]) && txState <= RTX_BACKOFF){ // If a packet was recently received txState machine should always be somewhere before RTX_SENDING
          // Create acknowledge in ackBuffer.
          // Do ackBuffer[PACKET_TO]=rxBuf[PACKET_FROM] and ackBuffer[PACKET_FROM]=rxBuf[PACKET_TO with int instructions.             
          UINT(ackBuffer[PACKET_TO]) = UINT(rxBuf[PACKET_FROM]);
          UINT(ackBuffer[PACKET_FROM]) = UINT(rxBuf[PACKET_TO]);
          ackBuffer[PACKET_NUM] = rxBuf[PACKET_NUM];                          
          txState = RTX_A_TURNON;
      }       
      rxState = RXSTATE_FULL;
      WAKEUPLPM1(radio_rx_isr, MF_RADIO_RX);
      cdCounter=8; // Safety gap after packet finish.
      return;
    case RXSTATE_FULL:  // nothing to do here, wait for someone to get the packet or error message
    case RXSTATE_ERROR: // wait until error is processed
      return;     
    default:
      Comm_print("! undefined rxState: %b\r\n", rxState); // should never occur
      rxClear();
      return;
  }
}

interrupt (UART0TX_VECTOR) radio_tx_isr (void) {
  // decrease cdCounter  
  if(cdCounter>0) cdCounter--; 
  // Send dummy byte to provide byte timing. Dont forget this in every case, else interrupt will starve.  
  switch(txState){
    // IDLE and SENT are processed by radio_tx_heartbeat!!!
    case RTX_IDLE: 
      TXBUF0 = 0x00;
      return;
    // WAITING: Check for free channel.
    case RTX_WAITING:
      TXBUF0 = 0x00;
      if(cdCounter == 0){                       // Channel is free!
        //txCounter = random(BACKOFF_LENGTH) + 1; // Calculate random backoff..
        txCounter = (ADC12MEM5 & 0x001F)+1;       // better, random contains % call
        txState = RTX_BACKOFF;                  // ..and go to RTX_BACKOFF.
      }
      return;
    // BACKOFF: Decrement backoff and check if elapsed, if channel gets busy again go back.
    case RTX_BACKOFF:
      TXBUF0 = 0x00;
      if(cdCounter > 0){ // Channel is busy again!
        txState = RTX_WAITING;
        return;
      } 
      else {
        txCounter--;
        if(txCounter==0){          // Backoff time elapsed.
          { extern UINT16 _skipADC; _skipADC = 0xFFFF; }
          #if defined (ESS430)
          P3SEL = 0xF0;
          P2OUT |= 0x80;
          P2OUT &= 0xBF;
          #else
          P5OUT |= 0x40;           // Switch Transceiver into TxOOK
          P5OUT &= 0x7F;           // Switch Transceiver into TxOOK
          P3OUT |= 0x10; // 1 on Transceiver TxD
          #endif
          txCounter = TURNON_LENGTH;
          txState = RTX_TURNON; 
        }
        return;
      }
    // TURNON: Keep Trasnmitter sending a 1 until counter is elapsed.
    case RTX_TURNON:
      txCounter--;
      TXBUF0 = 0xAA;
      if(txCounter==0) {
        #if !defined(ESS430) 
        P3SEL = 0xF0;  // Switch UART Tx Module onto Transceiver
        P3OUT &= ~0x10; // 0 on Transceiver TxD
        #endif
        txCounter = PREAMBLE_LENGTH;
        txState = RTX_PREAMBLE;
      }
      return;
    // PREAMBLE: Send PREAMBLE_LENGTH 0xAAs.    
    case RTX_PREAMBLE:
      TXBUF0 = 0xAA;
      txCounter--;
      if(txCounter==0){ // Preamble done, calc length and start the packet.
        // length to transmit is packet data length + header length (defined by PACKET_DATA)
        txLength = RADIOTX_RB_INT(rtxrb, PACKET_LENGTH) + PACKET_DATA + 2;
        txPos = 0;
        txState = RTX_FF;                
      }
      return;
    case RTX_FF:  TXBUF0 = STARTBYTE0; txState = RTX_01;       return;
    case RTX_01:  TXBUF0 = STARTBYTE1; txState = RTX_7F;       return;
    case RTX_7F:  TXBUF0 = STARTBYTE2;
                  if(RADIOTX_RB_I(rtxrb, PACKET_TYPE)==SYNCH_PACKET) txState = RTX_S_SENDING1;
                  else                                               txState = RTX_SENDING1; 
                  return; 
    // SENDING1: Encode byte and send the first half.      
    case RTX_SENDING1:
      txEncodeBuf = MAN_ENCODE(RADIOTX_RB_I(rtxrb, txPos));
      TXBUF0 = txEncodeBuf >> 8;
      txState = RTX_SENDING2;
      return;
    // SENDING2: Send the second half. If all bytes sent go to postamble.  
    case RTX_SENDING2:
      TXBUF0 = txEncodeBuf;
      txPos++;
      if(txPos==txLength){
        txCounter = POSTAMBLE_LENGTH;
        txState = RTX_POSTAMBLE;
      }
      else txState = RTX_SENDING1;
      return;
    // POSTAMBLE: Send POSTAMBLE_LENGTH 0x55s. If done continue to SENT
    // for unacknowledged packets (type<0x80) and ACKWAITING for acknowledged packets. 
    case RTX_POSTAMBLE:
      TXBUF0 = 0x00;
      txCounter--;
      if(txCounter==0){
        #if defined(ESS430)
        P3SEL = 0xE0; 
        P2OUT |= 0xC0; 
        #else
        P3SEL = 0xE0; 
        P5OUT |= 0xC0;     
        #endif
        { extern UINT16 _skipADC; _skipADC = 40; }
        if( RADIOTX_RB_INT(rtxrb, PACKET_TO) != 0xFFFF){          
          txCounter = ACKWAITING_LENGTH; 
          txState = RTX_ACKWAITING;
        }
        else {
          txState = RTX_SENT;
          WAKEUPLPM1(radio_tx_isr, MF_RADIO_TX);
        }
      }
      return;
    // ACKWAITING: If counter elapses start a new retry or go to NOTSENT
    // if max retries are reached. ::radio_rx_heartbeat gets us out of this
    // if the correct ack is received.      
    case RTX_ACKWAITING:
      TXBUF = 0x00;
      txCounter--;
      if(txCounter==0){
        txRetries++;
        if(txRetries == MAX_TX_RETRIES){
          txState = RTX_NOTSENT;
          WAKEUPLPM1(radio_tx_isr, MF_RADIO_TX);
        }
        else {
          txCounter = TURNON_LENGTH;
          txState = RTX_WAITING;                 
        }      
      }
      return;
    // Final state for successfull sending (unacknowledged packet or acknowledged packet
    // with ack received). ::radio_tx_heartbeat continues here.
    case RTX_SENT:
      TXBUF0 = 0x00;
      return;
    // Final state for unsucessfull sending (acknowledged packet without ack after max retries).
    // ::radio_tx_heartbeat continues here.
    case RTX_NOTSENT:
      TXBUF0 = 0x00;
      return;
    // Below is the state machine for sending the acks which is mainly a copy of the
    // above states. No Backoff is done here, the data is sent from the ackBuffer
    // and this is started by the radio_rx interrupt receiving a packet with an
    // type >= 0x80. This goes automatically back to RTX_IDLE if done.
    case RTX_A_TURNON:  // TODO
      TXBUF0 = 0xAA;
      { extern UINT16 _skipADC; _skipADC = 0xFFFF; }
      #if defined (ESS430)
      P3SEL = 0xF0;
      P2OUT |= 0x80;
      P2OUT &= 0xBF;
      #else
      P5OUT |= 0x40;           // Switch Transceiver into TxOOK
      P5OUT &= 0x7F;           // Switch Transceiver into TxOOK
      P3OUT |= 0x10; // 1 on Transceiver TxD
      #endif
      txCounter = PREAMBLE_LENGTH+1;
      txState = RTX_A_PREAMBLE;    
      return;
    case RTX_A_PREAMBLE:
      TXBUF0 = 0xAA;
      txCounter--;
      if(txCounter==0){
        // length to transmit is always PACKET_DATA
        txLength = PACKET_DATA;
        txPos = 0;
        txState = RTX_A_FF;                
      }
      return;
    case RTX_A_FF:  TXBUF0 = STARTBYTE0; txState = RTX_A_01;       return;
    case RTX_A_01:  TXBUF0 = STARTBYTE1; txState = RTX_A_7F;       return;
    case RTX_A_7F:  TXBUF0 = STARTBYTE2; txState = RTX_A_SENDING1; return; 
    case RTX_A_SENDING1:
      txEncodeBuf = MAN_ENCODE(ackBuffer[txPos]);
      TXBUF0 = txEncodeBuf >> 8;
      txState = RTX_A_SENDING2;
      return;
    case RTX_A_SENDING2:
      TXBUF0 = txEncodeBuf;
      txPos++;
      if(txPos==txLength){
        txCounter = POSTAMBLE_LENGTH;
        txState = RTX_A_POSTAMBLE;
      }
      else txState = RTX_A_SENDING1;
      return;
    case RTX_A_POSTAMBLE:
      TXBUF0 = 0x00;
      txCounter--;
      if(txCounter==0){
        #if defined(ESS430)
        P3SEL = 0xE0; 
        P2OUT |= 0xC0; 
        #else
        P3SEL = 0xE0; 
        P5OUT |= 0xC0;     
        #endif
        { extern UINT16 _skipADC; _skipADC = 40; }
        txState = RTX_IDLE;
        WAKEUPLPM1(radio_tx_isr, MF_RADIO_TX);
      }
      return;
    ////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////  
    // SENDING1: Encode byte and send the first half.      
    case RTX_S_SENDING1:
      if(txPos==0){ // Read time when 01 byte was sent ( UART is has a byte buffer)
        extern volatile time_t currentTime;
        synch.synchTime = currentTime; 
        // A bit too long for irq, but must be done here: Calculate checksum for synch. (Other way would be to omit crc for synchs)
        synch.checksum = crc_calc(synch.checksum, (UINT8*)&synch, sizeof(time_t));
      }
      if(txPos<PACKET_DATA) txEncodeBuf = MAN_ENCODE(RADIOTX_RB_I(rtxrb, txPos));
      else                  txEncodeBuf = MAN_ENCODE( ((UINT8*)&synch)[txPos-PACKET_DATA] );
      TXBUF0 = txEncodeBuf >> 8;
      txState = RTX_S_SENDING2;
      return;
    // SENDING2: Send the second half. If all bytes sent go to postamble.  
    case RTX_S_SENDING2:
      TXBUF0 = txEncodeBuf;
      txPos++;
      if(txPos==txLength){
        txCounter = POSTAMBLE_LENGTH;
        txState = RTX_POSTAMBLE;
      }
      else txState = RTX_S_SENDING1;
      return;      
      default:
      Comm_print("! undefined txState: %b\r\n", txState); // should never occur
      rxClear();
      break;

  }
}

bool Net_setTxPower(UINT8 value) {
  #if defined(ESS430)
  return False;
  #else
  int i;
  if(value>100) return False;
  POTI_PORT &= ~POTI_UD;                                // P25 = 0 (down selected)
  POTI_PORT &= ~POTI_CS;                                // P26 = 0 (chipselect on)
  for(i=0;i<102;i++) {
     POTI_PORT &= ~POTI_INC;                             // P24 = 0 (inc)
     POTI_PORT |= POTI_INC;    
  }
  POTI_PORT |= POTI_UD;                                // P25 = 1 (up selected)
  for(i=0;i<value;i++) {
    POTI_PORT &= ~POTI_INC;                              // P24 = 0 (inc)
    POTI_PORT |= POTI_INC;    
  }
  POTI_PORT |= POTI_CS;                                // P26 = 1 (chipselect off)
  if (Configuration.transmitPower!=value) {
    Configuration.transmitPower=value;
    Configuration_save();
  }
  return True;
  #endif
}

bool Net_rxHandler(void){
  UINT16 i;
	// If packet received, fill rxPacket struct.
  if(rxState==RXSTATE_FULL){ 
    rxPacket.to     = UINT(rxBuf[PACKET_TO]);
    rxPacket.from   = UINT(rxBuf[PACKET_FROM]);
    rxPacket.type   = rxBuf[PACKET_TYPE];
    rxPacket.num    = rxBuf[PACKET_NUM];
    rxPacket.header_length = 0;
    rxPacket.header = 0;
    rxPacket.data_length = UINT(rxBuf[PACKET_LENGTH]);
    rxPacket.data   = (UINT8*) &rxBuf[PACKET_DATA];
    // If debug flag is set provide info.         
    if(Comm_logLevel & DEBUG_RADIO) printRxPacket();
    if(rxPacket.type==ACK_PACKET){ rxClear(); return True; } 
    // If no broadcast check in rxNumTable for doubles and note packet num.
    // Check if already received
    for(i=0; i<MAX_RXNUM_TABLE; i++){
      if(rxNumTable[i].id == rxPacket.from && rxNumTable[i].num == rxPacket.num){ 
        rxClear(); // We already received that, remove and continue.
        return True;
      }
    }
    // First time received: enter current num into history.
    for(i=0; i<=MAX_RXNUM_TABLE; i++){
      if(i==MAX_RXNUM_TABLE){ // If no entry found from rxPacket.from create a new one..
        rxNumTable[rxNumTablePos].id = rxPacket.from;
        rxNumTable[rxNumTablePos].num = rxPacket.num; 
        rxNumTablePos++;
        if(rxNumTablePos==MAX_RXNUM_TABLE) rxNumTablePos = 0;           
      }
      else if(rxNumTable[i].id == rxPacket.from){ // else just overwrite old number.
        rxNumTable[i].num = rxPacket.num;
      }
    }    
    if(callbacks[C_RADIO]) callbacks[C_RADIO]((void*)&rxPacket);
  }
  // Also clear an erronous buffer, if wanted provide some info.
  else if(rxState==RXSTATE_ERROR){ // packet with errors
    if(Comm_logLevel & DEBUG_RADIO) printRxPacket();
    rxClear();    
  }
  return True;
}

char Net_send(packet_t* packet, fp_char_t callback){
  UINT16 i;
  UINT16 cb = (UINT16) callback;
  UINT16 length = packet->header_length + packet->data_length;
  UINT16 checksum = INIT_FCS;
  UINT16 offset;
  if(packet->type==ACK_PACKET) return -2; // ACKs are internal only
  if(length & 0x0001){ //if length is uneven make it even
    length++;
    packet->data_length++;
  }
  // if disabled return other error code
  if(RADIOTX_RB_FREE(rtxrb) > length + 8 + 2 + 2){ // we have space in buf, so queue it, length + 8 data bytes, 2 checksum and 2 callback
    // inc txnum
    txNum++;
    offset = RADIOTX_RB_LEN(rtxrb); 
    // Queue header
    RADIOTX_RB_PUT(rtxrb, packet->to); // to l
    RADIOTX_RB_PUT(rtxrb, packet->to >> 8); // to h      
    RADIOTX_RB_PUT(rtxrb, Configuration.id); // from l
    RADIOTX_RB_PUT(rtxrb, Configuration.id >> 8); // from h
    RADIOTX_RB_PUT(rtxrb, packet->type); // type
    RADIOTX_RB_PUT(rtxrb, txNum); // num
    RADIOTX_RB_PUT(rtxrb, length);      // lower 8 bits of length
    RADIOTX_RB_PUT(rtxrb, length >> 8); // upper 8 bits of length  
    // Queue data header and data
    for(i=0; i<packet->header_length; i++) RADIOTX_RB_PUT(rtxrb,packet->header[i]); 
    for(i=0; i<packet->data_length; i++)   RADIOTX_RB_PUT(rtxrb,packet->data[i]);
    // Calc and queue checksum, for SYNCs only precalc header
    if(packet->type==SYNCH_PACKET){
      for(i=0; i<8; i++) checksum = ccitt_fcs(checksum, RADIOTX_RB_I(rtxrb, offset+i));
      synch.checksum = checksum;    
    }
    else {
      for(i=0; i<(8 + packet->header_length + packet->data_length) ; i++) 
        checksum = ccitt_fcs(checksum, RADIOTX_RB_I(rtxrb, offset+i));
    }
    RADIOTX_RB_PUT(rtxrb, checksum); 
    RADIOTX_RB_PUT(rtxrb, checksum >> 8); 
    // Queue function callback 
    RADIOTX_RB_PUT(rtxrb, cb >> 8); // upper 8 bits of callback
    RADIOTX_RB_PUT(rtxrb, cb);      // lower 8 bits of callback  
    // Debug output
    if(Comm_logLevel & DEBUG_RADIO) 
      Comm_print("> %b[%i -> %i] %i,%i csum: %x cb: %x free: %i\r\n", 
        packet->type, Configuration.id, packet->to, length, txNum, checksum, cb, RADIOTX_RB_FREE(rtxrb));
    runModule |= MF_RADIO_TX;
    return 1;        
  }
  else {
    if(Comm_logLevel & DEBUG_RADIO) 
      Comm_print("| %b[%i -> %i] %i,%i cb: %x free: %i\r\n", 
        packet->type, Configuration.id, packet->to, length, txNum, cb, RADIOTX_RB_FREE(rtxrb));
    return -1;  
  }
}

bool Net_txHandler(){
  UINT8 tmp;
  UINT16 cb;
  fp_char_t callback;
  if(txState == RTX_SENT || txState == RTX_NOTSENT){ // if sending of one packet is finished do cleanup and go to idle
    // Copy out to sentPacket what was sent.
    sentPacket.to = RADIOTX_RB_INT(rtxrb, PACKET_TO);
    sentPacket.from = RADIOTX_RB_INT(rtxrb, PACKET_FROM);
    sentPacket.type = RADIOTX_RB_I(rtxrb, PACKET_TYPE);    
    sentPacket.num = RADIOTX_RB_I(rtxrb, PACKET_NUM);
    sentPacket.data = NULL;
    sentPacket.data_length = RADIOTX_RB_INT(rtxrb, PACKET_LENGTH);
    // txLength still contains length of packet!, use it to remove packet, TODO: rb skip operation
    // read header out to sentPacket
    while(txLength > 0){    
      RADIOTX_RB_GET(rtxrb, tmp);
      txLength--;
    }
    // remove callback and call it if set
    RADIOTX_RB_GET(rtxrb, tmp); cb = tmp << 8;
    RADIOTX_RB_GET(rtxrb, tmp); cb |= tmp;
    if(cb != 0){
      callback = (fp_char_t) cb;
      if(txState==RTX_SENT) callback(1);
      else                  callback(0);
    }
    txState = RTX_IDLE;
    if(Comm_logLevel & DEBUG_RADIO) Comm_print("radio-tx: packet SENT and removed - %i %i cb: %x\r\n", rtxrb.head, rtxrb.tail, cb);
  } 
  // if idle and something in buffer ==> try to send 
  if(txState == RTX_IDLE && !RADIOTX_RB_EMPTY(rtxrb)){ 
    if(Comm_logLevel & DEBUG_RADIO) Comm_print("radio-tx: something in buf - %i %i \r\n", rtxrb.head, rtxrb.tail);
    txRetries = 0;                
    txState = RTX_WAITING;
  }
  runModule &= ~MF_RADIO_TX;
  return True;
}

bool Net_init() {
  UCTL0 = CHAR;                         // 8-bit character
  UTCTL0 = SSEL1;                       // UCLK = SMCLK
  UBR00 = 0x80;                         // 2,457MHz/19200 = 128 -> 0x80
  UBR10 = 0x00;;                        //
  UMCTL0 = 0x00;                        // no modulation
  Net_setTxPower(Configuration.transmitPower);
  ME1 |= UTXE0 + URXE0;                 // Enable USART0 TXD/RXD, doesnt bring any power savings if switched off
  Net_on();
  return True;
}

bool Net_on(){
  rxNumTablePos = 0;
  cdCounter = 0;
  rxClear();  
  txClear();  
  setFunkRx();
  IFG1 |= UTXIFG0;           // SET TFIFG: ensure interrupt will start
  IE1  |= (URXIE0 + UTXIE0); // SET TXIE and RXIE: and enable it!
  return True;
}

bool Net_off(){
  IE1 &= ~(URXIE0 + UTXIE0); // Disable Rx&Tx Interrupt
  setFunkOff();
  return True;
}
