#include<Arduino.h>
#include <Wire.h>


#include "WEMOS_Motor.h"

//follow firmware flash guide for new wemos motor shield v1.0  https://github.com/thomasfredericks/wemos_motor_shield


#define BUTTON_DEBOUNCE 200
#define PIN_BUTTON1 D5
struct button{
  uint8_t pin;
  unsigned long last_time_read=0;
  bool down=false;
  bool changed=false;
};
button button1;

unsigned long last_sensor_read=0;
#define SENSOR_READ_INTERVAL 20 //in ms
#define SENSE_FILTER_SIZE 20 //value will have a delay of td = SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL

bool manual_drive_direction=false;

#define PIN_SENSE A0
unsigned long last_print=0;
#define SENSE_CLEAR_LOWER 40 //adc value lower limit for clear part. clear is around 70
#define SENSE_CLEAR_UPPER 100 //adc value upper limit for clear part

#define SENSE_OPAQUE_LOWER 600 //adc value lower limit for opaque part. opaque is around 675
#define SENSE_OPAQUE_UPPER 750 //adc value upper limit for opaque part

#define SENSE_END_LOWER 850 //adc value lower limit for end marker
#define SENSE_END_UPPER 1024 //adc value upper limit for end marker

//define directions for motors
#define _M1_UP _CCW;
#define _M1_DOWN _CW;

#define _M2_UP _CCW;
#define _M2_DOWN _CW;

#define SENSESTATUS_CLEAR 1;
#define SENSESTATUS_OPAQUE 2;
#define SENSESTATUS_END 3;
#define SENSESTATUS_UNKNOWN 0;



//model parameters/variables
struct blindmodel
{
  unsigned long lastreadtime=0;
  float position=0; //0 is furthest open. positive is down (closing). unit is mm

  float length_clear; //length of clear part in position units
  float length_opaque; //lengt of opaque part in position units

  float speed_estimate=1; //how much position units (mm) per second at pwm=100

  int sense_clear_lower; //adc value lower limit for clear part. clear is around 70
  int sense_clear_upper; //adc value upper limit for clear part

  int sense_opaque_lower; //adc value lower limit for opaque part. opaque is around 675
  int sense_opaque_upper; //adc value upper limit for opaque part

  int sense_end_lower; //adc value lower limit for end marker
  int sense_end_upper; //adc value upper limit for end marker

  uint8_t sense_status=0;
  int sense_read[SENSE_FILTER_SIZE] = {0};
  uint8_t sense_read_pos=0; //position of last element written to

  float set_position=0;

  unsigned long last_sense_ok=0; //last time sensor measured class ok
  //TODO: implement timeout if last_sense_ok gets too high. 
};

blindmodel blind1;


//Motor shield default I2C Address: 0x30
//PWM frequency: 1000Hz(1kHz)
Motor M1(0x30, _MOTOR_A, 1000); //Motor A
Motor M2(0x30, _MOTOR_B, 1000); //Motor B



int getFitered(int* values,uint8_t size);
uint8_t classifySensorValue(blindmodel &blind);

void setup() {
  
  
  Serial.begin(57600);
  Serial.println("Starting");

  button1.pin=PIN_BUTTON1;

  pinMode(button1.pin, INPUT_PULLUP);
  pinMode(PIN_SENSE, INPUT);

  M1.setmotor(_STOP);
  M2.setmotor(_STOP);


  //settings for blind
  blind1.length_clear=50;
  blind1.length_opaque=74;
  blind1.sense_clear_lower=40;
  blind1.sense_clear_upper=100;
  blind1.sense_opaque_lower=600;
  blind1.sense_opaque_upper=750;
  blind1.sense_end_lower=850;
  blind1.sense_end_upper=1024;
  blind1.speed_estimate=20;
  
}


void loop() {

  button1.changed=false;
  if (millis() > button1.last_time_read + BUTTON_DEBOUNCE) {
    bool new_pin_button_down=!digitalRead(button1.pin);
    if (button1.down != new_pin_button_down) { //changed
      button1.down = new_pin_button_down; //update
      button1.changed=true;
      button1.last_time_read=millis(); //delay next check
    }    
  }


  //Manual movement by button
  if (button1.changed) {
    if (button1.down) { //changed to pressed
      if (manual_drive_direction) {
        M1.setmotor( _CW, 100);
        Serial.print("CW PWM: ");
      }else{
        M1.setmotor( _CCW, 100);
        Serial.print("CCW PWM: ");
      }
      Serial.println(100);
      manual_drive_direction=!manual_drive_direction; //switch direction every new press
    }else{ //changed to released
      Serial.println("Motor STOP");
      M1.setmotor(_STOP);
    }
  }



  //Read sensor/encoder
  if (millis() > last_sensor_read + SENSOR_READ_INTERVAL) {
    blind1.sense_read_pos=(blind1.sense_read_pos+1)%SENSE_FILTER_SIZE; //next element
    blind1.sense_read[blind1.sense_read_pos]=analogRead(PIN_SENSE);

    classifySensorValue(blind1); //writes to blindmodel.sense_status

    
    last_sensor_read=millis();
  }

  if (millis() > last_print + 100) {
    Serial.println(blind1.sense_status);
    last_print=millis();
  }
}

int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
{
  // Need to cast the void * to int *
  int a = *((int *)cmp1);
  int b = *((int *)cmp2);
  // The comparison
  return a > b ? -1 : (a < b ? 1 : 0);
  // A simpler, probably faster way:
  //return b - a;
}

int getFitered(int* values,uint8_t size) {
  int copied_values[size];
  for(int i=0;i<size;i++) {
    copied_values[i] = values[i]; //TODO: maybe some value filtering/selection here
  }
  int copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
  qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);

  return copied_values[size/2];
}

uint8_t classifySensorValue(blindmodel &blind) {
  int filtered=getFitered(blind.sense_read, SENSE_FILTER_SIZE);
  if (filtered>=blind.sense_clear_lower && filtered<=blind.sense_clear_upper) {
    blind.sense_status=SENSESTATUS_CLEAR;
    blind.last_sense_ok=millis();
  } else if (filtered>=blind.sense_opaque_lower && filtered<=blind.sense_opaque_upper) {
    blind.sense_status=SENSESTATUS_OPAQUE;
    blind.last_sense_ok=millis();
  } else if (filtered>=blind.sense_end_lower && filtered<=blind.sense_end_upper) {
    blind.sense_status=SENSESTATUS_END;
    blind.last_sense_ok=millis();
  } 
}

/*
void loop() {

  int pwm;

  for (pwm = 0; pwm <= 100; pwm++)
  {
    M1.setmotor( _CW, pwm);
    M2.setmotor(_CW, pwm);
    Serial.print("Clockwise PWM: ");
    Serial.println(pwm);
    delay(100);
  }

  Serial.println("Motor STOP");
  M1.setmotor(_STOP);
  M2.setmotor( _STOP);

  delay(1000);

  
  for (pwm = 0; pwm <= 100; pwm++)
  {
    M1.setmotor(_CCW, pwm);
    //delay(1);
    M2.setmotor(_CCW, pwm);
    Serial.print("Counterclockwise PWM: ");
    Serial.println(pwm);
    delay(100);

  }

  Serial.println("Motor A&B STANDBY");
  M1.setmotor(_STANDBY);
  M2.setmotor( _STANDBY);
  delay(1000);

}
*/