/*
 * Sleeping Arduino - unless you jostle it!
 *
 * If left alone, make a light pummer "snore" in peaceful colors.  If
 * agitated by shaking the device, breath faster in hotter colors.
 *
 * A basic example sketch that reads from analog three-pin three-axis
 * accelerometers, such as the common ADXL330 chip found at Seeedstudio
 * or SparkFun online vendors.
 *
 * In addition, this sketch outputs PWM signals to three LEDs, such as
 * a self-contained RGB LED package or three independently wired LEDs,
 * one each of red, green and blue suggested.
 *
 * Optionally, wiring a switch from digital pin 8 to ground will give
 * this sketch a chance to "calibrate" the accelerometer.  Not required
 * but useful, since every accelerometer varies slightly.
 *
 * Released (cc) Creative Commons Attribution Only
 * Ed Halley
 * [ e d @ h a l l e y . c c ]
 *
 */

#include <math.h>

/* Pummer:
 * A simple RGB color-interpolating helper class.
 *
 * When creating one, tell it which three output pins to drive PWM signals.
 * If your RGB device is common-anode, it can reverse the PWM for you.
 * Don't forget to limit current to each LED with a resistor (e.g., 220ohm).
 *
 * At any time, tell it what color to become by calling the goal() method,
 * and how fast to transition to that color.
 *
 * Call the pummer's loop() method occasionally to let it set the PWM
 * outputs to the LEDs.
 */
class Pummer
{
    byte lR, lG, lB;
    byte nR, nG, nB;
    byte wR, wG, wB;
    int pR, pG, pB;
    unsigned long last, when;
    boolean reverse;

public:
    Pummer(int pinR, int pinG, int pinB, boolean anode=false)
    {
        pinMode(pR = pinR, OUTPUT);
        pinMode(pG = pinG, OUTPUT);
        pinMode(pB = pinB, OUTPUT);
        nR = nG = nB = 0;
        reverse = anode;
        show();
        goal(255, 255, 255);
    }

    void show()
    {
        analogWrite(pR, reverse? (255-nR) : nR);
        analogWrite(pG, reverse? (255-nG) : nG);
        analogWrite(pB, reverse? (255-nB) : nB);
    }

    boolean done() { return last == when; }

    void goal(byte r, byte g, byte b, unsigned long speed = 500)
    {
        lR = nR; lG = nG; lB = nB;
        wR = r; wG = g; wB = b;
        last = millis();
        when = last + speed;
    }

    void loop()
    {
        unsigned long now = millis();
        if (now > when)
        {
            if (last == when)
                return;
            nR = wR; nG = wG; nB = wB;
            last = when;
        }
        else
        {
            nR = map(now, last, when, lR, wR);
            nG = map(now, last, when, lG, wG);
            nB = map(now, last, when, lB, wB);
        }
        show();
    }
};

/* Accelerometer:
 * Receive input from a 3-axis device, and perform some useful calculations.
 *
 * Specify the three axis pins, and the VCC and GND pins, using analog pin
 * numbers.  These are usually adjacent on the common breakout boards.
 *
 * Call the accelerometer's update() method occasionally to update the
 * current values from the hardware.
 *
 * Get the axis acceleration features with the axis() method, or other useful
 * calculations such as milligee(), pitch() and roll().
 *
 * Note that some functions like pitch() and roll() use floating point math,
 * and can therefore really increase the size of the program on the chip.
 * You can save about 2000 bytes if you don't need any floating point math.
 */
#define ANALOG0 14

class Accelerometer
{
    int p[3]; // which analog pins
    int a[3]; // acceleration, zero-based
    int b[3]; // acceleration bias/calibration information
    int g, t, r; // cached copies of calculations
    int scale; // scaling factor between ADC and gravity

public:
    Accelerometer(int pinX, int pinY, int pinZ, int pinVCC, int pinGND)
    {
        pinMode(pinGND + ANALOG0, OUTPUT); digitalWrite(pinGND + ANALOG0, LOW);
        pinMode(pinVCC + ANALOG0, OUTPUT); digitalWrite(pinVCC + ANALOG0, HIGH);
        pinMode((p[0] = pinX) + ANALOG0, INPUT);
        pinMode((p[1] = pinY) + ANALOG0, INPUT);
        pinMode((p[2] = pinZ) + ANALOG0, INPUT);
        for (int i = 0; i < 3; i++)
            b[i] = 512;
        g = t = r = 0;
        scale = 100;
    }

    void update()
    {
        for (int i = 0; i < 3; i++)
            a[i] = analogRead(p[i]) - b[i];
        g = t = r = 0;
    }

    void dump()
    {
        Serial.print(  "x="); Serial.print(a[0]);
        Serial.print("\ty="); Serial.print(a[1]);
        Serial.print("\tz="); Serial.print(a[2]);
        Serial.print("\tmg="); Serial.print(milligee());
        Serial.print("\tpitch="); Serial.print(pitch());
        Serial.print("\troll="); Serial.print(roll());
        Serial.println();
    }

    void calibrate()
    {
        for (int i = 0; i < 3; i++)
            b[i] = analogRead(p[i]);
        b[2] -= scale;
        update();
    }

    int milligee()
    {
        if (g != 0) return g;
        long squared = 0.0;
        for (int i = 0; i < 3; i++)
            squared += (long)a[i] * (long)a[i];
        g = squared * 1000 / (scale*scale);
        return g;
    }

    int accel(int axis)
    {
        if (axis < 0 || axis > 3) return 0;
        return a[axis];
    }

    int roll()
    {
        if (r != 0) return r;
        r = (int)(atan2(a[0], a[2]) * 180. / M_PI);
        return r;
    }

    int pitch()
    {
        if (t != 0) return t;
        t = (int)(acos(a[1] / (float)scale) * 180. / M_PI);
        return t;
    }

    void loop()
    {
        update();
    }
};

/*
 * Example Program
 *
 * Sleeping Arduino - unless you jostle it!
 *
 * If you wire a button from ground to pin 8, you can press it
 * once when the device is at rest on a level table, to get
 * somewhat better calibration data.  We do not try to save the
 * calibration into EEPROM, but that would be handy for more
 * permanent uses of an accelerometer.
 *
 */

void loop() { ; } // we do our own loop below

void setup()
{
    Serial.begin(9600);

    boolean grow = true;
    int anger = 0;
    Pummer pummer = Pummer(5, 6, 3, true);
    Accelerometer accel = Accelerometer(3, 2, 1, 5, 4);

    pinMode(8, INPUT); digitalWrite(8, HIGH); // internal pullup

    int div = 0;
    while (1)
    {
        delay(20);
        accel.loop();
        if (LOW == digitalRead(8))
            accel.calibrate();
        if (--div <= 0) { div = 8; accel.dump(); } // print every 8th loop

        // get more and more annoyed at high gee forces,
        // get instantly annoyed at zero gee force (freefall),
        // fall back asleep at normal gee forces
        //
        if (accel.milligee() > 1800)
            anger += 5;
        else if (accel.milligee() < 200)
            anger = 255;
        else if (accel.milligee() < 1100)
            anger -= 1;
        anger = max(0, min(anger, 255));

        // show the state of mind with our pummer
        pummer.loop();
        if (pummer.done())
        {
            grow = !grow;
            if (grow)
                pummer.goal(anger, 64, 64-anger/4, 350-anger);
            else
                pummer.goal(anger/2, anger/4, 128-anger/2, 2000-anger*7);
        }
    }
}