#define led1  LATBbits.LATB5
 #define led2  LATBbits.LATB15

//--------------REGISTROS DEL ADXL345-------------------------------------------
#define WriteData   0xA6             //Direccion + bit de Escritura
#define ReadData    0xA7             //Direccion + bit de Lectura
#define OFSX        0x1E             //Offset X
#define OFSY        0x1F             //Offset Y
#define OFSZ        0x20             //Offset Z
#define BW_RATE     0x2C             //Ancho de Banda
#define POWER_CTL   0x2D             //Modo Medicion
#define INT_ENABLE  0x2E             //Habilita el control de interruptor
#define DATA_FORMAT 0x31             //Formato de los datos
#define DATAX0      0x32             //LSB Dato del eje X
#define DATAX1      0x33             //MSB Dato del eje x
#define DATAY0      0x34             //LSB Dato del eje y
#define DATAY1      0x35             //MSB Dato del eje y
#define DATAZ0      0x36             //LSB Dato del eje z
#define DATAZ1      0x37             //MSB Dato del eje z
#define FIFO_CTL    0x38             //Control FIFO
//------------------------------------------------------------------------------

//--------------VARIABLES-------------------------------------------------------
char StrLecturas[16];
int lecturas[3];
//------------------------------------------------------------------------------

//-------------FUNCIONES ADXL345------------------------------------------------
void ADXL345_Write(int address, int data1){
    I2C1_Start();                   //Inicia comunicacion por I2C
    I2C1_Write(WriteData);           //Envia byte por I2C (direccion + Escritura)
    I2C1_Write(address);             //Envia byte (direcion registro)
    I2C1_Write(data1);               //Envia informacion
    I2C1_Stop();                     //Para la comunicacion
}

int ADXL345_Read(int address){
    int temp;                        //Variable temporal
    
    I2C1_Start();                    //Inicia comunicacion por I2C
    I2C1_Write(WriteData);           //Envia byte por I2C (direccion + Escritura)
    I2C1_Write(address);             //Envia byte (direcion registro)
    I2C1_Restart();                  //Reinicia la comunicacion por I2C
    I2C1_Write(ReadData);            //Envia byte por I2C (direccion + Lectura)
    temp = I2C1_Read(0);             //Lee la informacion del registro
    I2C1_Stop();                     //Para la comunicacion

    return temp;
}

void ADXL345_Init(){
    ADXL345_Write(POWER_CTL,0x00);   //Modo Sleep.
    delay_ms(2);
    ADXL345_Write(DATA_FORMAT,0x0B); //Configurado a +/-16g. (0x0B Full resolution a +/- 16g)
    delay_ms(2);
    ADXL345_Write(INT_ENABLE,0x80);  //Activa Int DATA_READY.
    delay_ms(2);
    ADXL345_Write(BW_RATE,0x0A);     //100Hz Standard.
    delay_ms(2);    
    ADXL345_Write(POWER_CTL,0x08);   //Modo Medición.
    delay_ms(2);
}
//------------------------------------------------------------------------------

//-------------FUNCIONES DE CAPTURA DE DATOS------------------------------------
int LeerX(){
    char lsb, msb;
    int accX;              //Declaracion de variables
    
    msb = ADXL345_Read(DATAX1);      //Lee el MSB de la acceleracion en X
    lsb = ADXL345_Read(DATAX0);      //Lee el LSB de la acceleracion en X
    
    accX = (msb << 8);
    accX = (accX | lsb);             //Concatena el MSB y LSB
    
    return accX;                     //Regresa la acceleracion en X
}

int LeerY(){
    int lsb, msb, accY;              //Declaracion de variables

    msb = ADXL345_Read(DATAY1);      //Lee el MSB de la acceleracion en Y
    lsb = ADXL345_Read(DATAY0);      //Lee el MSB de la acceleracion en Y

    accY = (msb << 8);
    accY = (accY | lsb);             //Concatena el MSB y LSB

    return accY;                     //Regresa la acceleracion en Y
}

int LeerZ(){
    int lsb, msb, accZ;              //Declaracion de variables

    msb = ADXL345_Read(DATAZ1);      //Lee el MSB de la acceleracion en Z
    lsb = ADXL345_Read(DATAZ0);      //Lee el MSB de la acceleracion en Z

    accZ = (msb << 8);
    accZ = (accZ | lsb);             //Concatena el MSB y LSB

    return accZ;                     //Regresa la acceleracion en Y
}

void LeerDatos(){
    lecturas[0] = LeerX();
    lecturas[1] = LeerY();
    lecturas[2] = LeerZ();
}
//------------------------------------------------------------------------------

//-------------PROGRAMA PRINCIPAL-----------------------------------------------
void init(){
    JTAGEN_bit = 0;                 //Deshabilita JTAG
    
    ANSELA = 0x0000;
    ANSELB = 0x0000;                //Puertos A y B como Digitales
    
    TRISA = 0x0000;
    TRISB = 0x0000;                 //Puertos A y B como Salida
    
    LATA = 0x0000;
    LATB = 0x0000;                  //Puertos A y B como LOW
    
    //ANSELBbits.ANSB2 = 1;           //Puerto B2 como Analogico (SDA2)
    //ANSELBbits.ANSB3 = 1;           //Puerto B3 como Analogico (SCL2)

    TRISAbits.TRISA2 = 1;           //Puerto A2 como entrada (Xtal Input)
    TRISBbits.TRISB11 = 1;          //Puerto B11 como entrada (FTDI Tx)
    
    U2RXR = 0x03;                   //Habilita Rx1 en el puerto B11
    RPB10R = 0x02;                  //Habilita Tx1 en el puerto B10
    
    UART2_Init(9600);               //Inicializa la conexion UART1
    Delay_ms(100);
    
    I2C1_Init(100000);              //Inicializa I2C2
}

void main() {

     init();
     Delay_ms(50);
     ADXL345_Init();
     Delay_ms(50);
     
     while(1){
         LeerDatos();
         //int devid = ADXL345_Read(0x00);
         //int devid2 = ADXL345_Read(0x00);
         //long xm = ADXL345_Read(DATAX1);
         //IntToStr(devid, StrLecturas);
         IntToStr(lecturas[0], StrLecturas);
         UART2_Write_Text(StrLecturas);
         UART2_Write(13);
         IntToStr(lecturas[1], StrLecturas);
         //IntToStr(devid2, StrLecturas);
         UART2_Write_Text(StrLecturas);
         UART2_Write(13);
         IntToStr(lecturas[2], StrLecturas);
         //LongToStr(xm, StrLecturas);
         UART2_Write_Text(StrLecturas);
         UART2_Write(13);
         UART2_Write_Text("=================================");
         UART2_Write(13);
         
         Delay_ms(200);
     }
}