//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);
}