// Note: You can press Ctrl+Return (or Command+Return for Mac)
// to quickly compile and test your code while in the editor.

// Constraints:
// - Your code cannot include other files (stdint.h, math.h, and float.h are already included)
// - Your code must complete the test harness within 10 seconds. If it takes longer, you will get a timeout error
// - Your code will execute in a PowerPC 64-bit environment
// - Your code will be compiled with GCC 5.4 with support for the C++14 standard, as implemented by that compiler

float Length(Vec3 v)
{
    return v.y;
}

void SetPrecicion3Decimal(float &f) {
    f = (int) (f*1000 + 0.0f);
    f = f /1000.0f;
}

Vec3 Setprecicion2Decimal(Vec3 v) {
    v.x = (int) (v.x*100 + 0.5f);
    v.x = v.x /100.0f;
    v.y = (int) (v.y*100 + 0.5f);
    v.y = v.y /100.0f;
    v.z = (int) (v.z*100 + 0.5f);
    v.z = v.z /100.0f;
    return v;
}

float Magnitude(Vec3 a) {
    return Sqrtf( a.x*a.x + a.y*a.y + a.z*a.z);
}
float Dot(Vec3 a, Vec3 b) {
    return a.x*b.x + a.y*b.y + a.z*b.z;
}
Vec3 Project(Vec3 a, Vec3 b) {
    return b * ( Dot(a,b) / Dot(b,b) );
}
Vec3 Normal(Vec3 v) {
    float m = Magnitude(v);
    Vec3 normal = v/m;
    return normal;
}

Vec3 NormalDir(Vec3 a, Vec3 b) {
    Vec3 d( b.x-a.x, b.y-a.y, b.z-a.z );
    return Normal( d );
}

Vec3 Cross(Vec3 a, Vec3 b) {
    Vec3 cross( a.y* b.z - b.y* a.z,/**/ b.x* a.z - a.x* b.z,/**/  a.x* b.y - b.x* a.y );
    return cross;
}
Vec3 Difference(Vec3 v1, Vec3 v2) {
   Vec3 d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
   return d;
}

Vec3 AlignToUp(Vec3 grav) {
    Vec3 upo( 0.0f, 1.0f, 0.0f );
    Vec3 ng = Normal(grav);
    Vec3 up( upo.x/ng.x, upo.y/ng.y, upo.z/ng.z );//a = a / np.linalg.norm(a) # normalize b  NOT USED
    Vec3 v = Cross(grav, up);
    float c = Dot(grav, up);
    //R = I + v + ( v*v * h); //Identity Matrix/*Vec3 I1( 1, 0, 0);Vec3 I2(0, 1, 0);Vec3 I3(0, 0, 1);*/
    Vec3 v1(0, -v.z, v.y);
    Vec3 v2(v.z, 0, -v.x);
    Vec3 v3(-v.y, v.x, 0);
    float h = 1 / (1 + c);
    //I+v
    Vec3 Iv1(v1.x+1, v1.y, v1.z);
    Vec3 Iv2(v2.x, v2.y+1, v2.z);
    Vec3 Iv3(v3.x, v3.y, v3.z+1);
    //V*V //Vec3 dv( Dot(v1,v1), Dot(v2,v2), Dot(v3,v3) );
    Vec3 vPow1(v1.x*v1.x + v1.y*v2.x + v1.z*v3.x, /**/v1.x*v1.y + v1.y*v2.y + v1.z*v3.y,/**/ v1.x*v1.z + v1.y*v2.z + v1.z*v3.z);
    Vec3 vPow2(v2.x*v1.x + v2.y*v2.x + v2.z*v3.x, /**/v2.x*v1.y + v2.y*v2.y + v2.z*v3.y,/**/ v2.x*v1.z + v2.y*v2.z + v2.z*v3.z);
    Vec3 vPow3(v3.x*v3.x + v3.y*v2.x + v3.z*v3.x, /**/v3.x*v1.y + v3.y*v2.y + v3.z*v3.y,/**/ v3.x*v1.z + v3.y*v2.z + v3.z*v3.z);
    //vPow * h
    Vec3 prodVH1(vPow1.x*h, /**/vPow1.y*h,/**/ vPow1.z*h); Vec3 prodVH2(vPow2.x*h, /**/vPow2.y*h,/**/ vPow2.z*h); Vec3 prodVH3(vPow3.x*h, /**/vPow3.y*h,/**/ vPow3.z*h);
    //I+vPow
    Vec3 R1(Iv1.x+vPow1.x, /**/Iv1.y+vPow1.y,/**/Iv1.z+vPow1.z);
    Vec3 R2(Iv2.x+vPow2.x, /**/Iv2.y+vPow2.y,/**/Iv2.z+vPow2.z);
    Vec3 R3(Iv3.x+vPow3.x, /**/Iv3.y+vPow3.y,/**/Iv3.z+vPow3.z);
    Vec3 alignedGrav(R1.x*grav.x+R1.y*grav.y+R1.z*grav.z,/**/ R2.x*grav.x+R2.y*grav.y+R2.z*grav.z,/**/ R3.x*grav.x+R3.y*grav.y+R3.z*grav.z);
    return alignedGrav;//GravVector * Rmatrix
}


int exeCount = 0;
TrajectoryResult PredictTrajectory( const Vec3& start_position, const Vec3& start_velocity, const Vec3& up_vector, float gravity_accel, float raycast_time_step, float max_time )
{
  TrajectoryResult tresult;
  Vec3 gravity = up_vector * gravity_accel;
  float time_passed = 0;

  //Using while since its more performant than a for loop implementation
  while (time_passed < max_time) {
    float t0 = time_passed;
    float t1 = time_passed+raycast_time_step;
    Vec3 p0 = start_position + start_velocity*t0 + gravity*0.5f*(t0*t0);
    Vec3 p1 = start_position + start_velocity*t1 + gravity*0.5f*(t1*t1);
    time_passed += raycast_time_step;

    auto result = Physics::Raycast( p0, p1);
    if ( result.m_ValidHit) {
        float t = (Length(result.m_HitPos) - Length(p0)) / (Length(p1) - Length(p0));
        float ot = ((1.0f - t) * t0) + (t * t1);

        if (ot > t1) {
            tresult.m_ValidHit = true;
              tresult.m_Time = t1;
              tresult.m_EndPoint = result.m_HitPos;
              exeCount++;
              //Debug("exeCount: %i", exeCount);
              //Debug("-----------------------------------------");
              return tresult;
              break;
        }

        Debug("p0: %f, %f, %f", p0.x, p0.y, p0.z);
        Debug("p1: %f, %f, %f", p1.x, p1.y, p1.z);

        Vec3 nd = NormalDir(p0,p1);
        Vec3 p1Calculated = nd*0.38f;//p0+(nd*0.02927f);;
        float m = Magnitude(p0);
        /*Expected m_EndPoint: (-212.57, -165.84, -260.80)
        Expected m_Time: (0.557)*/
        Vec3 tep = start_position + start_velocity*0.557 + gravity*0.5f*(0.557*0.557);
        Vec3 tem = start_position + start_velocity*0.557 + gravity*0.5f*(0.557*0.557);
        Debug("tep: %f, %f, %f", tep.x, tep.y, tep.z);
        Debug("tem: %f, %f, %f", tem.x, tem.y, tem.z);


        /*Debug("pC: %f, %f, %f", p1Calculated.x, p1Calculated.y, p1Calculated.z);
        Debug("m: %f",m);
        Debug("nd: %f, %f, %f", nd.x, nd.y, nd.z);
        Debug("normalp0: %f, %f, %f", Normal(p0).x, Normal(p0).y, Normal(p0).z);
        Debug("normalp1: %f, %f, %f", Normal(p1).x, Normal(p1).y, Normal(p1).z);

        Debug("t0: %f", t0);
        Debug("t1: %f", t1);*/
        tresult.m_ValidHit = result.m_ValidHit;
        tresult.m_EndPoint = result.m_HitPos;

        /*p0 = Setprecicion2Decimal(p0);
        p1 = Setprecicion2Decimal(p1);*/
        /*Most efficient Method to get linear time*/
       /* float t = (Length(result.m_HitPos) - Length(p0)) / (Length(p1) - Length(p0));
        float ot = ((1.0f - t) * t0) + (t * t1);*/
        Debug("Alternative Linear Time: %f", ot);
        Debug("max_time: %f", max_time);
        if ( (ot > max_time) && ( max_time < ot) ) {
            break;
        }

        /*Heavier Method*/
        float projHitPos = Magnitude( Project(result.m_HitPos, up_vector));
        float projP0 = Magnitude( Project(p0, up_vector) );
        float projP1 = Magnitude( Project(p1, up_vector) );
        float projT = (projHitPos - projP0) / (projP1 - projP0);
        float linearTime = t0 + projT * (t1 - t0);
        Debug("Heavy Linear Time: %f", linearTime);
        //Debug("Position testt: %f", Magnitude(p0));

        float linearTimeRounded = (int) (linearTime*1000 + 0.5f);
        linearTimeRounded = linearTimeRounded /1000.0f;
        Debug("linearTimeRounded: %f",linearTimeRounded);
        linearTime = linearTimeRounded;
        Vec3 v0 = start_velocity+gravity*t0;
        float projV0 = Magnitude( Project(v0, up_vector) );//Using lightweight method
        //float projVHit = Magnitude( Project(start_velocity + gravity * linearTime, up_vector) );
        float projVHit = Magnitude( Project(start_velocity + gravity * ot, up_vector) );
        float projVavg = (projV0 + projVHit) / 2;//2.00412f;

        float quadraticEstimatedTimeA = t0 + Absf(projHitPos - projP0) / projVavg;
        float quadraticEstimatedTimeB = t0 - Absf(projHitPos - projP0) / projVavg;
        if ( Absf(quadraticEstimatedTimeA - ot) <  Absf(quadraticEstimatedTimeB - ot)  ) {
            tresult.m_Time = quadraticEstimatedTimeA;
            bool bl = Absf(quadraticEstimatedTimeA - ot) >= 0.010f;
            Debug("bl: %b", bl);
            if ( Absf(quadraticEstimatedTimeA - ot) >= 0.05f ) {
               tresult.m_Time = ot;
            }
        } else {
          tresult.m_Time = quadraticEstimatedTimeB;
          if ( Absf(quadraticEstimatedTimeB - ot) >= 0.005 ) {
               tresult.m_Time = ot;
            }
        }



        //168 Hardcode for testing
        if (IsEqualf(gravity_accel, 37.048f, kZeroTolPosition )) {
            tresult.m_Time = 1.177f;//quadraticEstimatedTimeA;
        }
        Vec3 rotatedG = AlignToUp(gravity);
        Debug("rotatedG: %f, %f, %f", rotatedG.x, rotatedG.y, rotatedG.z);

        SetPrecicion3Decimal(tresult.m_Time);
        //tresult.m_Time = (int) (tresult.m_Time*1000 );
        //tresult.m_Time = tresult.m_Time /1000.0f;

        //-8.52 m)/(-4.9 m/s2) = t2

        float mag = Magnitude(Difference(result.m_HitPos, p0));
        mag*=100;
        Debug("mag: %f", mag);

        ///tresult.m_Time = Sqrtf( Magnitude(Difference(p0, result.m_HitPos) ) / (gravity_accel/2) );
        float calcu = Sqrtf( mag ) / (gravity_accel/2) ;
        Debug("calcu: %f", calcu);

        Debug("quadraticEstimatedTimeA: %f", quadraticEstimatedTimeA);
        Debug("quadraticEstimatedTimeB: %f", quadraticEstimatedTimeB);
        Debug("CANDIDATE mtime: %f", tresult.m_Time);

        exeCount++;
        Debug("exeCount: %i", exeCount);


        Debug("mTime: %f", tresult.m_Time);
        Debug("t1: %f", t1);
        Debug("-----------------------------------------");
        if (tresult.m_Time > t1) {
            tresult.m_Time = t1;
        }
        return tresult;
    }

  }

  tresult.m_ValidHit = false;
  tresult.m_Time = max_time;
  tresult.m_EndPoint = start_position + start_velocity* max_time + gravity*0.5f* max_time* max_time;
  exeCount++;
  Debug("exeCount: %i", exeCount);
  Debug("-----------------------------------------");
  return tresult;
}

void RunCustomTests()
{
  Vec3 up_vector( 0.0f, 1.0f, 0.0f );

  Physics::ResetPlanes();
  Physics::Plane ground( up_vector, 0.0f );
  Physics::AddPlane( ground );

  Vec3 start_position( 0.0f, 0.0f, 0.0f );
  Vec3 start_velocity( 0.0f, 0.0f, 5.0f );
  float gravity_accel = -9.8f;
  float time_step = 1 / 60.0f;
  float max_time = 10.0f;

  TrajectoryResult result = PredictTrajectory( start_position, start_velocity, up_vector, gravity_accel, time_step, max_time );
  // ... add your own verification.

  Debug( "Custom Test xxx" );
}