#include "wagon.h"

#define SLOWVELOCITY 0.05

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;
  _spawntime=millis();
  _lasttimefast=millis();
  _wagoncolor=wagoncolor;

}

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 * G    https://de.wikipedia.org/wiki/Rollwiderstand   0.001
  #define AIRRES 0.18 //C_w*A*0.5*rho     Air resistance:  C_w * A * 0.5 * rho  (.... *v^2)
  //http://www.kfz-tech.de/Biblio/Formelsammlung/Luftwiderstand.htm   C_w 0.6 
  //rho  Massendichte luft  1,2041
  //A = 1m^2

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

  _acc=0;
  int cpos=(int)_pos;
  for (int cpos=(int)_pos;cpos>(int)(_pos-_trainlength);cpos--){ //for each 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 * cos(beta) *updatedelayms/1000; //Gravity  and m/s^2 time correction
    float bb=C_ROLL*CONST_G*updatedelayms/1000*sin(beta); //roll resistance
    if (_vel<0){
      bb*=-1;
    }
    float cc=AIRRES/m*pow(_vel,2); //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;
    

  }

  _acc*=updatedelayms/1000;

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

  float _testvel=_vel;
  if (_testvel<0){
    _testvel*=-1;
  }
  if (_testvel>SLOWVELOCITY){ //for despawn if slow
    _lasttimefast=millis();
  }


  if (_pos>=_numpixels){ //Wrap around edges
    _pos-=_numpixels;
  }
  if (_pos<0){ //warp around edges
    _pos=_numpixels+_pos;
  }
  
}

float Wagon::getHeight(int p){
  
  
  if (p<0){
    p=0; //straight edges
    //return (-p)*10; //edges as wall
  }else if(p>=_numpixels){
    p=_numpixels-1;  //straight edges
    return (p-_numpixels)*10; //edges as wall
  }
  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);

    uint32_t c=_wagoncolor;
    uint8_t _r = (uint8_t)(c >> 16);
    uint8_t _g = (uint8_t)(c >>  8);
    uint8_t _b = (uint8_t)c;

    _r*=featherbrightness;
    _g*=featherbrightness;
    _b*=featherbrightness;
    
    _strip->setPixelColor(i,_r,_g,_b);
  }
}

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 (millis()>_lasttimefast+5000 ){ //too long too slow
    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);
}