#ifndef _LED_H_
#define _LED_H_


//LED Ring Positions
/*
      3 2 1 
    4       0
   5         15
   6         14
   7         13
    8       12
     9 10 11
*/


#include <Adafruit_NeoPixel.h>


#define LED_PIN 3
#define LED_COUNT  16

// NeoPixel brightness, 0 (min) to 255 (max)
#define BRIGHTNESS 50 // Set BRIGHTNESS to about 1/5 (max = 255)

// Declare our NeoPixel strip object:
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRBW + NEO_KHZ800);

unsigned long last_ledupdate=0;
#define LEDUPDATEINTERVAL 100

uint8_t led_errorcount=0; //count led progress errors. used for delay at end if any errors occured

void led_dotcircle(unsigned long loopmillis);
void led_gauge(unsigned long loopmillis,float value,float value_min,float value_max,uint32_t colorGauge,uint32_t colorBG);

void init_led() {
  strip.begin();           // INITIALIZE NeoPixel strip object (REQUIRED)
  strip.show();            // Turn OFF all pixels ASAP
  strip.setBrightness(BRIGHTNESS);
}

void led_testLEDSBlocking(){
    strip.clear();
    strip.show();
    delay(10);
    uint32_t color=strip.Color(0,   0,   0, 0);

    for(int i=0; i<strip.numPixels()*4; i++) { // For each pixel in strip...
        switch(i/strip.numPixels()) {
            case 0:
                color=strip.Color(255,   0,   0, 0);
            break;
            case 1:
                color=strip.Color(0,   255,   0, 0);
            break;
            case 2:
                color=strip.Color(0,   0,   255, 0);
            break;
            case 3:
                color=strip.Color(0,   0,   0, 255);
            break;
        }
        strip.setPixelColor(i%strip.numPixels(), color);
        strip.show();
        delay(25);
    }
    strip.clear();
    strip.show();
}

void led_simpeProgress(uint8_t progressID,uint8_t result){
    if (result!=1) {
        led_errorcount++;
    }
    uint32_t color=strip.Color(0,   0,   0, 0);
    switch(result) {
        case 0: //fail
            color=strip.Color(255,   0,   0, 0);
        break;
        case 1: //success
            color=strip.Color(0,   255,   0, 0);
        break;
        case 2: //warning
            color=strip.Color(127,   127,   0, 0);
        break;
    }
    strip.setPixelColor(progressID, color);
    strip.show();
}

void led_simpleProgressWait() {
    if (led_errorcount>0) {
        delay(5000);
    }else{
        delay(100);
    }
}

void led_update(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear) {
    if (loopmillis>last_ledupdate+LEDUPDATEINTERVAL) {
        last_ledupdate=loopmillis;

        

        if (error_brake_outofrange || error_throttle_outofrange || error_ads_max_read_interval) {
            //Error
            
        }else{
                    
            if (armed) {
                if (loopmillis-last_notidle>5000) {
                    //Standing           
                    float vbat=min(escRear.getFeedback_batVoltage(),escFront.getFeedback_batVoltage());     
                    led_gauge(loopmillis,vbat,3*12,4.2*12,strip.Color(0,  255,   0, 0),strip.Color(100,  0,   0, 0));
                }else{
                    //Driving
                    float currentMean=escRear.getFiltered_curL()+escRear.getFiltered_curR()+escFront.getFiltered_curL()+escFront.getFiltered_curR();
                    led_gauge(loopmillis,currentMean,0,16.0,strip.Color(0,  0,   0, 255),strip.Color(0,  0,   20, 0));
                }
            }else{
                //Disarmed
                led_dotcircle(loopmillis); //fill background with animation

                uint32_t colorConnected=strip.Color(0,   255,   0, 0);
                if (escFront.getControllerConnected()) {
                    for(int i=1; i<=3; i++) {
                        strip.setPixelColor(i, colorConnected); 
                    }
                }

                if (escRear.getControllerConnected()) {
                    for(int i=1+8; i<=3+8; i++) {
                        strip.setPixelColor(i, colorConnected); 
                    }
                }
                
            }
        }
        

        strip.show();                          //  Update strip to match
    }
}

void led_gauge(unsigned long loopmillis,float value,float value_min,float value_max,uint32_t colorGauge,uint32_t colorBG) {
    uint8_t position=map( max(min(value,value_max),value_min)  ,value_min,value_max, 0,strip.numPixels()+1);
    for(int i=0; i<strip.numPixels(); i++) { // For each pixel in strip...
        uint8_t pp=(strip.numPixels()-i-1 + 10 )%strip.numPixels(); //Offset and invert
        
        if (i<position) {
            strip.setPixelColor(pp, colorGauge);         //  Set pixel's color (in RAM)
        }else{
            strip.setPixelColor(pp, colorBG);         //  Set pixel's color (in RAM)
        }
    }
}

void led_dotcircle(unsigned long loopmillis) {
    float position=(loopmillis/1000.0*strip.numPixels()/10.0);
    while (position>strip.numPixels()){ //modulo for float
        position-=strip.numPixels();
    }

    for(int i=0; i<strip.numPixels(); i++) { // For each pixel in strip...
        float _bright=min(abs(i-position),strip.numPixels()-abs(i-position));
        _bright/=strip.numPixels()/2.0; //1 if position is at i, 0 if position is oposite of i in circle
        _bright=pow(_bright,2);
        uint32_t color=strip.Color(0,   0,   0, _bright*255);
        strip.setPixelColor(i, color);         //  Set pixel's color (in RAM)
    }
}

#endif