#include <16F88.h>
//#define NODE_ADDR   0x42
//#define NODE_ADDR   0x44
#define NODE_ADDR   0x46
#include <..\Libs\I2CSlave.c>

#fuses HS,NOWDT,NOPROTECT,NOLVP, NOFCMEN, NOIESO, NOBROWNOUT, EC_IO
#use delay(clock=20000000)
#use rs232(baud=115200, xmit=PIN_B5, rcv=PIN_B2, ERRORS)

#define PIN_SYNC_LINE   PIN_B6

#define hi(x)  (*(&x+1))

#define STATUS       0x03
#define Z            2
#define FSR          0x04
#define INDF         0x00
#define W            0
#define F            1

#byte PORTA = 0x05
#byte PORTB = 0x06

unsigned long int ServoValues[6];

void CopyServoValues();
void ResetPinsAndADDRs();
void SortPulseWidths();
void PrunePulseWidths();
void CheckForZeros();
void StopPulseTrain(BYTE pwh, BYTE pwl, BYTE a, BYTE m, long int);
void ProcessRequest(void);

unsigned long int PulseWidths[6];

BYTE PinMasks[6];
BYTE PinADDRs[6];
BYTE CycleCount;
BYTE f0, f1, f2, f3, f4, f5;

void main() {
   BYTE pw0h, pw1h, pw2h, pw3h, pw4h, pw5h;
   BYTE pw0l, pw1l, pw2l, pw3l, pw4l, pw5l;
   BYTE m0, m1, m2, m3, m4, m5;
   BYTE a0, a1, a2, a3, a4, a5;
   BYTE index;

   output_a(0);
   output_b(0);
   TRISA = 0;
   TRISB = 0;

   setup_ccp1(CCP_OFF);
   setup_adc_ports(NO_ANALOGS);

   delay_ms(10);

   output_float(PIN_B2);  //RS232 RCV Pin

   i2c_initialize();
   enable_interrupts(GLOBAL);

   //setup_timer_1(T1_INTERNAL | T1_DIV_BY_8);
   //Makes every count = 1/20,000,000 * 4 = .2 Microseconds

   for (index=0; index<6; index++)
      ServoValues[index]=0;

   while(1)
   {
      //Do all the prep work before we start the loop
      CopyServoValues();
      ResetPinsAndADDRs();
      SortPulseWidths();
      CheckForZeros();
      PrunePulseWidths();

      output_low(PIN_SYNC_LINE);

      pw0h = hi(PulseWidths[0]);
      pw0l = PulseWidths[0];
      pw1h = hi(PulseWidths[1]);
      pw1l = PulseWidths[1];
      pw2h = hi(PulseWidths[2]);
      pw2l = PulseWidths[2];
      pw3h = hi(PulseWidths[3]);
      pw3l = PulseWidths[3];
      pw4h = hi(PulseWidths[4]);
      pw4l = PulseWidths[4];
      pw5h = hi(PulseWidths[5]);
      pw5l = PulseWidths[5];

      m0 = PinMasks[0];
      m1 = PinMasks[1];
      m2 = PinMasks[2];
      m3 = PinMasks[3];
      m4 = PinMasks[4];
      m5 = PinMasks[5];

      a0 = PinADDRs[0];
      a1 = PinADDRs[1];
      a2 = PinADDRs[2];
      a3 = PinADDRs[3];
      a4 = PinADDRs[4];
      a5 = PinADDRs[5];

      //Set pins high
      *a0 |= 0xFF - m0;
      *a1 |= 0xFF - m1;
      *a2 |= 0xFF - m2;
      *a3 |= 0xFF - m3;
      *a4 |= 0xFF - m4;
      *a5 |= 0xFF - m5;

      //set_timer1(0);

      StopPulseTrain(pw0h, pw0l, a0, m0, PulseWidths[0]);
      StopPulseTrain(pw1h, pw1l, a1, m1, PulseWidths[1]);
      StopPulseTrain(pw2h, pw2l, a2, m2, PulseWidths[2]);
      StopPulseTrain(pw3h, pw3l, a3, m3, PulseWidths[3]);
      StopPulseTrain(pw4h, pw4l, a4, m4, PulseWidths[4]);
      StopPulseTrain(pw5h, pw5l, a5, m5, PulseWidths[5]);

      output_high(PIN_SYNC_LINE);

      while (kbhit())
          getc();

      putc(0x41);
	  
      CycleCount = getc();
      f0 = getc();
      f1 = getc();
      f2 = getc();
      f3 = getc();
      f4 = getc();
      f5 = getc();

      // Re-enable I2C Request Processing
      i2c_initialize();

      delay_ms(15);

      //Disable I2C Request processing, but allow any outstanding request to
      //finish by disrupting address matching
      //The master will keep trying to send the next update
      //until we ACK

      set_i2c_address(0);

      //kill 1 ms letting any final I2C requests finish before we start the PWM
      delay_ms(1);
   }
}

void ResetPinsAndADDRs()
{
   PinADDRs[0] = &PORTA;
   PinMasks[0] = 255 - 1;
   PinADDRs[1] = &PORTA;
   PinMasks[1] = 255 - 2;
   PinADDRs[2] = &PORTA;
   PinMasks[2] = 255 - 4;
   PinADDRs[3] = &PORTA;
   PinMasks[3] = 255 - 8;
   PinADDRs[4] = &PORTA;
   PinMasks[4] = 255 - 16;
   PinADDRs[5] = &PORTB;
   PinMasks[5] = 255 - 128;
}

void StopPulseTrain(BYTE pwh, BYTE pwl, BYTE a, BYTE m, long int duration)
{
   /*

      It's as simple as this:
      there's a value that was split before hand into pw high and low

      while (--pw>0) { noop; }

      apply mask m to address a;

      But I wanted it to be alot faster yeilding the smoothest and highest
      resolution delay I can come up with.
   */

#asm

      movf     pwl, F
      btfsc    STATUS.Z ; is our low order byte 0
      goto     tfh      ; yes, lets test the high order one next

tfl:
      decfsz   pwl      ; run the low order byte to 0 then move on
      goto     tfl

tfh:  movf     pwh, F
      btfsc    STATUS.Z ; is our high order byte 0?
      goto     low      ; yes, so we're done. drop the pin low

msb:
      decfsz   pwh      ; no it wasn't so drop it by one and test again
                        ; (for the sake of the loop)
      goto     lsb      ; still not zero, zero the lsb again.
      goto     last     ; thats it, zero the lsb one last time.

lsb:  decfsz   pwl      ; decrement and test lsb
      goto     lsb      ; not zero yet, do it again
      goto     msb      ; it's zero, decrement that msb

last: decfsz   pwl      ; draining out the lsb one last time
      goto     last

low:                    ; now we set the pin low
      movf     a, W     ; load the address
      movwf    FSR      ; into the FSR for INDF access
      movf     INDF, W  ; copy the addresses value
      andwf    m, W     ; apply the mask
      movwf    INDF     ; put it back

#endasm
}

void PrunePulseWidths()
{
   BYTE i;

   PulseWidths[5] -= PulseWidths[4];
   PulseWidths[4] -= PulseWidths[3];
   PulseWidths[3] -= PulseWidths[2];
   PulseWidths[2] -= PulseWidths[1];
   PulseWidths[1] -= PulseWidths[0];
}

void SortPulseWidths()
{
   //Bubble sort em, we've got plenty of time.
   BYTE i, j;
   unsigned long int tmpWidth;
   BYTE tmpMask, tmpAddr;

   for (i=0; i<6; i++)
   {
      for (j=0; j<5-i; j++)
         if (PulseWidths[j+1] < PulseWidths[j])
         {
            tmpWidth = PulseWidths[j];
            PulseWidths[j] = PulseWidths[j+1];
            PulseWidths[j+1] = tmpWidth;

            tmpMask = PinMasks[j];
            PinMasks[j] = PinMasks[j+1];
            PinMasks[j+1] = tmpMask;

            tmpAddr = PinADDRs[j];
            PinADDRs[j] = PinADDRs[j+1];
            PinADDRs[j+1] = tmpAddr;
         }
   }
}

void CopyServoValues()
{
   BYTE i;

   for (i=0; i<6; i++)
      PulseWidths[i] = ServoValues[i];
}

void CheckForZeros()
{
   //We're checking to see if any of the servo's have 0's
   //for the position.  In that case, kill the mask to turn
   //the pulse train off.

   BYTE i;

   for (i=0; i<6; i++)
      if (PulseWidths[i] == 0)
         PinMasks[i] = 255;

}

void i2c_start_read()
{
   slave_buffer[0] = CycleCount;
   slave_buffer[1] = f0;
   slave_buffer[2] = f1;
   slave_buffer[3] = f2;
   slave_buffer[4] = f3;
   slave_buffer[5] = f4;
   slave_buffer[6] = f5;
}

void i2c_end_request()
{
   BYTE cmd;

   //output_high(PIN_B0);
   //if (request_direction == 1)
   {
      //Master Write
      cmd = slave_buffer[1];

      switch (cmd)
      {
         case 0x41:
            ServoValues[0] = slave_buffer[2];
            hi(ServoValues[0]) = slave_buffer[3];
            ServoValues[1] = slave_buffer[4];
            hi(ServoValues[1]) = slave_buffer[5];
            ServoValues[2] = slave_buffer[6];
            hi(ServoValues[2]) = slave_buffer[7];
            ServoValues[3] = slave_buffer[8];
            hi(ServoValues[3]) = slave_buffer[9];
            ServoValues[4] = slave_buffer[10];
            hi(ServoValues[4]) = slave_buffer[11];
            ServoValues[5] = slave_buffer[12];
            hi(ServoValues[5]) = slave_buffer[13];
      }

   }
   //else
   {
      //Master Read
   }
}