/***************************************************************************
 * This header file holds all the functions necesary for I2C to work as well
 * as communicating and getting data from the I2C sensors (accel, magnetometer,
 * gyro, and barometer).  As of the end of the semester, all of the sensors can
 * be written to and read from, but only the accel seems to be producing usable,
 * actual data.  Not sure what is wrong with the others, but the data they give
 * is just not right.  Probably in the functions, but could be an error in 
 * hardware as well.
 ***************************************************************************/

#include<math.h>

/***************************************************************************
 * I2C addresses of the sensors
 ***************************************************************************/
char GyroAddw = 0b11010000;
char GyroAddr = 0b11010001;
char AccAddw = 0b00110010;
char AccAddr = 0b00110011;
char MagAddw = 0b00111100;
char MagAddr = 0b00111101;

/***************************************************************************
 * Global variables for all the data vectors and any off sets
 ***************************************************************************/
float AccVec[3];    // <x,y,z> current acceleration vector
float MagVec[3];    // <x,y,z> current Magnetometer vector
int RotVec[3];      // <x,y,z> current Angular velocity vector
int RotOff[3];      // <x,y,z> 0 offset for angular rotation
float OrientVec[3]; // <x,y,z> approximate vector of absolute angles
float AccZoff = 0;
float Temperature;


/***************************************************************************
 * This function initializes I2C.  It takes in a char value for the baud rate
 * and sets the I2C to communicate at that speed.  Note, the equation given in
 * the data sheet to determine what the rate char needs to be is incorrect.  Not
 * really sure where the proper equation is found, but just kinda guessed at the
 * values.
 ***************************************************************************/
void init_I2C(char rate)
{
    TRISAbits.TRISA2 = 0;
    PORTAbits.RA2 = 0;
    int i;
    for (i = 0; i < 255; i++)               // Put in to make sure sensors don't 
    {                                       //think they're supposed to be outputing data
        PORTAbits.RA2 = !PORTAbits.RA2;
        __delay_us(1.25);
    }
    TRISAbits.TRISA2 = 1;
	I2C2BRG = rate;
	I2C2CONbits.I2CEN = 1;
        I2C2STATbits.IWCOL = 0;
        I2C2STATbits.I2COV = 0;
        IFS3bits.MI2C2IF = 0;
        IFS3bits.SI2C2IF = 0;
        return;
}
/***************************************************************************
 * Function to call whenever you want to issue a stop command in I2C
 ***************************************************************************/
void I2Cstop(void)
{
    	IFS3bits.MI2C2IF = 0;
	I2C2STATbits.BCL = 0;
	I2C2STATbits.IWCOL = 0;
        I2C2CONbits.PEN = 1;
}

/***************************************************************************
 * Function to call whenever you want to issue a start command in I2C
 ***************************************************************************/
void I2Cstart(void)
{
	IFS3bits.MI2C2IF = 0;
	I2C2STATbits.BCL = 0;
	I2C2STATbits.IWCOL = 0;
	I2C2CONbits.SEN = 1;
        return;
}

/***************************************************************************
 * Function to call whenever you want to issue a repeated start command in I2C
 ***************************************************************************/
void I2Crestart(void)
{
	IFS3bits.MI2C2IF = 0;
	I2C2STATbits.BCL = 0;
	I2C2STATbits.IWCOL = 0;
	I2C2CONbits.RSEN = 1;
        return;
}

/***************************************************************************
 * Function to get what was received by the microcontroler. Returns 8 bits in
 * the Receive register.
 ***************************************************************************/
short int I2Crx(bool ack)
{
	short int data;
	data = I2C2RCV;
	I2C2CONbits.ACKDT = ack;
	I2C2CONbits.ACKEN = 1;
	return data;
}

/***************************************************************************
 * Function to put the microcontroler in Rx mode
 ***************************************************************************/
void I2Csetrx(void)
{
	IFS3bits.MI2C2IF = 0;
	I2C2STATbits.BCL = 0;
	I2C2STATbits.IWCOL = 0;
	I2C2CONbits.RCEN = 1;
        return;
}

/***************************************************************************
 * Function to transmit data out to the SDA bus.  Takes in 8 bits of data to be
 * sent out, returns whether previous command was acknowledged.
 ***************************************************************************/
bool I2Ctx(char data)
{
	//bit ack;
	I2C2TRN = data;
	return I2C2STATbits.ACKSTAT;
	//return ack;
}

/***************************************************************************
 * Funciton that returns whether something was acknoledged.
 ***************************************************************************/
bool I2Cack(void)
{
	//bit ack;
	return I2C2STATbits.ACKSTAT;
	//return ack;
}

/***************************************************************************
 * Wait funciton used to have the microcontroller wait until I2C process has
 * been completed.  This has to be called after EVERY I2C command given.  This
 * was never straight up put in all the functions because a beter design practice
 * would be to establish a state machine and have the code run through states
 * instead of having the microcontroller kill time by simply waiting.  Never got
 * around to making the state machine though.
 ***************************************************************************/
void I2Cwait(void)
{	while (I2C2CONbits.SEN || I2C2CONbits.RSEN || I2C2CONbits.PEN || I2C2CONbits.RCEN || I2C2CONbits.ACKEN);
		{}
	while (!IFS3bits.MI2C2IF);
		{}
	IFS3bits.MI2C2IF = 0;		//IF flag set back to 0
}


////////////////////////////////
//  All of the Gyro Functions
//  Gyro_init
//  Gyro_getRotVec
//  Gyro_getOff
//  DeterminePos    <- doesn't work
///////////////////////////////

/***************************************************************************
 * Function to initialize the gyro.  No inputs or outputs needed.  For specifics
 * on what I am setting the gyro to do consult the datasheet
 ***************************************************************************/
void Gyro_init(void)
{
    bool ack;
    
    I2Cstart();
    I2Cwait();
    
    ack = I2Ctx(0b11010000);    //Device address
    I2Cwait();

    ack = I2Ctx(0x20);          //Mem address
    I2Cwait();

    ack = I2Ctx(0b11001111);          //Stuff written out
    I2Cwait();
    ack = I2Ctx(0x00);
    I2Cwait();
    ack = I2Ctx(0b00000000);
    I2Cwait();
    ack = I2Ctx(0x00);
    I2Cwait();
    ack = I2Ctx(0b01001010);
    I2Cwait();

    I2Cstop();
    I2Cwait();
    

    I2Cstart();
    I2Cwait();

    ack = I2Ctx(0b11010000);    //Device address
    I2Cwait();

    ack = I2Ctx(0x2E);          //Mem address
    I2Cwait();

    ack = I2Ctx(0b01011111);
    I2Cwait();

    I2Cstop();
    I2Cwait();
}

/***************************************************************************
 * Function to get the 'Rotation Vector.'  This is a vector of the angular
 * velocity of the GuadRotor.  This takes in no input, and changes the global
 * variable.
 * notes: Giving values about an order of magnitude off.  Might have to do with
 *          the way I'm getting 32 values at once and averaging.  Not sure if
 *          that is working properly.
 ***************************************************************************/
void Gyro_getRotVec(void)
{
    bool ack;
    int i;
    short int Xlsb, Xmsb, Ylsb, Ymsb, Zlsb, Zmsb;
    int TempX=0, TempY=0, TempZ=0;
    I2Cstart();
    I2Cwait();
    ack = I2Ctx(GyroAddw);
    I2Cwait();
    ack = I2Ctx(0b10101000);
    I2Cwait();
    I2Crestart();
    I2Cwait();
    ack = I2Ctx(GyroAddr);
    I2Cwait();

    for (i = 0; i<16; i++)
    {
        I2Csetrx();
        I2Cwait();
        Xlsb = I2Crx(0);
        I2Cwait();
        I2Csetrx();
        I2Cwait();
        Xmsb = I2Crx(0);
        I2Cwait();
        I2Csetrx();
        I2Cwait();
        Ylsb = I2Crx(0);
        I2Cwait();
        I2Csetrx();
        I2Cwait();
        Ymsb = I2Crx(0);
        I2Cwait();
        I2Csetrx();
        I2Cwait();
        Zlsb = I2Crx(0);
        I2Cwait();
        I2Csetrx();
        I2Cwait();
        Zmsb = I2Crx(1);
        I2Cwait();

        TempX = TempX + ((Xmsb << 8) | Xlsb );          //////////////////////
        TempY = TempY + ((Ymsb << 8) | Ylsb );          //  Create running average
        TempZ = TempZ + ((Zmsb << 8) | Zlsb );          //////////////////////

    }

    I2Cstop();
    I2Cwait();

    TempX >>= 4;                    ///////////////////////////////
    TempY >>= 4;                    //  Divide by 32
    TempZ >>= 4;                    ///////////////////////////////
    RotVec[0] = (TempX*7.629)-RotOff[0];
    RotVec[1] = (TempY*7.629)-RotOff[1];
    RotVec[2] = (TempZ*7.629)-RotOff[2];
}

/***************************************************************************
 * This function gets the offsets for the gyro by sampling 256 times, averaging,
 * and then setting that value to the offset vector.
 ***************************************************************************/
void Gyro_getOff(void)
{
    int DataX = 0;
    int DataY = 0;
    int DataZ = 0;
    int i;
    for (i=0;i<256;i++)
    {
    Gyro_getRotVec();
    DataX = DataX+RotVec[0];
    DataY = DataY+RotVec[1];
    DataZ = DataZ+RotVec[2];
    __delay_ms(80);
    myputc('.');
    }
    RotOff[0] = (DataX>>8);
    RotOff[1] = (DataY>>8);
    RotOff[2] = (DataZ>>8);
}

/***************************************************************************
 * This funciton is supposed to be a simple integration.  Doesn't work.  Not
 * sure if didn't work because gyro values sucked, or didn't work because some
 * part of algorightm sucked.  Look into this heavily before using
 ***************************************************************************/
void DeterminePos(void)
{
    int pulses;
    float time;
    T1CONbits.TON = 1;
    pulses = TMR1;
    TMR1 = 0;
    time = (float)pulses*8/16000000;
    OrientVec[0] = (float)RotVec[0]*time/1000 + OrientVec[0];
    OrientVec[1] = (float)RotVec[1]*time/1000 + OrientVec[1];
    OrientVec[2] = (float)RotVec[2]*time/1000 + OrientVec[2];
}


//////////////////////////////
//  Accel functions
//  Acc_init
//  Acc_getAccVec
//  Acc_getZoff
/////////////////////////////

/***************************************************************************
 * Function to initialize the accelerometer.  For specifics on what is being set
 * look at the datasheet.
 ***************************************************************************/
void Acc_init(void)
{
    bool ack;

    I2Cstart();
    I2Cwait();

    ack = I2Ctx(AccAddw);
    I2Cwait();
    ack = I2Ctx(0x20);
    I2Cwait();
    ack = I2Ctx(0b01000111);
    I2Cwait();
    I2Cstop();
    I2Cwait();
    
    return;
}

/***************************************************************************
 * Function to get the acceleration vector.  This gives acceleration in x, y,
 * and z.  No inputs needed, and changes the global variables.
 * notes: This function definitely works, and was working really well. This is
 *          why the entire PID controler is based off accelerations.
 */
void Acc_getAccVec(void)
{
    bool ack;
    short int Xlsb, Xmsb, Ylsb, Ymsb, Zlsb, Zmsb, Temp;
    float Tempf;
    I2Cstart();
    I2Cwait();
    ack = I2Ctx(AccAddw);
    I2Cwait();
    ack = I2Ctx(0b10101000);
    I2Cwait();
    I2Crestart();
    I2Cwait();
    ack = I2Ctx(AccAddr);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Xlsb = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Xmsb = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Ylsb = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Ymsb = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Zlsb = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Zmsb = I2Crx(1);
    I2Cwait();
    I2Cstop();
    I2Cwait();
    Temp = ((Xmsb << 8) | Xlsb );
    Tempf = (float)Temp;                            ///////////////////////////
    Tempf = ((Tempf*2)/32768);
    if (Tempf - AccVec[1] > 0.3 || AccVec[1] - Tempf > 0.3)
    {
        Tempf = AccVec[1];
    }
    AccVec[1] = Tempf;                               // Have to rotate axis
    Temp = ((Ymsb << 8) | Ylsb );                   //////////////////////////
    Tempf = (float)-1*Temp;
    Tempf = ((Tempf*2)/32768);
    if (Tempf - AccVec[0] > 0.3 || AccVec[0] - Tempf > 0.3)
    {
        Tempf = AccVec[0];
    }
    AccVec[0] = Tempf;
    Temp = ((Zmsb << 8) | Zlsb );
    AccVec[2] = (float)Temp;
    AccVec[2] = ((AccVec[2]*2)/32768) + AccZoff;

}

/***************************************************************************
 * Funcion to get Z offset of the accelerometer to ensure 1 g when stationary.
 * Runs 256 times, takes an averages, subtracts that from 1 g, then always adds
 * that number to Acceleration Vector.
 ***************************************************************************/
void Acc_getZoff(void)
{
    float Data, Temp;
    Data = 0;
    int i;
    for (i=0;i<256;i++)
    {
    Acc_getAccVec();
    Temp = AccVec[2];
    Data = Data+Temp;
    }
    AccZoff = 1-(Data/256);
}

////////////////////////////////
//  Magnetometer function
//  Mag_init
//  Mag_getMagVec
//  Mag_getTemp
///////////////////////////////

/***************************************************************************
 * Initialized magnetometer.  For specifics on what is initialized, look at
 * values sent, then look it up in the damn datasheet.  Magnetometer never
 * worked properly.  Not sure if that's because of a hardware issue (had a wire
 * runing right over sensor which could produce magnetic field) or if its a
 * software related thing.
 ***************************************************************************/
void Mag_init(void)
{
    bool ack;

    I2Cstart();
    I2Cwait();

    ack = I2Ctx(MagAddw);
    I2Cwait();
    ack = I2Ctx(0x00);
    I2Cwait();
    ack = I2Ctx(0b10011000);
    I2Cwait();
    ack = I2Ctx(0b00100000);
    I2Cwait();
    ack = I2Ctx(0x00);
    I2Cwait();
    I2Cstop();
    I2Cwait();

    return;
}

/***************************************************************************
 * This function returns the magnetometer vector in x, y, and z.  Again, this
 * never really returned reasonable values.  Possibly software related.  The
 * stupid fucking datasheet for this part says almost nothing about the data
 * received, or how precise (how many bits long) it actually is.  Therefore,
 * not entirely sure what prescaling needs to be....just to re-iterate-fuck the
 * datasheet.
 ***************************************************************************/
void Mag_getMagVec(void)
{
    bool ack;
    short int Xlsb, Xmsb, Ylsb, Ymsb, Zlsb, Zmsb, Temp;
    I2Cstart();
    I2Cwait();
    ack = I2Ctx(MagAddw);
    I2Cwait();
    ack = I2Ctx(0x83);
    I2Cwait();
    I2Crestart();
    I2Cwait();
    ack = I2Ctx(MagAddr);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Xlsb = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Xmsb = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Ylsb = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Ymsb = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Zlsb = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Zmsb = I2Crx(1);
    I2Cwait();
    I2Cstop();
    I2Cwait();
    Temp = ((Xmsb << 8) | Xlsb );
    //Temp >>= 4;
    MagVec[1] = (float)Temp;
    MagVec[1] = (MagVec[1]*1.3/2048);
    Temp = ((Ymsb << 8) | Ylsb );
    //Temp >>= 4;
    MagVec[0] = (float)Temp;
    MagVec[0] = -1*(MagVec[0]*1.3/2048);
    Temp = ((Zmsb << 8) | Zlsb );
    //Temp >>= 4;
    MagVec[2] = (float)Temp;
    MagVec[2] = MagVec[2]*1.3/2048;

}

/***************************************************************************
 * This function was written to test the magnetometer by getting the temperature
 * value from it.  Never returned a valid tempurature.  Again, not sure if that
 * is because of the function, or if the magnetometer just didn't want to
 * cooperate.  Again, fuck this datasheet, thought the little documentation it
 * had on the tempurature was more than for the actual magnetometer...
 ***************************************************************************/
float Mag_getTemp(void)
{
    bool ack;
    short int Xlsb, Xmsb, Temp;
    I2Cstart();
    I2Cwait();
    ack = I2Ctx(MagAddw);
    I2Cwait();
    ack = I2Ctx(0xB1);
    I2Cwait();
    I2Crestart();
    I2Cwait();
    ack = I2Ctx(GyroAddr);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Xmsb = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    Xlsb = I2Crx(1);
    I2Cwait();
    I2Cstop();
    I2Cwait();

    Temp = (Xmsb<<8) | Xlsb;
    Temp = Temp>>4;
    Temperature = (float)Temp;
    Temperature = Temperature/8;


}

//////////////////////////////////////////
//  Barometer functions
//  Bar_getParam
//  Bar_getTemp
///////////////////////////////////////////

short int AC1, AC2, AC3, B1, B2, MB, MC, MD;
unsigned short int AC4, AC5, AC6;

/***************************************************************************
 * Function to get all the parameters in order to calculate tempurature (and
 * later pressure and altitude).  Not sure if working right or not.  Never got
 * acurate tempurature reading, and kinda gave up on it to focus on more
 * pressing matters.  Also, if you thought the Mag datasheet sucked, wait till
 * you look at this one.  Has absolutely nothing on it.  Does tell what to do
 * to get temp and pressure values, but does so by assuming you have their code
 * and are using their functions (aka saying stuff along the lines of get temp
 * by using Bar.workPerfectly&GetTemp() funcition, or something equally useless
 ***************************************************************************/
void Bar_getParam(void){
    bool ack;
    short int h,l;

    ////////////////////////////////////////////
    //Code to get stuff from Barometer
    ////////////////////////////////////////////
    I2Cstart();
    I2Cwait();
    ack = I2Ctx(0xEE);
    I2Cwait();
    ack = I2Ctx(0xAA);
    I2Cwait();
    I2Crestart();
    I2Cwait();
    ack = I2Ctx(0xEF);
    I2Cwait();
    ////////////////////////////////////////////
    //Get lots of data
    ////////////////////////////////////////////
    I2Csetrx();
    I2Cwait();
    h = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    l = I2Crx(0);
    I2Cwait();
    AC1 = (h<<8) | l;


    I2Csetrx();
    I2Cwait();
    h = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    l = I2Crx(0);
    I2Cwait();
    AC2 = (h<<8) | l;

    I2Csetrx();
    I2Cwait();
    h = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    l = I2Crx(0);
    I2Cwait();
    AC3 = (h<<8) | l;

    I2Csetrx();
    I2Cwait();
    h = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    l = I2Crx(0);
    I2Cwait();
    AC4 = (h<<8) | l;

    I2Csetrx();
    I2Cwait();
    h = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    l = I2Crx(0);
    I2Cwait();
    AC5 = (h<<8) | l;

    I2Csetrx();
    I2Cwait();
    h = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    l = I2Crx(0);
    I2Cwait();
    AC6 = (h<<8) | l;

    I2Csetrx();
    I2Cwait();
    h = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    l = I2Crx(0);
    I2Cwait();
    B1 = (h<<8) | l;

    I2Csetrx();
    I2Cwait();
    h = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    l = I2Crx(0);
    I2Cwait();
    B2 = (h<<8) | l;

    I2Csetrx();
    I2Cwait();
    h = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    l = I2Crx(0);
    I2Cwait();
    MB = (h<<8) | l;

    I2Csetrx();
    I2Cwait();
    h = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    l = I2Crx(0);
    I2Cwait();
    MC = (h<<8) | l;

    I2Csetrx();
    I2Cwait();
    h = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    l = I2Crx(1);
    I2Cwait();
    MD = (h<<8) | l;

    ///////////////////////////////////////////
    //Finish up protocol
    //////////////////////////////////////////
    I2Cstop();
    I2Cwait();

}

/***************************************************************************
 * Function to determine temperature from Barometer data.  Not sure if it is
 * correct or not.  Consult the stupid fucking datasheet.  It should be noted
 * that if this were ever to be actually implemented, it should be split up into
 * 2 functions.  One to tell the barometer to measure temp. And the other to get
 * the data once it was finished.
 ***************************************************************************/
int Bar_getTemp(void){
    bool ack;
    short int UT;
    short int MSBdata, LSBdata;
    long int X1, X2, B5, T;

    ////////////////////////////////////////////
    //Code to get Barometer to collect data
    ////////////////////////////////////////////
    I2Cstart();
    I2Cwait();
    ack = I2Ctx(0xEE);
    I2Cwait();
    ack = I2Ctx(0xF4);
    I2Cwait();
    ack = I2Ctx(0x2E);
    I2Cwait();
    I2Cstop();
    I2Cwait();
    __delay_us(4500);

    ////////////////////////////////////////////
    //Code to get Temp data
    ////////////////////////////////////////////
    I2Cstart();
    I2Cwait();
    ack = I2Ctx(0xEE);
    I2Cwait();
    ack = I2Ctx(0xF6);
    I2Cwait();
    I2Crestart();
    I2Cwait();
    ack = I2Ctx(0xEF);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    MSBdata = I2Crx(0);
    I2Cwait();
    I2Csetrx();
    I2Cwait();
    LSBdata = I2Crx(1);
    I2Cwait();
    I2Cstop();
    I2Cwait();
    UT =(MSBdata<<8) | LSBdata;

    ////////////////////////////////////////
    // Calculate true temp
    ////////////////////////////////////////
    X1 = (UT - AC6)*AC5/pow(2,15);
    X2 = MC*pow(2,11)/(X1+MD);
    B5 = X1+X2;
    T = (B5+8)/pow(2,4);
    return (int)T;
}