#include <RCA.h>
#include <solenoids.h>
#include <LCDWIKI_GUI.h>
#include <LCDWIKI_SPI.h>

RCA modone;

solenoids P1U1(6, 22, 23, 24);
solenoids P1L1(5, 25, 26, 27);
solenoids P2U1(4, 28, 29, 30);
solenoids P2L1(2, 31, 32, 33);

#define MODEL ILI9488_18
#define CS   A0
#define CD   A2
#define RST  A1
#define LED  A3

LCDWIKI_SPI lcd(MODEL, CS, CD, RST, LED); //model,cs,dc,reset,led

//define some colour values
#define BLACK   0x0000
#define BLUE    0x001F
#define RED     0xF800
#define GREEN   0x07E0
#define CYAN    0x07FF
#define MAGENTA 0xF81F
#define YELLOW  0xFFE0
#define WHITE   0xFFFF

char s   [80];
char buf [80];
byte one, two;
bool pause;
bool abt;
bool res;
unsigned long msec;
//unsigned long pausestart = 0,pauselst = 0;
unsigned long pas;
enum { Off = HIGH, On = LOW };

#define MaxTok  20
char *toks [MaxTok];
int   vals [MaxTok];

#define NcmdQ 20
struct Q {
  char  cmdQ [NcmdQ][30];
  int   head;
  int   tail;
  int   cnt;
};
struct Pump {
  byte          pul, stp, dir;
  const char   *label;
  bool          active;
  Q             q;
  int           pwm;
  int           solenoid0;
  int           solenoid1;
  unsigned long msecLst;
  unsigned long period;
};
Pump pumps [] = {
  { 3, "pump-0" },
  { 7, 8, 9, "pump-1" },
  { 10, 11, 12, "pump-2" },
};
#define Npump   (sizeof(pumps)/sizeof(Pump))

void setup () {
  Serial.begin (115200);
  lcd.Init_LCD();
  lcdbggnd();
  modone.PinSetup();
  //    Serial.println ("ready");
  P1U1.Init();
  P1L1.Init();
  P2U1.Init();
  P2L1.Init();
  P1U1.disable();
  P1L1.disable();
  P2U1.disable();
  P2L1.disable();
}
//LCD
void lcdbggnd() {
  //BACKGROUND
  //  lcd.Fill_Screen(178,34,34);
  lcd.Set_Rotation(2);

  lcd.Fill_Screen(BLACK);
  //CENTER LINE
  lcd.Set_Text_Mode(1);
  lcd.Set_Text_colour(124, 252, 0);
  lcd.Print_String("------------------------------------------------------", 0, 239);
  lcd.Print_String("------------------------------------------------------", 0, 240);
  lcd.Print_String("------------------------------------------------------", 0, 241);
  //rectangle
  lcd.Set_Draw_color(124, 252, 0); //light green
  lcd.Draw_Round_Rectangle(9, 279, 41, 441, 7);
  lcd.Draw_Round_Rectangle(8, 278, 42, 442, 7);
  lcd.Draw_Round_Rectangle(9, 201, 41, 39, 7);
  lcd.Draw_Round_Rectangle(8, 200, 42, 40, 7);
  //lcd.Set_Draw_color(221,160,221);  //light pink
  lcd.Set_Draw_color(70, 130, 180); // dark cyan
  //  lcd.Set_Draw_color(0,206,209);  //cyan box color
  lcd.Fill_Round_Rectangle(10, 280, 40, 440, 5);
  lcd.Fill_Round_Rectangle(10, 199, 40, 41, 5);
  lcd.Set_Text_Mode(1);
  lcd.Set_Rotation(1);
  lcd.Set_Text_Size(3);
  lcd.Set_Text_colour(0, 0, 0);
  lcd.Print_String("LINE-1", 312, 15);
  lcd.Print_String("LINE-2", 68, 15);
  //Data line 1/2
  lcd.Set_Text_colour(CYAN);
  lcd.Set_Text_Mode(1);
  lcd.Set_Rotation(1);
  lcd.Set_Text_Size(2);
  lcd.Print_String("SPEED  :", 5, 80);
  lcd.Print_String("REAGENT:", 5, 120);
  lcd.Print_String("BEAKER :", 5, 160);
  lcd.Print_String("SPEED  :", 6, 80);
  lcd.Print_String("REAGENT:", 6, 120);
  lcd.Print_String("BEAKER :", 6, 160);
  lcd.Print_String("ml/min", 160, 80);
  lcd.Print_String("SPEED  :", 245, 80);
  lcd.Print_String("REAGENT:", 245, 120);
  lcd.Print_String("BEAKER :", 245, 160);
  lcd.Print_String("SPEED  :", 246, 80);
  lcd.Print_String("REAGENT:", 246, 120);
  lcd.Print_String("BEAKER :", 246, 160);
  lcd.Print_String("ml/min", 400, 80);
  //Time Box line 2
  lcd.Set_Draw_color(124, 252, 0); //light green
  lcd.Draw_Round_Rectangle(10, 200, 220, 300, 10);
  lcd.Draw_Round_Rectangle(11, 201, 221, 301, 10);
  lcd.Set_Draw_color(70, 130, 180); // dark cyan
  lcd.Fill_Round_Rectangle(12, 202, 219, 299, 10);
  lcd.Set_Text_Mode(0);
  lcd.Set_Text_Size(2);
  lcd.Set_Text_colour(0, 0, 0);
  lcd.Print_String("TIMER", 80, 210);
  //Time Box line 1
  lcd.Set_Draw_color(124, 252, 0); //light green
  lcd.Draw_Round_Rectangle(255, 200, 460, 300, 10);
  lcd.Draw_Round_Rectangle(256, 201, 461, 301, 10);
  lcd.Set_Draw_color(70, 130, 180); // dark cyan
  lcd.Fill_Round_Rectangle(257, 202, 459, 299, 10);
  lcd.Set_Text_Mode(0);
  lcd.Set_Text_Size(2);
  lcd.Set_Text_colour(0, 0, 0);
  lcd.Print_String("TIMER", 320, 210);
}
void line1cls() {
  lcd.Set_Draw_color(BLACK);
  lcd.Fill_Rectangle(340, 70, 390, 180);
  lcd.Set_Draw_color(70, 130, 180);
  lcd.Fill_Rectangle(310, 250, 400, 290);
}
void line2cls() {
  lcd.Set_Draw_color(BLACK);
  lcd.Fill_Rectangle(100, 70, 150, 180);
  lcd.Set_Draw_color(70, 130, 180);
  lcd.Fill_Rectangle(70, 250, 200, 280);
}
void line1print(int p0, int sol00, int sol11, unsigned long ttim) {
  lcd.Set_Draw_color(BLACK);
//  lcd.Fill_Rectangle(360, 70, 390, 180);
  lcd.Fill_Rectangle(340, 70, 390, 180);
  lcd.Set_Text_colour(CYAN);
  lcd.Set_Text_Mode(1);
  lcd.Set_Rotation(1);
  lcd.Set_Text_Size(2);
  lcd.Set_Text_Back_colour(BLACK);
  lcd.Print_Number_Int(p0, 350, 80, 2, 1, 10);
  lcd.Print_Number_Int(sol00, 350, 120, 2, 0, 10);
  lcd.Print_Number_Int(sol11, 350, 160, 2, 1, 10);
  lcd.Set_Text_colour(BLACK);
  lcd.Set_Text_Mode(1);
  lcd.Set_Rotation(1);
  lcd.Set_Text_Size(3);
  lcd.Set_Text_Back_colour(CYAN);
  lcd.Set_Draw_color(70, 130, 180);
  lcd.Fill_Rectangle(310, 250, 400, 290);
  lcd.Print_Number_Int(ttim, 310, 250, 2, 1, 10);
}
void line2print(int p, int sol0, int sol1, unsigned long tim) {

  lcd.Set_Draw_color(BLACK);
  lcd.Fill_Rectangle(100, 70, 150, 180);

  lcd.Set_Text_colour(CYAN);
  lcd.Set_Text_Mode(1);
  lcd.Set_Rotation(1);
  lcd.Set_Text_Size(2);
  lcd.Set_Text_Back_colour(BLACK);
  lcd.Print_Number_Int(p, 110, 80, 2, 1, 10);
  lcd.Print_Number_Int(sol0, 110, 120, 2, 0, 10);
  lcd.Print_Number_Int(sol1, 110, 160, 2, 1, 10);
  lcd.Set_Text_colour(BLACK);
  lcd.Set_Text_Mode(1);
  lcd.Set_Rotation(1);
  lcd.Set_Text_Size(3);
  lcd.Set_Text_Back_colour(CYAN);
  lcd.Set_Draw_color(70, 130, 180);
  lcd.Fill_Rectangle(70, 250, 200, 280);
  lcd.Print_Number_Int(tim, 70, 250, 2, 1, 10);
}

//--------------------------------------------------
int tokenize (char *s, const char *sep ) {
  unsigned n = 0;
  toks [n] = strtok (s, sep);
  vals [n] = atoi (toks [n]);

  for (n = 1; (toks [n] = strtok (NULL, sep)); n++)
    vals [n] = atoi (toks [n]);

  return n;
}
void queue (const char *buf, struct Q   *q, const char *label ) {
  sprintf (s, "queue: %s - %s", label, buf);
  Serial.println (s);

  if (NcmdQ <= q->cnt)  {
    sprintf (s, "queue: full");
    Serial.println (s);
  }

  memcpy (q->cmdQ [q->head++], buf, strlen(buf));
  if (NcmdQ <= q->head)
    q->head = 0;
  q->cnt++;
};
char *dequeue (Q *q) {
  if (0 == q->cnt)
    return NULL;

  char *buf = q->cmdQ [q->tail++];
  if (NcmdQ <= q->tail)
    q->tail = 0;
  q->cnt--;

  sprintf (s, " dequeue: %s", buf);
  Serial.println (s);

  return buf;
}
void Pumprun (char *buf ) {
  if (4 > tokenize (buf, ","))  {
    sprintf (s, "Error: wrong # of values - %s", s);
    Serial.println (s);
    return;
  }
  unsigned idx = vals [0];

  Pump *p = & pumps [idx];

  p->pwm       = vals [1];
  p->solenoid0 = vals [2];
  p->solenoid1 = vals [3];

  p->period    = vals [4] * 1000L;
  p->msecLst   = msec;
  p->active    = true;
  //    pause        = false;
  abt          = false;

  if (idx == 1) {
    one = true;
    P1U1.energize(p->solenoid0);
    P1L1.energize(p->solenoid1);
    line1print(p->pwm, p->solenoid0, p->solenoid1, p->period);
  }

  else if (idx == 2) {
    two = true;
    P2U1.energize(p->solenoid0);
    P2L1.energize(p->solenoid1);
    line2print(p->pwm, p->solenoid0, p->solenoid1, p->period);
  }

  analogWrite (p->pul, p->pwm);
  digitalWrite(p->stp, On);
  digitalWrite(p->dir, On);
//  modone.sgnled_on();
  sprintf (s, "  pumpRun: %s On (%lu ms)", p->label, p->period);
  Serial.println (s);
}
void ledoff() {
  if (one != true) {
    if (two != true) {
//      modone.sgnled_off();
    }
  }
}
void Pumpmonitor () {
  Pump *p = pumps;
  for (unsigned n = 0; n < Npump; n++, p++)  {
    if ((p->active && (msec - p->msecLst) >= p->period) || (p->active && abt == true))  {
      analogWrite (p->pul, 0);
      digitalWrite(p->stp, Off);
      digitalWrite(p->dir, Off);
      if (n == 1) {
        P1U1.disable();
        P1L1.disable();
        line1cls();
        one = false;
      }

      else if (n == 2) {
        P2U1.disable();
        P2L1.disable();
        line2cls();
        two = false;
      }
      p->active    = false;

      sprintf (s, "   pumpRun: %s Off", p->label);
      Serial.println (s);
      char *buf = dequeue (& p->q);
      if (NULL != buf) {
        Pumprun (buf);
      }
      else  {
        ledoff();
        sprintf (s, "   pumpRun: %s is done", p->label);
        Serial.println (s);
      }
    }
  }
}
void pumpabort(struct Q *q) {
  if (abt == false) {
//    modone.sgnled_off();
    Pump *p = pumps;
    for (unsigned n = 0; n < Npump; n++, p++)  {
      for (int i = 0; i <= 30; i++) {
        char *buf = dequeue (& p->q);
      }
    }
    abt = true;
    Serial.println("Abort");
  }
}
void pausefun() {
  pause = true;
  if (pause == true) {
    pas = msec;
    analogWrite (9, 0);
    digitalWrite(10, Off);
    digitalWrite(11, Off);
    analogWrite (8, 0);
    digitalWrite(12, Off);
    digitalWrite(13, Off);
    P1U1.disable();
    P1L1.disable();
    P2U1.disable();
    P2L1.disable();
    res = false;
  }
}
void resumefun() {
  res = true;
}
void Pumpstart () {
  Pump *p = pumps;
  for (unsigned n = 0; n < Npump; n++, p++)  {
    if (! p->active)  {
      char *buf = dequeue (& p->q);
      if (NULL != buf)
        Pumprun (buf);
    }
    else {
      sprintf (s, "%s: pump %d active", __func__, n);
      Serial.println (s);
    }
  }
}

//---------------------------------------------------
void loop () {
  msec = millis ();
  Pumpmonitor ();

  if (Serial.available () > 0)  {
    int n = Serial.readBytesUntil ('\n', buf, sizeof(buf) - 1);
    buf [n] = '\0';
    unsigned idx = atoi (buf);
    switch (idx) {
      case 0:
        if (Serial.availableForWrite()) {
          Serial.println("MODULE IS READY.");
        }
        else {
          Serial.println("MODULE IS BUSY");
        }
        break;
      case 3:
        Serial.println("Started");
        Pumpstart ();
        break;
      case 4:
        pumpabort(& pumps [idx].q);
        break;
      case 5:
        pausefun();
        break;
      case 6:
        resumefun();
        break;
      default:
        queue (buf, & pumps [idx].q, pumps [idx].label);
        break;
    }
  }
}