#include <time.h>
#include <TM1637Display.h>

#define VRX_PIN  A0 // Arduino pin connected to VRX pin
#define VRY_PIN  A1 // Arduino pin connected to VRY pin
#define THRUST_PIN A2

#define STARTINGPOS 5000

#define SPEED_MULTIPLIER 0.015
#define SHIP_SPEED 1
#define SPEED_SMOOTHNESS 5
#define COORDINATES_MULTIPLIER 0.00002


#define CLKDEPTH 11
#define DIODEPTH 10

#define CLKX 8
#define DIOX 9

#define CLKY 6
#define DIOY 7



struct Vector3
{
  float x;
  float y;
  float z;
}typedef Vector3;

struct Ship
{
  Vector3 pos;
  Vector3 targetVelocity;
  Vector3 velocity;

};

struct Ship myShip;

int xValue = 0; // To store value of the X axis
int yValue = 0; // To store value of the Y axis

TM1637Display DepthDisplay(CLKDEPTH, DIODEPTH);
TM1637Display XDisplay(CLKX, DIOX);
TM1637Display YDisplay(CLKY, DIOY);

char buf[5];

/* SURFACE RANDOMS */
float AngleA1;
float AngleA2;

float AngleB1;
float AngleB2;

float AngleC1;
float AngleC2;

float ScaleA1;
float ScaleA2;

float ScaleB1;
float ScaleB2;

float ScaleC1;
float ScaleC2;

float AngleD1;
float AngleD2;

float AngleE1;
float AngleE2;


float ScaleD1;
float ScaleD2;

float ScaleE1;
float ScaleE2;

long seed;



void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  myShip.pos = {STARTINGPOS, STARTINGPOS, 90};
  myShip.targetVelocity = {0,0,0};
  myShip.velocity = {0,0,0};

  randomSeed(analogRead(0));
  pinMode(THRUST_PIN, INPUT);

  DepthDisplay.setBrightness(0x0a);
  XDisplay.setBrightness(0x0a);
  YDisplay.setBrightness(0x0a);



  /* RANDOMS */
  AngleA1 = ( random() % (360*10) )/10;
  AngleA2 = ( random() % (360*10) )/10;
  AngleB1 = ( random() % (360*10) )/10;
  AngleB2 = ( random() % (360*10) )/10;
  AngleC1 = ( random() % (360*10) )/10;
  AngleC2 = ( random() % (360*10) )/10;

  AngleD1 = ( random() % (360*10) )/10;
  AngleD2 = ( random() % (360*10) )/10;
  AngleE1 = ( random() % (360*10) )/10;
  AngleE2 = ( random() % (360*10) )/10;

  ScaleA1 = ( random() % 25 + 25 ) * COORDINATES_MULTIPLIER;
  ScaleA2 = ( random() % 25 + 25 ) * COORDINATES_MULTIPLIER;
  ScaleB1 = ( random() % 25 + 25 ) * COORDINATES_MULTIPLIER;
  ScaleB2 = ( random() % 25 + 25 ) * COORDINATES_MULTIPLIER;
  ScaleC1 = ( random() % 25 + 25 ) * COORDINATES_MULTIPLIER;
  ScaleC2 = ( random() % 25 + 25 ) * COORDINATES_MULTIPLIER;

  ScaleD1 = ( random() % 200 + 200 ) * COORDINATES_MULTIPLIER;
  ScaleD2 = ( random() % 200 + 200 ) * COORDINATES_MULTIPLIER;
  ScaleE1 = ( random() % 200 + 200 ) * COORDINATES_MULTIPLIER;
  ScaleE2 = ( random() % 200 + 200 ) * COORDINATES_MULTIPLIER;


}

float lerp(float a, float b, float f)
{
    return a + f * (b - a);
}

float GetHeightValue(float x, float y, float Angle1, float Angle2, float Scale1, float Scale2, float Scale, float offsetX, float offsetY)
{
  return sin((x * cos(Angle1) * Scale1 - y * sin(Angle2) * Scale2) + offsetX) * cos( (x * sin(Angle1) * Scale1 - y * cos(Angle2) * Scale2 ) + offsetY) * Scale;
}

float GetHeight(float x, float y)
{
  /* OVEALL */

  float Height = GetHeightValue(x,y,AngleA1, AngleA2, ScaleA1, ScaleA2, 50, 2, 6);
  Height += GetHeightValue(x,y,AngleB1, AngleB2, ScaleB1, ScaleB2, 35, 124, 76);
  Height += GetHeightValue(x,y,AngleC1, AngleC2, ScaleC1, ScaleC2, 20, 12, 1);

  /* DETAILS */
  Height += GetHeightValue(x,y,AngleD1, AngleD2, ScaleD1, ScaleD2, 7, 0, 7);
  Height += GetHeightValue(x,y,AngleE1, AngleE2, ScaleE1, ScaleE2, 9, 0, 7);
  Height += GetHeightValue(x,y,AngleB1, AngleD2, 1200 * COORDINATES_MULTIPLIER, 6040 * COORDINATES_MULTIPLIER, 3, 7, 214);
  Height += GetHeightValue(x,y,AngleC1, AngleE2, 870 * COORDINATES_MULTIPLIER, 1230 * COORDINATES_MULTIPLIER, 2, 123, 34);
  Height += GetHeightValue(x,y,AngleE1, AngleA2, 450 * COORDINATES_MULTIPLIER, 4650 * COORDINATES_MULTIPLIER, 5, 96, 1);




  return(Height);
}

void loop() {
  // put your main code here, to run repeatedly:
  
  //float Height = GetHeight(myShip.pos.x, myShip.pos.y);
  //Serial.println(Height);

  xValue = (analogRead(VRX_PIN)) - 512;
  yValue = (analogRead(VRY_PIN)) - 512;
  float thrustInput = (analogRead(THRUST_PIN) / 1024.0)*3;

  float Height = GetHeight(myShip.pos.x, myShip.pos.y);


  myShip.targetVelocity.x = xValue/512.0 * SHIP_SPEED * SPEED_MULTIPLIER * thrustInput;
  myShip.targetVelocity.y = yValue/512.0 * SHIP_SPEED * SPEED_MULTIPLIER * thrustInput;

  MoveShip();

  Height = myShip.pos.z - Height;



  XDisplay.showNumberDecEx((int)(myShip.pos.x), 0, true, 4, 0);
  YDisplay.showNumberDecEx((int)(myShip.pos.y), 0, true, 4, 0);
  DepthDisplay.showNumberDecEx((int)(Height*10), 0b00100000, true, 4, 0);

  

  delay(1/60.0);

  
  
}

void MoveShip()
{
  myShip.velocity.x = lerp(myShip.velocity.x, myShip.targetVelocity.x, (1.0/60) * SPEED_SMOOTHNESS);
  myShip.velocity.y = lerp(myShip.velocity.y, myShip.targetVelocity.y, (1.0/60) * SPEED_SMOOTHNESS);

  //myShip.velocity.x = myShip.targetVelocity.x;

  myShip.pos.x += myShip.velocity.x;
  myShip.pos.y += myShip.velocity.y;
}