#include "Propulsion.h"
#include "ControlledMotor.h"

#include "FloatSingle.h"

#include <Arduino.h>

Propulsion::Propulsion() {
  this->placement = nullptr;
  this->motorCount = 0;
}

void Propulsion::AddMotors(Placement* things, unsigned int thingCount) {
  this->motorCount = 0;
  for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
    Thing* thing = things[thingIx].thing;
    if (thing->type == Thing::Type::Motor || thing->type == Thing::Type::ControlledMotor)
      motorCount++;
    if (thing->type == Thing::Type::ControlledMotor)
      hasOdometer = true;
  }
  this->placement = new Placement[motorCount];

  unsigned int motorIx = 0;
  for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
    Thing* thing = things[thingIx].thing;
    if (thing->type == Thing::Type::Motor || thing->type == Thing::Type::ControlledMotor)
      this->placement[motorIx++] = things[thingIx];
  }
}

void Propulsion::AddQuadcopter(Quadcopter* quadcopter) {
  this->quadcopter = quadcopter;
}

unsigned int Propulsion::GetMotorCount() {
  return this->motorCount;
}

Motor* Propulsion::GetMotor(unsigned int motorId) {
  if (motorId >= this->motorCount)
    return nullptr;

  Thing* thing = this->placement[motorId].thing;
  // if (thing->isMotor)
  if (thing->type == Thing::Type::Motor)
    return (Motor*)thing;

  return nullptr;
}

void Propulsion::Update(float currentTimeMs) {
  // time_t currentTime = time(NULL);
  float timeStep = currentTimeMs - this->lastUpdateTime;  // difftime(currentTime, this->lastUpdateTime);

  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    Thing* thing = placement[motorIx].thing;
    if (thing->type == Thing::Type::ControlledMotor) {
      ControlledMotor* motor = (ControlledMotor*)thing;
      motor->Update(currentTimeMs);
    }
  }
  this->lastUpdateTime = currentTimeMs;
}

void Propulsion::SetDiffDriveSpeed(float leftSpeed, float rightSpeed) {
  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    Thing* thing = placement[motorIx].thing;
    if (thing->type == Thing::Type::Motor) {
      Motor* motor = (Motor*)placement[motorIx].thing;
      if (motor == nullptr)
        continue;

      float xPosition = placement[motorIx].position.x;
      if (xPosition < 0)
        motor->SetSpeed(leftSpeed);
      else if (xPosition > 0)
        motor->SetSpeed(rightSpeed);

      Serial.printf("motor %d, speed = %f\n", motorIx, motor->GetSpeed());
    } else if (thing->type == Thing::Type::ControlledMotor) {
      ControlledMotor* motor = (ControlledMotor*)placement[motorIx].thing;
      if (motor == nullptr)
        continue;

      float xPosition = placement[motorIx].position.x;
      if (xPosition < 0)
        motor->SetTargetSpeed(leftSpeed);
      else if (xPosition > 0)
        motor->SetTargetSpeed(rightSpeed);

      Serial.printf("controlled motor %d, speed = %f\n", motorIx, motor->GetActualSpeed());
    }
  };
}

void Propulsion::SetDiffDriveVelocities(float leftVelocity,
                                        float rightVelocity) {
  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    // Placement placement = placement[motorIx];
    // if (placement.position.x < 0)
    //   placement.controlledMotor->SetTargetVelocity(leftVelocity);
    // else if (placement.position.x > 0)
    //   placement.controlledMotor->SetTargetVelocity(rightVelocity);
  };
}

void Propulsion::SetTwistSpeed(float forward, float yaw) {
  // This is configuration dependent, a drone will do something completely
  // different...
  float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
  float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
  SetDiffDriveSpeed(leftSpeed, rightSpeed);
}

void Propulsion::SetTwistSpeed(float forward, float yaw, float pitch) {
  float leftSpeed = Float::Clamp(forward - yaw, -1, 1);
  float rightSpeed = Float::Clamp(forward + yaw, -1, 1);
  SetDiffDriveSpeed(leftSpeed, rightSpeed);

  if (quadcopter != nullptr) {
    quadcopter->SetTwistSpeed(forward, yaw, pitch);
  }
}

void Propulsion::SetTwistSpeed(Vector3 linear, float yaw) {
  if (quadcopter != nullptr)
    quadcopter->SetTwistSpeed(linear, yaw);
  else
    SetTwistSpeed(linear.z, yaw);
}

void Propulsion::SetTwistVelocity(float forwardVelocity,
                                  float turningVelocity) {
  float leftVelocity = Float::Clamp(forwardVelocity - turningVelocity, -1, 1);
  float rightVelocity = Float::Clamp(forwardVelocity + turningVelocity, -1, 1);
  SetDiffDriveVelocities(leftVelocity, rightVelocity);
}

void Propulsion::SetLinearSpeed(Vector3 velocity,
                                float yawSpeed,
                                float rollSpeed) {
  if (quadcopter != nullptr)
    quadcopter->LinearMotion(velocity, yawSpeed, rollSpeed);
}

Quadcopter* Propulsion::GetQuadcopter() {
  return quadcopter;
}

float Propulsion::GetOdometer() {
  float odometer = 0;
  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
    Thing* thing = placement[motorIx].thing;
    if (thing->type == Thing::Type::ControlledMotor) {
      ControlledMotor* motor = (ControlledMotor*)thing;
      odometer += motor->encoder->GetDistance() / this->motorCount;
    }
  }
  return odometer;
}

bool Propulsion::Drive(Vector3 point, float rotation, float currentTimeMs) {
  if (!this->driving) {
    this->startTime = time(NULL);
    this->targetDistance = point.magnitude();
    if (hasOdometer)
      this->startOdometer = GetOdometer();
    this->driving = true;
  }
  if (hasOdometer) {
    float distance = GetOdometer() - this->startOdometer;
    Serial.printf("Odometer = %f\n", distance);
    if (distance >= this->targetDistance) {
      this->driving = false;
      return true;
    }
  } else {
    double duration = difftime(time(NULL), this->startTime);
    if (duration >= this->targetDistance) {
      this->driving = false;
      return true;
    }
  }

  if (rotation > 0)
    rotation = 1;
  if (rotation < 0)
    rotation = -1;
  SetTwistSpeed(point.normalized(), rotation);

  Update(currentTimeMs);

  return false;
}