#include <FastLED.h>
#include "FastLED_RGBW.h"

#define PIN_STRIP1 8
#define NUM_LEDS 4
CRGBW strip1[NUM_LEDS];  // FastLED with RGBW
CRGB *strip1B = (CRGB *) &strip1[0];

#define PUL_P 4
#define PUL_M 3
#define DIR_P 12
#define DIR_M 7
#define ENA_P 6
#define ENA_M 5
#define frameready_sensor 2
#define frameready_led 13

#define NUM_COUNTS 3

long delay_Micros = 20; // Set value
long currentMicros = 0; long previousMicros = 0;

int frame_ready = 1; //frame sensor variable
int transport_ready = 1; //projector transport switch variable
int REDval = 0;
int GREENval = 0;
int BLUEval = 0;
int REDval_ant = 255;
int GREENval_ant = 255;
int BLUEval_ant = 255;
int motor_run;
int motor_dir;
int mydata[7]; // serial input buffer
int lampMode = 1; // Set the lamp mode (0 = blink once per frame, 1 = constantly on)

void blink() {
  frame_ready = !digitalRead(frameready_sensor);
}

void setup()
{
  Serial.begin(250000);
  FastLED.addLeds<WS2812B, PIN_STRIP1, RGB>(strip1B, getRGBWsize(NUM_LEDS));
  pinMode(PUL_P, OUTPUT);
  pinMode(PUL_M, OUTPUT);
  pinMode(DIR_P, OUTPUT);
  pinMode(DIR_M, OUTPUT);
  pinMode(ENA_P, OUTPUT);
  pinMode(ENA_M, OUTPUT);
  pinMode(frameready_led, OUTPUT);
  pinMode(frameready_sensor, INPUT);
  digitalWrite(frameready_led, LOW);
  digitalWrite(PUL_P, HIGH);
  digitalWrite(PUL_M, LOW);
  digitalWrite(DIR_P, LOW);
  digitalWrite(DIR_M, LOW);
  digitalWrite(ENA_P, HIGH);
  digitalWrite(ENA_M, LOW);
  for(int i=0; i<NUM_LEDS; i++) strip1[i] = CRGBW(255, 0, 0, 0);
  FastLED.show();
  attachInterrupt(digitalPinToInterrupt(frameready_sensor), blink, CHANGE);
  while(!Serial);
}

int vel = 1;

void motor_pot()
{
  vel = analogRead(A0);
  vel = vel - 512;
  if (vel > 0)
  {
    digitalWrite(DIR_P, HIGH);
    vel = 1023 - abs(vel) * 2;
  }
  else
  {
    digitalWrite(DIR_P, LOW);
    vel = 1023 - abs(vel) * 2;
  }
  if (vel < 1)
  {
    vel = 0;
    delay_Micros = 1;
  }
  else
  {
    delay_Micros = 2 * vel;
  }
  currentMicros = micros();
  if (currentMicros - previousMicros >= delay_Micros)
  {
    previousMicros = currentMicros;
    digitalWrite(PUL_P, HIGH);
    if (vel > 0)delayMicroseconds(vel); //Set Value
    digitalWrite(PUL_P, LOW);
  }
}

int frame_ready_ant = 0;
int count = 0;
bool stop_count = 0;
int white = 255;

void loop()
{
  // check frame_ready sensor and light the LED if a frame is ready to be photographed
  //frame_ready = !digitalRead(frameready_sensor);

  if(frame_ready!=frame_ready_ant)
    {
      //transport_ready = !(digitalRead(transportready_switch)); // (logic high if transport switch goes low)
      digitalWrite(frameready_led, frame_ready); //light the frame-ready LED if frame is ready
      if(frame_ready) count++;
      Serial.print("frame_ready ");
      if((count==NUM_COUNTS)&&(frame_ready))
        {
          Serial.print(1, DEC);
          if (!lampMode)
            { // if lampmode is blink then check for frame_ready and update the lamp settings if frame is ready
              for(int i=0; i<NUM_LEDS; i++) strip1[i] = CRGBW(mydata[0], mydata[1], mydata[2], white);
              FastLED.show();
            }
          stop_count = 1;
          count = 0;
        }
      else
        {
          Serial.print(0, DEC);
           if (!lampMode)
            { // if lampmode is blink then check for frame_ready and update the lamp settings if frame is ready
              if(mydata[6]!=0) for(int i=0; i<NUM_LEDS; i++) strip1[i] = CRGBW(0, 0, 0, 0);
              FastLED.show();
            }
        }
      Serial.println();
      Serial.print("transport_ready ");
      Serial.print(transport_ready, DEC);
      Serial.println();
      frame_ready_ant = frame_ready;
      if((stop_count)&&(mydata[6]==1))
        {
          digitalWrite(ENA_P, HIGH); // emergency stop if transport is disengaged
          delay(200);
          digitalWrite(ENA_P, LOW); // emergency stop if transport is disengaged
        }
      stop_count = 0;
    }
  
  while(Serial.available()>=7)
    {
      //wait to make sure we have all the data - probably a bad idea
      //delay(5);
      //serial data is waiting, lets extract
      int len = Serial.available();
      int i = 0;
      while (i < len) { // There are 7 values arriving via serial, so serial message should be this long
        if (len==7) mydata[i] = Serial.read(); // "mydata" is an array with the serial info
        else Serial.read(); // "mydata" is an array with the serial info
        i++;
      }
      //Never let motor run if transport isn't engaged!
      if (transport_ready) {
        digitalWrite(ENA_P, (!mydata[3])); // Turn motor ON if serial says so
      }
      else {
        digitalWrite(ENA_P, HIGH); // emergency stop if transport is disengaged
      }
      digitalWrite(DIR_P, (mydata[4])); // Set motor direction according to serial
      lampMode = (mydata[5]); // Set the lamp mode according to serial (0 = blink once per frame, 1 = constantly on)
      REDval = mydata[0];
      GREENval = mydata[1];
      BLUEval = mydata[2];
      if((REDval=REDval_ant)||(GREENval!=GREENval_ant)||(BLUEval!=BLUEval_ant))
        {
          white = (mydata[0] + mydata[1] + mydata[2])/3;
          for(int i=0; i<NUM_LEDS; i++) strip1[i] = CRGBW(mydata[0], mydata[1], mydata[2], white);
          FastLED.show();
          REDval_ant = REDval;
          GREENval_ant = GREENval;
          BLUEval_ant = BLUEval;
        }
    }

  if(mydata[6]==1)
    {
      digitalWrite(PUL_P, HIGH); // Turn motor ON if serial says so
      delayMicroseconds(300); //Set Value
      digitalWrite(PUL_P, LOW); // Turn motor ON if serial says so
      delayMicroseconds(300); //Set Value
    }
  else if(mydata[6]==0)
    {
      digitalWrite(PUL_P, HIGH); // Turn motor ON if serial says so
      delayMicroseconds(20); //Set Value
      digitalWrite(PUL_P, LOW); // Turn motor ON if serial says so
      delayMicroseconds(20); //Set Value
    }
    
  if (lampMode == 1)
    { // if lampmode is constant then we update the lamp settings
         for(int i=0; i<NUM_LEDS; i++) strip1[i] = CRGBW(mydata[0], mydata[1], mydata[2], white);
        FastLED.show();
    }
//  else
//    { // if lampmode is blink then check for frame_ready and update the lamp settings if frame is ready
//      if (frame_ready == 1)
//        {
//          strip1[0] = CRGBW(mydata[0], mydata[1], mydata[2], 0);
//          FastLED.show();
//        }
//      else
//        {
//          strip1[0] = CRGBW(0, 0, 0, 0);
//          FastLED.show();
//        }
//    }
}