#pragma once
#include "motor.hpp"
#include <math.h>

class Robot {
    private: float size;
    private: Motor arm1;
    private: Motor arm2;
    private: Motor yaw;

    public: Robot(float size,
        const Motor& yaw, const Motor& arm1, const Motor& arm2
    ) : size(size), arm1(arm1), arm2(arm2), yaw(yaw) {}

    public: bool pose(int tYaw, int tArm1, int tArm2) {
        /* Calculate the absolute distance to target */
        const int d1 = abs(tArm1 - arm1.angle());
        const int d2 = abs(tArm2 - arm2.angle());
        const int d3 = abs(tYaw - yaw.angle());

        /* Rotate with according speeds */
        const int m = max(max(d1, d2), d3);
        arm1.update(tArm1, 4 * (m / d1));
        arm2.update(tArm2, 4 * (m / d2));
        yaw.update(tYaw, 4 * (m / d3));

        /* Is destination reached */
        return arm1.angle() == tArm1
        && arm2.angle() == tArm2
        && yaw.angle() == tYaw;
    }

    public: bool point(float x, float y) {
        /* Calculate pythagorean theorem */
        const float dist = sqrtf(x * x + y * y);
        const int tYaw = (int) degrees(atan2f(x, y));
        float r = (dist / 2.0f) / size;
        r = constrain(r, -1.0f, 1.0f);

        /* Calculate angles and rotate */
        const float a1 = degrees(acosf(r));
        const float a2 = (90.0f - a1) * 2.0f;
        const int tArm1 = 90 - (int) a1;
        const int tArm2 = (int) a2 - 90;
        return pose(tYaw, tArm1, tArm2);
    }

    public: bool reset() {
        /* Rotate to origin */
        return pose(0, 0, 0);
    }
};