//Program: test RGB Led to Spectrophotometry
//Author: João Pedro da Silva

//libs
#include "math.h"

//constants
const int deltaLambda = 5; //nm
const int initLambda = 380; //nm
const int finLambda = 780; //nm
const int ledBluePin = 9;
const int ledGreenPin = 10;
const int ledRedPin = 11;

//global variables
int lambda = initLambda;
String input;
int rgb[3] = {0,0,0}; //{red, green, blue}

//functions
void ledRGBControl(int redValue, int greenValue, int blueValue) {
  analogWrite(ledRedPin, redValue);
  analogWrite(ledGreenPin, greenValue);
  analogWrite(ledBluePin, blueValue);
}

//code adapted from https://academo.org/demos/wavelength-to-colour-relationship/
void lambdaToRgb(int wavelength) {
  float gamma = 0.80;
  int intensityMax = 255;
  float factor, red, blue, green;
  if((wavelength >= 380) && (wavelength < 440)){
    red = (- 1.0) * (wavelength - 440) / (440 - 380);
    green = 0.0;
    blue = 1.0;
  }else if((wavelength >= 440) && (wavelength < 490)){
    red = 0.0;
    green = 1.0 * (wavelength - 440) / (490 - 440);
    blue = 1.0;
  }else if((wavelength >= 490) && (wavelength < 510)){
    red = 0.0;
    green = 1.0;
    blue = (- 1.0) * (wavelength - 510) / (510 - 490);
  }else if((wavelength >= 510) && (wavelength < 580)){
    red = 1.0 * (wavelength - 510) / (580 - 510);
    green = 1.0;
    blue = 0.0;
  }else if((wavelength >= 580) && (wavelength < 645)){
    red = 1.0;
    green = (-1.0) * (wavelength - 645) / (645 - 580);
    blue = 0.0;
  }else if((wavelength >= 645) && (wavelength < 781)){
    red = 1.0;
    green = 0.0;
    blue = 0.0;
  }else{
    red = 0.0;
    green = 0.0;
    blue = 0.0;
  }
  // Let the intensity fall off near the vision limits
  if((wavelength >= 380) && (wavelength < 420)){
    factor = 0.3 + 0.7*(wavelength - 380) / (420 - 380);
  }else if((wavelength >= 420) && (wavelength < 701)){
    factor = 1.0;
  }else if((wavelength >= 701) && (wavelength < 781)){
    factor = 0.3 + 0.7*(780 - wavelength) / (780 - 700);
  }else{
    factor = 0.0;
  }
  if (red != 0){
    red = round(intensityMax * pow(red * factor, gamma));
  }
  if (green != 0){
    green = round(intensityMax * pow(green * factor, gamma));
  }
  if (blue != 0){
    blue = round(intensityMax * pow(blue * factor, gamma));
  }
  rgb[0] = (int) red;
  rgb[1] = (int) green;
  rgb[2] = (int) blue;
}

void setup() 
{
  Serial.begin(9600);
  pinMode(ledRedPin, OUTPUT); //led rgb red
  pinMode(ledGreenPin, OUTPUT); //led rgb green
  pinMode(ledBluePin, OUTPUT); //led rgb blue
  lambdaToRgb(initLambda);
  Serial.println("lambda = " + String(initLambda)  + " nm");
  ledRGBControl(rgb[0], rgb[1], rgb[2]);
}

void loop() 
{
  if (Serial.available()) {
    input = Serial.readStringUntil('\n');
    if(input.toInt()) {
      lambda = input.toInt();
      if(lambda >= initLambda && lambda <= finLambda) {
        lambdaToRgb(lambda);
        Serial.println("lambda = " + input  + " nm");
      } else {
        Serial.println("Only allowed values between " + String(initLambda) + " and " + String(finLambda) + " nm!");
      }
    } else {
      Serial.println("Invalid value!");
    }
  }
  ledRGBControl(rgb[0], rgb[1], rgb[2]);
  delay(500);
}
nano:12
nano:11
nano:10
nano:9
nano:8
nano:7
nano:6
nano:5
nano:4
nano:3
nano:2
nano:GND.2
nano:RESET.2
nano:0
nano:1
nano:13
nano:3.3V
nano:AREF
nano:A0
nano:A1
nano:A2
nano:A3
nano:A4
nano:A5
nano:A6
nano:A7
nano:5V
nano:RESET
nano:GND.1
nano:VIN
nano:12.2
nano:5V.2
nano:13.2
nano:11.2
nano:RESET.3
nano:GND.3
rgb1:R
rgb1:COM
rgb1:G
rgb1:B
r1:1
r1:2
r2:1
r2:2
r3:1
r3:2