//#include <RF24Network.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
//#include <SharpIR.h>
#include <Average.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>


//#define SCREEN_WIDTH 100 // OLED display width, in pixels
//#define SCREEN_HEIGHT 64 // OLED display height, in pixels
//#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
//#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(128, 64);

//Average setup
byte averageSize[2] = {10, 15};
Average<float> ave[2] = {
  Average<float> (averageSize[0]),
  Average<float> (averageSize[1])
};

//IR pin setup
#define IRPin A0
#define model 430
#define IRPin A1
#define model 430

//Potentiometers values
int potCState[2] = {0};
int potPState[2] = {0};
int pTurnPot = 0;

//IR values
byte IR_val[2] = {0};
byte IR_Pval[2] = {0};

//Network
RF24 radio(9, 8);
const byte address[6] = "00001";

int timer = 0;
byte cursorSetting[14] = {
  0,0,0,48,48,48,
  0,14,28,0,14,28
  };
int presetSetting = 0;
byte settingArray[6][6] = {
  {0,1,2,3,4,5},
  {0,1,2,3,4,5},
  {0,1,2,3,4,5},
  {0,1,2,3,4,5},
  {0,1,2,3,4,5},
  {0,1,2,3,4,5}
};

#define BUTTON_PIN 4
int pButton;

//////////////////////
void setup() {
  Serial.begin(9600);
  
  //RF24 network activation
  SPI.begin();
  radio.begin();
  radio.openReadingPipe(0, address);
  radio.startListening();

  //Serial.println("before oled");
  //Oled activation
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  pTurnPot = map(analogRead(A2), 0, 1027, 6, -1);
  pButton = digitalRead(BUTTON_PIN);
  menuSetting(pTurnPot);
  pinMode(BUTTON_PIN, INPUT_PULLUP);
}
//////////////////////

//////////////////////
void loop() {
  readIncomingData();
  calcValues();
}
//////////////////////

//////////////////////
void readIncomingData(){
  
  if (radio.available())
  {
    int incomingMessage;
    radio.read(&incomingMessage, sizeof(incomingMessage));
    checkRecievedMessage(incomingMessage);
  }
}
//////////////////////

//////////////////////

void checkRecievedMessage(int value){
  //Right hand
  if(value >= 1000 && value <= 1500){
    MIDImessage(176,settingArray[presetSetting][2],value-1000);
  }
  if(value >= 2000 && value <= 2500){
    MIDImessage(176,settingArray[presetSetting][3],value-2000);
  }
}

//////////////////////

//////////////////////
void calcValues(){
  readMenuSettings();
  for(int i = 0; i < 2; i++){
    readPotentiometers(i);
  }
  
  if(IR_val[0] != 0){
    if(IR_val[0] != IR_Pval[0]){
      //MIDImessage(128,IR_Pval[0], 127);
      //MIDImessage(144,IR_val[0],127);
      MIDImessage(176,settingArray[presetSetting][0],IR_val[0]);
    }
  }
  IR_Pval[0] = IR_val[0];
  potPState[0] = potCState[0];
  
  if(IR_val[1] != 0){
    if(IR_val[1] != IR_Pval[1]){
      //MIDImessage(128,IR_Pval[1],127);
      MIDImessage(176,settingArray[presetSetting][1],IR_val[1]);
    }
  }
  IR_Pval[1] = IR_val[1];
  potPState[1] = potCState[1];
  delay(10);
}
//////////////////////

//////////////////////
void readMenuSettings(){
  int sensorValue = analogRead(A2);
  sensorValue = map(sensorValue, 0, 1027, 6, -1);
  if(sensorValue != pTurnPot){
    pTurnPot = sensorValue;
    menuSetting(pTurnPot);
    //Serial.println(pTurnPot);
  }
  int button = digitalRead(BUTTON_PIN);
  if(button != pButton){
    if(button == 0){
      timer = 0;
      //Serial.println("pressed");
      changeSetting();
    }
    pButton = button;
  }
  if(pButton == 0){
    timer++;
    if(timer >= 50){
      timer = timer - 2;
      changeSetting();
    }
  }
}
//////////////////////

//////////////////////
void changeSetting(){
  if(pTurnPot > 5){
    //Serial.println("pressed");
    presetSetting++;
    if(presetSetting > 5){
      presetSetting = 0;
    }
    menuSetting(pTurnPot);
  }
  if(pTurnPot <= 5){
    settingArray[presetSetting][pTurnPot]++;
    if(settingArray[presetSetting][pTurnPot] >= 65){
      settingArray[presetSetting][pTurnPot] = 0;
    }
    menuSetting(pTurnPot);
  }
}
//////////////////////

//////////////////////
void MIDImessage(byte command, byte data1, byte data2) //pass values out through standard Midi Command
{
   Serial.write(command);
   Serial.write(data1);
   Serial.write(data2);
}
//////////////////////

//////////////////////
float fscale( float originalMin, float originalMax, float newBegin, float newEnd, float inputValue, float curve) {
  float OriginalRange = 0;
  float NewRange = 0;
  float zeroRefCurVal = 0;
  float normalizedCurVal = 0;
  float rangedValue = 0;
  boolean invFlag = 0;
  
  if (curve > 10) curve = 10;
  if (curve < -10) curve = -10;
  curve = (curve * -.1);
  curve = pow(10, curve);

  if (inputValue < originalMin) {
    inputValue = originalMin;
  }
  if (inputValue > originalMax) {
    inputValue = originalMax;
  }
  
  OriginalRange = originalMax - originalMin;
  
  if (newEnd > newBegin) {
    NewRange = newEnd - newBegin;
  }
  else
  {
    NewRange = newBegin - newEnd;
    invFlag = 1;
  }
  zeroRefCurVal = inputValue - originalMin;
  normalizedCurVal  =  zeroRefCurVal / OriginalRange;
  if (originalMin > originalMax ) {
    return 0;
  }
  if (invFlag == 0) {
    rangedValue =  (pow(normalizedCurVal, curve) * NewRange) + newBegin;

  }
  else     // invert the ranges
  {
    rangedValue =  newBegin - (pow(normalizedCurVal, curve) * NewRange);
  }
  return rangedValue;
}
//////////////////////

//////////////////////
void readPotentiometers(int i) {
  int reading[2] = {0};
  int filteredVal[2] = {0};
  int scaledVal[2] = {0};

  int IR_range[2] = {90, 530};
  int IR_min_val[2] = {0,0};
  int IR_max_val[2] = {127,127};
  
  reading[0] = analogRead(A0);
  reading[1] = analogRead(A1);// raw reading
  ave[i].push(reading[i]); // adds value to average pool
  
  filteredVal[i] = ave[i].mean();
  potCState[i] = filteredVal[i];
  
  scaledVal[i] = fscale(IR_range[0], IR_range[1], IR_min_val[i], IR_max_val[i], filteredVal[i], 1);
  byte temp_val = clipValue(scaledVal[i], IR_min_val[i], IR_max_val[i]);
  if(i == 1){
    IR_val[1] = temp_val;
  }
  if(i == 0){
    IR_val[0] = temp_val;//map(temp_val, 0, 127, 48, 59+12);
  }
}
//////////////////////

//////////////////////
int clipValue(int in, int minVal, int maxVal) {
  int out;
  if (in > maxVal) {
    out = maxVal;
  }
  else if (in < minVal) {
    out = minVal;
  }
  else {
    out = in;
  }
  return out;
}
//////////////////////

//////////////////////
void menuSetting(byte userPosition){
  display.clearDisplay();
  display.setRotation(2);
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(2,2);
  display.println("C1");
    display.setCursor(30,2);
    display.println(settingArray[presetSetting][0]);
  display.setCursor(2,16);
  display.println("C2");
    display.setCursor(30,16);
    display.println(settingArray[presetSetting][1]);
  display.setCursor(2,30);
  display.println("C3");
    display.setCursor(30,30);
    display.println(settingArray[presetSetting][2]);
  display.setCursor(50,2);
  display.println("C4");
    display.setCursor(78,2);
    display.println(settingArray[presetSetting][3]);
  display.setCursor(50,16);
  display.println("C5");
    display.setCursor(78,16);
    display.println(settingArray[presetSetting][4]);
  display.setCursor(50,30);
  display.println("C6");
    display.setCursor(78,30);
    display.println(settingArray[presetSetting][5]);
  for(int i = 0; i < 6; i++){
    display.drawRoundRect(2+i*21,43,19,19,4,1);
    display.fillRoundRect(2+presetSetting*21,43,19,19,4,1);
  }
  if(userPosition != 6){
    display.drawRect(cursorSetting[userPosition],cursorSetting[userPosition+6],44,11,1); 
  }
  if(userPosition == 6){
    display.drawRect(0,41,128,23,1);
  }
  //Serial.println(cursorSetting[userPosition]);
  //Serial.println(cursorSetting[userPosition*2]);
  display.display();
}
//////////////////////