#include "Tlc5940.h"
#include <SoftwareSerial.h>
#include <EEPROM.h>

#define pinon A3
#define pinv A4
#define pinarmed A0
#define pinbeep A1
#define pingps A2

// BT
char buff_name[100];
char buff_value[100];
int pos = 0;
boolean colon = false;

// POSITIONS
#define FRONT_LEFT 0b0001
#define FRONT_RIGHT 0b0010
#define BACK_RIGHT 0b0100
#define BACK_LEFT 0b1000
#define BACK 0b1100
#define FRONT 0b0011
#define RIGHT 0b0110
#define LEFT 0b1001
#define QUAD 0b1111
#define RED 0b001
#define GREEN 0b010
#define BLUE 0b100
#define RGB 0b111

// LIMITS
#define scale 4096
#define full 4095
#define maxhue 24576
#define modes 7
#define serialdelay 30

// SYSTEM
int mode = 0;
int txvalue = full;
boolean on = true;
boolean sendcolors = true;
boolean alert = true;
boolean startup = true;
long now;

boolean state_gpsled;
long change_gpsled;
boolean state_armedled;
long change_armed;
long lastbeep = 0;

// COPTER
boolean disarmed = true;
boolean nogps = true;


// RADIO CONTROL
boolean btn = false;
boolean txconnected;
boolean lasttx = true;
boolean btcontrol = false;
boolean valuefromtx = true;
int lastvaluetx = 0;

// TIMING
long lastloop;
long loopcounter = 0;
int txcountdown = 0;
int txintervall = 100;
int savetimer = 1000;

// animation variables
int police_laststate = -1;
int police_laststate2 = -1;
int strobe_timer = 0;
long auto_hue = 0;

// -- CUSTOM --
char *alphabet = "abcdefghijklmnopqrstuvwxyz";
#define c_values 16
int c_value[c_values];
long c_timer = 0;
int c_breaktimer = 0;
int c_breakindex = 0;
int c_address;

void custom_setstandard() {
  c_value[0] = 0;
  c_value[1] = 0;
  c_value[2] = 0;
  c_value[3] = 0;
  c_value[4] = 0;
  c_value[5] = 0;
  c_value[6] = 0;
  c_value[7] = 100;
  c_value[8] = 100;
  c_value[9] = 100;
  c_value[10] = 100;
  c_value[11] = 100;
  c_value[12] = 0;
  c_value[13] = 0;
  c_value[14] = 0;
  c_value[15] = 0;
}

void custom_send() {
  for (int i = 0; i < c_values; i++) {
    Serial.print(alphabet[i]);
    Serial.print(":");
    Serial.print(String(c_value[i]));
    Serial.print("\n");
  }
}

void custom_save() { // ADDRESS 0 - 30  
  for (int i = 0; i < c_values;  i++) {
    EEPROM.write(8 + c_address * c_values * 2 + i * 2,c_value[i] / 256);
    EEPROM.write(8 + c_address * c_values * 2 + i * 2+1,c_value[i] % 256);
  }
  EEPROM.write(6,c_address);
  Serial.print("log:" + String(c_address) + " saved.\n");
}

boolean custom_load() {  
  for (int i = 0; i < c_values;  i++) {
    c_value[i] = EEPROM.read(8 + c_address * c_values * 2 + i * 2+1);
    if (c_value[i] == 255) return false;
    c_value[i] += EEPROM.read(8 + c_address * c_values * 2 + i * 2) * 256;
  }
  custom_send();
  Serial.print("log:Done loading " + String(c_address) + ".\n");
  return true;
}

int brightness(int v) {
   return pow(v / (float)full,3) * full;
}

void setrgb(int r, int g, int b, int mask) {
  long cl = (long)(r / 16) * (long)0x10000 + (long)(g / 16) * (long)0x100 + (b / 16);
  int r2 = brightness(r);
  int g2 = brightness(g);
  int b2 = brightness(b);
  
  if (mask & FRONT_LEFT) {
    Tlc.set(1,r2);
    Tlc.set(2,g2);
    Tlc.set(3,b2);
    if (sendcolors) Serial.print("FL:" + String (cl) + "\n");
  }
  if (mask & FRONT_RIGHT) {
    Tlc.set(4,r2);
    Tlc.set(5,g2);
    Tlc.set(6,b2);
    if (sendcolors) Serial.print("FR:" + String (cl) + "\n");
  }
  if (mask & BACK_RIGHT) {
    Tlc.set(7,r2);
    Tlc.set(8,g2);
    Tlc.set(9,b2);
    if (sendcolors) Serial.print("BR:" + String (cl) + "\n");
  }
  if (mask & BACK_LEFT) {
    Tlc.set(10,r2);
    Tlc.set(11,g2);
    Tlc.set(12,b2);
    if (sendcolors) Serial.print("BL:" + String (cl) + "\n");
  }
  Tlc.update();
}

void setcolor(int value, int color, int mask) {
  for (int p = 0; p < 4; p++) {
    for (int cl = 0; cl < 3; cl++)
       if (1 << p & mask && 1 << cl & color)
         Tlc.set(1 + 3 * p + cl,brightness(value));
    if (sendcolors && mask & 1 << p) {
      long cl = (long)(Tlc.get(p * 3 + 1) / 16) * (long)0x10000 + (long)(Tlc.get(p * 3 + 2) / 16) * (long)0x100 + (Tlc.get(p * 3 + 3) / 16);
      switch (p) {
        case 0: Serial.print("FL:"); break;
        case 1: Serial.print("FR:"); break;
        case 2: Serial.print("BR:"); break;
        case 3: Serial.print("BL:"); break;
      }
      Serial.print(cl); Serial.print("\n");
    }
  }
  Tlc.update();
}

void startupanimation() {
  setcolor(full,RED,QUAD);
  delay(1);
  setcolor(full,RED,QUAD);
  delay(144);
  setcolor(full,GREEN,QUAD);
  delay(144);
  setcolor(full,BLUE,QUAD);
  delay(144);
  setcolor(0,RGB,QUAD);  
  delay(1592);
  setcolor(full,RGB,QUAD);
  delay(489);
  setcolor(0,RGB,QUAD);
}
/*
void saveprefs() {
  if (true) return;
  EEPROM.write(0,mode);
  EEPROM.write(1,txvalue % 256);
  EEPROM.write(2,txvalue / 256);
  EEPROM.write(3,(sendcolors ? 1 : 0));
  EEPROM.write(4,(btcontrol ? 1 : 0));
  EEPROM.write(5,(alert ? 1 : 0)); 
  EEPROM.write(7,42);
}

void loadprefs() {
  if (EEPROM.read(7) != 42) return;
  mode = EEPROM.read(0);
  txvalue = EEPROM.read(1) + EEPROM.read(2) * 256;
  sendcolors = EEPROM.read(3) == 1;
  btcontrol = EEPROM.read(4) == 1;
  alert = EEPROM.read(5) == 1;
  
  // Send prefs
  Serial.print("value:" + String(txvalue) + "\n");
  delay(serialdelay);
  Serial.print("live:" + String(sendcolors) + "\n");
  delay(serialdelay);
  Serial.print("alert:" + String(alert) + "\n");
  delay(serialdelay);
  Serial.print("bt:" + String(btcontrol) + "\n");
  delay(serialdelay);
  Serial.print("animation:" + String(startup) + "\n");
}
*/
void setup()
{
  Serial.begin(115200);
  Tlc.init();  
  
  pinMode(pinv,INPUT);
  pinMode(pinon,INPUT);
  pinMode(pinarmed,INPUT);
  pinMode(pinbeep,INPUT);
  pinMode(pingps,INPUT);
  state_armedled = digitalRead(pinarmed);
  state_gpsled = digitalRead(pingps);
  now = 0;
  
  if (startup)
    startupanimation();
    
  c_address = 0;  
  custom_setstandard();
  
  sendsetup();
  
  setmode(mode);
  getinputs();
}

void arminganimation() {
  setcolor(0,RGB, QUAD);
  delay(10);
  for (int i = 0; i < 3; i++) {
    setcolor(full,(disarmed ? GREEN : RED | GREEN),QUAD);
    delay(200);
    setcolor(0,RGB, QUAD);
    delay(200);
  }
}

void getinputs() { // time: 33ms
  // Check leds
  if (digitalRead(pinarmed) != state_armedled) {
     if (!disarmed && now > 5000) {
       disarmed = true;
       arminganimation();
     }
     change_armed = now;
     state_armedled = !state_armedled;
  }
  else if (state_armedled && disarmed && now - change_armed > 600) {
    disarmed = false;
    arminganimation();
  }
  
  if (digitalRead(pingps) != state_gpsled) {
     if (!nogps) {
       nogps = true;
     }
     change_gpsled = now;
     state_gpsled = !state_gpsled;
  }
  else if (state_gpsled && nogps && now - change_gpsled > 600)
    nogps = false;
    
  if (now < 5000 && now - lastbeep > 5000 && digitalRead(pinbeep)) {
     disarmed = !disarmed;
     arminganimation(); 
  }

  int v_pinon = pulseIn(pinon,HIGH,30000);
  txconnected = v_pinon != 0;
  v_pinon -= 990;
  if (lasttx != txconnected) {
    //if (!btcontrol)
    setcolor(0,RGB,QUAD);
    lasttx = txconnected;
  }
  
  if (!txconnected) {
    btn = true;
    return;
  }
  on = v_pinon > 500;
  boolean newbtn = v_pinon > 900;
  if (newbtn && !btn) {
    if (mode == 6) {
      c_address++;
      if (c_address >= 5 || !custom_load()) setmode(0); 
      Serial.print("address:" + String(c_address) + "\n");
    }
    else {
      setmode(mode + 1);
      if (mode == 6) {
        c_address = 0;
        if (c_address >= 5 || !custom_load()) setmode(0);        
        Serial.print("address:" + String(c_address) + "\n");
      }
    }
  }
  btn = newbtn;
  
  int newvalue = (pulseIn(pinv,HIGH) - 990) * scale / 980;
  if (newvalue < 0) newvalue = 0;
  if (newvalue > full) newvalue = full;
  if (!valuefromtx)
    if (abs(lastvaluetx - newvalue) > 200)
      valuefromtx = true;
  if (valuefromtx) {
    txvalue = newvalue;
    lastvaluetx = newvalue;
    Serial.print("value:" + String(txvalue) + "\n");
  }
}

void checkserial() {
  while(Serial.available()) {
     if (!colon) {
       buff_name[pos] = Serial.read();
       if (buff_name[pos] == ':') {
         colon = !colon;
         buff_name[pos] = '\0';
         pos = -1; 
       }
       if (buff_value[pos] == '\n') {
         buff_name[pos] = '\0';
         buff_value[0] = '\0';
         pos = -1;
         receiveline(buff_name,buff_value);
       }
     } else {
       buff_value[pos] = Serial.read();
       if (buff_value[pos] == '\n') {
         colon = !colon;
         buff_value[pos] = '\0';
         pos = -1;
         receiveline(buff_name,buff_value);
       }
     }
     pos++;
  }
  
void receiveline(String name, String value) {
  btcontrol = true;
  if (millis() < 4000) return;
  if (name == "requestsetup") { sendsetup(); return; }
   
  // Check events  
  if (name == "mirror") {
    Serial.print(value + "\n");
  }
  
  if (name == "event") {
    if (value == "reset") {
      custom_setstandard();
      custom_send();
      setmode(6);
    }
    if (value == "save") {
      custom_save();
      setmode(6);
    } 
    if (value == "load") {
      custom_load();
      custom_send(); 
      setmode(6);
    }
  }
  
  if (name == "mode") {
    setmode(value.toInt());
    return;
  }
  if (name == "value") {
     txvalue = value.toInt();
     valuefromtx = false;
     return;
  }/*
  if (name == "live") {
     sendcolors = value == "true";
     saveprefs();
     return;
  }
  if (name == "alert") {
     alert = value == "true";
     saveprefs();
  }
  
  if (name == "animation") {
     alert = value == "true";
     saveprefs();
     return;
  }
  
  if (name == "bt") {
    btcontrol = value == "true";    
    setcolor(0,RGB,QUAD);
    Serial.print("Status:" + String(btcontrol) + "\n");
    saveprefs();
    return;
  }*/
  
  if (name == "address") {
    c_address = value.toInt();
    Serial.print("log:" + String(c_address) + "\n");
  }
  
  if (name.length() == 1) {
    for (int i = 0; i < c_values; i++)
       if (alphabet[i] == name.charAt(0)) {
         c_value[i] = value.toInt();
         setmode(6);
         return;
       }
  }
}

void setmode(int m) {
  int old = mode;
  mode = m % modes;
  if (old == m) return;
  setcolor(0,RGB,QUAD);
  Serial.print("mode:" + String(mode) + "\n");
  Serial.print("Status:");
  switch(mode) {
    case 0: Serial.print("Farbton"); break;
    case 1: Serial.print("Farbton (Automatisch)"); break;
    case 2: Serial.print("Polizei"); break;
    case 3: Serial.print("Weiß"); break;
    case 4: Serial.print("KFZ-Beleuchtung"); break;
    case 5: Serial.print("Strobo"); break;
    case 6: Serial.print("Benutzerdefiniert");
  }
  Serial.print("\n");
}

void loop()
{   
    now = millis();
    int dt = now - lastloop;
    lastloop = now;
    /*
    if (savetimer < 0) {
      //saveprefs();
      savetimer += 10000; 
    }*/
    
    if (txcountdown < 0) {
      loopcounter++;
      getinputs();
      txcountdown += txintervall;
    }
    else {
      txcountdown -= dt;
      delay(10); // !!!
    }
    checkserial();
    
    if (!txconnected && !btcontrol) {
       if (alert)
         setcolor((sin(millis() / 100.0/* + i * 1.57*/) + 1.0) * (full / 3.0),RED,QUAD);
         else setcolor(0,RGB,QUAD);
       return;
    }
    else if (nogps && !disarmed) {
      setcolor((sin(millis() / 50.0/* + i * 1.57*/) + 1.0) * (full / 2.0),BLUE,QUAD);
      return;
    }
    
    if (!on && !btcontrol) {
       setcolor(0,RGB,QUAD);
       return;
    }
    
    switch (mode) {
      case 0: // Scroll Hue
        sethsv((long)txvalue * (maxhue) / scale,full,full,QUAD);
        break;
      case 1: {// AUSTOSCROLL HUE
        auto_hue = (int)(auto_hue + dt / (txvalue * 10.0 + 50) * maxhue) % maxhue;
        sethsv(auto_hue,full,full,QUAD);
        break;
        }
      case 2: { // Police
        if (millis() / 12000 % 2 == 0) {
          int ptime = 1500;
          int p = (millis() % ptime) / (ptime / 4);
          if (p != police_laststate) {
            setrgb(0,0,0,QUAD);
            police_laststate = p;
          }
          setcolor(  (0.5 - abs(millis() % (ptime / 4) / (ptime / 4.0) - 0.5)) * 2 * full,
                     (p % 2) * RED + ((p+1) % 2) * BLUE,
                     (p / 2 == 0) ? LEFT : RIGHT);
        }
        else {
          int ptime = 4000;
          int p = (millis() % ptime) / (ptime / 4);
          int p2 = ((millis() + ptime / 8) % ptime) / (ptime / 4);
          if (p != police_laststate || p2 != police_laststate2) {
            setrgb(0,0,0,QUAD);
            police_laststate = p;
            police_laststate2 = p2;
          }
          setcolor(  (0.5 - abs(millis() % (ptime / 4) / (ptime / 4.0) - 0.5)) * 2 * full,
                     (p % 2) * RED + ((p+1) % 2) * BLUE,
                     LEFT);
          setcolor(  (0.5 - abs((millis() + ptime / 8) % (ptime / 4) / (ptime / 4.0) - 0.5)) * 2 * full,
                     (p2 % 2) * BLUE + ((p2+1) % 2) * RED,
                     RIGHT);
        }
      break;
    }
    case 3: { // WHITE
      setcolor(min(txvalue,full),RGB,QUAD);
      break;
    }
    case 4: { // CARLIGHTS
      setcolor(full,RED,FRONT);
      setcolor(min(txvalue,full),RGB,BACK);      
      break;
    }
    case 5: { // STROBE
      if (txvalue / 5 < 80) {
        setcolor(full, RGB, QUAD);
        delay(40);
        setcolor(0,RGB,QUAD);
        delay(max(0,txvalue / 5 - 40));
        return;      
      }
    
      if (strobe_timer < 0) {
         setcolor(full, RGB, QUAD);
         delay(40);
         setcolor(0,RGB,QUAD);
         strobe_timer += txvalue / 5;
      }
      else strobe_timer -= dt;   
      break;
    }
   case 6: { // CUSTOM
     if (c_value[14] != 0) {
        if (c_breaktimer > 0) {
          c_breaktimer -= dt;
          setcolor(0,RGB,QUAD);
          break;
        }
        else
          if (c_breakindex != floor((float)c_timer / c_value[14])) {
            c_breaktimer = c_value[15];
            c_breakindex = floor((float)c_timer / c_value[14]);
        }
     }
     c_timer += dt;
     int hue[4];
     int v[4];
     
     int autohueoffset = (c_timer) % (500 + c_value[5]) * ((float)maxhue / (500 + c_value[5]));
     if (c_value[6] != 0)
       autohueoffset -= autohueoffset % (int)(c_value[6] * 68.266);
     for (int i = 0; i < 4; i++) {
        hue[i] = (long)(c_value[0] * 68.266 + c_value[1] * i * 68.266 + (i > 0 ? c_value[1 + i] * 68.266 : 0) + (c_value[5] == 0 ? 0 : autohueoffset)) % maxhue;
        v[i] = full * (txvalue / (float)full) * (c_value[8+i] / 100.0) * (c_value[12] != 0 ? (0.5 * sin(((float)c_timer / c_value[12] + i * c_value[13] / 100.0) * 6.283 - 1.57) + 0.5) : 1.0);
     }
     
     sethsv(hue[0],c_value[7] / 100.0 * full,v[0],FRONT_LEFT);
     sethsv(hue[1],c_value[7] / 100.0 * full,v[1],FRONT_RIGHT);
     sethsv(hue[2],c_value[7] / 100.0 * full,v[2],BACK_RIGHT);
     sethsv(hue[3],c_value[7] / 100.0 * full,v[3],BACK_LEFT);
     
     break;
   }   
  }
  //Serial.print("loop:" + String(millis() - now) + "\n"); 
}

void sethsv(int h, int s, int v, int mask) {
  // h: 0 - maxhue
  // s: 0 - 4095
  // v: 0 - 4095
 
  int r = 0;
  int g = 0;
  int b = 0;
  
  int i = h / scale;
  int high = v;
  int low = 0;
  if (s != full) low = v * ((float)(full - s) / full);
  int up = low + (high - low) * (float)((h % scale) / (float)scale);
  int down = high - (high - low) * (float)((h % scale) / (float)scale);  
  
  switch (i) {
     case 1: r = down; g = high; b = low; break;
     case 2: r = low; g = high; b = up; break;
     case 3: r = low; g = down; b = high; break;
     case 4: r = up; g = low; b = high; break; 
     case 5: r = high; g = low; b = down; break;
     default: r = high, g = up; b = low; break;
  }
  
  setrgb(r,g,b,mask);  
}


void sendsetup() {
  Serial.print("Status:Quadcopter\n");
  Serial.print("FL:#000000\n");
  Serial.print("FR:#000000\n");
  Serial.print("BR:#000000\n");
  Serial.print("BL:#000000\n");
  
  Serial.print("requestint:mode\n");
  Serial.print("setminmode:0\n");
  delay(serialdelay);
  Serial.print("setmaxmode:" + String(modes - 1) + "\n");
  Serial.print("requestint:value\n");
  delay(serialdelay);
  Serial.print("setminvalue:0\n");
  Serial.print("setmaxvalue:" + String(full) + "\n");  
  delay(serialdelay);
  /*Serial.print("requestbool:live\n");
  Serial.print("requestbool:bt\n");
  delay(serialdelay);
  Serial.print("requestbool:alert\n");
  Serial.print("requestbool:animation\n");*/
  
  for (int i = 0; i < c_values; i++) {
    Serial.print("requestint:" + String(alphabet[i]) + "\n");
    delay(serialdelay);  
  }
  
  if (millis() > 1000) senddesc();
}

void senddesc() {
  int t = 30;
  Serial.print("a?Startfarbwert\n");
  Serial.print("setmaxa:360\n");
  delay(t);
  
  Serial.print("b?Farbwert Offset\n");
  Serial.print("setmaxb:180\n");
  delay(t);
  
  Serial.print("c?Farbwert Offset FR\n");
  Serial.print("setmaxc:360\n");
  delay(t);
  
  Serial.print("d?Farbwert Offset BL\n");
  Serial.print("setmaxd:360\n");
  delay(t);
  
  Serial.print("e?Farbwert Offset BR\n");
  Serial.print("setmaxe:360\n");
  delay(t);
  
  Serial.print("f?Farbwert Geschwindigkeit (Periodendauer - 500)\n");
  Serial.print("setmaxf:3500\n");
  delay(t);
  
  Serial.print("g?Farbwert Schritt\n");
  Serial.print("setmaxg:180\n");
  delay(t);
  
  Serial.print("h?Sättigung Gesamt\n");
  Serial.print("setmaxh:100\n");
  delay(t);
  
  Serial.print("i?Helligkeit FL\n");
  Serial.print("setmaxi:100\n");
  delay(t);
  
  Serial.print("j?Helligkeit FR\n");
  Serial.print("setmaxj:100\n");
  delay(t);
  
  Serial.print("k?Helligkeit BR\n");
  Serial.print("setmaxk:100\n");
  delay(t);
  
  Serial.print("l?Helligkeit BL\n");
  Serial.print("setmaxl:100\n");  
  delay(t);
    
  Serial.print("m?Helligkeit Sinus (Periodendauer)\n");
  Serial.print("setmaxm:2000\n");
  delay(t);
  
  Serial.print("n?Offset Helligkeit Sinus (% von 2pi)\n");
  Serial.print("setmaxn:100\n");
  delay(t);
  
  Serial.print("o?Abstand Pausen\n");
  Serial.print("setmaxo:4000\n");
  delay(t);
  
  Serial.print("p?Dauer Pausen\n");
  Serial.print("setmaxp:2000\n");
  
  delay(t);
  Serial.print("registerevent:reset\n");
  delay(t);
  Serial.print("requestint:address\n");
  Serial.print("setmaxaddress:5\n");
  delay(t);
  Serial.print("address?Speicheraddresse\n");
  delay(t);
  Serial.print("registerevent:save\n");
  delay(t);
  Serial.print("registerevent:load\n");
  delay(t);  
}