/*
   Arduino and MPU6050 Accelerometer and Gyroscope Sensor Tutorial
   by Dejan, https://howtomechatronics.com
*/

#include <Wire.h>
#include <FastLED.h>
#include <MPU6050_light.h>

#define LED_PIN     7

byte mode = 5; // 0 for acceleration, 1 for fire, 2 for waterfall, 3 pride, 4 glitter, 5 pulse, 6 off
const byte NUM_MODES = 8;
const byte debug = 3;
const byte STRIPS = 6;
const byte NUM_LEDS = 10;
const byte FRAMES_PER_SECOND = 60;
const float range = 0.5; //accelleration range in g

const byte BRIGHTNESS = 50;
const byte COOLING = 80;
const byte SPARKING = 50;
uint8_t gHue = 0;


const float GRAVITY = -9.81;              // Downward (negative) acceleration of gravity in m/s^2
const float h0 = 1;                  // Starting height, in meters, of the ball (strip length)
const byte NUM_BALLS = STRIPS;                 // Number of bouncing balls you want (recommend < 7, but 20 is fun in its own way)

float h[NUM_BALLS] ;                         // An array of heights
float vImpact0 = sqrt( -2 * GRAVITY * h0 );  // Impact velocity of the ball when it hits the ground if "dropped" from the top of the strip
float vImpact[NUM_BALLS] ;                   // As time goes on the impact velocity will change, so make an array to store those values
float tCycle[NUM_BALLS] ;                    // The time since the last time the ball struck the ground
int   pos[NUM_BALLS] ;                       // The integer position of the dot on the strip (LED index)
long  tLast[NUM_BALLS] ;                     // The clock time of the last ground strike
float COR[NUM_BALLS] ;                       // Coefficient of Restitution (bounce damping)

float accelerationHistory [STRIPS];

MPU6050 mpu(Wire);
CRGB leds[NUM_LEDS * STRIPS];
CRGBPalette16 gPal;
CRGB flagcolors[6] = {CRGB::Red, CRGB::DarkOrange, CRGB::Yellow,  CRGB::DarkGreen, CRGB::Blue, CRGB::DarkViolet};

const byte modeSwitchPin = 2;

void setup() {
	Serial.begin(19200);
	//setup LEDs
	FastLED.addLeds<WS2812, LED_PIN, GRB>(leds, NUM_LEDS*STRIPS).setCorrection( TypicalLEDStrip );
	FastLED.setBrightness( BRIGHTNESS );


	pinMode(modeSwitchPin, INPUT_PULLUP);
	attachInterrupt(digitalPinToInterrupt(modeSwitchPin), setMode, RISING);

	for (int i = 0; i<STRIPS; i++) {
		accelerationHistory[i] = 1;
	}
	

	Wire.begin();
	byte status = mpu.begin();
	Serial.print(F("MPU6050 status: "));
	Serial.println(status);
	while(status!=0){ } // stop everything if could not connect to MPU6050

	Serial.println(F("Calculating offsets, do not move MPU6050"));
	delay(1000);
	mpu.calcOffsets(true,true); // gyro and accelero

	for (int i = 0 ; i < NUM_BALLS ; i++) {    // Initialize variables
		tLast[i] = millis();
		h[i] = h0;
		pos[i] = 0;                              // Balls start on the ground
		vImpact[i] = vImpact0;                   // And "pop" up at vImpact0
		tCycle[i] = 0;
		COR[i] = 0.90 - float(i)/pow(NUM_BALLS,2); 
	}

	Serial.println("Done!\n");

}

float calculateOrientationData() {
	mpu.update();
	float accCombined = sqrt(pow(mpu.getAccX(), 2) + pow(mpu.getAccY(), 2) + pow(mpu.getAccZ(), 2));

	// Print the values on the serial monitor
	if (debug <= 1) {
		Serial.print(F("TEMPERATURE: "));Serial.println(mpu.getTemp());
		Serial.print(F("ACCELERO  X: "));Serial.print(mpu.getAccX());
		Serial.print("\tY: ");Serial.print(mpu.getAccY());
		Serial.print("\tZ: ");Serial.println(mpu.getAccZ());

		Serial.print(F("GYRO      X: "));Serial.print(mpu.getGyroX());
		Serial.print("\tY: ");Serial.print(mpu.getGyroY());
		Serial.print("\tZ: ");Serial.println(mpu.getGyroZ());

		Serial.print(F("ACC ANGLE X: "));Serial.print(mpu.getAccAngleX());
		Serial.print("\tY: ");Serial.println(mpu.getAccAngleY());

		Serial.print(F("ANGLE     X: "));Serial.print(mpu.getAngleX());
		Serial.print("\tY: ");Serial.print(mpu.getAngleY());
		Serial.print("\tZ: ");Serial.println(mpu.getAngleZ());
	}
	if (debug <= 2) {
		Serial.print("ACC COMBINED: ");
		Serial.println(accCombined, 3);
	}
	return accCombined;
}

void drawAccelerationOnStrip(int strip, float acceleration){ // 0 1
	if (debug <= 2) {
		Serial.print("drawing strip ");
		Serial.print(strip);
		Serial.print("\t");
		Serial.print(acceleration, 4);
		Serial.print("\t");
	}

	int stripStart = NUM_LEDS * strip; // 10*0  0
	if (debug <= 2) {
		Serial.print(stripStart);
		Serial.print("\t");
	}

	int ledcutoff = stripStart + int((NUM_LEDS/(2*range))*acceleration - (NUM_LEDS/2)); // 0+(10/(2*0,5)*1-10/2)  5
	if (debug <= 2) {
		Serial.print(ledcutoff);
		Serial.print("\t");
	}

	int stripEnd = (NUM_LEDS * (strip + 1)) - 1; // (10*(0+1))-1  9
	if (debug <= 2) {
		Serial.println(stripEnd);
	}

	if ((ledcutoff < stripStart) || (ledcutoff > stripEnd)){
		for (int i = stripStart; i <= stripEnd; i++) {
			leds[i] = CRGB(0, 0, 64);
		}
	}
	else {
		for (int i = stripStart; i <= ledcutoff; i++) {
			leds[i] = CRGB(64, 0, 0);
		}
		for (int i = ledcutoff; i <= stripEnd; i++) {
			leds[i] = CRGB(0, 64, 0);
		}
	}
}

void enableLEDsOnAcceleration(float accCombined){
	// draw all stored accelerations
	for (int i = 0; i < STRIPS; i++){
		drawAccelerationOnStrip(i, accelerationHistory[i]);
	}
	//shift them to the front
	for (int i = 0; i < STRIPS-1; i++) {
		accelerationHistory[i] = accelerationHistory[i+1];
	}
	//add new entry at the end
	accelerationHistory[STRIPS-1] = accCombined;
}

void calculateFire(int strip){
	// Array of temperature readings at each simulation cell
	static byte heat[STRIPS][NUM_LEDS];

	// Step 1.  Cool down every cell a little
	for( int i = 0; i < NUM_LEDS; i++) {
		heat[strip][i] = qsub8( heat[strip][i],  random8(0, ((COOLING * 10) / NUM_LEDS) + 2));
	}

	// Step 2.  Heat from each cell drifts 'up' and diffuses a little
	for( int k= NUM_LEDS - 1; k >= 2; k--) {
		heat[strip][k] = (heat[strip][k - 1] + heat[strip][k - 2] + heat[strip][k - 2] ) / 3;
	}

	// Step 3.  Randomly ignite new 'sparks' of heat near the bottom
	if( random8() < SPARKING ) {
		int y = random8(7);
		heat[strip][y] = qadd8( heat[strip][y], random8(160,255) );
	}

	// Step 4.  Map from heat cells to LED colors
	for( int j = 0; j < NUM_LEDS; j++) {
		// Scale the heat value from 0-255 down to 0-240
		// for best results with color palettes.
		byte colorindex = scale8( heat[strip][j], 240);
		CRGB color = ColorFromPalette( gPal, colorindex);
		int pixelnumber = (strip * NUM_LEDS) + j;
		leds[pixelnumber] = color;
	}
}

void drawFire(){
	if (mode == 1) {
		gPal = CRGBPalette16( CRGB::Black, CRGB::Red, CRGB::Yellow, CRGB::White);
	}
	else { 
		gPal = CRGBPalette16( CRGB::Black, CRGB::Blue, CRGB::Aqua,  CRGB::White);
	}
	for (int i = 0; i < STRIPS; i++){
		calculateFire(i);
	}
}

void drawPride(){
	for (int strip = 0; strip < STRIPS; strip++){
		CRGB color = flagcolors[strip];
		for( int j = 0; j < NUM_LEDS; j++) {
			int pixelnumber =  (strip * NUM_LEDS) + j;
			leds[pixelnumber] = color;
		}
	}
}

void drawGlitter(){
	fadeToBlackBy( leds, STRIPS*NUM_LEDS, 10);
	int pos = random16(STRIPS*NUM_LEDS);
	leds[pos] += CHSV( gHue + random8(64), 200, 255);
}

void drawPulse(){
	static int current_step;
	float steps = 2000;
	CRGB color;
	for (int strip = 0; strip < STRIPS; strip++){
		//float f = 0.5-0.5*cos(360/steps*float(current_step) + 360/steps*360/float(STRIPS)*float(strip));
		float f = cos(360/steps*float(current_step) + 360/steps*360/float(2*STRIPS)*float(strip));
		if (f < 0) f = 0;
		color = CHSV(gHue , 255, int(255*f));
		for( int j = 0; j < NUM_LEDS; j++) {
			int pixelnumber =  (strip * NUM_LEDS) + j;
			leds[pixelnumber] = color;
		}
	}
	current_step++;
}

void drawKunsisLila(){
	static int current_step;
	float steps = 2000;
	CRGB color;
	for (int strip = 0; strip < STRIPS; strip++){
		float f = cos(360/steps*float(current_step) + 360/steps*360/float(2*STRIPS)*float(strip));
		if (f < 0) f = 0;
		color = CHSV(224 , 255, int(255*f));
		for( int j = 0; j < NUM_LEDS; j++) {
			int pixelnumber =  (strip * NUM_LEDS) + j;
			leds[pixelnumber] = color;
		}
	}
	current_step++;
}

void bounceBalls(){
	for (int i = 0 ; i < NUM_BALLS ; i++) {
		tCycle[i] =  millis() - tLast[i] ;     // Calculate the time since the last time the ball was on the ground

		// A little kinematics equation calculates positon as a function of time, acceleration (gravity) and intial velocity
		h[i] = 0.5 * GRAVITY * pow( tCycle[i]/1000 , 2.0 ) + vImpact[i] * tCycle[i]/1000;

		if ( h[i] < 0 ) {                      
			h[i] = 0;                            // If the ball crossed the threshold of the "ground," put it back on the ground
			vImpact[i] = COR[i] * vImpact[i] ;   // and recalculate its new upward velocity as it's old velocity * COR
			tLast[i] = millis();

			if ((vImpact[i] < 0.01 ) && (calculateOrientationData()>1.5)) {
				vImpact[i] = vImpact0;  // If the ball is barely moving, "pop" it back up at vImpact0
			}
		}
		pos[i] = round( h[i] * (NUM_LEDS - 1) / h0);       // Map "h" to a "pos" integer index position on the LED strip
	}

	for (int i = 0 ; i < STRIPS*NUM_LEDS ; i++) {
		leds[i] = CRGB::Black;
	}	
	//Choose color of LEDs, then the "pos" LED on
	for (int i = 0 ; i < NUM_BALLS ; i++) leds[(i * NUM_LEDS)+pos[i]] = CHSV( uint8_t (i * 40) , 255, 255);
	//Then off for the next loop around
}

void drawOff(){
	fadeToBlackBy( leds, STRIPS*NUM_LEDS, 10);
}

void setMode(){
	mode++;
	if (mode >= NUM_MODES) {
		mode = 0;
	}
	if (debug <= 3) {
		Serial.print("Blinkmode:\t");
		Serial.println(mode);
	}

}

void loop() {
	if (mode == 0) {
		// === Read acceleromter data === //
		float accCombined = calculateOrientationData();
		enableLEDsOnAcceleration(accCombined);
	}
	else if ((mode == 1) || (mode == 2)) {
		random16_add_entropy( random());
		drawFire();
	}
	else if (mode == 3) {
		drawPride();
	}
	else if (mode == 4) {
		drawGlitter();
	}
	else if (mode == 5) {
		drawPulse();
	}
	else if (mode == 6) {
		drawKunsisLila();
	}
	else if (mode == 7) {
		bounceBalls();
	}
	else if (mode == 8) {
		drawOff();
	}
	FastLED.show();
	gHue++;
	FastLED.delay(1000 / FRAMES_PER_SECOND);
}