/***************************************************************************
 * This header file is primarily a place where all the miscellaneous functions
 * I wrote ended up, hence the name.
 ***************************************************************************/


/***************************************************************************
 * Function to print the offsets to the screen.  Got tired of this giant block
 * of text in the main, so put it in a function.
 ***************************************************************************/
void PrintOffs(void)
{
    char string[20];
    myputc('\n');
    myputc('\r');
    myputs("Start:");
    myputc('\n');
    myputc('\r');
    sprintf(string, "Zoff: %f",AccZoff);
    myputs(string);
    myputc('\n');
    myputc('\r');
    myputs("Rotational Offsets: ");
    myputi(RotOff[0]);
    myputc(',');
    myputi(RotOff[1]);
    myputc(',');
    myputi(RotOff[2]);
    myputc('.');
    myputc('\n');
    myputc('\r');
}

/***************************************************************************
 * Function to print out most, if not all, of the data to the screen.  Again
 * wrote the funciton because I was tired of the big block sitting in the main.
 ***************************************************************************/
void PrintData(void)
{
    char string[20];
     myputc('<');
        sprintf(string, "%f",AccVec[0]);
        myputs(string);
        myputc(',');
        sprintf(string, "%f",AccVec[1]);
        myputs(string);
        myputc(',');
        sprintf(string, "%f",AccVec[2]);
        myputs(string);
        myputc('>');
        myputc('\t');
        myputc('<');
        sprintf(string, "%f",MagVec[0]);
        myputs(string);
        myputc(',');
        sprintf(string, "%f",MagVec[1]);
        myputs(string);
        myputc(',');
        sprintf(string, "%f",MagVec[2]);
        myputs(string);
        myputc('>');
        myputc('\t');
        myputc('<');
        myputi(RotVec[0]);
        myputc(',');
        myputi(RotVec[1]);
        myputc(',');
        myputi(RotVec[2]);
        myputc('>');
        /*
        myputc('\t');
        myputc('<');
        sprintf(string, "%f",OrientVec[0]);
        myputs(string);
        myputc(',');
        sprintf(string, "%f",OrientVec[1]);
        myputs(string);
        myputc(',');
        sprintf(string, "%f",OrientVec[2]);
        myputs(string);
        myputc('>');
         */
        myputc('\n');
        myputc('\r');
}

/***************************************************************************
 * Function I put together to round a number.  Pretty sure it works.  Can't
 * belive the math.h header didn't have this function in it.
 ***************************************************************************/
float round(float num)
{
    int numi;
    numi = num*10;
    numi = numi%10;

    if (numi >= 5)
    {
        if (num >=0)
        {
            return (float)ceil(num);
        }
        else
        {
            return (float)floor(num);
        }
    }
    else
    {
        if (num >= 0)
        {
            return (float)floor(num);
        }
        else
        {
            return (float)ceil(num);
        }
    }
}

/***************************************************************************
 * Function written to initialize timer 1.  This is used to calculate time it
 * takes for the loop to run to calculate integral and derivative portions of
 * the PID controler
 ***************************************************************************/
void init_TMR1(void)
{
    TMR1                = 0;            //Set Timer2 to 0
    T1CONbits.TCKPS     = 0b10;         //Prescale value is 8
    T1CONbits.TON       = 1;            //Turn Timer2 off
}