#include <math.h>
#include <float.h>
#include <limits.h>
 
 
int valx = 0;
int valy = 0;
int valz = 0;
 
int xaxis = 0;
int yaxis = 1;   
int zaxis = 2;   
 
float xa=0;
float ya=0;
float za=0;
 
 
 
 
int minx = INT_MAX;
int maxx = 0;
int miny = INT_MAX;
int maxy = 0;
int minz = INT_MAX;
int maxz = 0;
 
int g0x = 0;
int g0y = 0;
int g0z = 0;
 
long time1 = 0;
long time2 = 0;
 
float rho = 0;
float phi = 0;
float theta = 0;
 
 
void setup()
{
  Serial.begin(9600);
  Serial.println("RST\r\n");
  pinMode(xaxis,INPUT);
  pinMode(yaxis,INPUT);
  pinMode(zaxis,INPUT);
  time1=millis();
  time2=millis();
}
 
void loop()
{
 
  valx = analogRead(xaxis);    // read the value from the sensor
  valy = analogRead(yaxis);    // read the value from the sensor
  valz = analogRead(zaxis);    // read the value from the sensor
 
  int pfx = valx;
  int pfy = valy;
  int pfz = valz;
 
  autoZeroCalibration(pfx,pfy,pfz);
 
  int fx = (pfx - g0x);
  int fy = (pfy - g0y);
  int fz = (pfz - g0z);
 
  float ax = fx*(3.3/(1024.0*0.800));
  float ay = fy*(3.3/(1024.0*0.800));
  float az = fz*(3.3/(1024.0*0.800));
 
  //
  rho =   atan(ax/sqrt(pow(ay,2)+pow(az,2)))*(360/(2*3.1592));  
  phi =   atan(ay/sqrt(pow(ax,2)+pow(az,2)))*(360/(2*3.1592));  
  theta = atan(sqrt(pow(ay,2)+pow(ax,2))/az)*(360/(2*3.1592));  
 
  printAngles();
 
}
 
void autoZeroCalibration(int pfx, int pfy, int pfz)
{
    if ((pfx < minx)||(pfy < miny)||(pfz < minz)||(pfx > maxx)||(pfy > maxy)||(pfz > maxz)) {
     // autozero calibration
     if (pfx < minx) minx = pfx;
     if (pfy < miny) miny = pfy;
     if (pfz < minz) minz = pfz;
 
     if (pfx > maxx) maxx = pfx;
     if (pfy > maxy) maxy = pfy;
     if (pfz > maxz) maxz = pfz;
 
     g0x = ((maxx - minx)/2)+minx;
     g0y = ((maxy - miny)/2)+miny;
     g0z = ((maxz - minz)/2)+minz;
 
     printValues();
  }
}
void printFloat(float value, int places) {
   // this is used to cast digits
   int digit;
   float tens = 0.1;
   int tenscount = 0;
   int i;
   float tempfloat = value;
 
   // if value is negative, set tempfloat to the abs value
   // make sure we round properly. this could use pow from  
   //<math.h>, but doesn't seem worth the import
   // if this rounding step isn't here, the value  54.321 prints as  
   //54.3209
 
   // calculate rounding term d:   0.5/pow(10,places)
   float d = 0.5;
   if (value < 0)
     d *= -1.0;
   // divide by ten for each decimal place
   for (i = 0; i < places; i++)
     d/= 10.0;
   // this small addition, combined with truncation will round our  
   // values properly
   tempfloat +=  d;
 
   // first get value tens to be the large power of ten less than value
   // tenscount isn't necessary but it would be useful if you wanted  
   // to know after this how many chars the number will take
 
   if (value < 0)
     tempfloat *= -1.0;
   while ((tens * 10.0) <= tempfloat) {
     tens *= 10.0;
     tenscount += 1;
   }
 
 
   // write out the negative if needed
   if (value < 0)
     Serial.print('-');
 
   if (tenscount == 0)
     Serial.print(0, DEC);
 
   for (i=0; i< tenscount; i++) {
     digit = (int) (tempfloat/tens);
     Serial.print(digit, DEC);
     tempfloat = tempfloat - ((float)digit * tens);
     tens /= 10.0;
   }
 
   // if no places after decimal, stop now and return
   if (places <= 0)
     return;
 
   // otherwise, write the point and continue on
   Serial.print('.');
 
   // now write out each decimal place by shifting digits one by one  
   // into the ones place and writing the truncated value
   for (i = 0; i < places; i++) {
     tempfloat *= 10.0;
     digit = (int) tempfloat;
     Serial.print(digit,DEC);
     // once written, subtract off that digit
     tempfloat = tempfloat - (float) digit;
   }
}
 
void printAngles()
{
   if (millis()-time1 > 1000) {
    time1 = millis();
    Serial.print("rho: ");
    printFloat(rho,3);
    Serial.print(" phi: ");
    printFloat(phi,3);
    Serial.print(" theta: ");
    printFloat(theta,3);
    Serial.print("\r\n");
  }
}
 
void printValues()
{
  if ((millis() - time2) > 800) {
    time2=millis();
    Serial.print("minx: ");
    Serial.print((int)minx,DEC);
    Serial.print(".");
    Serial.print((minx-(int)minx)*1000,DEC);
    Serial.print(" miny: ");
    Serial.print((int)miny,DEC);
    Serial.print(".");
    Serial.print((miny-(int)miny)*1000,DEC);
    Serial.print(" minz: ");
    Serial.print((int)minz,DEC);
    Serial.print(".");
    Serial.print((minz-(int)minz)*1000,DEC);
    Serial.print("\r\n");
 
    Serial.print("maxx: ");
    Serial.print((int)maxx,DEC);
    Serial.print(".");
    Serial.print((maxx-(int)maxx)*1000,DEC);
    Serial.print(" maxy: ");
    Serial.print((int)maxy,DEC);
    Serial.print(".");
    Serial.print((maxy-(int)maxy)*1000,DEC);
    Serial.print(" maxz: ");
    Serial.print((int)maxz,DEC);
    Serial.print(".");
    Serial.print((maxz-(int)maxz)*1000,DEC);
    Serial.print("\r\n");
  }
}

/* Sleep Demo Serial
 * -----------------
 * Example code to demonstrate the sleep functions in a Arduino. Arduino will wake up
 * when new data is received in the serial port USART
 * Based on Sleep Demo Serial from http://www.arduino.cc/playground/Learning/ArduinoSleepCode 
 *
 * Copyright (C) 2006 MacSimski 2006-12-30 
 * Copyright (C) 2007 D. Cuartielles 2007-07-08 - Mexico DF
 * 
 *  With modifications from Ruben Laguna  2008-10-15
 * 
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 * 
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 * 
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include <avr/power.h>
#include <avr/sleep.h>
 
 
 
int sleepStatus = 0;             // variable to store a request for sleep
int count = 0;                   // counter
 
 
void setup()
{
 
  Serial.begin(9600);
}
 
void sleepNow()
{
    /* Now is the time to set the sleep mode. In the Atmega8 datasheet
     * http://www.atmel.com/dyn/resources/prod_documents/doc2486.pdf on page 35
     * there is a list of sleep modes which explains which clocks and 
     * wake up sources are available in which sleep modus.
     *
     * In the avr/sleep.h file, the call names of these sleep modus are to be found:
     *
     * The 5 different modes are:
     *     SLEEP_MODE_IDLE         -the least power savings 
     *     SLEEP_MODE_ADC
     *     SLEEP_MODE_PWR_SAVE
     *     SLEEP_MODE_STANDBY
     *     SLEEP_MODE_PWR_DOWN     -the most power savings
     *
     *  the power reduction management <avr/power.h>  is described in 
     *  http://www.nongnu.org/avr-libc/user-manual/group__avr__power.html
     */  
 
  set_sleep_mode(SLEEP_MODE_IDLE);   // sleep mode is set here
 
  sleep_enable();          // enables the sleep bit in the mcucr register
                             // so sleep is possible. just a safety pin 
 
  power_adc_disable();
  power_spi_disable();
  power_timer0_disable();
  power_timer1_disable();
  power_timer2_disable();
  power_twi_disable();
 
 
  sleep_mode();            // here the device is actually put to sleep!!
 
                             // THE PROGRAM CONTINUES FROM HERE AFTER WAKING UP
  sleep_disable();         // first thing after waking from sleep:
                            // disable sleep...
 
  power_all_enable();
 
}
 
 
void loop()
{
  // display information about the counter
  Serial.print("Awake for ");
  Serial.print(count);
  Serial.println("sec");
  count++;
  delay(1000);                           // waits for a second
 
  // compute the serial input
  if (Serial.available()) {
    int val = Serial.read();
    if (val == 'S') {
      Serial.println("Serial: Entering Sleep mode");
      delay(100);     // this delay is needed, the sleep 
                      //function will provoke a Serial error otherwise!!
      count = 0;
      sleepNow();     // sleep function called here
    }
    if (val == 'A') {
      Serial.println("Hola Caracola"); // classic dummy message
    }
  }
 
  // check if it should go asleep because of time
  if (count >= 10) {
      Serial.println("Timer: Entering Sleep mode");
      delay(100);     // this delay is needed, the sleep 
                      //function will provoke a Serial error otherwise!!
      count = 0;
      sleepNow();     // sleep function called here
  }
}