#define joyX A4
#define joyY A5

const int redPin = 1; // Declare the pin number for thr RBG LED
const int greenPin = 2;
const int bluePin = 3;

int delayVal = 1000;


// Insert the colors values in the map
const byte BLACK = 0b000;
const byte RED = 0b100;
const byte GREEN = 0b010;
const byte BLUE = 0b001;
const byte MAGENTA = 0b101;
const byte CYAN = 0b011;
const byte YELLOW = 0b110;
const byte WHITE = 0b111;

// Input variable 
String command;
float xValue;
float yValue;


// the position of the RGB colors
const int redX = 511;
const int redY = 1023;
const int greenX = 1023;
const int greenY = 0;
const int blueX = 0;
const int blueY = 0;

// Store the current RGB brightness
int brightRed;
int brightGreen;
int brightBlue;


// the setup function runs once when you press reset or power the board
void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(redPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  pinMode(bluePin, OUTPUT);
  Serial.begin(9600);
  delay(1000);
  Serial.println("Type the color");
  analogWriteResolution(10); // Set analog out resolution to max, 10-bits 

}

// the loop function runs over and over again forever
void loop() {
  //get the analog value from the pins
  xValue = analogRead(joyX);
  yValue = analogRead(joyY);
 
  //print the values with to plot or view
  Serial.print(xValue);
  Serial.print("\t");
  Serial.println(yValue);
  // set the current RGB from the position of the joystick
  setColorsFromAxis(xValue,yValue);
  // display the current color
  displayColor(brightRed, brightGreen, brightBlue);

  delay(250);
}


void displayColor(int r, int g, int b) {
  // Set the appropriate bit from the byte
  analogWrite(redPin, r);
  analogWrite(greenPin, g);
  analogWrite(bluePin, b);
}

// function that return the right color byte
void commandToColorByte(String command) {

}

void setColorsFromAxis(float xValue, float yValue) {
  // Flip orientation the X axe
  xValue = map(xValue, 0, 1023, 1023, 0);
  // Do the Pythagore geometry to get the distance between the color and the position of the joystick.
  int distanceRed = sqrt(pow(abs(redX - xValue), 2) +  pow(abs(redY - yValue), 2));
  int distanceGreen = sqrt(pow(abs(greenX - xValue), 2) +  pow(abs(greenY - yValue), 2));
  int distanceBlue = sqrt(pow(abs(blueX - xValue), 2) +  pow(abs(blueY - yValue), 2));

  // remap the distance on a 8 bit scale (0-255) and set the brighness of the colors
  brightRed = 255 - constrain(map(distanceRed, 0, 1023, 0, 255), 0, 255);
  brightGreen = 255 - constrain(map(distanceGreen, 0, 1023, 0, 255), 0, 255);
  brightBlue = 255 - constrain(map(distanceBlue, 0, 1023, 0, 255), 0, 255);
}