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

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

#define PUL_P 12
#define DIR_P 11
#define ENA_P 10
#define MODE   9
#define CAMERA 3  //PD1
#define RED   A0
#define GREEN A1
#define BLUE  A2
#define WHITE A3
#define SP1   A4
#define SP2   A5

#define frameready_sensor 2

#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 WHITEval = 255;
int SPEED1val = 0;
int SPEED2val = 0;
int REDval_ant = 255;
int GREENval_ant = 255;
int BLUEval_ant = 255;
int WHITEval_ant = 0;
int motor_run;
int motor_dir;
int mydata[20]; // serial input buffer
int mydata_temp[20]; // serial input buffer
int frame_ready_ant = frame_ready;
int count = 0;
bool stop_count = 0;
byte c;
byte c_inc = 0;
unsigned long time_frame_ready = millis();

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

void search_frame()
  {
    digitalWrite(ENA_P, LOW); // Turn motor ON if serial says so
    digitalWrite(DIR_P, HIGH);
    while((count<NUM_COUNTS))
      {
        if(frame_ready!=frame_ready_ant)
          {
            if(frame_ready) count++;
            frame_ready_ant = frame_ready;
          }
        else
          {
            digitalWrite(PUL_P, HIGH); // Turn motor ON if serial says so
            delayMicroseconds(100); //Set Value
            digitalWrite(PUL_P, LOW); // Turn motor ON if serial says so
            delayMicroseconds(100); //Set Value
          }
      }
    count = 0;
    digitalWrite(ENA_P, HIGH); // Turn motor ON if serial says so
  }

void setup()
{
  //Serial.begin(115200);
  Serial.begin(250000);
  FastLED.addLeds<WS2812B, PIN_STRIP1, RGB>(strip1B, getRGBWsize(NUM_LEDS));
  pinMode(PUL_P, OUTPUT);
  pinMode(DIR_P, OUTPUT);
  pinMode(ENA_P, OUTPUT);
  pinMode(CAMERA, OUTPUT);
  pinMode(MODE,  INPUT_PULLUP);
  pinMode(frameready_sensor, INPUT_PULLUP);
  digitalWrite(CAMERA, HIGH);
  digitalWrite(PUL_P, HIGH);
  digitalWrite(DIR_P, LOW);
  digitalWrite(ENA_P, HIGH);
  for(int i=0; i<NUM_LEDS; i++) strip1[i] = CRGBW(255, 0, 0, 0);
  FastLED.show();
  attachInterrupt(digitalPinToInterrupt(frameready_sensor), blink, CHANGE);
  search_frame();
  Serial.print("frame_ready ");
  Serial.print(1, DEC);
  Serial.println();
  Serial.print("transport_ready ");
  Serial.print(transport_ready, DEC);
  Serial.println();
}

unsigned time_speed = 100;
int vel_rew = 3;
bool VAL_PUL = true;

void loop()
{
  // check frame_ready sensor and light the LED if a frame is ready to be photographed
  //frame_ready = !digitalRead(frameready_sensor);
  if (digitalRead(MODE))
    {
      if(frame_ready!=frame_ready_ant)
        {
          if(frame_ready) count++;
          time_frame_ready = millis();
          if((count>=NUM_COUNTS)&&(frame_ready))
            {
              WHITEval = map(analogRead(WHITE), 0, 1023, 0, 255);
              if(WHITEval!=WHITEval_ant)
                {
                  for(int i=0; i<NUM_LEDS; i++) strip1[i] = CRGBW(REDval, GREENval, BLUEval, WHITEval);
                  WHITEval_ant = WHITEval;
                  FastLED.show();
                }
              Serial.print("frame_ready ");
              Serial.print(1, DEC);
              Serial.println();
              Serial.print("transport_ready ");
              Serial.print(transport_ready, DEC);
              Serial.println();
              if(mydata[6]==1)
                {
                  time_speed = map(analogRead(SP1), 0, 1023, 0, 250);
                  //digitalWrite(CAMERA, LOW);
                  PORTD &= B11110111; 
                  delay(map(analogRead(SP2), 0, 1023, 0, 250));
                  //digitalWrite(CAMERA, HIGH);
                  PORTD |= B00001000; 
                }
              else
                {
                  //digitalWrite(CAMERA, LOW);
                  PORTD &= B11110111; 
                  delayMicroseconds(100);
                  //digitalWrite(CAMERA, HIGH);
                  PORTD |= B00001000; 
                }
              count = 0;
            }
          else if((count==1)&&(frame_ready))
            {
              Serial.print("frame_ready ");
              Serial.print(0, DEC);
              Serial.println();
              Serial.print("transport_ready ");
              Serial.print(transport_ready, DEC);
              Serial.println();
            }
          frame_ready_ant = frame_ready;
        }
      
      if(Serial.available())
        {
          c = Serial.read();
          if ((c == '\r') || (c == '\n'))
            {
              if (c_inc==7)
                {
                  time_frame_ready = millis();
                  int check_diff = 0;
                  for(int i=0; i<7; i++) 
                    {
                      if(mydata[i] != mydata_temp[i]) check_diff++;
                    }
                  if (check_diff>0)
                    {
                      bool slow_ramp = false;
                      bool fast_ant = false;
                      if(mydata[3] != mydata_temp[3]) slow_ramp = true;
                      if(mydata[6] == 0) fast_ant = true;
                      for(int i=0; i<7; i++) mydata[i] = mydata_temp[i];
                      digitalWrite(DIR_P, (mydata[4])); // Set motor direction according to serial
                      REDval = mydata[0];
                      GREENval = mydata[1];
                      BLUEval = mydata[2];
                      if((REDval!=REDval_ant)||(GREENval!=GREENval_ant)||(BLUEval!=BLUEval_ant))
                        {
                          //WHITEval = (mydata[0] + mydata[1] + mydata[2])/3;
                          for(int i=0; i<NUM_LEDS; i++) strip1[i] = CRGBW(mydata[0], mydata[1], mydata[2], WHITEval);
                          FastLED.show();
                          REDval_ant = REDval;
                          GREENval_ant = GREENval;
                          BLUEval_ant = BLUEval;
                        }

                      if(fast_ant&&slow_ramp)
                        {
                          if(mydata[4]==1) vel_rew = 3;
                          else vel_rew = 10;
                          if(mydata[3]==1)
                            {
                              digitalWrite(ENA_P, (!mydata[3])); // Turn motor ON if serial says so
                              for(int i=200; i>=vel_rew; i--)
                                {
                                  PORTB |= B00010000; 
                                  delayMicroseconds(i); //Set Value
                                  PORTB &= B11101111; 
                                  delayMicroseconds(i); //Set Value
                                }
                            }
                          else if(mydata[3]==0)
                            {
                              for(int i=vel_rew; i<=500; i++)
                                {
                                  PORTB |= B00010000; 
                                  delayMicroseconds(i); //Set Value
                                  PORTB &= B11101111; 
                                  delayMicroseconds(i); //Set Value
                                }
                            }
                        }
                      digitalWrite(ENA_P, (!mydata[3])); // Turn motor ON if serial says so
                        
                    }
                }
              c_inc=0;
            }
          else
            {
              mydata_temp[c_inc] = c;
              c_inc++;
            }
        }
      
      if((millis()-time_frame_ready)>=1000)
        {
          Serial.print("frame_ready ");
          Serial.print(1, DEC);
          Serial.println();
          Serial.print("transport_ready ");
          Serial.print(transport_ready, DEC);
          Serial.println();
          time_frame_ready = millis();
        }
        
      if(mydata[3]==1)
        {
          if(mydata[6]==1)
            {
              PORTB |= B00010000; 
              delayMicroseconds(time_speed); //Set Value
              PORTB &= B11101111; 
              delayMicroseconds(time_speed); //Set Value
            }
          else if(mydata[6]==0)
            {
              PORTB |= B00010000; 
              delayMicroseconds(vel_rew); //Set Value
              PORTB &= B11101111; 
              delayMicroseconds(vel_rew); //Set Value
            }
        }
       else 
        {
          WHITEval = map(analogRead(WHITE), 0, 1023, 0, 255);
          if(WHITEval!=WHITEval_ant)
            {
              for(int i=0; i<NUM_LEDS; i++) strip1[i] = CRGBW(REDval, GREENval, BLUEval, WHITEval);
              WHITEval_ant = WHITEval;
              FastLED.show();
            }
        }
    }
  else
    {      
      digitalWrite(ENA_P, LOW);
      digitalWrite(DIR_P, HIGH);
      if(frame_ready!=frame_ready_ant)
        {
          if(frame_ready) count++;
          time_frame_ready = millis();
          if((count>=NUM_COUNTS)&&(frame_ready))
            {
              time_speed = map(analogRead(SP1), 0, 1023, 0, 250);
              REDval = map(analogRead(RED), 0, 1023, 0, 255);
              GREENval = map(analogRead(GREEN), 0, 1023, 0, 255);
              BLUEval = map(analogRead(BLUE), 0, 1023, 0, 255);
              WHITEval = map(analogRead(WHITE), 0, 1023, 0, 255);
              if((REDval!=REDval_ant)||(GREENval!=GREENval_ant)||(BLUEval!=BLUEval_ant)||(WHITEval!=WHITEval_ant))
                {
                  for(int i=0; i<NUM_LEDS; i++) strip1[i] = CRGBW(REDval, GREENval, BLUEval, WHITEval);
                  FastLED.show();
                  REDval_ant = REDval;
                  GREENval_ant = GREENval;
                  BLUEval_ant = BLUEval;
                  WHITEval_ant = WHITEval;
                }
              //digitalWrite(CAMERA, LOW);
              PORTD &= B11110111; 
              delay(map(analogRead(SP2), 0, 1023, 0, 250));
              //digitalWrite(CAMERA, HIGH);
              PORTD |= B00001000; 
              count = 0;
            }
          else if((count==1)&&(frame_ready))
            {
              Serial.print("frame_ready ");
              Serial.print(0, DEC);
              Serial.println();
              Serial.print("transport_ready ");
              Serial.print(transport_ready, DEC);
              Serial.println();
            }
          frame_ready_ant = frame_ready;
        }
        PORTB |= B00010000; 
        delayMicroseconds(time_speed); //Set Value
        PORTB &= B11101111; 
        delayMicroseconds(time_speed); //Set Value
    }
  
}