#pragma once

#include "Adafruit_NeoPixel.h"
#include "CurieIMU.h"
#include "CurieTime.h"
#include <Wire.h>

// Minimal class to replace std::vector for regular Arduino's not needed for Arduino 101
#include <vector>
using namespace std;

#define Vector vector

long lastStepCount = 0;
Adafruit_NeoPixel *pixels;

class Color {
  public:
  unsigned short R,G,B;
  Color(unsigned short newR=255,unsigned short newG=255,unsigned short newB=255) {
    R=newR;
    G=newG;
    B=newB;
  }
};

class Colors {
  public:
  Vector<Color> values;
  void add(Color c) {
    values.push_back(c);
  }
  void transfigure(Colors &other,float t) {
     for (unsigned i=0;i<values.size(); i++) {
        float R1=values[i].R;
        float G1=values[i].G;
        float B=values[i].B;
        float R2=other.values[i].R;
        float G2=other.values[i].G;
        float B2=other.values[i].B;
        float r=(R2-R1)*t+R1;
        if (r<0.0) r=0.0;
        if (r>255.0) r=255.0;
        float g=(G2-G1)*t+G1;
        if (g<0.0) g=0.0;
        if (g>255.0) g=255.0;
        float b=(B2-B)*t+B;
        if (b<0.0) b=0.0;
        if (b>255.0) b=255.0;
        // Neopixel code to set LED colors
     }
  }
  int size() {
    return values.size();
  }
};

class Length {
  public:
  Length(unsigned short newL) {
    l=newL;
  }
  unsigned short l;  // in inches
};

class Lengths {
 public:
 Vector<Length> values;  // in inches
 void add(Length l) {
  values.push_back(l);
 }
 void transfigure(Lengths &otherValues,float t) {
   for (unsigned i=0;i<values.size(); i++) {
      float l1=(float) values[i].l;
      float l2=(float) otherValues.values[i].l;
      float l=(l2-l1)*t+l1;
      if (l<0.0) l=0.0;
      if (l>255.0) l=255.0;
      Wire.write(byte(i+1)); // Transmit the interpolated lengths
      Wire.write(byte(l));
      Wire.endTransmission();  
   }
 }
 int size() {
  return values.size();
 }
};

class Look { // The state of the dress/garment
  Colors colors;
  Lengths lengths;
  public:
  int getColorCount() {
    return colors.size();
  }
  int getLengthCount() {
    return lengths.size();
  }
  void add(Color newColor) {
    colors.add(newColor);
  }
  void add(Length newL) {
    lengths.add(newL);
  }
  void transfigure(Look *other,float t){
    colors.transfigure(other->colors,t);
    lengths.transfigure(other->lengths,t);
  }
};

#define PM true
#define AM false
#define STEP 0
#define SPIN 1
#define TIME 2

class Situation { // A situation that causes a need for a new look
  protected:
  int type, count, ghour,gminute,gsecond;
  bool pm;
  public:
  bool hasOccurred() {
    bool retval=false;
    switch(type) {
      case STEP:
          retval=(lastStepCount<count);
        break;
      case SPIN:
      // To Do KJC
        break;
      case TIME:
          long goal=gsecond+(gminute+ghour*60)*60;
          if (PM) goal+=12*60*60; // seconds in 1/2 day
          long current;
          current=second()+(minute()+hour()*60)*60;
          retval=(goal<current);
        break;   
    }
    return retval;
  }
};

class Step:public Situation {
 public: 
  Step(int newCount) {
    type=STEP;
    count=newCount;
  }
};

class Spin:public Situation {
  public:
  Spin(int newCount) {
    type=SPIN;
    count=newCount;
  }
};

class Time:public Situation {
  public:
  Time(int newhour,int newminute,int newsecond,bool newpm) {
    type=TIME;
    ghour=newhour;
    gminute=newminute;
    gsecond=newsecond;
    pm=newpm;
  }
};

class Transition {
  public:
  Look *current,*next;
  Situation *occurs;
  Transition(Situation &newOccurs,Look &newCurrent,Look &newNext) {
    occurs=&newOccurs;
    current=&newCurrent;
    next=&newNext;
  }
};

static void updateStepCount() {
  int stepCount = CurieIMU.getStepCount();
  if (stepCount != lastStepCount) {
    lastStepCount = stepCount;
  }
}

static void eventCallback(void) {
  if (CurieIMU.stepsDetected())
    updateStepCount();
}

class Occasion {
  Vector<Look*> looks;
  Vector<Situation*> situations;
  Vector<Transition> transitions;
  Look *current;
  Look *last;
  float t,peg,transitionTime;
  float getPeg() {
    return ((float)millis())/1000.0;
  }
  void  setPeg() {
    peg=getPeg();
  }
  public:
  Occasion(float newTransitionTime=3.0) {
    transitionTime=newTransitionTime;
  }
  void startWith(Look &l) {
    current=&l;
    last=&l;
    setPeg();
  }
  void add(Look &l) {
    looks.push_back(&l);
  }
  void add(Situation &s) {
    situations.push_back(&s);
  }
  void loop(){ // move through the FSM
    for (unsigned s=0;s<situations.size();s++) {
      if (situations[s]->hasOccurred()) {
        for (unsigned t=0;t<transitions.size();t++) {
          if (transitions[t].current==current && transitions[t].occurs==situations[s]) {
            setPeg();
            lastStepCount=0;
            last=current;
            current=transitions[t].next;
          }
        }
      }
    }
    float t=(getPeg()-peg)/transitionTime;
    if (t<0.0) t=0.0;
    if (t>1.0) t=1.0;
    current->transfigure(last,t);
  }
  void setup(){
    int NumPixels=current->getColorCount();
    Adafruit_NeoPixel temp=Adafruit_NeoPixel(NumPixels, 3, NEO_GRB + NEO_KHZ800);
    pixels=&temp;
    Wire.begin();
    CurieIMU.begin();
    CurieIMU.setStepDetectionMode(CURIE_IMU_STEP_MODE_NORMAL);
    CurieIMU.setStepCountEnabled(true);
    CurieIMU.attachInterrupt(eventCallback);
    CurieIMU.interrupts(CURIE_IMU_STEP);
    
    pixels->begin();
  }
  void when(Situation &occurs,Look &current,Look &next) {
    Transition t(occurs,current,next);
    transitions.push_back(t);
  }
};