#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <utility>
#include <limits>
#include <stdexcept>
#include <vector>
#include  <iostream>

struct Vec3 { double x, y, z; };
using Formation = std::vector<Vec3>;
constexpr double pi = 3.14159265358979323846;
Vec3 add(Vec3 a, Vec3 b) {
return {a.x + b.x, a.y + b.y, a.z + b.z};
}
Vec3 sub(Vec3 a, Vec3 b) {
return {a.x - b.x, a.y - b.y, a.z - b.z};
}
Vec3 scale(Vec3 v, double k) {
return {v.x * k, v.y * k, v.z * k};
}
double dot(Vec3 a, Vec3 b) {
return a.x*b.x + a.y*b.y + a.z*b.z;
}
double length(Vec3 v) { return std::sqrt(dot(v,v)); }

bool near(Vec3 a, Vec3 b) {
return length(sub(a,b)) < 1e-8;
}

Formation square(Vec3 a, int l){

    if(l<0) throw std::invalid_argument("Il lato non può essere negativo!");
    
    Formation vertici(4); 

    for(int i=0; i<4; ++i){

        double rad = (((90*i)+225)*pi)/180;

        vertici[i].x = a.x + ((l/sqrt(2))*cos(rad));
        vertici[i].y = a.y + ((l/sqrt(2))*sin(rad));
        vertici[i].z = a.z; 
    }

    return {vertici};}

Vec3 rotazione(Vec3 a, Vec3 b, double angolo){

    double rad = (angolo*pi)/180;
    Vec3 dist = sub(a, b);

    Vec3 new_pos;
   
    new_pos.x = b.x + (dist.x*cos(rad)) - (dist.y*sin(rad));
    new_pos.y = b.y + (dist.x*sin(rad)) + (dist.y*cos(rad));
    new_pos.z = a.z; 

    return {new_pos};}

Vec3 lerp(Vec3 a, Vec3 b, double u){
    Vec3 pos = scale(sub(b,a),u);
    pos = add(a,pos); 
    return{pos};}

int main() {

    Vec3 punto{12,5,3}; 
    Vec3 centro{10,5,3}; 
    double deg = 90; 
    int lato = 4;

    Vec3 pos = rotazione(punto, centro, deg);

    std::cout << "(" << pos.x << ", "
                     << pos.y << ", "
                     << pos.z << ")\n";

    Formation quadrato = square(centro, lato);

    for(const Vec3& v:quadrato)
        std::cout << "(" << v.x << ", "
                     << v.y << ", "
                     << v.z << ")\n";

    Formation new_square(4);
    for (int i = 0; i < 4; ++i){
        new_square[i] = rotazione(quadrato[i], centro, deg);
        std::cout << "Il vertice " << (i+1)<< " del nuovo quadrato ha coordinate (" 
                    << new_square[i].x << ", "
                    << new_square[i].y << ", "
                    << new_square[i].z << ")\n";
        
    }

    Vec3 metaArco = rotazione(punto, centro, 45.0);
    Vec3 metaSegmento = lerp(punto, pos, 0.5);

    std::cout << "(" << metaArco.x << ", "
                 << metaArco.y << ", "
                 << metaArco.z << ")\n";

    std::cout << "(" << metaSegmento.x << ", "
                 << metaSegmento.y << ", "
                 << metaSegmento.z << ")\n";
    
    assert(near(metaSegmento, {11.0, 6.0, 3.0}));
    assert(near(metaArco, {10.0 + std::sqrt(2.0), 5.0 + std::sqrt(2.0), 3.0}));

}