/////////////////////////////ATyiny1614
//define pins
////// I2C
#define SCL_pin B0
#define SDA_pin B1
////// SERIAL
#define Rx_pin B3 //4
#define Tx_pin B2 //5
////// UPDI
// #define UPDI_pin A0
////// OUTPUTS
#define B_LED_pin A3
#define R_LED_pin A4
#define G_LED_pin A5
#define MOTOR_pin A7 //5
/////// INPUT
#define BTN_pin A6
/////// ADXL343 3 axis ///////
#define BUFFER_SIZE 25
#define ADXL343_ADDRESS                 (0x53)    // I2C ADXL343 ALT Address
#define ADXL343_REG_DATAX0              (0x32)    // X-axis data 0
#define ADXL343_REG_DATAY0              (0x34)    // Y-axis data 0
#define ADXL343_REG_DATAZ0              (0x36)    // Z-axis data 0
//////////////////////////////
///////// INCLUDES ///////////
// Library for I2C - to speak to sensor
#include <Wire.h>
///////////// VARIABLES //////
//// ADXL3434 SENSOR
int minValue = 0;
int maxValue = 511; // +/-2g range
int sensorCollectData = 0; // turn on data collection (0 = off).
int16_t x = 0;
int16_t y = 0;
int16_t z = 0;
int xyzArray[] = {x, y, z};
int xyzArrayLength = sizeof(xyzArray) / (sizeof(xyzArray[0]));
char buffer[10];
int holdLight = 500; // hold colour delay
//// SERIAL
int serialDebug = 1; // 0 for off
int baudRate = 9600;
//// OTHERS
int MODUS = 0;
//// MOTOR
int motorOn = 0; // turn on motor (0 = off).
//int totalModes = 2;
//// Interrupts
int lastInterrupt = 0;
int currentInterupt = 0;
int timeBetweenInterrupts = 0;
//////////////////////////////
void setup() {
  // put your setup code here, to run once:
  //// INPUT
  pinMode(BTN_pin, INPUT_PULLUP);
  //// Attach interrupt
  attachInterrupt(digitalPinToInterrupt(BTN_pin), increaseMODUS, RISING);
  //// OUPUTS
  // RGB LED
  pinMode(R_LED_pin, OUTPUT);
  pinMode(G_LED_pin, OUTPUT);
  pinMode(B_LED_pin, OUTPUT);
  // TURN OFF RGB LEDs
  digitalWrite(R_LED_pin, HIGH);
  digitalWrite(G_LED_pin, HIGH);
  digitalWrite(B_LED_pin, HIGH);
  // Motor
  pinMode(MOTOR_pin, OUTPUT);
  //// I2C COMMS
  Wire.begin(); // join i2c bus (address optional for master)
  // register sensor
  uint8_t deviceId = adxl343ReadRegister(0x00);
  adxl343WriteRegister(0x2D, 0x08); // POWER_CTL register
  adxl343WriteRegister(0x31, 0x08); // Full res register

  ////////////////////////////////////////////////////////////
  //// FEATURES //////
  sensorCollectData = 1;
  motorOn = 0;
  /////////////////
  //// Serial - FTDI
  if (serialDebug == 1) {
    Serial.begin(baudRate);
    while (!Serial);
    Serial.flush();
  }
}
////////////////////////////////////////////////////////////
///////////////////////// LOOP /////////////////////////////
void loop() {

  if (sensorCollectData == 1 ) {

    x = adxl343ReadWord(ADXL343_REG_DATAX0);
    y = adxl343ReadWord(ADXL343_REG_DATAY0);
    z = adxl343ReadWord(ADXL343_REG_DATAZ0);
    delay(50);
    
  }
  // put your main code here, to run repeatedly:
  if (digitalRead(BTN_pin) == LOW) {
    // do nothing
  } else {
    ChangeMode();
  }
}
////////////////////////////////////////////////////////////
////////////////Increase MODUS + ChangeMode ////////////////
void increaseMODUS() {
  // pause interrupts
  //  noInterrupts();
  //  lastInterrupt = millis();
  //  timeBetweenInterrupts = lastInterrupt;
  //
  //  if ((timeBetweenInterrupts + 500) <= lastInterrupt) {
  //    // allow interrupts
  //    interrupts();
  //  }
  // increase MODUS and start new mode
  MODUS++;
  Serial.print("btn pressed - interrupt - ");
  Serial.println(MODUS);
  //
  ChangeMode();

}
////////////////////////////////////////////////////////////
///////////////////////// ChangeMode ///////////////////////
void ChangeMode() {

  // Modes
  switch (MODUS) {

    case 0:
      //allOff();
      // OFF STATE
      break;

    case 1:
      rainbowLight();
      break;

    case 2:
      greenRed();
      break;

    default:
      // reset MODUS
      MODUS = 0;
      allOff();
      ChangeMode();
      break;
  }
}
/////////////////////////////////////////////////////////////
///////////////////////// allOff ////////////////////////////
// just to be sure
void allOff() {
  // LEDs off
  digitalWrite(R_LED_pin, HIGH);
  digitalWrite(G_LED_pin, HIGH);
  digitalWrite(B_LED_pin, HIGH);
  // motor off
  digitalWrite(MOTOR_pin, LOW);
}
////////////////////////////////////////////////////////////
//////////////// RAINBOWLIGHT //////////////////////////////
/////////////// sensor /////////////////////////////////////
void  rainbowLight() {

  // reverse numbers
  if (x <= 0) {
    x = x * -1;
  }
  if (y <= 0) {
    y = y * -1;
  }
  if (z <= 0) {
    z = z * -1;
  }
  // map numbers
  x = map(x, minValue, maxValue, 0, 255);
  y = map(y, minValue, maxValue, 0, 255);
  z = map(z, minValue, maxValue, 0, 255);
  //
  if (motorOn == 1) {
    int j = 0;
    for (int k = 0; k >= xyzArrayLength; k++) {

      if (xyzArray[k] >= j) {
        j = xyzArray[k];
      }
    }
    // motor
    analogWrite(MOTOR_pin, j);
  }
  //
  analogWrite(R_LED_pin, x);
  analogWrite(G_LED_pin, y);
  analogWrite(B_LED_pin, z);
  //////////
  delay(holdLight); // delay to see the colour
}
////////////////GREENRED //////////////////////////////
/////////////// sensor /////////////////////////////////////
void  greenRed() {
  digitalWrite(B_LED_pin, HIGH);// need to turn off Blue!
  int switchThreshold = 255 / 10;
  // reverse numbers
  if (x <= 0) {
    x = x * -1;
  }
  if (y <= 0) {
    y = y * -1;
  }
  if (z <= 0) {
    z = z * -1;
  }
  // map numbers
  x = map(x, minValue, maxValue, 0, 255);
  y = map(y, minValue, maxValue, 0, 255);
  z = map(z, minValue, maxValue, 0, 255);

  // find greatest
  int tempMax = 0;
  int xyzArray[] = {x, y, z};
  int xyzArrayLength = sizeof(xyzArray) / (sizeof(xyzArray[0]));
  // find through array
  for (int k = 0; k <= xyzArrayLength - 2; k++) {
    // no z, values always in the middle?
    if (tempMax <= xyzArray[k]) {
      tempMax = xyzArray[k];
    }
  }
  //
  if (tempMax >= switchThreshold || xyzArray[2] >= (switchThreshold+127)|| xyzArray[2] <= (switchThreshold-127)) {
    // includes z now
    digitalWrite(G_LED_pin, HIGH);
    digitalWrite(R_LED_pin, LOW);
    //digitalWrite(MOTOR_pin, HIGH);
  } else {
    digitalWrite(G_LED_pin, LOW);
    digitalWrite(R_LED_pin, HIGH);
    //digitalWrite(MOTOR_pin, LOW);
  }
  //////////
  delay(50); // delay to see the colour
}
////////////////////////////////////////////////////////////
//////////////////////// ADXL343 - 3 axis //////////////////
uint8_t adxl343WriteRegister(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(ADXL343_ADDRESS);
  Wire.write((uint8_t)reg);
  Wire.write((uint8_t)value);
  Wire.endTransmission();
}

uint8_t adxl343ReadRegister(uint8_t reg) {
  Wire.beginTransmission(ADXL343_ADDRESS);
  Wire.write((uint8_t)reg);
  Wire.endTransmission();
  Wire.requestFrom(ADXL343_ADDRESS, 1);
  return Wire.read();
}

int16_t adxl343ReadWord(uint8_t reg) {
  Wire.beginTransmission(ADXL343_ADDRESS);
  Wire.write((uint8_t)reg);
  Wire.endTransmission();
  Wire.requestFrom(ADXL343_ADDRESS, 2);
  return (uint16_t)(Wire.read() | (Wire.read() << 8));
}
////////////////////////////////////////////////////////////
///////////////////////// SERIAL DEBUG /////////////////////
// Debug by viewing serial output
void debugOutput(int debugValue) {

  Serial.write(debugValue);

}