#include <SolarCalculator.h>
#include <TimeLib.h>


// this class is for linear actuators in a "triangle" configuration
class InverseCinematics{
  
  public:

  InverseCinematics(){}
  
  // Geometry:
  // .-a2-.
  // |     \
  // |      c <- Motor
  // a1      \
  // |        |
  // |        b2
  // '--b1----'
  InverseCinematics(const double a1, const double a2, const double b1, const double b2){
    this->a = sqrt(a1*a1 + a2*a2);
    this->b = sqrt(b1*b1 + b2*b2);
    const double offsetAngleA = acos((a1*a1+a*a-a2*a2) / (2*a1*a));
    const double offsetAngleB = acos((b1*b1+b*b-b2*b2) / (2*b1*b));
    this->gammaOffset = offsetAngleA + offsetAngleB;
  }
  
  // calculate length of motor c for desired angle gamma to move motor
  // .
  // |\
  // a c
  // |  \
  // '-b-'
  const double operator()(const double value) const{
    const double gammaWithoutOffset = value-this->gammaOffset;
    const double c = sqrt(b*b + a*a - 2*b*a*cos(gammaWithoutOffset));
    return c;
  }

  void operator=(InverseCinematics other){
    this->a = other.a;
    this->b = other.b;
    this->gammaOffset = other.gammaOffset;
  }
  
  inline const double getA(void)const{
    return a;
  }
  inline const double getB(void)const{
    return b;
  }
  inline const double getGammaOffset(void)const{
    return gammaOffset;
  }
  
  private:
  
  double a, b;
  double gammaOffset;
};

  

class vec3{
  public:
  
    // default constructor
    vec3(){}
    
    // set values
    vec3(const double x1, const double x2, const double x3){
      set(x1,x2,x3);
    }
    
    void set(const double x1, const double x2, const double x3){
      val[0] = x1;
      val[1] = x2;
      val[2] = x3;
    }
    
    void operator=(const vec3 & other){
      val[0] = other.val[0];
      val[1] = other.val[1];
      val[2] = other.val[2];
    }

    vec3 operator+ (const vec3 & first) const{
        return vec3(val[0] + first.val[0], val[1] + first.val[1], val[2] + first.val[2]);
    }
    double operator[](const size_t index) const{
      return val[index];
    }
    double & operator[](const size_t index){
      return val[index];
    }
    
    // convert (r,theta,phi) from degree to radians
    void degToRad(void){
      val[1] *= PI / 180.0; // theta
      val[2] *= PI / 180.0; // phi
    }
    
    // (r,theta,phi) to (x,y,z)
    void toCartesianCoordinates(void){
      const double x = val[0]*sin(val[1])*cos(val[2]);
      const double y = val[0]*sin(val[1])*sin(val[2]);
      val[2] = val[0]*cos(val[1]);
      val[1] = y;
      val[0] = x;
    }

    // (x,y,z) to (r,theta,phi)
    // phi starts anticlockwise from the x axis
    // theta starts from z axis downwards
    void toSphericalCoordinates(void){
      const double r = getLengthCartesian();
      const double sqrtx2y2 = sqrt(val[0]*val[0]+val[1]*val[1]);
      const double theta = acos(val[2] / r);
      val[2] = (val[1]>=0) ? acos(val[0] / sqrtx2y2)
        : 2*PI - acos(val[0] / sqrtx2y2);
      val[1] = theta;
      val[0] = r;
    }

    const double getLengthCartesian() const{
      return sqrt(val[0]*val[0] + val[1]*val[1] + val[2]*val[2]);
    }

    // normalize cartesian vector
    void normalize(void){
      const double len = getLengthCartesian();
      val[0] /= len;
      val[1] /= len;
      val[2] /= len;
    }

    const String str(void) const{
      const String s = "("+String(val[0],3)+","+String(val[1],3)+","+String(val[2],3)+")";
      return s;
    }

  private:
    double val[3] = {0,0,0};

};



class Motor{
  
  public:

  Motor(){}
  
  // please note that the inverse kinematiks should use the same length units as the motor
  // speed is in lengthUnits per second
  Motor(const int pinContract, const int pinExpand, const double minLength, const double maxLength, const double speed, InverseCinematics inverseCinematics){
    this->pinContract       = pinContract;
    this->pinExpand         = pinExpand;
    this->minLength         = minLength;
    this->maxLength         = maxLength;
    this->speed             = speed;
    this->inverseCinematics = inverseCinematics;
    pinMode(pinContract, OUTPUT);
    digitalWrite(pinContract, HIGH);
    pinMode(pinExpand, OUTPUT);
    digitalWrite(pinExpand, HIGH);
  }

  void operator=(const Motor other){
    this->pinContract       = other.pinContract;
    this->pinExpand         = other.pinExpand;
    this->minLength         = other.minLength;
    this->maxLength         = other.maxLength;
    this->speed             = other.speed;
    this->inverseCinematics = other.inverseCinematics;
    
  }
  
  void setAngle(const double angle){
    const double targetLength = inverseCinematics(angle);
    setLength(targetLength);
  }
  
  // contract to minimal length
  void init(){
    const double maxDistance = (minLength - maxLength) * 1.1; // 10% buffer
    move(maxDistance);
  }
  
  void setLength(double targetLength){
    targetLength = max(targetLength, minLength);
    targetLength = min(targetLength, maxLength);
    const double distance = targetLength-currentLength;
    move(distance);
  }

  inline const double getMinLength(void)const{
    return minLength;
  }
  inline const double getMaxLength(void)const{
    return maxLength;
  }
  inline const double getLength(void)const{
    return currentLength;
  }
  
  private:
  
  // distance>0: expand
  // distance<0: expand
  void move(const double distance){
    const double moveTime = abs(distance) / this->speed;
    if(moveTime>minMoveTime){
      const int motorPin = (distance>0) ? this->pinExpand : this->pinContract;
      currentLength += distance;
      currentLength = max(currentLength, minLength);
      currentLength = min(currentLength, maxLength);
      Serial.println("move: "+String(moveTime)+"s Distance: "+String(distance)+"cm to: "+String(currentLength));
      digitalWrite(motorPin, LOW);
      delay(moveTime * 1000); // moveTime seconds
      digitalWrite(motorPin, HIGH);
    }else{
      Serial.println("skip move, distance too short");
      
    }
  }
  
  double currentLength;
  
  int pinExpand;
  int pinContract;
  double minLength;
  double maxLength;
  double speed;
  InverseCinematics inverseCinematics;
  double minMoveTime = 0.2; // minimum move time for the motor (in seconds), if too short, the relais won't close fast enough, too long and the drive system becomes inaccurate
};

inline void swapDouble(double &a, double &b){
  double c = a;
  a = b;
  b = c;
}


class Reflector{
  public:
  
    Reflector(){}
    
    Reflector(const Motor motorRoll, const Motor motorPitch){
      //Serial.println("Reflector()");
      setMotors(motorRoll, motorPitch);
    }
  
    void updateMotors(){
      Serial.println("updateMotors()");
      vec3 sunPosition;
      getSunCoordinates(sunPosition);
      vec3 mirrorTargetPosition = (sunPosition + this->windowPosition);
      Serial.println("SunPositionCartesian: " + sunPosition.str());
      Serial.println("WindowPosition:       " + windowPosition.str());
      Serial.println("mirrorTargetPosition: " + mirrorTargetPosition.str() + " (cartesian)");
      // x1 axis is north/south, x2 is east/west and x3 is up/down
      // rotate coordinate system 90° for inverse kinematics so x1 is down/up and x3 is south/north
      swapDouble(mirrorTargetPosition[0], mirrorTargetPosition[2]);
      mirrorTargetPosition.toSphericalCoordinates(); // (r, theta, phi)
      Serial.println("mirrorTargetPosition: " + mirrorTargetPosition.str() + " (spherical, rotated)");
      // move motors
      Serial.println("MotorRollAngle:       " + String(mirrorTargetPosition[2]*180.0/PI, 1) + "°"); // output angle is relative to zenith not the actual angle within the triangles
      Serial.println("MotorPitchAngle:      " + String((mirrorTargetPosition[1]-PI/2.0)*180.0/PI, 1) + "°");
      // add 90° because 0° means straight up which means 90° for the motors
      motorRoll .setAngle(PI/2.0 + mirrorTargetPosition[2]); // left   /right    (west /east ), add 90° because the triangle has 90 degrees then facing up
      motorPitch.setAngle(mirrorTargetPosition[1]); // forward/backward (north/south), add 90° because at 90° the mirror is facing upwards. then calculate 90°-angle because we want to rotate our coordinate system back and angle is meassured from zenitz downwards but the angle insige the triangle gets smaller then moving downwards
    }
    
    void setMotors(const Motor motorRoll, const Motor motorPitch){
      this->motorRoll = motorRoll;
      this->motorPitch = motorPitch;
      initMotors();
    }
  
    // init the motors and move to 0 position
    void initMotors(){
      this->motorRoll.init();
      this->motorPitch.init();
    }
  
    // set the window coordinates
    // @param windowCoordinates coordinates of the center of the window relative to the center of the mirror
    //        first coordinate: distance perpendicular to wall
    //        second coordinate: horizontal distance allong the wall
    //        third coordinate: vertical distance
    // @param wallOffsetAngle the offset angle of the wall relative to the rotation axis of the mirror in degree
    void setWindowCoordinates(const vec3 windowCoordinates, const double wallOffsetAngle){
      this->windowPosition = windowCoordinates;
      this->windowPosition.toSphericalCoordinates();
      this->windowPosition[2] += wallOffsetAngle * PI / 180.0;
      this->windowPosition.toCartesianCoordinates();
      this->windowPosition.normalize();
    }
    
    void setMirrorOffsetAngle(const double mirrorOffsetAngle){
      this->mirrorOffsetAngle = mirrorOffsetAngle; // in degree
    }

    void setCoordinates(const double latitude, const double longitude){
      this->latitude = latitude;
      this->longitude = longitude;
    }
    
  
  private:
  
    // get normalized sun coordinates relative to the coordinates system of the mirror in cartesian coordinates
    void getSunCoordinates(vec3 &position){
      const time_t utc = now();
      double az; // clockwise relative from north
      double el; // relative to horizon (0=horizon)
      calcHorizontalCoordinates(utc, this->latitude, this->longitude, az, el); // Calculate the solar position, in degrees
      position[0] = 1; // set length to 1 (normalized vector)
      position[1] = 90-el; // in spherical coordinates theta=0 is up and 90° is the horizon
      position[2] = fmodf(360.0+180.0-az-this->mirrorOffsetAngle,360); // in spherical coordinates x is facing in the plane (south) with 0 degree facing south
      Serial.println("Time (UTC):           " + String(utc));
      Serial.println("Sun elevation:        " + String(el) + "°");
      Serial.println("Sun azimuth:          " + String(az) + "°");
      //Serial.println("SunPositionSpherical: " + position.str());
      position.degToRad();
      //Serial.println("SunPosition:" + position.str());
      position.toCartesianCoordinates();
      //Serial.println("SunPositionCartesian:" + position.str());
    }
    
    // normalized window coordinates relative to the coordinates system of the mirror in cartesian coordinates
    vec3 windowPosition;
    
    // the offset angle of the mirror rotation axis relative to north/south in degree
    // positive value: mirror rotated clockwise, negative value: rotated anticlockwise
    double mirrorOffsetAngle;
    
    // Location
    double latitude = 0.0;
    double longitude = 0.0;
    
    Motor motorRoll;
    Motor motorPitch;
  
};


// for debugging
const void testGeometry(InverseCinematics cinematics){
  Serial.println("Length A:"+String(cinematics.getA()));
  Serial.println("Length B:"+String(cinematics.getB()));
  Serial.println("GammaOffset:"+String(cinematics.getGammaOffset()));
}


// for debugging
// run the motors from 500 to 50 ms so you can check how fast your relais react.
// usually the minimum time for the motor is 200ms to still be accurate
const void testMotorPin(const int pin){
  pinMode(pin, OUTPUT);
  digitalWrite(pin, HIGH);
  for(int interval = 500; interval-=50; interval>0){
    digitalWrite(pin, LOW);
    delay(interval);
    digitalWrite(pin, HIGH);
    delay(1000);
  }
}

// for debugging
// perform coordinate transformation and print result over serial
const void testVec3(){
  vec3 test(1,90,270); // radius, inclenation, rotation
  Serial.println(test.str());
  test.degToRad();
  Serial.println(test.str());
  test.toCartesianCoordinates(); // x, y, z
  Serial.println(test.str());
  test.toSphericalCoordinates(); // (r, theta, phi)
  Serial.println(test.str());
}

// for debugging
const void testMotor(Motor motor){
  motor.init(); // move to min position
  const double minLen = motor.getMinLength();
  const double maxLen = motor.getMaxLength();
  motor.setLength(maxLen); // move to max position
  Serial.println("maxLen:"+String(motor.getLength()));
  delay(1000);
  motor.setLength(minLen); // move to min position
  Serial.println("minLen:"+String(motor.getLength()));
  delay(1000);
  motor.setAngle(90.0*PI/180.0); // set to right angle (mirror facing straigt up)
  Serial.println("90°Len:"+String(motor.getLength()));
}

// for debugging
const time_t toUtc(time_t local, const int utcOffset){
  return local - utcOffset * 3600L;
}
// Code from JChristensen/Timezone Clock example
const time_t compileTime(){
  const uint8_t COMPILE_TIME_DELAY = 8;
  const char *compDate = __DATE__, *compTime = __TIME__, *months = "JanFebMarAprMayJunJulAugSepOctNovDec";
  char chMon[4], *m;
  tmElements_t tm;

  strncpy(chMon, compDate, 3);
  chMon[3] = '\0';
  m = strstr(months, chMon);
  tm.Month = ((m - months) / 3 + 1);

  tm.Day = atoi(compDate + 4);
  tm.Year = atoi(compDate + 7) - 1970;
  tm.Hour = atoi(compTime);
  tm.Minute = atoi(compTime + 3);
  tm.Second = atoi(compTime + 6);
  const time_t t = makeTime(tm);
  return t + COMPILE_TIME_DELAY;
}