#include <iostream>
#include "ServoJoint.h"

int main() {
    //2D array containing the 7 calibrated positions for each of the 10 servo motors
    size_t servoCalibrations[10][7]={{143, 190, 245, 305, 365, 410, 467},
                                     {143, 205, 260, 320, 377, 423, 475},
                                     {115, 168, 230, 280, 340, 392, 440},
                                     {130, 182, 235, 290, 340, 397, 455},
                                     {135, 190, 235, 290, 345, 400, 455},
                                     {140, 190, 250, 305, 355, 415, 465},
                                     {130, 185, 240, 300, 355, 415, 465},
                                     {140, 200, 245, 300, 355, 410, 460},
                                     {140, 200, 245, 300, 355, 410, 460},
                                     {140, 200, 245, 300, 355, 410, 460}};

    //Array of pointers to 10 size_t arrays of size 7
    //This is because C++ doesn't allow accessing an entire row/column of a 2D array
    //or I don't know how to
    size_t * calibrationArrays[10];
    for(size_t i=0; i<10; i++){
        size_t * tmp= new size_t[7];
        for(size_t j=0; j<7; j++){
            tmp[j]= servoCalibrations[i][j];
        }
        calibrationArrays[i]= tmp;
    }

    //Array of pointers to 10 ServoJoint objects
    ServoJoint * servos[10];

    //Dynamically allocate 10 ServoJoint objects
    for(size_t i=0; i<10; i++){
        servos[i]= new ServoJoint(i, 7, calibrationArrays[i]);
    }

    for(size_t i=0; i<10; i++){
        std::cout << "Servo motor " << i << " has the following PWM values: ";
        for(size_t j=0; j<7; j++){
            std::cout << servos[i]->getPWM(-90+j*30) << " ";
        }
        std::cout<< std::endl;
    };

    return 0;
}

#ifndef QUADRUPEDROBOT_SERVOJOINT_H
#define QUADRUPEDROBOT_SERVOJOINT_H

#include <cstdint>
#include <map>
//#include "PWMDriver.h"

class ServoJoint{

private:

    //servo motor channel number on the PWM/Servo driver; [0, 15]
    size_t mChannel;

    //number of calibrated positions
    size_t mSize;

    //pointer to a map of the servo motor's angular position with its corresponding pulse width value
    std::map<int, size_t>*  mPWM;

public:

    //size indicates the number of calibrated positions in [-90 deg, 90deg];
    //calibrated positions must be at equal intervals. i.e. -90, -45, -30, ... is not valid
    ServoJoint(size_t givenChannel, size_t size, size_t givenPWM[]);

};

#endif //QUADRUPEDROBOT_SERVOJOINT_H

undefined reference to `ServoJoint::ServoJoint(unsigned long long, unsigned long long, unsigned long long*)'
undefined reference to `ServoJoint::getPWM(double)'