#include "wagon.h"

#define WAGONLENGTH 3

#define EDGE_KILL
//#define EDGE_WALL
//#define EDGE_BOUNCE
//#define EDGE_WRAP
#define WRAPLEDENDPOS _numpixels //default
  //#define WRAPLEDENDPOS (_numpixels-5) //led index which is last led
#define WRAPLEDSTARTPOS 0 //default
  //#define WRAPLEDSTARTPOS 23 //led index which is the same as WRAPLEDENDPOS (from start)

#define TYPE_RAINBOW 1

/*uint8_t GammaE[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2,
2, 2, 2, 3, 3, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5,
6, 6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 11, 11,
11, 12, 12, 13, 13, 13, 14, 14, 15, 15, 16, 16, 17, 17, 18, 18,
19, 19, 20, 21, 21, 22, 22, 23, 23, 24, 25, 25, 26, 27, 27, 28,
29, 29, 30, 31, 31, 32, 33, 34, 34, 35, 36, 37, 37, 38, 39, 40,
40, 41, 42, 43, 44, 45, 46, 46, 47, 48, 49, 50, 51, 52, 53, 54,
55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70,
71, 72, 73, 74, 76, 77, 78, 79, 80, 81, 83, 84, 85, 86, 88, 89,
90, 91, 93, 94, 95, 96, 98, 99,100,102,103,104,106,107,109,110,
111,113,114,116,117,119,120,121,123,124,126,128,129,131,132,134,
135,137,138,140,142,143,145,146,148,150,151,153,155,157,158,160,
162,163,165,167,169,170,172,174,176,178,179,181,183,185,187,189,
191,193,194,196,198,200,202,204,206,208,210,212,214,216,218,220, 
222,224,227,229,231,233,235,237,239,241,244,246,248,250,252,255}; */

Wagon::Wagon(int id,int numpixels, Adafruit_NeoPixel *strip,uint8_t *height,float pos, float trainlength,float startvel,float startacc, float wagonmass, uint32_t wagoncolor)
{
  _id = id;
  _numpixels=numpixels;
  _pos = pos;
  _trainlength = trainlength;
  _strip=strip;
  _height=height;
  _vel=startvel;
  _acc=startacc;
  _wagonmass=wagonmass; //mass of whole train
  _spawntime=millis();
  _lastvel=startvel;
  _lastpositivedirchangepos=_numpixels+1000;
  _lastpositivedirchangePosDifference=1000;
  _wagoncolor=wagoncolor;
  _health=1.0;
  _type=0; //normal
}

void Wagon::setType(uint8_t t){
  _type=t;
}
void Wagon::setLength(float l){
  _trainlength=l;
}

bool Wagon::operator==(const Wagon &r) const {
  return (r._id == _id);
}

void Wagon::updatePhysics(float updatedelayms)
{
  
  /*
  float acceleration=0;
  for (int cpos=int(_pos);cpos>int(_pos-_trainlength);cpos--){
    acceleration=(_height[(int)cpos]-_height[(int)cpos+1])*0.03;
  }
  acceleration/=int(_trainlength);
  _vel+= acceleration; //Acceleration from height difference
  _vel*=1-0.001; //resistance
  float airresistance=_vel*_vel *0.005;//air resistance
  if (_vel>=0){
    _vel-=airresistance;
  }else{
    _vel+=airresistance;
  }*/
  #define CONST_G 9.81
  #define PIXELDISTANCE 1.6666667 // 1/60.0 * 100 
  #define C_ROLL 0.001 // = Croll    https://de.wikipedia.org/wiki/Rollwiderstand   0.001 (zug)
  //#define AIRRESFIRST 0.018 //C_w*A*0.5*rho     Air resistance:  C_w * A * 0.5 * rho  (.... *v^2)   0.18
  #define AIRRESFIRST 0.05 //C_w*A*0.5*rho     Air resistance:  C_w * A * 0.5 * rho  (.... *v^2)   0.18 test
  #define AIRRES 0.001 //for slipstream at second wagon
  #define AIRRESMUL 0.2 //how much of air resistance the next wagon has
  //http://www.kfz-tech.de/Biblio/Formelsammlung/Luftwiderstand.htm   C_w 0.6 
  //rho  Massendichte luft  1,2041
  //A = 1m^2

  float m=_wagonmass; //mass of part of a wagon

  _acc=0;
  int cpos=(int)_pos;
  uint8_t wagonnumber=0;
  for (int cpos=(int)_pos;cpos>(int)(_pos-_trainlength);cpos--){ //for each wagon
    if (wagonnumber%WAGONLENGTH==0) { //every n-th pixel is a wagon
  
      float hdiff=getHeight((int) (cpos-0.5)) - getHeight((int)(cpos+0.5));
  
      //Serial.print("hdiff=");
      //Serial.print(hdiff);
    
      float beta=atan2(PIXELDISTANCE, hdiff);
  
      //Serial.print(" beta=");
      //Serial.println(beta);
    
      //_acc += CONST_G * cos(beta) - C_ROLLG*sin(beta) - AIRRES/m*_vel*_vel;
  
      float aa=CONST_G *m* cos(beta) *updatedelayms/1000; //Gravity  and m/s^2 time correction
  
      //Roll Resistance
      float bb=C_ROLL*m*CONST_G*updatedelayms/1000*sin(beta); //roll resistance
      
      if (_vel<0){
        bb*=-1;
      }
  
      //Air Resistance
      float cc=0;
      if (wagonnumber==0){ //first wagon
        //cc=AIRRESFIRST/m*pow(_vel,2); //air resistance for first wagon
        cc=AIRRESFIRST*pow(_vel,2); //air resistance for first wagon
      }else {
        //cc=AIRRES/m*pow(_vel,2) *pow(AIRRESMUL,wagonnumber-1); //air resistance 
        cc=AIRRES*pow(_vel,2) *pow(AIRRESMUL,wagonnumber-1); //air resistance 
      }
        
      if (_vel<0){
        cc*=-1;
      }
  
      //Serial.print("aa="); Serial.print(aa);
      //Serial.print(" bb="); Serial.print(bb);
      //Serial.print(" cc="); Serial.println(cc);
      
  
      _acc += aa - bb - cc;
    }
    
    wagonnumber++;
  }

  _acc*=updatedelayms/1000;

  
  
  _vel += _acc;
  _pos += _vel/PIXELDISTANCE;
  /*Serial.print(" Vel=");
  Serial.print(_vel);
  Serial.print(" Acc=");
  Serial.println(_acc);*/

    
  if (_lastvel*_vel<0 && _lastvel>=0){ //direction changed, positive
    _lastpositivedirchangePosDifference=_lastpositivedirchangepos-_pos;
    Serial.print("DC pos=");
    Serial.print(_pos);
    Serial.print(" last=");
    Serial.print(_lastpositivedirchangepos);
    
    Serial.print(" diff=");
    Serial.println(_lastpositivedirchangePosDifference);
    _lastpositivedirchangepos=_pos;
  }
  _lastvel=_vel;


  
  if (_pos>=WRAPLEDENDPOS){ 
    #ifdef EDGE_WRAP
      _pos-=WRAPLEDENDPOS; //Wrap around edges
    #endif
    #ifdef EDGE_BOUNCE
      _vel*=-1; //bounce at edges
    #endif
    #ifdef EDGE_WALL
      //nothing
    #endif
    #ifdef EDGE_KILL
      if (_pos>=WRAPLEDENDPOS+(_trainlength*WAGONLENGTH)){
        _health=0;
      }
    #endif
  }
  if (_pos<WRAPLEDSTARTPOS){ 
    #ifdef EDGE_WRAP
      _pos=WRAPLEDENDPOS+_pos; //warp around edges
    #endif
    #ifdef EDGE_BOUNCE
      _vel*=-1;; //bounce at edges
    #endif
    #ifdef EDGE_WALL
      //nothing
    #endif
    #ifdef EDGE_KILL
      _health=0;
    #endif
  }

  //fade out if health got low
  if (_health<1.0 && _health>0.0){
    _health-=0.0001; //reduce until 0
  }
  
}

float Wagon::getHeight(int p){
  
  
  if (p<0){
    #ifdef EDGE_WRAP
      p=_numpixels+p; //wrap edge
    #endif
    #ifdef EDGE_WALL
      return _height[0]+p*-100.0; //edges as wall
    #endif
    #ifdef EDGE_KILL
      return _height[0]+p*-100.0; //same like edges as wall
    #endif
  }else if(p>=WRAPLEDENDPOS){
    #ifdef EDGE_WRAP
      p=p-WRAPLEDENDPOS; //wrap edge
    #endif
    #ifdef EDGE_WALL
      return _height[WRAPLEDENDPOS-1]+(p-WRAPLEDENDPOS)*100.0; //edges as wall
    #endif
    #ifdef EDGE_KILL
      return _height[WRAPLEDENDPOS-1]+(p-WRAPLEDENDPOS)*100.0; //samel like edges as wall
    #endif
  }
  return _height[p];
}

void Wagon::updateGraphics()
{
  float wagonfeathering=2;
  for(int i=_pos+wagonfeathering;i>_pos-_trainlength-wagonfeathering;i--){
    float featherbrightness=1;
    if (i>_pos){ //in front of wagon
      featherbrightness=1 - (i-_pos)/wagonfeathering;
      
    }else if (i<_pos-_trainlength){ //behind of wagon
      featherbrightness=1 - (_pos-_trainlength -i)/wagonfeathering;
    }
    
    if (featherbrightness<=0){ //distpercent between 0 and 1. 1-> full brightness, 0-> feathering distance away
      featherbrightness=0;
    }
    
    //uint32_t c=_strip->Color(0,255*featherbrightness,0);
    
    //uint32_t c=Wheel(_height[i]/45.0*255,featherbrightness);
    
    //uint8_t b=_height[i]*255/45;
    //uint8_t b=abs(_vel)*255.0;
    //uint32_t c=_strip->Color(b*featherbrightness,(255-b)*featherbrightness,0);

    float healtpositive=_health;
    if(healtpositive<0){ //only positive values for color
      healtpositive=0;
    }
    
    uint32_t c=_wagoncolor;
    if (_type==TYPE_RAINBOW){
      c=Wheel(((int)((i-_pos)*256/_trainlength))%256,1.0);
    }
    uint8_t _r = (uint8_t)(c >> 16)*healtpositive;
    uint8_t _g = (uint8_t)(c >>  8)*healtpositive;
    uint8_t _b = (uint8_t)c*healtpositive;

    

    _r*=featherbrightness;
    _g*=featherbrightness;
    _b*=featherbrightness;

    /*_r=GammaE[_r];
    _g=GammaE[_g];
    _b=GammaE[_b];*/

    int iCorrected=i;
    #ifdef EDGE_WRAP
      if (i<0){
        iCorrected=WRAPLEDENDPOS+i+1; //Wrap around edges
      }
    #endif

    uint32_t _pxcolor=_strip->getPixelColor(iCorrected); //get current color of that pixel
    uint8_t _pxr = _pxcolor >> 16;
    uint8_t _pxg = _pxcolor >> 8;
    uint8_t _pxb = _pxcolor;
    uint16_t _tmpr=_pxr+_r; //add colors
    uint16_t _tmpg=_pxg+_g;
    uint16_t _tmpb=_pxb+_b;

    double _brightnesscorrection = 0;
    if ( (_tmpr+_tmpg+_tmpb) > 0){
      _brightnesscorrection = max((double)_pxr+(double)_pxg+(double)_pxb, (double)_r+(double)_g+(double)_b)/(_tmpr+_tmpg+_tmpb);
    }
    
    
    _tmpr=(uint16_t)(_min(255,_tmpr*_brightnesscorrection));
    _tmpg=(uint16_t)(_min(255,_tmpg*_brightnesscorrection));
    _tmpb=(uint16_t)(_min(255,_tmpb*_brightnesscorrection));

    _strip->setPixelColor(iCorrected,_tmpr,_tmpg,_tmpb); //draw pixel
  }
}

int Wagon::pos()
{
  return _pos;
}

int Wagon::id()
{
  return _id;
}

long Wagon::spawntime()
{
  return _spawntime;
}

bool Wagon::alive()
{
  /*if (_pos>_numpixels){
    return false;
  }*/

  /*if (millis()>_spawntime+30*1000){ //too old
    return false;
  }*/


  if (_lastpositivedirchangePosDifference<=5){ //reduce health
    //return false;
    _health-=0.1;
  }
  if (_health<=0) {
    return false;
  }
  
  return true;
}

uint32_t Wagon::Wheel(byte WheelPos,float brightness) {
  WheelPos = 255 - WheelPos;
  if(WheelPos < 85) {
    return _strip->Color(255 - WheelPos * 3*brightness, 0, WheelPos * 3*brightness);
  }
  if(WheelPos < 170) {
    WheelPos -= 85;
    return _strip->Color(0, WheelPos * 3*brightness, 255 - WheelPos * 3*brightness);
  }
  WheelPos -= 170;
  return _strip->Color(WheelPos * 3*brightness, 255 - WheelPos * 3*brightness, 0);
}