#include "xc.h"
#include <stdio.h>
#include <string.h>
#include "extern.h"
#include "I2C.h"
#include "UART.h"


char s[64];

//#ifdef DEBUG //Not ready for prime time...
#if 1 //For testing

void __attribute__((interrupt, no_auto_psv)) _MI2C1Interrupt(void)
{
    __asm__ volatile ("push CORCON");
    
    switch(I2C_State)
    {
        case 0:  //Completed Send of Start Condition for data read;
//            putU1('0');
            I2C_State = 1;
            I2C1TRN = I2C_DeviceAddr; // Send Device Address with a write indication
            break;
        case 1:  //Completed Send of Device Address
//            putU1('1');
//            if(I2C1STATbits.ACKSTAT == 0)
//               putU1('a'); 
//            else
//               putU1('n'); 
            I2C_State = 2;
            I2C1TRN = (char) ((I2C_RegisterAddr & 0xff00) << 8); //Send Address High Byte
            break;
        case 2:
//            putU1('2');
//            if(I2C1STATbits.ACKSTAT == 0)
//               putU1('a'); 
//            else
//               putU1('n'); 
            I2C_State = 3;
            I2C1TRN = (char) (I2C_RegisterAddr & 0xff); //Send Address Low Byte
            break;
        
         case 3:
//            putU1('3');
            
//            if(I2C1STATbits.ACKSTAT == 0)
//               putU1('a'); 
//            else
//               putU1('n'); 
            I2C_State = 4;
            I2C_data_ctr = 0;
            I2C1TRN = I2C_Data[I2C_data_ctr]; //Send Data
            break;
            
        case 4:
//            putU1('4');  
//            if(I2C1STATbits.ACKSTAT == 0)
//               putU1('a'); 
//            else
//               putU1('n'); 
            if(++I2C_data_ctr < I2C_NumDataBytes) // More bytes to send
            {
            I2C_State = 4;
            I2C1TRN = I2C_Data[I2C_data_ctr]; //Send Data
            }
            else
            {
            I2C_State = 5;
            I2C1CONbits.PEN = 1; //Send Stop Condition              
            }
            break;
            
        case 5:
//            putU1('5');
            I2C_State = 255;  //Done
//            putU1('\r');
//            putU1('\n');
            break;
  
            
        case 128:  //Completed Send of Start Condition for data read;
//            putU1('8');
            I2C_State = 129;
            I2C1TRN = I2C_DeviceAddr; // Send Device Address with a write indication
            break;
        case 129:  //Completed Send of Device Address
//            putU1('9');

//            if(I2C1STATbits.ACKSTAT == 0)
//               putU1('a'); 
//            else
//               putU1('n'); 
            I2C_State = 130;

            I2C1TRN = (char) ((I2C_RegisterAddr & 0xff00) << 8); //Send Address High Byte
            break;
        case 130:
//            putU1('0');

//            if(I2C1STATbits.ACKSTAT == 0)
//               putU1('a'); 
//            else
//               putU1('n'); 
            I2C_State = 131;
            I2C1TRN = (char) (I2C_RegisterAddr & 0xff); //Send Address Low Byte
            break;
           
        case 131:  
//            putU1('1');
//            if(I2C1STATbits.ACKSTAT == 0)
//               putU1('a'); 
//            else
//               putU1('n'); 
            I2C_State = 132;
            I2C1CONbits.RSEN = 1; //Send restart
            break;
        case 132:
//            putU1('2');
            I2C_State = 133;
            I2C1TRN = I2C_DeviceAddr + 1; // Send Device Address with a read indication
            break;
            
        case 133:   
//            putU1('3');
//            if(I2C1STATbits.ACKSTAT == 0)
//               putU1('a'); 
//            else
//               putU1('n');             
            I2C_State = 134;
            I2C_data_ctr = 0; 
            I2C1CONbits.RCEN = 1; //Enable Master Reception
            break;
                    
        case 134:   
//            putU1('4'); 
         
            I2C_Data[I2C_data_ctr] = I2C1RCV;
            
            if(I2C_data_ctr < I2C_NumDataBytes-1)  //More bytes to read
            { 
               I2C_State = 135; 
               I2C_data_ctr++;
               I2C1CONbits.ACKDT = 0; //Set ACK
               I2C1CONbits.ACKEN = 1; //Send Ack
            }
            else
            {   
                I2C_State = 136; // Move on to Stop condition when NACK completes
                //I2C1CONbits.ACKDT = 1;  //Send NACK
                //I2C1CONbits.ACKEN = 1; //Send Not-Acknowledge
                I2C1CONbits.PEN = 1; //Send Stop Condition  
           }
            break;    
            
        case 135:
  //          putU1('5');
            I2C_State = 134;  //Done
            I2C1CONbits.RCEN = 1; //Enable Master Reception
            break;     

            
        case 136: 
 //           putU1('6');
            I2C_State = 137;
            I2C1CONbits.PEN = 1; //Send Stop Condition       
           
            break;
        case 137:
 //           putU1('7');
            I2C_State = 255;  //Done
 //           putU1('\r');
 //           putU1('\n');
            break;
    }
 
    _MI2C1IF = 0;  //Clear Interrupt

    __asm__ volatile ("pop CORCON");
}



void WriteI2CData(char DeviceAddr, int RegisterAddr, char *data,int NumBytes)
{
    I2C_DeviceAddr = DeviceAddr;
    I2C_RegisterAddr = RegisterAddr;
    I2C_NumDataBytes = NumBytes;  
    for(int i = 0;i<NumBytes;i++)
        I2C_Data[i] = data[i];   
    I2C_State = 0;
    I2C1CONbits.SEN = 1; //Send Start Condition, initiates write, ISR takes care of the rest and sets I2C_State to 255 when done.
    while(I2C_State < 255);  // Wait for byte to be written, making this a blocking write.
    return;
}

void ReadI2CData(char DeviceAddr, int RegisterAddr, char *data, int NumBytes)
{
    char s[64];

    I2C_DeviceAddr = DeviceAddr;
    I2C_RegisterAddr = RegisterAddr;
    I2C_NumDataBytes = NumBytes;
    I2C_State = 128;
    I2C1CONbits.SEN = 1; //Send Start Condition, initiates read, ISR takes care of the rest and sets I2C_State to 255 when all bytes read.
    while(I2C_State < 255);  // Wait for byte to be read, making this a blocking read.
    for(int i = 0;i<NumBytes;i++)
      data[i] = I2C_Data[i];   
}

#endif