#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "Arduino.h" // for digitalWrite
#include "pins_arduino.h"


#ifndef __SYNC__
#define __SYNC__        SCK
#endif
#ifndef __SDA__
#define __SDA__         MOSI
#endif
#ifndef __SCL__
#define __SCL__         MISO
#endif

#define BUFFER_LENGTH   24  
#define kinnoDelay      5
#define kTimeout        5000
 
#define __kInnoTimeOut    1200
uint8_t __innoCommandBuff__[BUFFER_LENGTH], __innoNumByteToRead__;
uint8_t __initI2C__  = 0, __i2cModuleStatus__=0, __i2cBusy__=0;

void ResetModule(void)
{
    pinMode(__SYNC__, OUTPUT);
    _delay_us(20);
    pinMode(__SYNC__, INPUT);   
    _delay_us(200);    
}
uint8_t CheckModule(void)
{
    return 1;     
}
void InitArmCommander(void)
{   
  if(!__initI2C__)
  {
    __initI2C__ = 1;
    
    pinMode(__SDA__, INPUT);
    pinMode(__SCL__, INPUT);      
    pinMode(__SYNC__, INPUT);  
            
    digitalWrite(__SDA__, LOW);
    digitalWrite(__SCL__, LOW);
    digitalWrite(__SYNC__, LOW);  
    ResetModule();                                
  }
}

void __i2cWriteBuffer(uint8_t* pBuffer, uint8_t I2C_DEV_ADDR, uint8_t NumByteToWrite)
{
  uint8_t i, ii, Data;	
  uint8_t *ptr;
  uint8_t CheckSum;      
  uint16_t Timeout;
  
  if(__i2cBusy__) return;
  __i2cModuleStatus__ = 0;     
  __i2cBusy__ = 1;    

  *(pBuffer+1) = 255 - *pBuffer - I2C_DEV_ADDR - I2C_DEV_ADDR;
  if(NumByteToWrite >= 5)
  {
      i = NumByteToWrite - 4;
      ptr = pBuffer + 2;
      CheckSum = 255;
      do
      {
        CheckSum -= *ptr++;  
      }while(--i);
      *ptr = CheckSum;
  } 
  Timeout = __kInnoTimeOut;
  while(--Timeout)  {
// START
    pinMode(__SDA__, OUTPUT);
    delayMicroseconds(kinnoDelay*2); 
    pinMode(__SCL__, OUTPUT);
    delayMicroseconds(kinnoDelay*2);    
    Data = I2C_DEV_ADDR + I2C_DEV_ADDR;
    for(ii = 0 ; ii < 8 ; ii++, Data<<=1 )
    {
       if(Data & 0x80)  pinMode(__SDA__, INPUT);
       else             pinMode(__SDA__, OUTPUT);
       delayMicroseconds(kinnoDelay);           
       pinMode(__SCL__, INPUT); 
       delayMicroseconds(kinnoDelay);      
       pinMode(__SCL__, OUTPUT);
       delayMicroseconds(kinnoDelay); 
    }
    pinMode(__SDA__, INPUT);
    delayMicroseconds(kinnoDelay);   
    pinMode(__SCL__, INPUT); 
    delayMicroseconds(kinnoDelay);    
    Data = digitalRead(__SDA__);
    pinMode(__SCL__, OUTPUT);
    delayMicroseconds(kinnoDelay); 
    if(!Data)   
    {
        __i2cModuleStatus__ = 0;
        ptr = pBuffer;
        for(i = 0 ; (i < NumByteToWrite) && !Data ; i++)
        {
            Data = *ptr++;
            for(ii = 0 ; ii < 8 ; ii++, Data <<= 1)
            {
               if(Data & 0x80)  pinMode(__SDA__, INPUT);
               else             pinMode(__SDA__, OUTPUT);
               delayMicroseconds(kinnoDelay); 
               pinMode(__SCL__, INPUT); 
               delayMicroseconds(kinnoDelay);      
               pinMode(__SCL__, OUTPUT);
               delayMicroseconds(kinnoDelay); 
            } 
            pinMode(__SDA__, INPUT);
            delayMicroseconds(kinnoDelay*2);
            pinMode(__SCL__, INPUT); 
            delayMicroseconds(kinnoDelay);    
            Data = digitalRead(__SDA__);
            if(Data)  __i2cModuleStatus__ = 1;
            pinMode(__SCL__, OUTPUT); 
            delayMicroseconds(kinnoDelay);             
        }
    }
    else    __i2cModuleStatus__ = 2;

// STOP    
    pinMode(__SDA__, OUTPUT);
     delayMicroseconds(kinnoDelay*2);

    pinMode(__SCL__, INPUT);          
    delayMicroseconds(kinnoDelay*2); 
    pinMode(__SDA__, INPUT);    
	delayMicroseconds(kinnoDelay*2); 
    if(!Data) break;
  } 
  __i2cBusy__ = 0;
  return;  
}

uint32_t __i2cReadBuffer(uint8_t* pBuffer, uint8_t I2C_DEV_ADDR, uint8_t NumByteToWrite, uint8_t *NumByteToRead)
{  
  uint8_t* ptr;    
  uint8_t Data, CheckSum, ii;
  uint8_t i, SkipWrite = 0;
  uint16_t Timeout;
  
  if(__i2cBusy__) return 0;
  __i2cModuleStatus__ = 0;     
  __i2cBusy__ = 1;  

  *(pBuffer+1) = 255 - *pBuffer  - I2C_DEV_ADDR  - I2C_DEV_ADDR; 
  if(NumByteToWrite >= 5)   
  {
    i = NumByteToWrite - 4;
    CheckSum = 255;
    ptr = pBuffer + 2;
    do
    {
        CheckSum -= *ptr++;
    }while(--i);
    *ptr = CheckSum;
  }
   
  Timeout = __kInnoTimeOut;
  while(--Timeout)
  {
    if(!SkipWrite)
    {
// START
        pinMode(__SDA__, OUTPUT);
        delayMicroseconds(kinnoDelay*2); 
        pinMode(__SCL__, OUTPUT);   
        delayMicroseconds(kinnoDelay*2);  
        Data = I2C_DEV_ADDR + I2C_DEV_ADDR;
        for(ii = 0 ; ii < 8 ; ii++, Data<<=1 )
        {
           if(Data & 0x80)  pinMode(__SDA__, INPUT);
           else             pinMode(__SDA__, OUTPUT);
           delayMicroseconds(kinnoDelay);
           pinMode(__SCL__, INPUT); 
           delayMicroseconds(kinnoDelay);      
           pinMode(__SCL__, OUTPUT);
           delayMicroseconds(kinnoDelay); 
        }
        pinMode(__SDA__, INPUT);
        delayMicroseconds(kinnoDelay);    
        pinMode(__SCL__, INPUT); 
        delayMicroseconds(kinnoDelay);    
        Data = digitalRead(__SDA__);
        pinMode(__SCL__, OUTPUT);
        delayMicroseconds(kinnoDelay);   
        i = 0 ;
        if(!Data)   
        {
            __i2cModuleStatus__ = 0;
            ptr = pBuffer;
            for(i = 0 ; (i < NumByteToWrite) && !Data ; i++)
            {
                Data = *ptr++;
                for(ii = 0 ; ii < 8 ; ii++, Data<<=1)
                {
                   if(Data & 0x80)  pinMode(__SDA__, INPUT);
                   else             pinMode(__SDA__, OUTPUT);
                   delayMicroseconds(kinnoDelay);
                   pinMode(__SCL__, INPUT); 
                   delayMicroseconds(kinnoDelay);      
                   pinMode(__SCL__, OUTPUT);
                   delayMicroseconds(kinnoDelay);
                } 
                pinMode(__SDA__, INPUT);
                delayMicroseconds(kinnoDelay*2);  
                pinMode(__SCL__, INPUT); 
                delayMicroseconds(kinnoDelay);    
                Data = digitalRead(__SDA__);
                if(Data) __i2cModuleStatus__ = 1;
                pinMode(__SCL__, OUTPUT);             
                delayMicroseconds(kinnoDelay); 
            }
        }
        else __i2cModuleStatus__ = 2;
    // STOP
        pinMode(__SDA__, OUTPUT);
        delayMicroseconds(kinnoDelay*2);         
        pinMode(__SCL__, INPUT);
        delayMicroseconds(kinnoDelay*2);  
        pinMode(__SDA__, INPUT);
         delayMicroseconds(kinnoDelay*2); 
        
        if(i != NumByteToWrite) continue;
    }
    delayMicroseconds(4*kinnoDelay);
    SkipWrite = 1;

// START
    pinMode(__SDA__, OUTPUT);
    delayMicroseconds(kinnoDelay*2); 
    pinMode(__SCL__, OUTPUT);   
    delayMicroseconds(kinnoDelay*2); 
    Data = I2C_DEV_ADDR + I2C_DEV_ADDR + 1;
    for(ii = 0 ; ii < 8 ; ii++, Data<<=1 )
    {
       if(Data & 0x80)  pinMode(__SDA__, INPUT);
       else             pinMode(__SDA__, OUTPUT);
       delayMicroseconds(kinnoDelay); 
       pinMode(__SCL__, INPUT); 
       delayMicroseconds(kinnoDelay);      
       pinMode(__SCL__, OUTPUT);
       delayMicroseconds(kinnoDelay); 
    }
    pinMode(__SDA__, INPUT);
    delayMicroseconds(kinnoDelay); 
    pinMode(__SCL__, INPUT); 
    delayMicroseconds(kinnoDelay);    
    Data = digitalRead(__SDA__);
    pinMode(__SCL__, OUTPUT);   
    delayMicroseconds(kinnoDelay); 


    if(!Data)   
    {
        __i2cModuleStatus__ = 0;
        ptr = pBuffer;
        for(i = *NumByteToRead ; i ; i--)
        {
            pinMode(__SDA__, INPUT); 
            delayMicroseconds(kinnoDelay); 
            for(ii = 0 ; ii < 8 ; ii++)
            {                                                              
               pinMode(__SCL__, INPUT); 
               Data <<= 1;   
               delayMicroseconds(kinnoDelay); 
               if(digitalRead(__SDA__))  Data |= 1;     
               pinMode(__SCL__, OUTPUT);
               delayMicroseconds(kinnoDelay);
            } 
            *ptr++ = Data; 
            if(i == 1)      pinMode(__SDA__, INPUT);
            else            pinMode(__SDA__, OUTPUT);
            delayMicroseconds(kinnoDelay*2);  
            pinMode(__SCL__, INPUT); 
             delayMicroseconds(kinnoDelay); 
            pinMode(__SCL__, OUTPUT);
            delayMicroseconds(kinnoDelay); 
        }
        Data = 0;
    }
    else __i2cModuleStatus__ = 2;

// STOP
    pinMode(__SDA__, OUTPUT);
     delayMicroseconds(kinnoDelay*2);

    pinMode(__SCL__, INPUT);
    delayMicroseconds(kinnoDelay*2); 
    pinMode(__SDA__, INPUT);
	delayMicroseconds(kinnoDelay*2);    
    if(!Data) break;
  } 

  __i2cBusy__ = 0;  
  if(!Timeout)   return 1;

  CheckSum = 255 - I2C_DEV_ADDR;
  i = *NumByteToRead; 
  ptr = pBuffer;      
  do {
    CheckSum -= *ptr++;
  }while(--i);


  if(CheckSum) {
    __i2cModuleStatus__ = 2;
    return 1;
  }
  return 0;
}

