#include <mesh452.h>

#use fast_io(a)
#use fast_io(b)
#use fast_io(c)
#use rs232(STREAM=serial0, BAUD=240, XMIT=PIN_A0, RCV=PIN_B0, FORCE_SW, PARITY=N, BITS=8)
#use rs232(STREAM=serial1, BAUD=240, XMIT=PIN_A2, RCV=PIN_B1, FORCE_SW, PARITY=N, BITS=8)
#use rs232(STREAM=serial2, BAUD=240, XMIT=PIN_E0, RCV=PIN_B2, FORCE_SW, PARITY=N, BITS=8)
#use rs232(STREAM=pc, BAUD=9600, XMIT=PIN_D0, RCV=PIN_D1, FORCE_SW, PARITY=N, BITS=8)

#define txLED PIN_C0
#define rxLED PIN_C1
#define successLED PIN_C7
#define localAddr_0 PIN_B4
#define localAddr_1 PIN_B5
#define localAddr_2 PIN_B6
#define localAddr_3 PIN_B7
#define sendTo_0 PIN_C2
#define sendTo_1 PIN_C3
#define sendTo_2 PIN_C4
#define sendTo_3 PIN_C5
#define doSend PIN_C6

#define tx0 PIN_A0
#define tx1 PIN_A2
#define tx2 PIN_E0
#define txAck0 PIN_A1
#define txAck1 PIN_A3
#define txAck2 PIN_E1
#define rx0 PIN_B0
#define rx1 PIN_B1
#define rx2 PIN_B2

#define numStreams 3

#define ackTimeout 200
#define packetSize 8
#define maxHopCount 32

int localAddress = 0;
short readyToSend = 0;

void main() {
   setup_adc_ports(NO_ANALOGS);
   setup_adc(ADC_OFF);
   setup_spi(FALSE);
   setup_wdt(WDT_OFF);
   setup_timer_0(RTCC_INTERNAL);
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DISABLED,0,1);
   setup_timer_3(T3_DISABLED|T3_DIV_BY_1);

   init();

   while(true) {
      loop();
   }
}

void init() {
   output_a(0);
   output_b(0);
   output_c(0);
   output_d(0);
   output_e(0);
   set_tris_a(0b10101010);
   set_tris_b(0b11111111);
   set_tris_c(0b01111100);
   set_tris_d(0b00000000);
   set_tris_e(0b00000010);
   delay_ms(100);

   fprintf(pc,"hello world\r\n");

   localAddress = getLocalAddress();

   fprintf(pc,"localAddress=%d\r\n",localAddress);

   output_high(rxLED);
   delay_ms(200);
   output_high(txLED);
   delay_ms(200);
   output_high(successLED);
   delay_ms(400);
   output_low(rxLED);
   delay_ms(50);
   output_low(txLED);
   delay_ms(50);
   output_low(successLED);
}


void loop() {
   int* receivedPacket;
   int* payload;
   int streamCounter = 0;
   int blinkCounter = 0;
   int startStream = 0;

   if(input(doSend)) {
      if(readyToSend) {
         payload = calloc(4,sizeof(int));

         startStream = getRandomStream();

         sendPacket(preparePacket(getSendTo(),0,payload),startStream,100);

         free(payload);
      }
   } else {
      readyToSend = 1;
   }

   receivedPacket = 0;
   for(streamCounter = 0; streamCounter < numStreams; streamCounter++) {
      if(getRx(streamCounter)) {
         receivedPacket = receivePacket(streamCounter);
         if(receivedPacket > 0) {
            if(receivedPacket[0] == localAddress) {
               // BOOM.  It's our packet baby.
               for(blinkCounter = 0; blinkCounter < 10; blinkCounter++) {
                  output_high(rxLED);
                  output_high(successLED);
                  delay_ms(75);
                  output_low(rxLED);
                  output_low(successLED);
                  delay_ms(75);
               }
               free(receivedPacket);
            } else {
               // forward it on
               receivedPacket[2]++;
               

               do {
                  startStream = getRandomStream();
               } while(startStream == streamCounter);


               if(receivedPacket[2] < maxHopCount) {
                  sendPacket(receivedPacket,startStream,streamCounter);
               }
            }
         }
         break;
      }
   }
}

int getRandomStream() {
   int retVal;
   retVal = get_timer0() % numStreams;
   return retVal;
}

int* receivePacket(int curStream) {
   int* retVal;
   retVal = 0;

   setRxTris(curStream,0);
   output_high(rxLED);
   setRx(curStream,0);
   delay_us(250);
   setRxTris(curStream,1);
   delay_us(250);
   // read data serially
   retVal = receiveSerial(curStream);

   delay_us(50);
   setRxTris(curStream,0);
   setRx(curStream,0);
   delay_ms(1);

   output_high(successLED);
   delay_ms(250);
   setRxTris(curStream,1);
   output_low(rxLED);
   output_low(successLED);

   fprintf(pc,"packet received - %d, %d, %d, %d, %d, %d, %d, %d\r\n",retVal[0],retVal[1],retVal[2],retVal[3],retVal[4],retVal[5],retVal[6],retVal[7]);

   return retVal;
}

int* receiveSerial(int curStream) {
   int receiveCounter = 0;
   int* packet;
   packet = malloc(packetSize);

   switch(curStream) {
      case 0:
         for(receiveCounter = 0; receiveCounter < packetSize; receiveCounter++) {
            packet[receiveCounter] = fgetc(serial0);
         }
         break;
      case 1:
         for(receiveCounter = 0; receiveCounter < packetSize; receiveCounter++) {
            packet[receiveCounter] = fgetc(serial1);
         }
         break;
      case 2:
         for(receiveCounter = 0; receiveCounter < packetSize; receiveCounter++) {
            packet[receiveCounter] = fgetc(serial2);
         }
         break;
   }

   return packet;
}

void setRxTris(int curStream, short direction) {
   if(direction) {
      set_tris_b(0b11111111);
   } else {
      switch(curStream) {
         case 0:
            set_tris_b(0b11111110);
            break;
         case 1:
            set_tris_b(0b11111101);
            break;
         case 2:
            set_tris_b(0b11111011);
            break;
      }
   }
}

void sendPacket(int* packet, int startStream, int receivedFromStream) {
   long ackTimeoutCounter = 0;
   short sendSucceeded = 0;
   int attemptCounter = 0;
   int curStream;
   curStream = startStream;

   fprintf(pc,"sending packet (startStream=%d) - %d, %d, %d, %d, %d, %d, %d, %d\r\n",startStream,packet[0],packet[1],packet[2],
      packet[3],packet[4],packet[5],packet[6],packet[7]);

   while(!sendSucceeded && attemptCounter < 25) {
      output_high(txLED);
      output_low(successLED);


      setTx(curStream,1);
      //delay_ms(10);
      ackTimeoutCounter = 0;

      // wait for ready to listen signal
      for(ackTimeoutCounter = 0; ackTimeoutCounter < ackTimeout; ackTimeoutCounter++) {
         if(!getTxAck(curStream)) {
            break;
         }
         delay_us(50);
      }

      // if we got it
      if(!getTxAck(curStream)) {
         // wait for line to be released
         //delay_ms(1);
         for(ackTimeoutCounter = 0; ackTimeoutCounter < ackTimeout; ackTimeoutCounter++) {
            if(getTxAck(curStream)) {
               break;
            }
            delay_us(50);
         }

         if(getTxAck(curStream)) {
            delay_ms(1); // wait for remote to be ready to listen

            // here we send the data
            sendSerial(packet,curStream);

            setTx(curStream,1);
            //delay_ms(1);
            // wait for I got it signal
            for(ackTimeoutCounter = 0; ackTimeoutCounter < ackTimeout; ackTimeoutCounter++) {
               if(!getTxAck(curStream)) {
                  break;
               }
               delay_us(50);
            }

            if(!getTxAck(curStream)) {
               sendSucceeded = 1;
            }
         }
      }
      setTx(curStream,0);
      output_low(txLED);
      delay_ms(200);
      curStream = (++curStream) % numStreams;
      if(attemptCounter < 6 && curStream == receivedFromStream) {
         curStream = (++curStream) % numStreams;
      }
      attemptCounter++;
   }

   if(sendSucceeded) {
      output_high(txLED);
      output_high(successLED);
      delay_ms(250);
   }

   output_low(txLED);
   output_low(successLED);

   free(packet);
}

void sendSerial(int* packet, int curStream) {
   int sendCounter = 0;

   switch(curStream) {
      case 0:
         for(sendCounter = 0; sendCounter < packetSize; sendCounter++) {
            fputc(packet[sendCounter],serial0);
         }
         break;
      case 1:
         for(sendCounter = 0; sendCounter < packetSize; sendCounter++) {
            fputc(packet[sendCounter],serial1);
         }
         break;
      case 2:
         for(sendCounter = 0; sendCounter < packetSize; sendCounter++) {
            fputc(packet[sendCounter],serial2);
         }
         break;
   }
}

void setRx(int curStream, short value) {
   switch(curStream) {
      case 0:
         output_bit(rx0,value);
         break;
      case 1:
         output_bit(rx1,value);
         break;
      case 2:
         output_bit(rx2,value);
         break;
   }
}

short getRx(int curStream) {
   short retVal = 0;

   switch(curStream) {
      case 0:
         retVal = input(rx0);
         break;
      case 1:
         retVal = input(rx1);
         break;
      case 2:
         retVal = input(rx2);
         break;
   }

   return retVal;
}

short getTxAck(int curStream) {
   short retVal = 0;
   switch (curStream) {
      case 0:
         retVal = input(txAck0);
         break;
      case 1:
         retVal = input(txAck1);
         break;
      case 2:
         retVal = input(txAck2);
         break;
   }

   return retVal;
}

void setTx(int curStream, short value) {
   switch (curStream) {
      case 0:
         output_bit(tx0,value);
         break;
      case 1:
         output_bit(tx1,value);
         break;
      case 2:
         output_bit(tx2,value);
         break;
   }
}

int* preparePacket(int sendTo, int hopCount, int payload[]) {
   int* newPacket;
   newPacket = malloc(packetSize);
   newPacket[0] = sendTo;
   newPacket[1] = localAddress;
   newPacket[2] = hopCount;
   newPacket[3] = 0;
   newPacket[4] = payload[0];
   newPacket[5] = payload[1];
   newPacket[6] = payload[2];
   newPacket[7] = payload[3];

   return newPacket;
}

int getSendTo() {
   int retVal = 0;
   int adder = 0;

   retVal = input(sendTo_0);
   adder = input(sendTo_1);
   adder *= 2;
   retVal += adder;
   adder = 0;
   adder = input(sendTo_2);
   adder *= 4;
   retVal += adder;
   adder = 0;
   adder = input(sendTo_3);
   adder *= 8;
   retVal += adder;

   return retVal;
}


int getLocalAddress() {
   int retVal = 0;
   int adder = 0;

   retVal = input(localAddr_0);
   adder = input(localAddr_1);
   adder *= 2;
   retVal += adder;
   adder = 0;
   adder = input(localAddr_2);
   adder *= 4;
   retVal += adder;
   adder = 0;
   adder = input(localAddr_3);
   adder *= 8;
   retVal += adder;

   return retVal;
}